diff --git a/.github/workflows/conda/environment_macos_linux.yml b/.github/workflows/conda/environment_macos_linux.yml
index 18191fd5a65009f876d21e30abf18d6b4c5b683b..34a295bada4c29d92a28ae300426f8bda89db2f1 100644
--- a/.github/workflows/conda/environment_macos_linux.yml
+++ b/.github/workflows/conda/environment_macos_linux.yml
@@ -1,4 +1,4 @@
-name: fcl
+name: coal
 channels:
   - conda-forge
   - nodefaults
@@ -10,7 +10,7 @@ dependencies:
   - boost
   - eigenpy
   - python
-  - doxygen
+  - doxygen<1.9.8|>=1.11
   - lxml
   - pylatexenc
   - qhull
diff --git a/.github/workflows/conda/environment_windows.yml b/.github/workflows/conda/environment_windows.yml
index 8c6fccaede6be219700d402eb53df1d3cb08adea..febe634ee46b330f149b8b20dc7c6ab321944383 100644
--- a/.github/workflows/conda/environment_windows.yml
+++ b/.github/workflows/conda/environment_windows.yml
@@ -1,4 +1,4 @@
-name: fcl
+name: coal
 channels:
   - conda-forge
   - nodefaults
@@ -10,7 +10,7 @@ dependencies:
   - boost
   - eigenpy
   - python
-  - doxygen
+  - doxygen<1.9.8|>=1.11
   - lxml
   - pylatexenc
   - qhull
diff --git a/.github/workflows/macos-linux-conda.yml b/.github/workflows/macos-linux-conda.yml
index a875f175c46f8b1a56554f24efa45f5c74d29a58..913ed513b03343f08ed6addf74ed8ce62eb6fba4 100644
--- a/.github/workflows/macos-linux-conda.yml
+++ b/.github/workflows/macos-linux-conda.yml
@@ -1,9 +1,9 @@
-name: Build hpp-fcl for Mac OS X/Linux via Conda
+name: Build Coal for Mac OS X/Linux via Conda
 
 on: [push,pull_request]
 
 jobs:
-  hpp-fcl-conda:
+  coal-conda:
     name: CI on ${{ matrix.os }} with Conda Python ${{ matrix.python-version }} - ${{ matrix.build_type }} ${{ matrix.cxx_options }}
     runs-on: ${{ matrix.os }}
     env:
@@ -40,13 +40,13 @@ jobs:
 
     - uses: conda-incubator/setup-miniconda@v3
       with:
-        activate-environment: fcl
+        activate-environment: coal
         auto-update-conda: true
         environment-file: .github/workflows/conda/environment_macos_linux.yml
         python-version: ${{ matrix.python-version }}
         auto-activate-base: false
 
-    - name: Build hpp-fcl
+    - name: Build Coal
       shell: bash -el {0}
       run: |
         conda list
@@ -65,14 +65,14 @@ jobs:
           -DCMAKE_CXX_FLAGS=${{ matrix.cxx_options }} \
           -DPYTHON_EXECUTABLE=$(which python3) \
           -DGENERATE_PYTHON_STUBS=ON \
-          -DHPP_FCL_HAS_QHULL=ON \
+          -DCOAL_HAS_QHULL=ON \
           -DBUILD_DOCUMENTATION=ON \
           -DINSTALL_DOCUMENTATION=ON
         cmake --build . -j2
         ctest --output-on-failure
         cmake --install .
 
-    - name: Uninstall hpp-fcl
+    - name: Uninstall Coal
       shell: bash -el {0}
       run: |
         cd build
@@ -83,7 +83,7 @@ jobs:
     name: check-macos-linux-conda
 
     needs:
-    - hpp-fcl-conda
+    - coal-conda
 
     runs-on: Ubuntu-latest
 
diff --git a/.github/workflows/macos-linux-pip.yml b/.github/workflows/macos-linux-pip.yml
index 2ced0264f7986e68860e64808050071ca8f650bf..156a86c1ed2c2de40e0163386cbd0d4ebff6343b 100644
--- a/.github/workflows/macos-linux-pip.yml
+++ b/.github/workflows/macos-linux-pip.yml
@@ -1,4 +1,4 @@
-name: Build hpp-fcl for Mac OS X/Linux via pip
+name: Build Coal for Mac OS X/Linux via pip
 
 on: [push, pull_request]
 
@@ -7,7 +7,7 @@ env:
     CTEST_PARALLEL_LEVEL: 4
 
 jobs:
-  hpp-fcl-pip:
+  coal-pip:
     name: "CI on ${{ matrix.os }} / py ${{ matrix.python-version }} with pip"
     runs-on: "${{ matrix.os }}-latest"
 
@@ -29,6 +29,6 @@ jobs:
       - run: python -m pip install -U pip
       - run: python -m pip install cmeel-assimp cmeel-octomap cmeel-qhull eigenpy[build]
       - run: echo "CMAKE_PREFIX_PATH=$(cmeel cmake)" >> $GITHUB_ENV
-      - run: cmake -B build -S . -DHPP_FCL_HAS_QHULL=ON
+      - run: cmake -B build -S . -DCOAL_HAS_QHULL=ON
       - run: cmake --build build -j 4
       - run: cmake --build build -t test
diff --git a/.github/workflows/windows-conda-clang.yml b/.github/workflows/windows-conda-clang.yml
index 33fc913ca4889d6ff0379910054ed584a11a3e0c..7d7a8d87a3f533feaca43172efb296953d287f80 100644
--- a/.github/workflows/windows-conda-clang.yml
+++ b/.github/workflows/windows-conda-clang.yml
@@ -1,4 +1,4 @@
-name: Build FCL for Windows (CLANG) via Conda
+name: Build Coal for Windows (CLANG) via Conda
 on: [push,pull_request]
 
 jobs:
@@ -33,13 +33,13 @@ jobs:
 
     - uses: conda-incubator/setup-miniconda@v3
       with:
-        activate-environment: fcl
+        activate-environment: coal
         auto-update-conda: true
         environment-file: .github/workflows/conda/environment_windows.yml
         python-version: "3.10"
         auto-activate-base: false
 
-    - name: Build FCL
+    - name: Build Coal
       shell: cmd /C CALL {0}
       run: |
         call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64
@@ -59,7 +59,7 @@ jobs:
           -DGENERATE_PYTHON_STUBS=ON ^
           -DPYTHON_SITELIB=%CONDA_PREFIX%\Lib\site-packages ^
           -DPYTHON_EXECUTABLE=%CONDA_PREFIX%\python.exe ^
-          -DHPP_FCL_HAS_QHULL=ON ^
+          -DCOAL_HAS_QHULL=ON ^
           -DBUILD_PYTHON_INTERFACE=ON ^
           ..
         if errorlevel 1 exit 1
@@ -74,7 +74,7 @@ jobs:
 
         :: Test Python import
         cd ..
-        python -c "import hppfcl"
+        python -c "import coal"
         if errorlevel 1 exit 1
 
   check:
diff --git a/.github/workflows/windows-conda-v142.yml b/.github/workflows/windows-conda-v142.yml
index e09444ba3bcec0831fc33f9626957bd95c90a1cc..e1191d014e69b187f0c709c8ce441e830b8bbb28 100644
--- a/.github/workflows/windows-conda-v142.yml
+++ b/.github/workflows/windows-conda-v142.yml
@@ -1,4 +1,4 @@
-name: Build FCL for Windows (v142) via Conda
+name: Build Coal for Windows (v142) via Conda
 on: [push,pull_request]
 
 jobs:
@@ -32,13 +32,13 @@ jobs:
 
     - uses: conda-incubator/setup-miniconda@v3
       with:
-        activate-environment: fcl
+        activate-environment: coal
         auto-update-conda: true
         environment-file: .github/workflows/conda/environment_windows.yml
         python-version: "3.10"
         auto-activate-base: false
 
-    - name: Build FCL
+    - name: Build Coal
       shell: cmd /C CALL {0}
       run: |
         call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64
@@ -56,7 +56,7 @@ jobs:
           -DGENERATE_PYTHON_STUBS=ON ^
           -DPYTHON_SITELIB=%CONDA_PREFIX%\Lib\site-packages ^
           -DPYTHON_EXECUTABLE=%CONDA_PREFIX%\python.exe ^
-          -DHPP_FCL_HAS_QHULL=ON ^
+          -DCOAL_HAS_QHULL=ON ^
           -DBUILD_PYTHON_INTERFACE=ON ^
           ..
         if errorlevel 1 exit 1
@@ -71,7 +71,7 @@ jobs:
 
         :: Test Python import
         cd ..
-        python -c "import hppfcl"
+        python -c "import coal"
         if errorlevel 1 exit 1
 
   check:
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 1f360380180c048c86afdab23f6a789c0309f719..990ca9d7be36b71380cf91ec3dfe3b9e0568d1c3 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -7,6 +7,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 ## [Unreleased]
 
 ### Added
+- Renaming the library from `hpp-fcl` to `coal`. Created a `COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL` CMake option for retro compatibility. This allows to still do `find_package(hpp-fcl)` and `#include <hpp/fcl/...>` in C++ and it allows to still do `import hppfcl` in python ([#596](https://github.com/humanoid-path-planner/hpp-fcl/pull/596)).
 - Added `Transform3f::Random` and `Transform3f::setRandom` ([#584](https://github.com/humanoid-path-planner/hpp-fcl/pull/584))
 - New feature: computation of contact surfaces for any pair of primitive shapes (triangle, sphere, ellipsoid, plane, halfspace, cone, capsule, cylinder, convex) ([#574](https://github.com/humanoid-path-planner/hpp-fcl/pull/574)).
 - Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/humanoid-path-planner/hpp-fcl/pull/570))
@@ -22,8 +23,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Optimize EPA: ignore useless faces in EPA's polytope; warm-start support computation for `Convex`; fix edge-cases witness points computation.
 - Add `Serializable` trait to transform, collision data, collision geometries, bounding volumes, bvh models, hfields. Collision problems can now be serialized from C++ and sent to python and vice versa.
 - CMake: allow use of installed jrl-cmakemodules ([#564](https://github.com/humanoid-path-planner/hpp-fcl/pull/564))
+- CMake: Add compatibility with jrl-cmakemodules workspace ([#610](https://github.com/humanoid-path-planner/hpp-fcl/pull/610))
 - Python: add id() support for geometries ([#618](https://github.com/humanoid-path-planner/hpp-fcl/pull/618)).
 
+
 ### Fixed
 
 - Fix Fix serialization unit test when running without Qhull support ([#611](https://github.com/humanoid-path-planner/hpp-fcl/pull/611))
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7d5031b66f78481a8eba152d452dbb63e1efcc2b..72f69ead47ff6202615f642ee9e91ee975c43c27 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -35,12 +35,16 @@
 cmake_minimum_required(VERSION 3.10)
 set(CXX_DISABLE_WERROR TRUE)
 
-set(PROJECT_NAME hpp-fcl)
+set(PROJECT_NAME coal)
 set(PROJECT_DESCRIPTION
-  "HPP fork of FCL -- The Flexible Collision Library"
+  "Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library"
   )
 SET(PROJECT_USE_CMAKE_EXPORT TRUE)
 SET(PROJECT_COMPATIBILITY_VERSION AnyNewerVersion)
+# To enable jrl-cmakemodules compatibility with workspace we must define the two
+# following lines
+set(PROJECT_AUTO_RUN_FINALIZE FALSE)
+set(PROJECT_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR})
 
 SET(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE)
 SET(DOXYGEN_USE_TEMPLATE_CSS TRUE)
@@ -50,8 +54,9 @@ SET(DOXYGEN_USE_TEMPLATE_CSS TRUE)
 # Need to be set before including base.cmake
 # ----------------------------------------------------
 option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF)
-option(HPP_FCL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical HPP-FCL asserts to exception." FALSE)
-option(HPP_FCL_ENABLE_LOGGING "Activate logging for warnings or error messages. Turned on by default in Debug." FALSE)
+option(COAL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical Coal asserts to exception." FALSE)
+option(COAL_ENABLE_LOGGING "Activate logging for warnings or error messages. Turned on by default in Debug." FALSE)
+option(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL "Make Coal retro-compatible with HPP-FCL." FALSE)
 
 # Check if the submodule cmake have been initialized
 set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake")
@@ -85,6 +90,7 @@ else()
 endif()
 
 include("${JRL_CMAKE_MODULES}/boost.cmake")
+include("${JRL_CMAKE_MODULES}/python.cmake")
 include("${JRL_CMAKE_MODULES}/hpp.cmake")
 include("${JRL_CMAKE_MODULES}/apple.cmake")
 include("${JRL_CMAKE_MODULES}/ide.cmake")
@@ -121,14 +127,16 @@ CMAKE_DEPENDENT_OPTION(GENERATE_PYTHON_STUBS "Generate the Python stubs associat
 ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.0")
 
 if(BUILD_PYTHON_INTERFACE)
-  FIND_PACKAGE(eigenpy 2.9.2 REQUIRED)
+  SET(PYTHON_COMPONENTS Interpreter Development NumPy)
+  FINDPYTHON(REQUIRED)
+  ADD_PROJECT_PRIVATE_DEPENDENCY(eigenpy 2.9.2 REQUIRED)
 endif()
 
 # Required dependencies
 SET_BOOST_DEFAULT_OPTIONS()
 EXPORT_BOOST_DEFAULT_OPTIONS()
 ADD_PROJECT_DEPENDENCY(Boost REQUIRED chrono thread date_time serialization filesystem)
-if (HPP_FCL_ENABLE_LOGGING)
+if (COAL_ENABLE_LOGGING)
   ADD_PROJECT_DEPENDENCY(Boost REQUIRED log)
 endif()
 if(BUILD_PYTHON_INTERFACE)
@@ -146,31 +154,31 @@ endif()
 # Optional dependencies
 ADD_PROJECT_DEPENDENCY(octomap PKG_CONFIG_REQUIRES "octomap >= 1.6")
 if(octomap_FOUND)
-  SET(HPP_FCL_HAS_OCTOMAP TRUE)
+  SET(COAL_HAS_OCTOMAP TRUE)
   string(REPLACE "." ";" VERSION_LIST ${octomap_VERSION})
   list(GET VERSION_LIST 0 OCTOMAP_MAJOR_VERSION)
   list(GET VERSION_LIST 1 OCTOMAP_MINOR_VERSION)
   list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION)
-  message(STATUS "HPP-FCL uses Octomap")
+  message(STATUS "COAL uses Octomap")
 else()
-  SET(HPP_FCL_HAS_OCTOMAP FALSE)
-  message(STATUS "HPP-FCL does not use Octomap")
+  SET(COAL_HAS_OCTOMAP FALSE)
+  message(STATUS "COAL does not use Octomap")
 endif()
 
-option(HPP_FCL_HAS_QHULL "use qhull library to compute convex hulls." FALSE)
-if(HPP_FCL_HAS_QHULL)
+option(COAL_HAS_QHULL "use qhull library to compute convex hulls." FALSE)
+if(COAL_HAS_QHULL)
   find_package(Qhull COMPONENTS qhull_r qhullcpp)
   if(Qhull_FOUND)
-    set(HPP_FCL_USE_SYSTEM_QHULL TRUE)
-    message(STATUS "HPP-FCL uses system Qhull")
+    set(COAL_USE_SYSTEM_QHULL TRUE)
+    message(STATUS "COAL uses system Qhull")
   else()
     message(STATUS "Qhullcpp not found: it will be build from sources, if Qhull_r is found")
     file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/third-parties)
     execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink
-      ${CMAKE_SOURCE_DIR}/third-parties/qhull/src/libqhullcpp
-      ${CMAKE_CURRENT_BINARY_DIR}/third-parties/libqhullcpp
+      ${PROJECT_SOURCE_DIR}/third-parties/qhull/src/libqhullcpp
+      ${PROJECT_BINARY_DIR}/third-parties/libqhullcpp
       )
-    set(Qhullcpp_PREFIX ${CMAKE_BINARY_DIR}/third-parties)
+    set(Qhullcpp_PREFIX ${PROJECT_BINARY_DIR}/third-parties)
     find_path(Qhull_r_INCLUDE_DIR
       NAMES libqhull_r/libqhull_r.h
       PATHS ${Qhull_PREFIX}
@@ -180,7 +188,7 @@ if(HPP_FCL_HAS_QHULL)
       PATHS ${Qhull_PREFIX}
       )
     if(NOT Qhull_r_LIBRARY)
-      message(FATAL_ERROR "Qhull_r not found, please install it or turn HPP_FCL_HAS_QHULL OFF")
+      message(FATAL_ERROR "Qhull_r not found, please install it or turn COAL_HAS_QHULL OFF")
     endif()
   endif()
 endif()
@@ -188,133 +196,265 @@ endif()
 FIND_PACKAGE(assimp REQUIRED)
 
 SET(${PROJECT_NAME}_HEADERS
-  include/hpp/fcl/collision_data.h
-  include/hpp/fcl/BV/kIOS.h
-  include/hpp/fcl/BV/BV.h
-  include/hpp/fcl/BV/RSS.h
-  include/hpp/fcl/BV/OBBRSS.h
-  include/hpp/fcl/BV/BV_node.h
-  include/hpp/fcl/BV/AABB.h
-  include/hpp/fcl/BV/OBB.h
-  include/hpp/fcl/BV/kDOP.h
-  include/hpp/fcl/broadphase/broadphase.h
-  include/hpp/fcl/broadphase/broadphase_SSaP.h
-  include/hpp/fcl/broadphase/broadphase_SaP.h
-  include/hpp/fcl/broadphase/broadphase_bruteforce.h
-  include/hpp/fcl/broadphase/broadphase_collision_manager.h
-  include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h
-  include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h
-  include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h
-  include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
-  include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
-  include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
-  include/hpp/fcl/broadphase/broadphase_interval_tree.h
-  include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h
-  include/hpp/fcl/broadphase/broadphase_spatialhash.h
-  include/hpp/fcl/broadphase/broadphase_callbacks.h
-  include/hpp/fcl/broadphase/default_broadphase_callbacks.h
-  include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h
-  include/hpp/fcl/broadphase/detail/hierarchy_tree.h
-  include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h
-  include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h
-  include/hpp/fcl/broadphase/detail/interval_tree.h
-  include/hpp/fcl/broadphase/detail/interval_tree_node.h
-  include/hpp/fcl/broadphase/detail/morton-inl.h
-  include/hpp/fcl/broadphase/detail/morton.h
-  include/hpp/fcl/broadphase/detail/node_base-inl.h
-  include/hpp/fcl/broadphase/detail/node_base.h
-  include/hpp/fcl/broadphase/detail/node_base_array-inl.h
-  include/hpp/fcl/broadphase/detail/node_base_array.h
-  include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h
-  include/hpp/fcl/broadphase/detail/simple_hash_table.h
-  include/hpp/fcl/broadphase/detail/simple_interval-inl.h
-  include/hpp/fcl/broadphase/detail/simple_interval.h
-  include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h
-  include/hpp/fcl/broadphase/detail/sparse_hash_table.h
-  include/hpp/fcl/broadphase/detail/spatial_hash-inl.h
-  include/hpp/fcl/broadphase/detail/spatial_hash.h
-  include/hpp/fcl/narrowphase/narrowphase.h
-  include/hpp/fcl/narrowphase/gjk.h
-  include/hpp/fcl/narrowphase/narrowphase_defaults.h
-  include/hpp/fcl/narrowphase/minkowski_difference.h
-  include/hpp/fcl/narrowphase/support_functions.h
-  include/hpp/fcl/shape/convex.h
-  include/hpp/fcl/shape/details/convex.hxx
-  include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
-  include/hpp/fcl/shape/geometric_shapes.h
-  include/hpp/fcl/shape/geometric_shapes_traits.h
-  include/hpp/fcl/shape/geometric_shapes_utility.h
-  include/hpp/fcl/distance_func_matrix.h
-  include/hpp/fcl/collision.h
-  include/hpp/fcl/collision_func_matrix.h
-  include/hpp/fcl/contact_patch.h
-  include/hpp/fcl/contact_patch_func_matrix.h
-  include/hpp/fcl/contact_patch/contact_patch_solver.h
-  include/hpp/fcl/contact_patch/contact_patch_solver.hxx
-  include/hpp/fcl/distance.h
-  include/hpp/fcl/math/matrix_3f.h
-  include/hpp/fcl/math/vec_3f.h
-  include/hpp/fcl/math/types.h
-  include/hpp/fcl/math/transform.h
-  include/hpp/fcl/data_types.h
-  include/hpp/fcl/BVH/BVH_internal.h
-  include/hpp/fcl/BVH/BVH_model.h
-  include/hpp/fcl/BVH/BVH_front.h
-  include/hpp/fcl/BVH/BVH_utility.h
-  include/hpp/fcl/collision_object.h
-  include/hpp/fcl/collision_utility.h
-  include/hpp/fcl/hfield.h
-  include/hpp/fcl/fwd.hh
-  include/hpp/fcl/logging.h
-  include/hpp/fcl/mesh_loader/assimp.h
-  include/hpp/fcl/mesh_loader/loader.h
-  include/hpp/fcl/internal/BV_fitter.h
-  include/hpp/fcl/internal/BV_splitter.h
-  include/hpp/fcl/internal/shape_shape_func.h
-  include/hpp/fcl/internal/shape_shape_contact_patch_func.h
-  include/hpp/fcl/internal/intersect.h
-  include/hpp/fcl/internal/tools.h
-  include/hpp/fcl/internal/traversal_node_base.h
-  include/hpp/fcl/internal/traversal_node_bvh_shape.h
-  include/hpp/fcl/internal/traversal_node_bvhs.h
-  include/hpp/fcl/internal/traversal_node_hfield_shape.h
-  include/hpp/fcl/internal/traversal_node_setup.h
-  include/hpp/fcl/internal/traversal_node_shapes.h
-  include/hpp/fcl/internal/traversal_recurse.h
-  include/hpp/fcl/internal/traversal.h
-  include/hpp/fcl/serialization/fwd.h
-  include/hpp/fcl/serialization/serializer.h
-  include/hpp/fcl/serialization/archive.h
-  include/hpp/fcl/serialization/transform.h
-  include/hpp/fcl/serialization/AABB.h
-  include/hpp/fcl/serialization/BV_node.h
-  include/hpp/fcl/serialization/BV_splitter.h
-  include/hpp/fcl/serialization/BVH_model.h
-  include/hpp/fcl/serialization/collision_data.h
-  include/hpp/fcl/serialization/contact_patch.h
-  include/hpp/fcl/serialization/collision_object.h
-  include/hpp/fcl/serialization/convex.h
-  include/hpp/fcl/serialization/eigen.h
-  include/hpp/fcl/serialization/geometric_shapes.h
-  include/hpp/fcl/serialization/memory.h
-  include/hpp/fcl/serialization/OBB.h
-  include/hpp/fcl/serialization/RSS.h
-  include/hpp/fcl/serialization/OBBRSS.h
-  include/hpp/fcl/serialization/kIOS.h
-  include/hpp/fcl/serialization/kDOP.h
-  include/hpp/fcl/serialization/hfield.h
-  include/hpp/fcl/serialization/quadrilateral.h
-  include/hpp/fcl/serialization/triangle.h
-  include/hpp/fcl/timings.h
+  include/coal/collision_data.h
+  include/coal/BV/kIOS.h
+  include/coal/BV/BV.h
+  include/coal/BV/RSS.h
+  include/coal/BV/OBBRSS.h
+  include/coal/BV/BV_node.h
+  include/coal/BV/AABB.h
+  include/coal/BV/OBB.h
+  include/coal/BV/kDOP.h
+  include/coal/broadphase/broadphase.h
+  include/coal/broadphase/broadphase_SSaP.h
+  include/coal/broadphase/broadphase_SaP.h
+  include/coal/broadphase/broadphase_bruteforce.h
+  include/coal/broadphase/broadphase_collision_manager.h
+  include/coal/broadphase/broadphase_continuous_collision_manager-inl.h
+  include/coal/broadphase/broadphase_continuous_collision_manager.h
+  include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h
+  include/coal/broadphase/broadphase_dynamic_AABB_tree.h
+  include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
+  include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h
+  include/coal/broadphase/broadphase_interval_tree.h
+  include/coal/broadphase/broadphase_spatialhash-inl.h
+  include/coal/broadphase/broadphase_spatialhash.h
+  include/coal/broadphase/broadphase_callbacks.h
+  include/coal/broadphase/default_broadphase_callbacks.h
+  include/coal/broadphase/detail/hierarchy_tree-inl.h
+  include/coal/broadphase/detail/hierarchy_tree.h
+  include/coal/broadphase/detail/hierarchy_tree_array-inl.h
+  include/coal/broadphase/detail/hierarchy_tree_array.h
+  include/coal/broadphase/detail/interval_tree.h
+  include/coal/broadphase/detail/interval_tree_node.h
+  include/coal/broadphase/detail/morton-inl.h
+  include/coal/broadphase/detail/morton.h
+  include/coal/broadphase/detail/node_base-inl.h
+  include/coal/broadphase/detail/node_base.h
+  include/coal/broadphase/detail/node_base_array-inl.h
+  include/coal/broadphase/detail/node_base_array.h
+  include/coal/broadphase/detail/simple_hash_table-inl.h
+  include/coal/broadphase/detail/simple_hash_table.h
+  include/coal/broadphase/detail/simple_interval-inl.h
+  include/coal/broadphase/detail/simple_interval.h
+  include/coal/broadphase/detail/sparse_hash_table-inl.h
+  include/coal/broadphase/detail/sparse_hash_table.h
+  include/coal/broadphase/detail/spatial_hash-inl.h
+  include/coal/broadphase/detail/spatial_hash.h
+  include/coal/narrowphase/narrowphase.h
+  include/coal/narrowphase/gjk.h
+  include/coal/narrowphase/narrowphase_defaults.h
+  include/coal/narrowphase/minkowski_difference.h
+  include/coal/narrowphase/support_functions.h
+  include/coal/shape/convex.h
+  include/coal/shape/details/convex.hxx
+  include/coal/shape/geometric_shape_to_BVH_model.h
+  include/coal/shape/geometric_shapes.h
+  include/coal/shape/geometric_shapes_traits.h
+  include/coal/shape/geometric_shapes_utility.h
+  include/coal/distance_func_matrix.h
+  include/coal/collision.h
+  include/coal/collision_func_matrix.h
+  include/coal/contact_patch.h
+  include/coal/contact_patch_func_matrix.h
+  include/coal/contact_patch/contact_patch_solver.h
+  include/coal/contact_patch/contact_patch_solver.hxx
+  include/coal/distance.h
+  include/coal/math/matrix_3f.h
+  include/coal/math/vec_3f.h
+  include/coal/math/types.h
+  include/coal/math/transform.h
+  include/coal/data_types.h
+  include/coal/BVH/BVH_internal.h
+  include/coal/BVH/BVH_model.h
+  include/coal/BVH/BVH_front.h
+  include/coal/BVH/BVH_utility.h
+  include/coal/collision_object.h
+  include/coal/collision_utility.h
+  include/coal/hfield.h
+  include/coal/fwd.hh
+  include/coal/logging.h
+  include/coal/mesh_loader/assimp.h
+  include/coal/mesh_loader/loader.h
+  include/coal/internal/BV_fitter.h
+  include/coal/internal/BV_splitter.h
+  include/coal/internal/shape_shape_func.h
+  include/coal/internal/shape_shape_contact_patch_func.h
+  include/coal/internal/intersect.h
+  include/coal/internal/tools.h
+  include/coal/internal/traversal_node_base.h
+  include/coal/internal/traversal_node_bvh_shape.h
+  include/coal/internal/traversal_node_bvhs.h
+  include/coal/internal/traversal_node_hfield_shape.h
+  include/coal/internal/traversal_node_setup.h
+  include/coal/internal/traversal_node_shapes.h
+  include/coal/internal/traversal_recurse.h
+  include/coal/internal/traversal.h
+  include/coal/serialization/fwd.h
+  include/coal/serialization/serializer.h
+  include/coal/serialization/archive.h
+  include/coal/serialization/transform.h
+  include/coal/serialization/AABB.h
+  include/coal/serialization/BV_node.h
+  include/coal/serialization/BV_splitter.h
+  include/coal/serialization/BVH_model.h
+  include/coal/serialization/collision_data.h
+  include/coal/serialization/contact_patch.h
+  include/coal/serialization/collision_object.h
+  include/coal/serialization/convex.h
+  include/coal/serialization/eigen.h
+  include/coal/serialization/geometric_shapes.h
+  include/coal/serialization/memory.h
+  include/coal/serialization/OBB.h
+  include/coal/serialization/RSS.h
+  include/coal/serialization/OBBRSS.h
+  include/coal/serialization/kIOS.h
+  include/coal/serialization/kDOP.h
+  include/coal/serialization/hfield.h
+  include/coal/serialization/quadrilateral.h
+  include/coal/serialization/triangle.h
+  include/coal/timings.h
   )
 
-IF(HPP_FCL_HAS_OCTOMAP)
-  LIST(APPEND ${PROJECT_NAME}_HEADERS
+if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL)
+  SET(HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS
+    include/hpp/fcl/broadphase/broadphase_bruteforce.h
+    include/hpp/fcl/broadphase/broadphase_callbacks.h
+    include/hpp/fcl/broadphase/broadphase_collision_manager.h
+    include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h
+    include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h
+    include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
+    include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
+    include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
+    include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h
+    include/hpp/fcl/broadphase/broadphase.h
+    include/hpp/fcl/broadphase/broadphase_interval_tree.h
+    include/hpp/fcl/broadphase/broadphase_SaP.h
+    include/hpp/fcl/broadphase/broadphase_spatialhash.h
+    include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h
+    include/hpp/fcl/broadphase/broadphase_SSaP.h
+    include/hpp/fcl/broadphase/default_broadphase_callbacks.h
+    include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h
+    include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h
+    include/hpp/fcl/broadphase/detail/hierarchy_tree.h
+    include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h
+    include/hpp/fcl/broadphase/detail/interval_tree.h
+    include/hpp/fcl/broadphase/detail/interval_tree_node.h
+    include/hpp/fcl/broadphase/detail/morton.h
+    include/hpp/fcl/broadphase/detail/morton-inl.h
+    include/hpp/fcl/broadphase/detail/node_base_array.h
+    include/hpp/fcl/broadphase/detail/node_base_array-inl.h
+    include/hpp/fcl/broadphase/detail/node_base.h
+    include/hpp/fcl/broadphase/detail/node_base-inl.h
+    include/hpp/fcl/broadphase/detail/simple_hash_table.h
+    include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h
+    include/hpp/fcl/broadphase/detail/simple_interval.h
+    include/hpp/fcl/broadphase/detail/simple_interval-inl.h
+    include/hpp/fcl/broadphase/detail/sparse_hash_table.h
+    include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h
+    include/hpp/fcl/broadphase/detail/spatial_hash.h
+    include/hpp/fcl/broadphase/detail/spatial_hash-inl.h
+    include/hpp/fcl/BV/AABB.h
+    include/hpp/fcl/BV/BV.h
+    include/hpp/fcl/BV/BV_node.h
+    include/hpp/fcl/BVH/BVH_front.h
+    include/hpp/fcl/BVH/BVH_internal.h
+    include/hpp/fcl/BVH/BVH_model.h
+    include/hpp/fcl/BVH/BVH_utility.h
+    include/hpp/fcl/BV/kDOP.h
+    include/hpp/fcl/BV/kIOS.h
+    include/hpp/fcl/BV/OBB.h
+    include/hpp/fcl/BV/OBBRSS.h
+    include/hpp/fcl/BV/RSS.h
+    include/hpp/fcl/coal.hpp
+    include/hpp/fcl/collision_data.h
+    include/hpp/fcl/collision_func_matrix.h
+    include/hpp/fcl/collision.h
+    include/hpp/fcl/collision_object.h
+    include/hpp/fcl/collision_utility.h
+    include/hpp/fcl/config.hh
+    include/hpp/fcl/contact_patch/contact_patch_solver.h
+    include/hpp/fcl/contact_patch/contact_patch_solver.hxx
+    include/hpp/fcl/contact_patch_func_matrix.h
+    include/hpp/fcl/contact_patch.h
+    include/hpp/fcl/data_types.h
+    include/hpp/fcl/deprecated.hh
+    include/hpp/fcl/distance_func_matrix.h
+    include/hpp/fcl/distance.h
+    include/hpp/fcl/fwd.hh
+    include/hpp/fcl/hfield.h
+    include/hpp/fcl/internal/BV_fitter.h
+    include/hpp/fcl/internal/BV_splitter.h
+    include/hpp/fcl/internal/intersect.h
+    include/hpp/fcl/internal/shape_shape_contact_patch_func.h
+    include/hpp/fcl/internal/shape_shape_func.h
+    include/hpp/fcl/internal/tools.h
+    include/hpp/fcl/internal/traversal.h
+    include/hpp/fcl/internal/traversal_node_base.h
+    include/hpp/fcl/internal/traversal_node_bvhs.h
+    include/hpp/fcl/internal/traversal_node_bvh_shape.h
+    include/hpp/fcl/internal/traversal_node_hfield_shape.h
+    include/hpp/fcl/internal/traversal_node_setup.h
+    include/hpp/fcl/internal/traversal_node_shapes.h
+    include/hpp/fcl/internal/traversal_recurse.h
+    include/hpp/fcl/internal/traversal_node_octree.h
+    include/hpp/fcl/logging.h
+    include/hpp/fcl/math/matrix_3f.h
+    include/hpp/fcl/math/transform.h
+    include/hpp/fcl/math/types.h
+    include/hpp/fcl/math/vec_3f.h
+    include/hpp/fcl/mesh_loader/assimp.h
+    include/hpp/fcl/mesh_loader/loader.h
+    include/hpp/fcl/narrowphase/gjk.h
+    include/hpp/fcl/narrowphase/minkowski_difference.h
+    include/hpp/fcl/narrowphase/narrowphase_defaults.h
+    include/hpp/fcl/narrowphase/narrowphase.h
+    include/hpp/fcl/narrowphase/support_functions.h
     include/hpp/fcl/octree.h
+    include/hpp/fcl/serialization/AABB.h
+    include/hpp/fcl/serialization/archive.h
+    include/hpp/fcl/serialization/BVH_model.h
+    include/hpp/fcl/serialization/BV_node.h
+    include/hpp/fcl/serialization/BV_splitter.h
+    include/hpp/fcl/serialization/collision_data.h
+    include/hpp/fcl/serialization/collision_object.h
+    include/hpp/fcl/serialization/contact_patch.h
+    include/hpp/fcl/serialization/convex.h
+    include/hpp/fcl/serialization/eigen.h
+    include/hpp/fcl/serialization/fwd.h
+    include/hpp/fcl/serialization/geometric_shapes.h
+    include/hpp/fcl/serialization/hfield.h
+    include/hpp/fcl/serialization/kDOP.h
+    include/hpp/fcl/serialization/kIOS.h
+    include/hpp/fcl/serialization/memory.h
+    include/hpp/fcl/serialization/OBB.h
+    include/hpp/fcl/serialization/OBBRSS.h
     include/hpp/fcl/serialization/octree.h
-    include/hpp/fcl/internal/traversal_node_octree.h
+    include/hpp/fcl/serialization/quadrilateral.h
+    include/hpp/fcl/serialization/RSS.h
+    include/hpp/fcl/serialization/serializer.h
+    include/hpp/fcl/serialization/transform.h
+    include/hpp/fcl/serialization/triangle.h
+    include/hpp/fcl/shape/convex.h
+    include/hpp/fcl/shape/details/convex.hxx
+    include/hpp/fcl/shape/geometric_shapes.h
+    include/hpp/fcl/shape/geometric_shapes_traits.h
+    include/hpp/fcl/shape/geometric_shapes_utility.h
+    include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
+    include/hpp/fcl/timings.h
+    include/hpp/fcl/warning.hh
+    )
+  LIST(APPEND ${PROJECT_NAME}_HEADERS ${HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS})
+  HEADER_INSTALL(COMPONENT hpp-fcl-compatibility ${HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS})
+endif()
+
+IF(COAL_HAS_OCTOMAP)
+  LIST(APPEND ${PROJECT_NAME}_HEADERS
+    include/coal/octree.h
+    include/coal/serialization/octree.h
+    include/coal/internal/traversal_node_octree.h
   )
-ENDIF(HPP_FCL_HAS_OCTOMAP)
+ENDIF(COAL_HAS_OCTOMAP)
 
 add_subdirectory(doc)
 add_subdirectory(src)
@@ -325,12 +465,12 @@ if(BUILD_TESTING)
   add_subdirectory(test)
 endif(BUILD_TESTING)
 
-pkg_config_append_libs("hpp-fcl")
-IF(HPP_FCL_HAS_OCTOMAP)
+pkg_config_append_libs("coal")
+IF(COAL_HAS_OCTOMAP)
   # FCL_HAVE_OCTOMAP kept for backward compatibility reasons.
   PKG_CONFIG_APPEND_CFLAGS(
-    "-DHPP_FCL_HAS_OCTOMAP -DHPP_FCL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}")
-ENDIF(HPP_FCL_HAS_OCTOMAP)
+    "-DCOAL_HAS_OCTOMAP -DCOAL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}")
+ENDIF(COAL_HAS_OCTOMAP)
 
 # Install catkin package.xml
 INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
@@ -341,3 +481,17 @@ file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/ament_prefix_p
 install(FILES ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/ament_prefix_path.dsv DESTINATION share/${PROJECT_NAME}/hook)
 file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/python_path.dsv "prepend-non-duplicate;PYTHONPATH;${PYTHON_SITELIB}")
 install(FILES ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/python_path.dsv DESTINATION share/${PROJECT_NAME}/hook)
+
+if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL)
+  include(CMakePackageConfigHelpers)
+  write_basic_package_version_file(hpp-fclConfigVersion.cmake
+      VERSION 3.0.0
+      COMPATIBILITY AnyNewerVersion)
+  install(FILES hpp-fclConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/hpp-fclConfigVersion.cmake
+          DESTINATION lib/cmake/hpp-fcl
+          COMPONENT hpp-fcl-compatibility)
+  include("${JRL_CMAKE_MODULES}/install-helpers.cmake")
+  add_install_target(NAME hpp-fcl-compatibility COMPONENT hpp-fcl-compatibility)
+endif()
+
+setup_project_finalize()
diff --git a/README.md b/README.md
index d8a6d1af465d7bd5805cc15aa72781b6d183b285..88e27ccac25d59a506fd046bb70c88d4c6cea35a 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
-# HPP-FCL — An extension of the Flexible Collision Library
+# Coal — An extension of the Flexible Collision Library
 
 <p align="center">
   <a href="https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/commits/master/"><img src="https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/pipeline.svg" alt="Pipeline status"/></a>
@@ -11,16 +11,16 @@
   <a href="https://github.com/astral-sh/ruff"><img alt="ruff" src="https://img.shields.io/endpoint?url=https://raw.githubusercontent.com/astral-sh/ruff/main/assets/badge/v2.json"></a>
 </p>
 
-[FCL](https://github.com/flexible-collision-library/fcl) was forked in 2015.
-Since then, a large part of the code has been rewritten or removed (for the unused and untested part).
-The broad phase was reintroduced by [Justin Carpentier](https://github.com/jcarpent) in 2022 based on the FCL version 0.7.0.
+[FCL](https://github.com/flexible-collision-library/fcl) was forked in 2015, creating a new project called HPP-FCL.
+Since then, a large part of the code has been rewritten or removed (for the unused and untested part), and new features have been developped (see below).
+Due to these major changes, it was decided in 2024 to rename the HPP-FCL project to **Coal**.
 
-If you use **HPP-FCL** in your projects and research papers, we would appreciate it if you'd [cite it](https://raw.githubusercontent.com/humanoid-path-planner/hpp-fcl/devel/CITATION.bib).
+If you use **Coal** in your projects and research papers, we would appreciate it if you'd [cite it](https://raw.githubusercontent.com/coal-library/coal/devel/CITATION.bib).
 
 ## New features
 
 Compared to the original [FCL](https://github.com/flexible-collision-library/fcl) library, the main new features are:
-- a dedicated and efficient implementation of the GJK algorithm (we do not rely anymore on [libccd](https://github.com/danfis/libccd))
+- dedicated and efficient implementations of the GJK and the EPA algorithms (we do not rely on [libccd](https://github.com/danfis/libccd))
 - the support of safety margins for collision detection
 - an accelerated version of collision detection *à la Nesterov*, which leads to increased performances (up to a factor of 2). More details are available in this [paper](https://hal.archives-ouvertes.fr/hal-03662157/)
 - the computation of a lower bound of the distance between two objects when collision checking is performed, and no collision is found
@@ -30,13 +30,15 @@ Compared to the original [FCL](https://github.com/flexible-collision-library/fcl
 - efficient computation of **contact points** and **contact patches** between objects
 - full support of object serialization via Boost.Serialization
 
-This project is now used in many robotics frameworks such as [Pinocchio](https://github.com/stack-of-tasks/pinocchio), an open-source software that implements efficient and versatile rigid body dynamics algorithms, the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc), an open-source software for Motion and Manipulation Planning. **HPP-FCL** has also been recently used to develop [Simple](https://github.com/Simple-Robotics/Simple), a new (differentiable) and efficient simulator for robotics and beyond.
+Note: the broad phase was reintroduced by [Justin Carpentier](https://github.com/jcarpent) in 2022, based on the FCL version 0.7.0.
+
+This project is now used in many robotics frameworks such as [Pinocchio](https://github.com/stack-of-tasks/pinocchio), an open-source software that implements efficient and versatile rigid body dynamics algorithms, the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc), an open-source software for Motion and Manipulation Planning. **Coal** has also been recently used to develop [Simple](https://github.com/Simple-Robotics/Simple), a new (differentiable) and efficient simulator for robotics and beyond.
 
 ## A high-performance library
 
-Unlike the original FCL library, HPP-FCL implements the well-established [GJK algorithm](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm) and [its variants](https://hal.archives-ouvertes.fr/hal-03662157/) for collision detection and distance computation. These implementations lead to state-of-the-art performances, as depicted by the figures below.
+Unlike the original FCL library, Coal implements the well-established [GJK algorithm](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm) and [its variants](https://hal.archives-ouvertes.fr/hal-03662157/) for collision detection and distance computation. These implementations lead to state-of-the-art performances, as depicted by the figures below.
 
-On the one hand, we have benchmarked HPP-FCL against major software alternatives of the state of the art:
+On the one hand, we have benchmarked Coal against major software alternatives of the state of the art:
 1. the [Bullet simulator](https://github.com/bulletphysics/bullet3),
 2. the original [FCL library](https://github.com/flexible-collision-library/fcl) (used in the [Drake framework]()),
 3. the [libccd library](https://github.com/danfis/libccd) (used in [MuJoCo](http://mujoco.org/)).
@@ -45,13 +47,13 @@ The results are depicted in the following figure, which notably shows that the a
 Please notice that the y-axis is in log scale.
 
 <p align="center">
-  <img src="./doc/images/hpp-fcl-vs-the-rest-of-the-world.png" width="600" alt="HPP-FCL vs the rest of the world" align="center"/>
+  <img src="./doc/images/coal-vs-the-rest-of-the-world.png" width="600" alt="Coal vs the rest of the world" align="center"/>
 </p>
 
-On the other hand, why do we care about dedicated collision detection solvers like GJK for the narrow phase? Why can't we simply formulate the collision detection problem as a quadratic problem and call an off-the-shelf optimization solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite))? Here is why.
+On the other hand, why do we care about dedicated collision detection solvers like GJK for the narrow phase? Why can't we simply formulate the collision detection problem as a quadratic problem and call an off-the-shelf optimization solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite))? Here is why:
 
 <p align="center">
-  <img src="./doc/images/hpp-fcl-performances.jpg" width="600" alt="HPP-FCL vs generic QP solvers" align="center"/>
+  <img src="./doc/images/coal-performances.jpg" width="600" alt="Coal vs generic QP solvers" align="center"/>
 </p>
 
 One can observe that GJK-based approaches largely outperform solutions based on classic optimization solvers (e.g., QP solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite)), notably for large geometries composed of tens or hundreds of vertices.
@@ -67,16 +69,16 @@ One can observe that GJK-based approaches largely outperform solutions based on
 - [ocs2](https://github.com/leggedrobotics/ocs2) A toolbox for Optimal Control for Switched Systems (OCS2)
 
 ## C++ example
-Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install hpp-fcl`.
+Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install coal`.
 The `.so` library, include files and python bindings will then be installed under `$CONDA_PREFIX/lib`, `$CONDA_PREFIX/include` and `$CONDA_PREFIX/lib/python3.XX/site-packages`.
 
-Here is an example of using HPP-FCL in C++:
+Here is an example of using Coal in C++:
 ```cpp
-#include "hpp/fcl/math/transform.h"
-#include "hpp/fcl/mesh_loader/loader.h"
-#include "hpp/fcl/BVH/BVH_model.h"
-#include "hpp/fcl/collision.h"
-#include "hpp/fcl/collision_data.h"
+#include "coal/math/transform.h"
+#include "coal/mesh_loader/loader.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/collision.h"
+#include "coal/collision_data.h"
 #include <iostream>
 #include <memory>
 
@@ -94,29 +96,29 @@ Here is an example of using HPP-FCL in C++:
 // GJK or EPA can be called with this object.
 // Consequently, after creating the BVH structure from the point cloud, this function
 // also computes its convex hull.
-std::shared_ptr<hpp::fcl::ConvexBase> loadConvexMesh(const std::string& file_name) {
-  hpp::fcl::NODE_TYPE bv_type = hpp::fcl::BV_AABB;
-  hpp::fcl::MeshLoader loader(bv_type);
-  hpp::fcl::BVHModelPtr_t bvh = loader.load(file_name);
+std::shared_ptr<coal::ConvexBase> loadConvexMesh(const std::string& file_name) {
+  coal::NODE_TYPE bv_type = coal::BV_AABB;
+  coal::MeshLoader loader(bv_type);
+  coal::BVHModelPtr_t bvh = loader.load(file_name);
   bvh->buildConvexHull(true, "Qt");
   return bvh->convex;
 }
 
 int main() {
-  // Create the hppfcl shapes.
-  // Hppfcl supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes,
+  // Create the coal shapes.
+  // Coal supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes,
   // halfspace and convex meshes (i.e. convex hulls of clouds of points).
   // It also supports BVHs (bounding volumes hierarchies), height-fields and octrees.
-  std::shared_ptr<hpp::fcl::Ellipsoid> shape1 = std::make_shared<hpp::fcl::Ellipsoid>(0.7, 1.0, 0.8);
-  std::shared_ptr<hpp::fcl::ConvexBase> shape2 = loadConvexMesh("../path/to/mesh/file.obj");
+  std::shared_ptr<coal::Ellipsoid> shape1 = std::make_shared<coal::Ellipsoid>(0.7, 1.0, 0.8);
+  std::shared_ptr<coal::ConvexBase> shape2 = loadConvexMesh("../path/to/mesh/file.obj");
 
   // Define the shapes' placement in 3D space
-  hpp::fcl::Transform3f T1;
-  T1.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom());
-  T1.setTranslation(hpp::fcl::Vec3f::Random());
-  hpp::fcl::Transform3f T2 = hpp::fcl::Transform3f::Identity();
-  T2.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom());
-  T2.setTranslation(hpp::fcl::Vec3f::Random());
+  coal::Transform3s T1;
+  T1.setQuatRotation(coal::Quaternion3f::UnitRandom());
+  T1.setTranslation(coal::Vec3s::Random());
+  coal::Transform3s T2 = coal::Transform3s::Identity();
+  T2.setQuatRotation(coal::Quaternion3f::UnitRandom());
+  T2.setTranslation(coal::Vec3s::Random());
 
   // Define collision requests and results.
   //
@@ -127,19 +129,19 @@ int main() {
   // Setting a positive security margin can be usefull in motion planning,
   // i.e to prevent shapes from getting too close to one another.
   // In physics simulation, allowing a negative security margin may be usefull to stabilize the simulation.
-  hpp::fcl::CollisionRequest col_req;
+  coal::CollisionRequest col_req;
   col_req.security_margin = 1e-1;
   // A collision result stores the result of the collision test (signed distance between the shapes,
   // witness points location, normal etc.)
-  hpp::fcl::CollisionResult col_res;
+  coal::CollisionResult col_res;
 
   // Collision call
-  hpp::fcl::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res);
+  coal::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res);
 
   // We can access the collision result once it has been populated
   std::cout << "Collision? " << col_res.isCollision() << "\n";
   if (col_res.isCollision()) {
-    hpp::fcl::Contact contact = col_res.getContact(0);
+    coal::Contact contact = col_res.getContact(0);
     // The penetration depth does **not** take into account the security margin.
     // Consequently, the penetration depth is the true signed distance which separates the shapes.
     // To have the distance which takes into account the security margin, we can simply add the two together.
@@ -158,47 +160,47 @@ int main() {
 ```
 
 ## Python example
-Here is the C++ example from above translated in python using HPP-FCL's python bindings:
+Here is the C++ example from above translated in python using the python bindings of Coal:
 ```python
 import numpy as np
-import hppfcl
+import coal
 # Optional:
 # The Pinocchio library is a rigid body algorithms library and has a handy SE3 module.
 # It can be installed as simply as `conda -c conda-forge install pinocchio`.
-# Installing pinocchio also installs hpp-fcl.
+# Installing pinocchio also installs coal.
 import pinocchio as pin
 
 def loadConvexMesh(file_name: str):
-    loader = hppfcl.MeshLoader()
-    bvh: hppfcl.BVHModelBase = loader.load(file_name)
+    loader = coal.MeshLoader()
+    bvh: coal.BVHModelBase = loader.load(file_name)
     bvh.buildConvexHull(True, "Qt")
     return bvh.convex
 
 if __name__ == "__main__":
-    # Create hppfcl shapes
-    shape1 = hppfcl.Ellipsoid(0.7, 1.0, 0.8)
+    # Create coal shapes
+    shape1 = coal.Ellipsoid(0.7, 1.0, 0.8)
     shape2 = loadConvexMesh("../path/to/mesh/file.obj")
 
     # Define the shapes' placement in 3D space
-    T1 = hppfcl.Transform3f()
+    T1 = coal.Transform3s()
     T1.setTranslation(pin.SE3.Random().translation)
     T1.setRotation(pin.SE3.Random().rotation)
-    T2 = hppfcl.Transform3f();
+    T2 = coal.Transform3s();
     # Using np arrays also works
     T1.setTranslation(np.random.rand(3))
     T2.setRotation(pin.SE3.Random().rotation)
 
     # Define collision requests and results
-    col_req = hppfcl.CollisionRequest()
-    col_res = hppfcl.CollisionResult()
+    col_req = coal.CollisionRequest()
+    col_res = coal.CollisionResult()
 
     # Collision call
-    hppfcl.collide(shape1, T1, shape2, T2, col_req, col_res)
+    coal.collide(shape1, T1, shape2, T2, col_req, col_res)
 
     # Accessing the collision result once it has been populated
     print("Is collision? ", {col_res.isCollision()})
     if col_res.isCollision():
-        contact: hppfcl.Contact = col_res.getContact(0)
+        contact: coal.Contact = col_res.getContact(0)
         print("Penetration depth: ", contact.penetration_depth)
         print("Distance between the shapes including the security margin: ", contact.penetration_depth + col_req.security_margin)
         print("Witness point shape1: ", contact.getNearestPoint1())
@@ -211,4 +213,4 @@ if __name__ == "__main__":
 
 ## Acknowledgments
 
-The development of **HPP-FCL** is actively supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr) and, to some extend, [Eureka Robotics](https://eurekarobotics.com/).
+The development of **Coal** is actively supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr) and, to some extend, [Eureka Robotics](https://eurekarobotics.com/).
diff --git a/cmake b/cmake
index baf48a3ecb96f8dcad616aec006d43cce3307e19..f1f95f942fabd3d4dc7e39aa7f1c20a5b0735165 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit baf48a3ecb96f8dcad616aec006d43cce3307e19
+Subproject commit f1f95f942fabd3d4dc7e39aa7f1c20a5b0735165
diff --git a/doc/gjk.py b/doc/gjk.py
index b69bcc2b3897c76cb94656dbd34bbef0abc342b7..bee0d547a932622f58deec84d5196c7527d59a84 100644
--- a/doc/gjk.py
+++ b/doc/gjk.py
@@ -444,34 +444,34 @@ def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]):
         for v in "abcd":
             print(
                 indent
-                + "const Vec3f& {} (current.vertex[{}]->w);".format(v.upper(), v),
+                + "const Vec3s& {} (current.vertex[{}]->w);".format(v.upper(), v),
                 file=file,
             )
-        print(indent + "const FCL_REAL aa = A.squaredNorm();".format(), file=file)
+        print(indent + "const CoalScalar aa = A.squaredNorm();".format(), file=file)
         for v in "dcb":
             for m in "abcd":
                 if m <= v:
                     print(
                         indent
-                        + "const FCL_REAL {0}{1}    = {2}.dot({3});".format(
+                        + "const CoalScalar {0}{1}    = {2}.dot({3});".format(
                             v, m, v.upper(), m.upper()
                         ),
                         file=file,
                     )
                 else:
                     print(
-                        indent + "const FCL_REAL& {0}{1}    = {1}{0};".format(v, m),
+                        indent + "const CoalScalar& {0}{1}    = {1}{0};".format(v, m),
                         file=file,
                     )
-            print(indent + "const FCL_REAL {0}a_aa = {0}a - aa;".format(v), file=file)
+            print(indent + "const CoalScalar {0}a_aa = {0}a - aa;".format(v), file=file)
         for l0, l1 in zip("bcd", "cdb"):
             print(
-                indent + "const FCL_REAL {0}a_{1}a = {0}a - {1}a;".format(l0, l1),
+                indent + "const CoalScalar {0}a_{1}a = {0}a - {1}a;".format(l0, l1),
                 file=file,
             )
         for v in "bc":
             print(
-                indent + "const Vec3f a_cross_{0} = A.cross({1});".format(v, v.upper()),
+                indent + "const Vec3s a_cross_{0} = A.cross({1});".format(v, v.upper()),
                 file=file,
             )
         print("", file=file)
diff --git a/doc/images/hpp-fcl-performances.jpg b/doc/images/coal-performances.jpg
similarity index 100%
rename from doc/images/hpp-fcl-performances.jpg
rename to doc/images/coal-performances.jpg
diff --git a/doc/images/hpp-fcl-vs-the-rest-of-the-world.pdf b/doc/images/coal-vs-the-rest-of-the-world.pdf
similarity index 100%
rename from doc/images/hpp-fcl-vs-the-rest-of-the-world.pdf
rename to doc/images/coal-vs-the-rest-of-the-world.pdf
diff --git a/doc/images/hpp-fcl-vs-the-rest-of-the-world.png b/doc/images/coal-vs-the-rest-of-the-world.png
similarity index 100%
rename from doc/images/hpp-fcl-vs-the-rest-of-the-world.png
rename to doc/images/coal-vs-the-rest-of-the-world.png
diff --git a/doc/mesh-mesh-collision-call.dot b/doc/mesh-mesh-collision-call.dot
index 55c455482c893809ebbe887a6b7e3ebeae219639..e70206145a73c478061f5572ef1758baf2dc7460 100644
--- a/doc/mesh-mesh-collision-call.dot
+++ b/doc/mesh-mesh-collision-call.dot
@@ -4,35 +4,35 @@ digraph CD  {
 	compound=true
         size = 11.7
 
-        "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
-        "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box]
-        "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box]
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" [shape = box]
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" [shape = box]
-        "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box]
-        "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
-        "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
+        "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
+        "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
         "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" [shape = box]
-        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" [shape = box]
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" [shape = box]
         "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" [shape = box]
-        "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
-        "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
-        "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)" [shape = box]
+        "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box]
 
-         "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"
-        "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)"
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)"
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)"
+         "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"
+        "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
         "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"-> "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)"
-        "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)"
-        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result"
-        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" -> "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result"
-        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)"
-        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result"
-"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)"
-      "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" [color=red]
-      "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" [color = red]
-      "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const"
-      "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const"
-      "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n  -request\n  - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const"
+        "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)"
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)"
+        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
+      "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [color=red]
+      "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [color = red]
+      "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
+      "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
+      "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
 }
\ No newline at end of file
diff --git a/doc/shape-mesh-collision-call.dot b/doc/shape-mesh-collision-call.dot
index 839f575a9c0af7f46287aa2c50838e7ed5fc40c3..2a16e9f98ffcb57d46b012e5d67567f539ab1d4e 100644
--- a/doc/shape-mesh-collision-call.dot
+++ b/doc/shape-mesh-collision-call.dot
@@ -4,33 +4,33 @@ digraph CD  {
 	compound=true
         size = 11.7
 
-        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box]
-        "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box]
+        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box]
+        "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box]
         "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" [shape = box]
         "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" [shape = box]
-        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box]
-        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box]
-        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box]
-        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
-        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box]
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box]
-        "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nFCL_REAL& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box]
-        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box]
-        "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" [shape = box]
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box]
+        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box]
+        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box]
+        "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box]
+        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box]
+        "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box]
 
-        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp"
-        "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp"
+        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp"
+        "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp"
         "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp"
-        "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
-        "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
-        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h"
-        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h"
-        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red]
-        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red]
-        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp"
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp"
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nFCL_REAL& squaredLowerBoundDistance)\nBV/OBB.cpp"
-        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293"
-        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156"
+        "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
+        "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h"
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h"
+        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red]
+        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp"
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293"
+        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156"
 }
diff --git a/doc/shape-shape-collision-call.dot b/doc/shape-shape-collision-call.dot
index 6690f4eb3b4bc94ed392d5b74fbaff9670de01e4..58d666331f7053b7bf814e40b54d0a828d950d8c 100644
--- a/doc/shape-shape-collision-call.dot
+++ b/doc/shape-shape-collision-call.dot
@@ -4,16 +4,16 @@ digraph CD  {
 	compound=true
         size = 11.7
 
-        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
-        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box]
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box]
         "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box]
         "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box]
         "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box]
-        "template<typename S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" [shape = box]
+        "template<typename S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box]
 
-        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)"
-        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)"
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)"
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)"
         "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" -> "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)"
         "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" -> "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h"
-        "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template<typename S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h"
+        "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template<typename S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h"
 }
diff --git a/hpp-fclConfig.cmake b/hpp-fclConfig.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..1e2664285755bd8172a338ad8157c1eb3c1b003c
--- /dev/null
+++ b/hpp-fclConfig.cmake
@@ -0,0 +1,14 @@
+# This file provide bacward compatiblity for `find_package(hpp-fcl)`.
+
+message(WARNING "Please update your CMake from 'hpp-fcl' to 'coal'")
+
+find_package(coal REQUIRED)
+
+if(NOT TARGET hpp-fcl::hpp-fcl)
+  add_library(hpp-fcl::hpp-fcl SHARED IMPORTED)
+  target_link_libraries(hpp-fcl::hpp-fcl INTERFACE coal::coal)
+  get_property(_cfg TARGET coal::coal PROPERTY IMPORTED_CONFIGURATIONS)
+  get_property(_loc TARGET coal::coal PROPERTY "IMPORTED_LOCATION_${_cfg}")
+  set_property(TARGET hpp-fcl::hpp-fcl PROPERTY IMPORTED_LOCATION "${_loc}")
+  target_compile_definitions(hpp-fcl::hpp-fcl INTERFACE COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL)
+endif()
diff --git a/include/coal/BV/AABB.h b/include/coal/BV/AABB.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d46a53234624127b830ec52d268dd6197afcd84
--- /dev/null
+++ b/include/coal/BV/AABB.h
@@ -0,0 +1,266 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_AABB_H
+#define COAL_AABB_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+struct CollisionRequest;
+class Plane;
+class Halfspace;
+
+/// @defgroup Bounding_Volume Bounding volumes
+/// Classes of different types of bounding volume.
+/// @{
+
+/// @brief A class describing the AABB collision structure, which is a box in 3D
+/// space determined by two diagonal points
+class COAL_DLLAPI AABB {
+ public:
+  /// @brief The min point in the AABB
+  Vec3s min_;
+  /// @brief The max point in the AABB
+  Vec3s max_;
+
+  /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf)
+  AABB();
+
+  /// @brief Creating an AABB at position v with zero size
+  AABB(const Vec3s& v) : min_(v), max_(v) {}
+
+  /// @brief Creating an AABB with two endpoints a and b
+  AABB(const Vec3s& a, const Vec3s& b)
+      : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {}
+
+  /// @brief Creating an AABB centered as core and is of half-dimension delta
+  AABB(const AABB& core, const Vec3s& delta)
+      : min_(core.min_ - delta), max_(core.max_ + delta) {}
+
+  /// @brief Creating an AABB contains three points
+  AABB(const Vec3s& a, const Vec3s& b, const Vec3s& c)
+      : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {}
+
+  AABB(const AABB& other) = default;
+
+  AABB& operator=(const AABB& other) = default;
+
+  AABB& update(const Vec3s& a, const Vec3s& b) {
+    min_ = a.cwiseMin(b);
+    max_ = a.cwiseMax(b);
+    return *this;
+  }
+
+  /// @brief Comparison operator
+  bool operator==(const AABB& other) const {
+    return min_ == other.min_ && max_ == other.max_;
+  }
+
+  bool operator!=(const AABB& other) const { return !(*this == other); }
+
+  /// @name Bounding volume API
+  /// Common API to BVs.
+  /// @{
+
+  /// @brief Check whether the AABB contains a point
+  inline bool contain(const Vec3s& p) const {
+    if (p[0] < min_[0] || p[0] > max_[0]) return false;
+    if (p[1] < min_[1] || p[1] > max_[1]) return false;
+    if (p[2] < min_[2] || p[2] > max_[2]) return false;
+
+    return true;
+  }
+
+  /// @brief Check whether two AABB are overlap
+  inline bool overlap(const AABB& other) const {
+    if (min_[0] > other.max_[0]) return false;
+    if (min_[1] > other.max_[1]) return false;
+    if (min_[2] > other.max_[2]) return false;
+
+    if (max_[0] < other.min_[0]) return false;
+    if (max_[1] < other.min_[1]) return false;
+    if (max_[2] < other.min_[2]) return false;
+
+    return true;
+  }
+
+  /// @brief Check whether AABB overlaps a plane
+  bool overlap(const Plane& p) const;
+
+  /// @brief Check whether AABB overlaps a halfspace
+  bool overlap(const Halfspace& hs) const;
+
+  /// @brief Check whether two AABB are overlap
+  bool overlap(const AABB& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const;
+
+  /// @brief Distance between two AABBs
+  CoalScalar distance(const AABB& other) const;
+
+  /// @brief Distance between two AABBs; P and Q, should not be NULL, return the
+  /// nearest points
+  CoalScalar distance(const AABB& other, Vec3s* P, Vec3s* Q) const;
+
+  /// @brief Merge the AABB and a point
+  inline AABB& operator+=(const Vec3s& p) {
+    min_ = min_.cwiseMin(p);
+    max_ = max_.cwiseMax(p);
+    return *this;
+  }
+
+  /// @brief Merge the AABB and another AABB
+  inline AABB& operator+=(const AABB& other) {
+    min_ = min_.cwiseMin(other.min_);
+    max_ = max_.cwiseMax(other.max_);
+    return *this;
+  }
+
+  /// @brief Return the merged AABB of current AABB and the other one
+  inline AABB operator+(const AABB& other) const {
+    AABB res(*this);
+    return res += other;
+  }
+
+  /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
+  inline CoalScalar size() const { return (max_ - min_).squaredNorm(); }
+
+  /// @brief Center of the AABB
+  inline Vec3s center() const { return (min_ + max_) * 0.5; }
+
+  /// @brief Width of the AABB
+  inline CoalScalar width() const { return max_[0] - min_[0]; }
+
+  /// @brief Height of the AABB
+  inline CoalScalar height() const { return max_[1] - min_[1]; }
+
+  /// @brief Depth of the AABB
+  inline CoalScalar depth() const { return max_[2] - min_[2]; }
+
+  /// @brief Volume of the AABB
+  inline CoalScalar volume() const { return width() * height() * depth(); }
+
+  /// @}
+
+  /// @brief Check whether the AABB contains another AABB
+  inline bool contain(const AABB& other) const {
+    return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) &&
+           (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) &&
+           (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
+  }
+
+  /// @brief Check whether two AABB are overlap and return the overlap part
+  inline bool overlap(const AABB& other, AABB& overlap_part) const {
+    if (!overlap(other)) {
+      return false;
+    }
+
+    overlap_part.min_ = min_.cwiseMax(other.min_);
+    overlap_part.max_ = max_.cwiseMin(other.max_);
+    return true;
+  }
+
+  /// @brief Check whether two AABB are overlapped along specific axis
+  inline bool axisOverlap(const AABB& other, int axis_id) const {
+    if (min_[axis_id] > other.max_[axis_id]) return false;
+    if (max_[axis_id] < other.min_[axis_id]) return false;
+
+    return true;
+  }
+
+  /// @brief expand the half size of the AABB by delta, and keep the center
+  /// unchanged.
+  inline AABB& expand(const Vec3s& delta) {
+    min_ -= delta;
+    max_ += delta;
+    return *this;
+  }
+
+  /// @brief expand the half size of the AABB by a scalar delta, and keep the
+  /// center unchanged.
+  inline AABB& expand(const CoalScalar delta) {
+    min_.array() -= delta;
+    max_.array() += delta;
+    return *this;
+  }
+
+  /// @brief expand the aabb by increase the thickness of the plate by a ratio
+  inline AABB& expand(const AABB& core, CoalScalar ratio) {
+    min_ = min_ * ratio - core.min_;
+    max_ = max_ * ratio - core.max_;
+    return *this;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief translate the center of AABB by t
+static inline AABB translate(const AABB& aabb, const Vec3s& t) {
+  AABB res(aabb);
+  res.min_ += t;
+  res.max_ += t;
+  return res;
+}
+
+static inline AABB rotate(const AABB& aabb, const Matrix3s& R) {
+  AABB res(R * aabb.min_);
+  Vec3s corner(aabb.min_);
+  const Eigen::DenseIndex bit[3] = {1, 2, 4};
+  for (Eigen::DenseIndex ic = 1; ic < 8;
+       ++ic) {  // ic = 0 corresponds to aabb.min_. Skip it.
+    for (Eigen::DenseIndex i = 0; i < 3; ++i) {
+      corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i];
+    }
+    res += R * corner;
+  }
+  return res;
+}
+
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
+/// and b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
+                         const AABB& b2);
+
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
+/// and b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
+                         const AABB& b2, const CollisionRequest& request,
+                         CoalScalar& sqrDistLowerBound);
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BV/BV.h b/include/coal/BV/BV.h
new file mode 100644
index 0000000000000000000000000000000000000000..2224f791888c812ec220a9b637a61921da0bd9ca
--- /dev/null
+++ b/include/coal/BV/BV.h
@@ -0,0 +1,289 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_BV_H
+#define COAL_BV_H
+
+#include "coal/BV/kDOP.h"
+#include "coal/BV/AABB.h"
+#include "coal/BV/OBB.h"
+#include "coal/BV/RSS.h"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BV/kIOS.h"
+#include "coal/math/transform.h"
+
+/** @brief Main namespace */
+namespace coal {
+
+/// @cond IGNORE
+namespace details {
+
+/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a
+/// bounding volume of type BV2 in I configuration.
+template <typename BV1, typename BV2>
+struct Converter {
+  static void convert(const BV1& bv1, const Transform3s& tf1, BV2& bv2);
+  static void convert(const BV1& bv1, BV2& bv2);
+};
+
+/// @brief Convert from AABB to AABB, not very tight but is fast.
+template <>
+struct Converter<AABB, AABB> {
+  static void convert(const AABB& bv1, const Transform3s& tf1, AABB& bv2) {
+    const Vec3s& center = bv1.center();
+    CoalScalar r = (bv1.max_ - bv1.min_).norm() * 0.5;
+    const Vec3s center2 = tf1.transform(center);
+    bv2.min_ = center2 - Vec3s::Constant(r);
+    bv2.max_ = center2 + Vec3s::Constant(r);
+  }
+
+  static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; }
+};
+
+template <>
+struct Converter<AABB, OBB> {
+  static void convert(const AABB& bv1, const Transform3s& tf1, OBB& bv2) {
+    bv2.To = tf1.transform(bv1.center());
+    bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
+    bv2.axes = tf1.getRotation();
+  }
+
+  static void convert(const AABB& bv1, OBB& bv2) {
+    bv2.To = bv1.center();
+    bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
+    bv2.axes.setIdentity();
+  }
+};
+
+template <>
+struct Converter<OBB, OBB> {
+  static void convert(const OBB& bv1, const Transform3s& tf1, OBB& bv2) {
+    bv2.extent = bv1.extent;
+    bv2.To = tf1.transform(bv1.To);
+    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
+  }
+
+  static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; }
+};
+
+template <>
+struct Converter<OBBRSS, OBB> {
+  static void convert(const OBBRSS& bv1, const Transform3s& tf1, OBB& bv2) {
+    Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
+  }
+
+  static void convert(const OBBRSS& bv1, OBB& bv2) {
+    Converter<OBB, OBB>::convert(bv1.obb, bv2);
+  }
+};
+
+template <>
+struct Converter<RSS, OBB> {
+  static void convert(const RSS& bv1, const Transform3s& tf1, OBB& bv2) {
+    bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius,
+                       bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
+    bv2.To = tf1.transform(bv1.Tr);
+    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
+  }
+
+  static void convert(const RSS& bv1, OBB& bv2) {
+    bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius,
+                       bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
+    bv2.To = bv1.Tr;
+    bv2.axes = bv1.axes;
+  }
+};
+
+template <typename BV1>
+struct Converter<BV1, AABB> {
+  static void convert(const BV1& bv1, const Transform3s& tf1, AABB& bv2) {
+    const Vec3s& center = bv1.center();
+    CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
+    const Vec3s center2 = tf1.transform(center);
+    bv2.min_ = center2 - Vec3s::Constant(r);
+    bv2.max_ = center2 + Vec3s::Constant(r);
+  }
+
+  static void convert(const BV1& bv1, AABB& bv2) {
+    const Vec3s& center = bv1.center();
+    CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
+    bv2.min_ = center - Vec3s::Constant(r);
+    bv2.max_ = center + Vec3s::Constant(r);
+  }
+};
+
+template <typename BV1>
+struct Converter<BV1, OBB> {
+  static void convert(const BV1& bv1, const Transform3s& tf1, OBB& bv2) {
+    AABB bv;
+    Converter<BV1, AABB>::convert(bv1, bv);
+    Converter<AABB, OBB>::convert(bv, tf1, bv2);
+  }
+
+  static void convert(const BV1& bv1, OBB& bv2) {
+    AABB bv;
+    Converter<BV1, AABB>::convert(bv1, bv);
+    Converter<AABB, OBB>::convert(bv, bv2);
+  }
+};
+
+template <>
+struct Converter<OBB, RSS> {
+  static void convert(const OBB& bv1, const Transform3s& tf1, RSS& bv2) {
+    bv2.Tr = tf1.transform(bv1.To);
+    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
+
+    bv2.radius = bv1.extent[2];
+    bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
+    bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
+  }
+
+  static void convert(const OBB& bv1, RSS& bv2) {
+    bv2.Tr = bv1.To;
+    bv2.axes = bv1.axes;
+
+    bv2.radius = bv1.extent[2];
+    bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
+    bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
+  }
+};
+
+template <>
+struct Converter<RSS, RSS> {
+  static void convert(const RSS& bv1, const Transform3s& tf1, RSS& bv2) {
+    bv2.Tr = tf1.transform(bv1.Tr);
+    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
+
+    bv2.radius = bv1.radius;
+    bv2.length[0] = bv1.length[0];
+    bv2.length[1] = bv1.length[1];
+  }
+
+  static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; }
+};
+
+template <>
+struct Converter<OBBRSS, RSS> {
+  static void convert(const OBBRSS& bv1, const Transform3s& tf1, RSS& bv2) {
+    Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
+  }
+
+  static void convert(const OBBRSS& bv1, RSS& bv2) {
+    Converter<RSS, RSS>::convert(bv1.rss, bv2);
+  }
+};
+
+template <>
+struct Converter<AABB, RSS> {
+  static void convert(const AABB& bv1, const Transform3s& tf1, RSS& bv2) {
+    bv2.Tr = tf1.transform(bv1.center());
+
+    /// Sort the AABB edges so that AABB extents are ordered.
+    CoalScalar d[3] = {bv1.width(), bv1.height(), bv1.depth()};
+    Eigen::DenseIndex id[3] = {0, 1, 2};
+
+    for (Eigen::DenseIndex i = 1; i < 3; ++i) {
+      for (Eigen::DenseIndex j = i; j > 0; --j) {
+        if (d[j] > d[j - 1]) {
+          {
+            CoalScalar tmp = d[j];
+            d[j] = d[j - 1];
+            d[j - 1] = tmp;
+          }
+          {
+            Eigen::DenseIndex tmp = id[j];
+            id[j] = id[j - 1];
+            id[j - 1] = tmp;
+          }
+        }
+      }
+    }
+
+    const Vec3s extent = (bv1.max_ - bv1.min_) * 0.5;
+    bv2.radius = extent[id[2]];
+    bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
+    bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
+
+    const Matrix3s& R = tf1.getRotation();
+    const bool left_hand = (id[0] == (id[1] + 1) % 3);
+    if (left_hand)
+      bv2.axes.col(0) = -R.col(id[0]);
+    else
+      bv2.axes.col(0) = R.col(id[0]);
+    bv2.axes.col(1) = R.col(id[1]);
+    bv2.axes.col(2) = R.col(id[2]);
+  }
+
+  static void convert(const AABB& bv1, RSS& bv2) {
+    convert(bv1, Transform3s(), bv2);
+  }
+};
+
+template <>
+struct Converter<AABB, OBBRSS> {
+  static void convert(const AABB& bv1, const Transform3s& tf1, OBBRSS& bv2) {
+    Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
+    Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
+  }
+
+  static void convert(const AABB& bv1, OBBRSS& bv2) {
+    Converter<AABB, OBB>::convert(bv1, bv2.obb);
+    Converter<AABB, RSS>::convert(bv1, bv2.rss);
+  }
+};
+
+}  // namespace details
+
+/// @endcond
+
+/// @brief Convert a bounding volume of type BV1 in configuration tf1 to
+/// bounding volume of type BV2 in identity configuration.
+template <typename BV1, typename BV2>
+static inline void convertBV(const BV1& bv1, const Transform3s& tf1, BV2& bv2) {
+  details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
+}
+
+/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2
+/// in identity configuration.
+template <typename BV1, typename BV2>
+static inline void convertBV(const BV1& bv1, BV2& bv2) {
+  details::Converter<BV1, BV2>::convert(bv1, bv2);
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BV/BV_node.h b/include/coal/BV/BV_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..b858308c5790a8a2076f56863e2a2900f1be79ef
--- /dev/null
+++ b/include/coal/BV/BV_node.h
@@ -0,0 +1,166 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_BV_NODE_H
+#define COAL_BV_NODE_H
+
+#include "coal/data_types.h"
+#include "coal/BV/BV.h"
+
+namespace coal {
+
+/// @defgroup Construction_Of_BVH Construction of BVHModel
+/// Classes which are used to build a BVHModel (Bounding Volume Hierarchy)
+/// @{
+
+/// @brief BVNodeBase encodes the tree structure for BVH
+struct COAL_DLLAPI BVNodeBase {
+  /// @brief An index for first child node or primitive
+  /// If the value is positive, it is the index of the first child bv node
+  /// If the value is negative, it is -(primitive index + 1)
+  /// Zero is not used.
+  int first_child;
+
+  /// @brief The start id the primitive belonging to the current node. The index
+  /// is referred to the primitive_indices in BVHModel and from that we can
+  /// obtain the primitive's index in original data indirectly.
+  unsigned int first_primitive;
+
+  /// @brief The number of primitives belonging to the current node
+  unsigned int num_primitives;
+
+  /// @brief Default constructor
+  BVNodeBase()
+      : first_child(0),
+        first_primitive(
+            (std::numeric_limits<unsigned int>::max)())  // value we should help
+                                                         // to raise an issue
+        ,
+        num_primitives(0) {}
+
+  /// @brief Equality operator
+  bool operator==(const BVNodeBase& other) const {
+    return first_child == other.first_child &&
+           first_primitive == other.first_primitive &&
+           num_primitives == other.num_primitives;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const BVNodeBase& other) const { return !(*this == other); }
+
+  /// @brief Whether current node is a leaf node (i.e. contains a primitive
+  /// index
+  inline bool isLeaf() const { return first_child < 0; }
+
+  /// @brief Return the primitive index. The index is referred to the original
+  /// data (i.e. vertices or tri_indices) in BVHModel
+  inline int primitiveId() const { return -(first_child + 1); }
+
+  /// @brief Return the index of the first child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline int leftChild() const { return first_child; }
+
+  /// @brief Return the index of the second child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline int rightChild() const { return first_child + 1; }
+};
+
+/// @brief A class describing a bounding volume node. It includes the tree
+/// structure providing in BVNodeBase and also the geometry data provided in BV
+/// template parameter.
+template <typename BV>
+struct COAL_DLLAPI BVNode : public BVNodeBase {
+  typedef BVNodeBase Base;
+
+  /// @brief bounding volume storing the geometry
+  BV bv;
+
+  /// @brief Equality operator
+  bool operator==(const BVNode& other) const {
+    return Base::operator==(other) && bv == other.bv;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const BVNode& other) const { return !(*this == other); }
+
+  /// @brief Check whether two BVNode collide
+  bool overlap(const BVNode& other) const { return bv.overlap(other.bv); }
+  /// @brief Check whether two BVNode collide
+  bool overlap(const BVNode& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const {
+    return bv.overlap(other.bv, request, sqrDistLowerBound);
+  }
+
+  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
+  /// the underlying BV supports distance, return the nearest points.
+  CoalScalar distance(const BVNode& other, Vec3s* P1 = NULL,
+                      Vec3s* P2 = NULL) const {
+    return bv.distance(other.bv, P1, P2);
+  }
+
+  /// @brief Access to the center of the BV
+  Vec3s getCenter() const { return bv.center(); }
+
+  /// @brief Access to the orientation of the BV
+  const Matrix3s& getOrientation() const {
+    static const Matrix3s id3 = Matrix3s::Identity();
+    return id3;
+  }
+
+  /// \cond
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  /// \endcond
+};
+
+template <>
+inline const Matrix3s& BVNode<OBB>::getOrientation() const {
+  return bv.axes;
+}
+
+template <>
+inline const Matrix3s& BVNode<RSS>::getOrientation() const {
+  return bv.axes;
+}
+
+template <>
+inline const Matrix3s& BVNode<OBBRSS>::getOrientation() const {
+  return bv.obb.axes;
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BV/OBB.h b/include/coal/BV/OBB.h
new file mode 100644
index 0000000000000000000000000000000000000000..8255f023b0bd7a2a54fd0931e59802e93b49cb8f
--- /dev/null
+++ b/include/coal/BV/OBB.h
@@ -0,0 +1,150 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_OBB_H
+#define COAL_OBB_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief Oriented bounding box class
+struct COAL_DLLAPI OBB {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief Orientation of OBB. axis[i] is the ith column of the orientation
+  /// matrix for the box; it is also the i-th principle direction of the box. We
+  /// assume that axis[0] corresponds to the axis with the longest box edge,
+  /// axis[1] corresponds to the shorter one and axis[2] corresponds to the
+  /// shortest one.
+  Matrix3s axes;
+
+  /// @brief Center of OBB
+  Vec3s To;
+
+  /// @brief Half dimensions of OBB
+  Vec3s extent;
+
+  OBB() : axes(Matrix3s::Zero()), To(Vec3s::Zero()), extent(Vec3s::Zero()) {}
+
+  /// @brief Equality operator
+  bool operator==(const OBB& other) const {
+    return axes == other.axes && To == other.To && extent == other.extent;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const OBB& other) const { return !(*this == other); }
+
+  /// @brief Check whether the OBB contains a point.
+  bool contain(const Vec3s& p) const;
+
+  /// Check collision between two OBB
+  /// @return true if collision happens.
+  bool overlap(const OBB& other) const;
+
+  /// Check collision between two OBB
+  /// @return true if collision happens.
+  /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
+  ///         they do not overlap.
+  bool overlap(const OBB& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const;
+
+  /// @brief Distance between two OBBs, not implemented.
+  CoalScalar distance(const OBB& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
+
+  /// @brief A simple way to merge the OBB and a point (the result is not
+  /// compact).
+  OBB& operator+=(const Vec3s& p);
+
+  /// @brief Merge the OBB and another OBB (the result is not compact).
+  OBB& operator+=(const OBB& other) {
+    *this = *this + other;
+    return *this;
+  }
+
+  /// @brief Return the merged OBB of current OBB and the other one (the result
+  /// is not compact).
+  OBB operator+(const OBB& other) const;
+
+  /// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
+  inline CoalScalar size() const { return extent.squaredNorm(); }
+
+  /// @brief Center of the OBB
+  inline const Vec3s& center() const { return To; }
+
+  /// @brief Width of the OBB.
+  inline CoalScalar width() const { return 2 * extent[0]; }
+
+  /// @brief Height of the OBB.
+  inline CoalScalar height() const { return 2 * extent[1]; }
+
+  /// @brief Depth of the OBB
+  inline CoalScalar depth() const { return 2 * extent[2]; }
+
+  /// @brief Volume of the OBB
+  inline CoalScalar volume() const { return width() * height() * depth(); }
+};
+
+/// @brief Translate the OBB bv
+COAL_DLLAPI OBB translate(const OBB& bv, const Vec3s& t);
+
+/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
+/// b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
+                         const OBB& b2);
+
+/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
+/// b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
+                         const OBB& b2, const CollisionRequest& request,
+                         CoalScalar& sqrDistLowerBound);
+
+/// Check collision between two boxes
+/// @param B, T orientation and position of first box,
+/// @param a half dimensions of first box,
+/// @param b half dimensions of second box.
+/// The second box is in identity configuration.
+COAL_DLLAPI bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                             const Vec3s& b);
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BV/OBBRSS.h b/include/coal/BV/OBBRSS.h
new file mode 100644
index 0000000000000000000000000000000000000000..97ea6bb1036bb04288ed9000af2d6a478a5a31ec
--- /dev/null
+++ b/include/coal/BV/OBBRSS.h
@@ -0,0 +1,160 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_OBBRSS_H
+#define COAL_OBBRSS_H
+
+#include "coal/BV/OBB.h"
+#include "coal/BV/RSS.h"
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief Class merging the OBB and RSS, can handle collision and distance
+/// simultaneously
+struct COAL_DLLAPI OBBRSS {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief OBB member, for rotation
+  OBB obb;
+
+  /// @brief RSS member, for distance
+  RSS rss;
+
+  /// @brief Equality operator
+  bool operator==(const OBBRSS& other) const {
+    return obb == other.obb && rss == other.rss;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const OBBRSS& other) const { return !(*this == other); }
+
+  /// @brief Check whether the OBBRSS contains a point
+  inline bool contain(const Vec3s& p) const { return obb.contain(p); }
+
+  /// @brief Check collision between two OBBRSS
+  bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); }
+
+  /// Check collision between two OBBRSS
+  /// @retval sqrDistLowerBound squared lower bound on distance between
+  ///         objects if they do not overlap.
+  bool overlap(const OBBRSS& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const {
+    return obb.overlap(other.obb, request, sqrDistLowerBound);
+  }
+
+  /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the
+  /// nearest points
+  CoalScalar distance(const OBBRSS& other, Vec3s* P = NULL,
+                      Vec3s* Q = NULL) const {
+    return rss.distance(other.rss, P, Q);
+  }
+
+  /// @brief Merge the OBBRSS and a point
+  OBBRSS& operator+=(const Vec3s& p) {
+    obb += p;
+    rss += p;
+    return *this;
+  }
+
+  /// @brief Merge two OBBRSS
+  OBBRSS& operator+=(const OBBRSS& other) {
+    *this = *this + other;
+    return *this;
+  }
+
+  /// @brief Merge two OBBRSS
+  OBBRSS operator+(const OBBRSS& other) const {
+    OBBRSS result;
+    result.obb = obb + other.obb;
+    result.rss = rss + other.rss;
+    return result;
+  }
+
+  /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
+  inline CoalScalar size() const { return obb.size(); }
+
+  /// @brief Center of the OBBRSS
+  inline const Vec3s& center() const { return obb.center(); }
+
+  /// @brief Width of the OBRSS
+  inline CoalScalar width() const { return obb.width(); }
+
+  /// @brief Height of the OBBRSS
+  inline CoalScalar height() const { return obb.height(); }
+
+  /// @brief Depth of the OBBRSS
+  inline CoalScalar depth() const { return obb.depth(); }
+
+  /// @brief Volume of the OBBRSS
+  inline CoalScalar volume() const { return obb.volume(); }
+};
+
+/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0)
+/// and b2 is in indentity
+inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,
+                    const OBBRSS& b2) {
+  return overlap(R0, T0, b1.obb, b2.obb);
+}
+
+/// Check collision between two OBBRSS
+/// @param  R0, T0 configuration of b1
+/// @param  b1 first OBBRSS in configuration (R0, T0)
+/// @param  b2 second OBBRSS in identity position
+/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do
+/// not overlap.
+inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,
+                    const OBBRSS& b2, const CollisionRequest& request,
+                    CoalScalar& sqrDistLowerBound) {
+  return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
+}
+
+/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0)
+/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points
+inline CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+                           const OBBRSS& b1, const OBBRSS& b2, Vec3s* P = NULL,
+                           Vec3s* Q = NULL) {
+  return distance(R0, T0, b1.rss, b2.rss, P, Q);
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BV/RSS.h b/include/coal/BV/RSS.h
new file mode 100644
index 0000000000000000000000000000000000000000..8bf2bd07a8d720b979acc6923e3c14c5208fb5f8
--- /dev/null
+++ b/include/coal/BV/RSS.h
@@ -0,0 +1,173 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_RSS_H
+#define COAL_RSS_H
+
+#include "coal/data_types.h"
+
+#include <boost/math/constants/constants.hpp>
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief A class for rectangle sphere-swept bounding volume
+struct COAL_DLLAPI RSS {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief Orientation of RSS. axis[i] is the ith column of the orientation
+  /// matrix for the RSS; it is also the i-th principle direction of the RSS. We
+  /// assume that axis[0] corresponds to the axis with the longest length,
+  /// axis[1] corresponds to the shorter one and axis[2] corresponds to the
+  /// shortest one.
+  Matrix3s axes;
+
+  /// @brief Origin of the rectangle in RSS
+  Vec3s Tr;
+
+  /// @brief Side lengths of rectangle
+  CoalScalar length[2];
+
+  /// @brief Radius of sphere summed with rectangle to form RSS
+  CoalScalar radius;
+
+  ///  @brief Default constructor with default values
+  RSS() : axes(Matrix3s::Zero()), Tr(Vec3s::Zero()), radius(-1) {
+    length[0] = 0;
+    length[1] = 0;
+  }
+
+  /// @brief Equality operator
+  bool operator==(const RSS& other) const {
+    return axes == other.axes && Tr == other.Tr &&
+           length[0] == other.length[0] && length[1] == other.length[1] &&
+           radius == other.radius;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const RSS& other) const { return !(*this == other); }
+
+  /// @brief Check whether the RSS contains a point
+  bool contain(const Vec3s& p) const;
+
+  /// @brief Check collision between two RSS
+  bool overlap(const RSS& other) const;
+
+  /// Not implemented
+  bool overlap(const RSS& other, const CollisionRequest&,
+               CoalScalar& sqrDistLowerBound) const {
+    sqrDistLowerBound = sqrt(-1);
+    return overlap(other);
+  }
+
+  /// @brief the distance between two RSS; P and Q, if not NULL, return the
+  /// nearest points
+  CoalScalar distance(const RSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
+
+  /// @brief A simple way to merge the RSS and a point, not compact.
+  /// @todo This function may have some bug.
+  RSS& operator+=(const Vec3s& p);
+
+  /// @brief Merge the RSS and another RSS
+  inline RSS& operator+=(const RSS& other) {
+    *this = *this + other;
+    return *this;
+  }
+
+  /// @brief Return the merged RSS of current RSS and the other one
+  RSS operator+(const RSS& other) const;
+
+  /// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
+  inline CoalScalar size() const {
+    return (std::sqrt(length[0] * length[0] + length[1] * length[1]) +
+            2 * radius);
+  }
+
+  /// @brief The RSS center
+  inline const Vec3s& center() const { return Tr; }
+
+  /// @brief Width of the RSS
+  inline CoalScalar width() const { return length[0] + 2 * radius; }
+
+  /// @brief Height of the RSS
+  inline CoalScalar height() const { return length[1] + 2 * radius; }
+
+  /// @brief Depth of the RSS
+  inline CoalScalar depth() const { return 2 * radius; }
+
+  /// @brief Volume of the RSS
+  inline CoalScalar volume() const {
+    return (length[0] * length[1] * 2 * radius +
+            4 * boost::math::constants::pi<CoalScalar>() * radius * radius *
+                radius);
+  }
+
+  /// @brief Check collision between two RSS and return the overlap part.
+  /// For RSS, we return nothing, as the overlap part of two RSSs usually is not
+  /// a RSS.
+  bool overlap(const RSS& other, RSS& /*overlap_part*/) const {
+    return overlap(other);
+  }
+};
+
+/// @brief distance between two RSS bounding volumes
+/// P and Q (optional return values) are the closest points in the rectangles,
+/// not the RSS. But the direction P - Q is the correct direction for cloest
+/// points Notice that P and Q are both in the local frame of the first RSS (not
+/// global frame and not even the local frame of object 1)
+COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+                                const RSS& b1, const RSS& b2, Vec3s* P = NULL,
+                                Vec3s* Q = NULL);
+
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
+/// b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
+                         const RSS& b2);
+
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
+/// b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
+                         const RSS& b2, const CollisionRequest& request,
+                         CoalScalar& sqrDistLowerBound);
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BV/kDOP.h b/include/coal/BV/kDOP.h
new file mode 100644
index 0000000000000000000000000000000000000000..cb837752654f98eb8045794636fcf48e9e3a10a5
--- /dev/null
+++ b/include/coal/BV/kDOP.h
@@ -0,0 +1,190 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_KDOP_H
+#define COAL_KDOP_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief KDOP class describes the KDOP collision structures. K is set as the
+/// template parameter, which should be 16, 18, or 24
+///  The KDOP structure is defined by some pairs of parallel planes defined by
+///  some axes.
+/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off
+/// some space of the edges:
+/// (-1,0,0) and (1,0,0)  -> indices 0 and 8
+/// (0,-1,0) and (0,1,0)  -> indices 1 and 9
+/// (0,0,-1) and (0,0,1)  -> indices 2 and 10
+/// (-1,-1,0) and (1,1,0) -> indices 3 and 11
+/// (-1,0,-1) and (1,0,1) -> indices 4 and 12
+/// (0,-1,-1) and (0,1,1) -> indices 5 and 13
+/// (-1,1,0) and (1,-1,0) -> indices 6 and 14
+/// (-1,0,1) and (1,0,-1) -> indices 7 and 15
+/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off
+/// some space of the edges:
+/// (-1,0,0) and (1,0,0)  -> indices 0 and 9
+/// (0,-1,0) and (0,1,0)  -> indices 1 and 10
+/// (0,0,-1) and (0,0,1)  -> indices 2 and 11
+/// (-1,-1,0) and (1,1,0) -> indices 3 and 12
+/// (-1,0,-1) and (1,0,1) -> indices 4 and 13
+/// (0,-1,-1) and (0,1,1) -> indices 5 and 14
+/// (-1,1,0) and (1,-1,0) -> indices 6 and 15
+/// (-1,0,1) and (1,0,-1) -> indices 7 and 16
+/// (0,-1,1) and (0,1,-1) -> indices 8 and 17
+/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off
+/// some space of the edges:
+/// (-1,0,0) and (1,0,0)  -> indices 0 and 12
+/// (0,-1,0) and (0,1,0)  -> indices 1 and 13
+/// (0,0,-1) and (0,0,1)  -> indices 2 and 14
+/// (-1,-1,0) and (1,1,0) -> indices 3 and 15
+/// (-1,0,-1) and (1,0,1) -> indices 4 and 16
+/// (0,-1,-1) and (0,1,1) -> indices 5 and 17
+/// (-1,1,0) and (1,-1,0) -> indices 6 and 18
+/// (-1,0,1) and (1,0,-1) -> indices 7 and 19
+/// (0,-1,1) and (0,1,-1) -> indices 8 and 20
+/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21
+/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22
+/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23
+template <short N>
+class COAL_DLLAPI KDOP {
+ protected:
+  /// @brief Origin's distances to N KDOP planes
+  Eigen::Array<CoalScalar, N, 1> dist_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief Creating kDOP containing nothing
+  KDOP();
+
+  /// @brief Creating kDOP containing only one point
+  KDOP(const Vec3s& v);
+
+  /// @brief Creating kDOP containing two points
+  KDOP(const Vec3s& a, const Vec3s& b);
+
+  /// @brief Equality operator
+  bool operator==(const KDOP& other) const {
+    return (dist_ == other.dist_).all();
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const KDOP& other) const {
+    return (dist_ != other.dist_).any();
+  }
+
+  /// @brief Check whether two KDOPs overlap.
+  bool overlap(const KDOP<N>& other) const;
+
+  /// @brief Check whether two KDOPs overlap.
+  /// @return true if collision happens.
+  /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
+  ///         they do not overlap.
+  bool overlap(const KDOP<N>& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const;
+
+  /// @brief The distance between two KDOP<N>. Not implemented.
+  CoalScalar distance(const KDOP<N>& other, Vec3s* P = NULL,
+                      Vec3s* Q = NULL) const;
+
+  /// @brief Merge the point and the KDOP
+  KDOP<N>& operator+=(const Vec3s& p);
+
+  /// @brief Merge two KDOPs
+  KDOP<N>& operator+=(const KDOP<N>& other);
+
+  /// @brief Create a KDOP by mergin two KDOPs
+  KDOP<N> operator+(const KDOP<N>& other) const;
+
+  /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
+  inline CoalScalar size() const {
+    return width() * width() + height() * height() + depth() * depth();
+  }
+
+  /// @brief The (AABB) center
+  inline Vec3s center() const {
+    return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2;
+  }
+
+  /// @brief The (AABB) width
+  inline CoalScalar width() const { return dist_[N / 2] - dist_[0]; }
+
+  /// @brief The (AABB) height
+  inline CoalScalar height() const { return dist_[N / 2 + 1] - dist_[1]; }
+
+  /// @brief The (AABB) depth
+  inline CoalScalar depth() const { return dist_[N / 2 + 2] - dist_[2]; }
+
+  /// @brief The (AABB) volume
+  inline CoalScalar volume() const { return width() * height() * depth(); }
+
+  inline CoalScalar dist(short i) const { return dist_[i]; }
+
+  inline CoalScalar& dist(short i) { return dist_[i]; }
+
+  //// @brief Check whether one point is inside the KDOP
+  bool inside(const Vec3s& p) const;
+};
+
+template <short N>
+bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
+             const KDOP<N>& /*b2*/) {
+  COAL_THROW_PRETTY("not implemented", std::logic_error);
+}
+
+template <short N>
+bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
+             const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/,
+             CoalScalar& /*sqrDistLowerBound*/) {
+  COAL_THROW_PRETTY("not implemented", std::logic_error);
+}
+
+/// @brief translate the KDOP BV
+template <short N>
+COAL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t);
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BV/kIOS.h b/include/coal/BV/kIOS.h
new file mode 100644
index 0000000000000000000000000000000000000000..83277ab6a632917fd84f96b0452fda6c211a4adf
--- /dev/null
+++ b/include/coal/BV/kIOS.h
@@ -0,0 +1,193 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_KIOS_H
+#define COAL_KIOS_H
+
+#include "coal/BV/OBB.h"
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief A class describing the kIOS collision structure, which is a set of
+/// spheres.
+class COAL_DLLAPI kIOS {
+  /// @brief One sphere in kIOS
+  struct COAL_DLLAPI kIOS_Sphere {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    Vec3s o;
+    CoalScalar r;
+
+    bool operator==(const kIOS_Sphere& other) const {
+      return o == other.o && r == other.r;
+    }
+
+    bool operator!=(const kIOS_Sphere& other) const {
+      return !(*this == other);
+    }
+  };
+
+  /// @brief generate one sphere enclosing two spheres
+  static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0,
+                                   const kIOS_Sphere& s1) {
+    Vec3s d = s1.o - s0.o;
+    CoalScalar dist2 = d.squaredNorm();
+    CoalScalar diff_r = s1.r - s0.r;
+
+    /** The sphere with the larger radius encloses the other */
+    if (diff_r * diff_r >= dist2) {
+      if (s1.r > s0.r)
+        return s1;
+      else
+        return s0;
+    } else /** spheres partially overlapping or disjoint */
+    {
+      float dist = (float)std::sqrt(dist2);
+      kIOS_Sphere s;
+      s.r = dist + s0.r + s1.r;
+      if (dist > 0)
+        s.o = s0.o + d * ((s.r - s0.r) / dist);
+      else
+        s.o = s0.o;
+      return s;
+    }
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief Equality operator
+  bool operator==(const kIOS& other) const {
+    bool res = obb == other.obb && num_spheres == other.num_spheres;
+    if (!res) return false;
+
+    for (size_t k = 0; k < num_spheres; ++k) {
+      if (spheres[k] != other.spheres[k]) return false;
+    }
+
+    return true;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const kIOS& other) const { return !(*this == other); }
+
+  static constexpr size_t max_num_spheres = 5;
+
+  /// @brief The (at most) five spheres for intersection
+  kIOS_Sphere spheres[max_num_spheres];
+
+  /// @brief The number of spheres, no larger than 5
+  unsigned int num_spheres;
+
+  /// @ OBB related with kIOS
+  OBB obb;
+
+  /// @brief Check whether the kIOS contains a point
+  bool contain(const Vec3s& p) const;
+
+  /// @brief Check collision between two kIOS
+  bool overlap(const kIOS& other) const;
+
+  /// @brief Check collision between two kIOS
+  bool overlap(const kIOS& other, const CollisionRequest&,
+               CoalScalar& sqrDistLowerBound) const;
+
+  /// @brief The distance between two kIOS
+  CoalScalar distance(const kIOS& other, Vec3s* P = NULL,
+                      Vec3s* Q = NULL) const;
+
+  /// @brief A simple way to merge the kIOS and a point
+  kIOS& operator+=(const Vec3s& p);
+
+  /// @brief Merge the kIOS and another kIOS
+  kIOS& operator+=(const kIOS& other) {
+    *this = *this + other;
+    return *this;
+  }
+
+  /// @brief Return the merged kIOS of current kIOS and the other one
+  kIOS operator+(const kIOS& other) const;
+
+  /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
+  CoalScalar size() const;
+
+  /// @brief Center of the kIOS
+  const Vec3s& center() const { return spheres[0].o; }
+
+  /// @brief Width of the kIOS
+  CoalScalar width() const;
+
+  /// @brief Height of the kIOS
+  CoalScalar height() const;
+
+  /// @brief Depth of the kIOS
+  CoalScalar depth() const;
+
+  /// @brief Volume of the kIOS
+  CoalScalar volume() const;
+};
+
+/// @brief Translate the kIOS BV
+COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3s& t);
+
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
+/// and b2 is in identity.
+/// @todo Not efficient
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
+                         const kIOS& b2);
+
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
+/// and b2 is in identity.
+/// @todo Not efficient
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
+                         const kIOS& b2, const CollisionRequest& request,
+                         CoalScalar& sqrDistLowerBound);
+
+/// @brief Approximate distance between two kIOS bounding volumes
+/// @todo P and Q is not returned, need implementation
+COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+                                const kIOS& b1, const kIOS& b2, Vec3s* P = NULL,
+                                Vec3s* Q = NULL);
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BVH/BVH_front.h b/include/coal/BVH/BVH_front.h
new file mode 100644
index 0000000000000000000000000000000000000000..fe471b9b31261ebe427b527402c3db50f5c110ee
--- /dev/null
+++ b/include/coal/BVH/BVH_front.h
@@ -0,0 +1,76 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_BVH_FRONT_H
+#define COAL_BVH_FRONT_H
+
+#include <list>
+
+#include "coal/config.hh"
+
+namespace coal {
+
+/// @brief Front list acceleration for collision
+/// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where
+/// the traversal terminates while performing a query during a given time
+/// instance. The front list reflects the subset of a BVTT that is traversed for
+/// that particular proximity query.
+struct COAL_DLLAPI BVHFrontNode {
+  /// @brief The nodes to start in the future, i.e. the wave front of the
+  /// traversal tree.
+  unsigned int left, right;
+
+  /// @brief The front node is not valid when collision is detected on the front
+  /// node.
+  bool valid;
+
+  BVHFrontNode(unsigned int left_, unsigned int right_)
+      : left(left_), right(right_), valid(true) {}
+};
+
+/// @brief BVH front list is a list of front nodes.
+typedef std::list<BVHFrontNode> BVHFrontList;
+
+/// @brief Add new front node into the front list
+inline void updateFrontList(BVHFrontList* front_list, unsigned int b1,
+                            unsigned int b2) {
+  if (front_list) front_list->push_back(BVHFrontNode(b1, b2));
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BVH/BVH_internal.h b/include/coal/BVH/BVH_internal.h
new file mode 100644
index 0000000000000000000000000000000000000000..49884639a3cb702d0a0d5a2b911a742abb65c4be
--- /dev/null
+++ b/include/coal/BVH/BVH_internal.h
@@ -0,0 +1,87 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_BVH_INTERNAL_H
+#define COAL_BVH_INTERNAL_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+/// @brief States for BVH construction
+/// empty->begun->processed ->replace_begun->processed -> ......
+///                        |
+///                        |-> update_begun -> updated -> .....
+enum BVHBuildState {
+  BVH_BUILD_STATE_EMPTY,      /// empty state, immediately after constructor
+  BVH_BUILD_STATE_BEGUN,      /// after beginModel(), state for adding geometry
+                              /// primitives
+  BVH_BUILD_STATE_PROCESSED,  /// after tree has been build, ready for cd use
+  BVH_BUILD_STATE_UPDATE_BEGUN,  /// after beginUpdateModel(), state for
+                                 /// updating geometry primitives
+  BVH_BUILD_STATE_UPDATED,  /// after tree has been build for updated geometry,
+                            /// ready for ccd use
+  BVH_BUILD_STATE_REPLACE_BEGUN  /// after beginReplaceModel(), state for
+                                 /// replacing geometry primitives
+};
+
+/// @brief Error code for BVH
+enum BVHReturnCode {
+  BVH_OK = 0,  /// BVH is valid
+  BVH_ERR_MODEL_OUT_OF_MEMORY =
+      -1,  /// Cannot allocate memory for vertices and triangles
+  BVH_ERR_BUILD_OUT_OF_SEQUENCE =
+      -2,  /// BVH construction does not follow correct sequence
+  BVH_ERR_BUILD_EMPTY_MODEL = -3,  /// BVH geometry is not prepared
+  BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME =
+      -4,  /// BVH geometry in previous frame is not prepared
+  BVH_ERR_UNSUPPORTED_FUNCTION = -5,  /// BVH funtion is not supported
+  BVH_ERR_UNUPDATED_MODEL = -6,       /// BVH model update failed
+  BVH_ERR_INCORRECT_DATA = -7,        /// BVH data is not valid
+  BVH_ERR_UNKNOWN = -8                /// Unknown failure
+};
+
+/// @brief BVH model type
+enum BVHModelType {
+  BVH_MODEL_UNKNOWN,    /// @brief unknown model type
+  BVH_MODEL_TRIANGLES,  /// @brief triangle model
+  BVH_MODEL_POINTCLOUD  /// @brief point cloud model
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BVH/BVH_model.h b/include/coal/BVH/BVH_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..722578f48ebc101dd1e35e5f454a769bff30504e
--- /dev/null
+++ b/include/coal/BVH/BVH_model.h
@@ -0,0 +1,539 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2020-2022, INRIA
+ *  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 COAL_BVH_MODEL_H
+#define COAL_BVH_MODEL_H
+
+#include "coal/fwd.hh"
+#include "coal/collision_object.h"
+#include "coal/BVH/BVH_internal.h"
+#include "coal/BV/BV_node.h"
+
+#include <vector>
+#include <memory>
+#include <iostream>
+
+namespace coal {
+
+/// @addtogroup Construction_Of_BVH
+/// @{
+
+class ConvexBase;
+
+template <typename BV>
+class BVFitter;
+template <typename BV>
+class BVSplitter;
+
+/// @brief A base class describing the bounding hierarchy of a mesh model or a
+/// point cloud model (which is viewed as a degraded version of mesh)
+class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
+ public:
+  /// @brief Geometry point data
+  std::shared_ptr<std::vector<Vec3s>> vertices;
+
+  /// @brief Geometry triangle index data, will be NULL for point clouds
+  std::shared_ptr<std::vector<Triangle>> tri_indices;
+
+  /// @brief Geometry point data in previous frame
+  std::shared_ptr<std::vector<Vec3s>> prev_vertices;
+
+  /// @brief Number of triangles
+  unsigned int num_tris;
+
+  /// @brief Number of points
+  unsigned int num_vertices;
+
+  /// @brief The state of BVH building process
+  BVHBuildState build_state;
+
+  /// @brief Convex<Triangle> representation of this object
+  shared_ptr<ConvexBase> convex;
+
+  /// @brief Model type described by the instance
+  BVHModelType getModelType() const {
+    if (num_tris && num_vertices)
+      return BVH_MODEL_TRIANGLES;
+    else if (num_vertices)
+      return BVH_MODEL_POINTCLOUD;
+    else
+      return BVH_MODEL_UNKNOWN;
+  }
+
+  /// @brief Constructing an empty BVH
+  BVHModelBase();
+
+  /// @brief copy from another BVH
+  BVHModelBase(const BVHModelBase& other);
+
+  /// @brief deconstruction, delete mesh data related.
+  virtual ~BVHModelBase() {}
+
+  /// @brief Get the object type: it is a BVH
+  OBJECT_TYPE getObjectType() const { return OT_BVH; }
+
+  /// @brief Compute the AABB for the BVH, used for broad-phase collision
+  void computeLocalAABB();
+
+  /// @brief Begin a new BVH model
+  int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
+
+  /// @brief Add one point in the new BVH model
+  int addVertex(const Vec3s& p);
+
+  /// @brief Add points in the new BVH model
+  int addVertices(const MatrixX3s& points);
+
+  /// @brief Add triangles in the new BVH model
+  int addTriangles(const Matrixx3i& triangles);
+
+  /// @brief Add one triangle in the new BVH model
+  int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
+
+  /// @brief Add a set of triangles in the new BVH model
+  int addSubModel(const std::vector<Vec3s>& ps,
+                  const std::vector<Triangle>& ts);
+
+  /// @brief Add a set of points in the new BVH model
+  int addSubModel(const std::vector<Vec3s>& ps);
+
+  /// @brief End BVH model construction, will build the bounding volume
+  /// hierarchy
+  int endModel();
+
+  /// @brief Replace the geometry information of current frame (i.e. should have
+  /// the same mesh topology with the previous frame)
+  int beginReplaceModel();
+
+  /// @brief Replace one point in the old BVH model
+  int replaceVertex(const Vec3s& p);
+
+  /// @brief Replace one triangle in the old BVH model
+  int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
+
+  /// @brief Replace a set of points in the old BVH model
+  int replaceSubModel(const std::vector<Vec3s>& ps);
+
+  /// @brief End BVH model replacement, will also refit or rebuild the bounding
+  /// volume hierarchy
+  int endReplaceModel(bool refit = true, bool bottomup = true);
+
+  /// @brief Replace the geometry information of current frame (i.e. should have
+  /// the same mesh topology with the previous frame). The current frame will be
+  /// saved as the previous frame in prev_vertices.
+  int beginUpdateModel();
+
+  /// @brief Update one point in the old BVH model
+  int updateVertex(const Vec3s& p);
+
+  /// @brief Update one triangle in the old BVH model
+  int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
+
+  /// @brief Update a set of points in the old BVH model
+  int updateSubModel(const std::vector<Vec3s>& ps);
+
+  /// @brief End BVH model update, will also refit or rebuild the bounding
+  /// volume hierarchy
+  int endUpdateModel(bool refit = true, bool bottomup = true);
+
+  /// @brief Build this \ref Convex "Convex<Triangle>" representation of this
+  /// model. The result is stored in attribute \ref convex. \note this only
+  /// takes the points of this model. It does not check that the
+  ///       object is convex. It does not compute a convex hull.
+  void buildConvexRepresentation(bool share_memory);
+
+  /// @brief Build a convex hull
+  /// and store it in attribute \ref convex.
+  /// \param keepTriangle whether the convex should be triangulated.
+  /// \param qhullCommand see \ref ConvexBase::convexHull.
+  /// \return \c true if this object is convex, hence the convex hull represents
+  ///         the same object.
+  /// \sa ConvexBase::convexHull
+  /// \warning At the moment, the return value only checks whether there are as
+  ///          many points in the convex hull as in the original object. This is
+  ///          neither necessary (duplicated vertices get merged) nor sufficient
+  ///          (think of a U with 4 vertices and 3 edges).
+  bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
+
+  virtual int memUsage(const bool msg = false) const = 0;
+
+  /// @brief This is a special acceleration: BVH_model default stores the BV's
+  /// transform in world coordinate. However, we can also store each BV's
+  /// transform related to its parent BV node. When traversing the BVH, this can
+  /// save one matrix transformation.
+  virtual void makeParentRelative() = 0;
+
+  Vec3s computeCOM() const {
+    CoalScalar vol = 0;
+    Vec3s com(0, 0, 0);
+    if (!(vertices.get())) {
+      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+                   "vertices."
+                << std::endl;
+      return com;
+    }
+    const std::vector<Vec3s>& vertices_ = *vertices;
+    if (!(tri_indices.get())) {
+      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+                   "triangles."
+                << std::endl;
+      return com;
+    }
+    const std::vector<Triangle>& tri_indices_ = *tri_indices;
+
+    for (unsigned int i = 0; i < num_tris; ++i) {
+      const Triangle& tri = tri_indices_[i];
+      CoalScalar d_six_vol =
+          (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
+      vol += d_six_vol;
+      com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) *
+             d_six_vol;
+    }
+
+    return com / (vol * 4);
+  }
+
+  CoalScalar computeVolume() const {
+    CoalScalar vol = 0;
+    if (!(vertices.get())) {
+      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+                   "vertices."
+                << std::endl;
+      return vol;
+    }
+    const std::vector<Vec3s>& vertices_ = *vertices;
+    if (!(tri_indices.get())) {
+      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+                   "triangles."
+                << std::endl;
+      return vol;
+    }
+    const std::vector<Triangle>& tri_indices_ = *tri_indices;
+    for (unsigned int i = 0; i < num_tris; ++i) {
+      const Triangle& tri = tri_indices_[i];
+      CoalScalar d_six_vol =
+          (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
+      vol += d_six_vol;
+    }
+
+    return vol / 6;
+  }
+
+  Matrix3s computeMomentofInertia() const {
+    Matrix3s C = Matrix3s::Zero();
+
+    Matrix3s C_canonical;
+    C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0,
+        1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0;
+
+    if (!(vertices.get())) {
+      std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
+                   "not contain vertices."
+                << std::endl;
+      return C;
+    }
+    const std::vector<Vec3s>& vertices_ = *vertices;
+    if (!(vertices.get())) {
+      std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
+                   "not contain vertices."
+                << std::endl;
+      return C;
+    }
+    const std::vector<Triangle>& tri_indices_ = *tri_indices;
+    for (unsigned int i = 0; i < num_tris; ++i) {
+      const Triangle& tri = tri_indices_[i];
+      const Vec3s& v1 = vertices_[tri[0]];
+      const Vec3s& v2 = vertices_[tri[1]];
+      const Vec3s& v3 = vertices_[tri[2]];
+      Matrix3s A;
+      A << v1.transpose(), v2.transpose(), v3.transpose();
+      C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
+    }
+
+    return C.trace() * Matrix3s::Identity() - C;
+  }
+
+ protected:
+  virtual void deleteBVs() = 0;
+  virtual bool allocateBVs() = 0;
+
+  /// @brief Build the bounding volume hierarchy
+  virtual int buildTree() = 0;
+
+  /// @brief Refit the bounding volume hierarchy
+  virtual int refitTree(bool bottomup) = 0;
+
+  unsigned int num_tris_allocated;
+  unsigned int num_vertices_allocated;
+  unsigned int num_vertex_updated;  /// for ccd vertex update
+
+ protected:
+  /// \brief Comparison operators
+  virtual bool isEqual(const CollisionGeometry& other) const;
+};
+
+/// @brief A class describing the bounding hierarchy of a mesh model or a point
+/// cloud model (which is viewed as a degraded version of mesh) \tparam BV one
+/// of the bounding volume class in \ref Bounding_Volume.
+template <typename BV>
+class COAL_DLLAPI BVHModel : public BVHModelBase {
+  typedef BVHModelBase Base;
+
+ public:
+  using bv_node_vector_t =
+      std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;
+
+  /// @brief Split rule to split one BV node into two children
+  shared_ptr<BVSplitter<BV>> bv_splitter;
+
+  /// @brief Fitting rule to fit a BV node to a set of geometry primitives
+  shared_ptr<BVFitter<BV>> bv_fitter;
+
+  /// @brief Default constructor to build an empty BVH
+  BVHModel();
+
+  /// @brief Copy constructor from another BVH
+  ///
+  /// \param[in] other BVHModel to copy.
+  ///
+  BVHModel(const BVHModel& other);
+
+  /// @brief Clone *this into a new BVHModel
+  virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
+
+  /// @brief deconstruction, delete mesh data related.
+  ~BVHModel() {}
+
+  /// @brief We provide getBV() and getNumBVs() because BVH may be compressed
+  /// (in future), so we must provide some flexibility here
+
+  /// @brief Access the bv giving the its index
+  const BVNode<BV>& getBV(unsigned int i) const {
+    assert(i < num_bvs);
+    return (*bvs)[i];
+  }
+
+  /// @brief Access the bv giving the its index
+  BVNode<BV>& getBV(unsigned int i) {
+    assert(i < num_bvs);
+    return (*bvs)[i];
+  }
+
+  /// @brief Get the number of bv in the BVH
+  unsigned int getNumBVs() const { return num_bvs; }
+
+  /// @brief Get the BV type: default is unknown
+  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+  /// @brief Check the number of memory used
+  int memUsage(const bool msg) const;
+
+  /// @brief This is a special acceleration: BVH_model default stores the BV's
+  /// transform in world coordinate. However, we can also store each BV's
+  /// transform related to its parent BV node. When traversing the BVH, this can
+  /// save one matrix transformation.
+  void makeParentRelative() {
+    Matrix3s I(Matrix3s::Identity());
+    makeParentRelativeRecurse(0, I, Vec3s::Zero());
+  }
+
+ protected:
+  void deleteBVs();
+  bool allocateBVs();
+
+  unsigned int num_bvs_allocated;
+  std::shared_ptr<std::vector<unsigned int>> primitive_indices;
+
+  /// @brief Bounding volume hierarchy
+  std::shared_ptr<bv_node_vector_t> bvs;
+
+  /// @brief Number of BV nodes in bounding volume hierarchy
+  unsigned int num_bvs;
+
+  /// @brief Build the bounding volume hierarchy
+  int buildTree();
+
+  /// @brief Refit the bounding volume hierarchy
+  int refitTree(bool bottomup);
+
+  /// @brief Refit the bounding volume hierarchy in a top-down way (slow but
+  /// more compact)
+  int refitTree_topdown();
+
+  /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but
+  /// less compact)
+  int refitTree_bottomup();
+
+  /// @brief Recursive kernel for hierarchy construction
+  int recursiveBuildTree(int bv_id, unsigned int first_primitive,
+                         unsigned int num_primitives);
+
+  /// @brief Recursive kernel for bottomup refitting
+  int recursiveRefitTree_bottomup(int bv_id);
+
+  /// @ recursively compute each bv's transform related to its parent. For
+  /// default BV, only the translation works. For oriented BV (OBB, RSS,
+  /// OBBRSS), special implementation is provided.
+  void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+                                 const Vec3s& parent_c) {
+    bv_node_vector_t& bvs_ = *bvs;
+    if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
+      makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
+                                parent_axes,
+                                bvs_[static_cast<size_t>(bv_id)].getCenter());
+
+      makeParentRelativeRecurse(
+          bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
+          bvs_[static_cast<size_t>(bv_id)].getCenter());
+    }
+
+    bvs_[static_cast<size_t>(bv_id)].bv =
+        translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c);
+  }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const BVHModel& other = *other_ptr;
+
+    bool res = Base::isEqual(other);
+    if (!res) return false;
+
+    // unsigned int other_num_primitives = 0;
+    // if(other.primitive_indices)
+    // {
+
+    //   switch(other.getModelType())
+    //   {
+    //     case BVH_MODEL_TRIANGLES:
+    //       other_num_primitives = num_tris;
+    //       break;
+    //     case BVH_MODEL_POINTCLOUD:
+    //       other_num_primitives = num_vertices;
+    //       break;
+    //     default:
+    //       ;
+    //   }
+    // }
+
+    //    unsigned int num_primitives = 0;
+    //    if(primitive_indices)
+    //    {
+    //
+    //      switch(other.getModelType())
+    //      {
+    //        case BVH_MODEL_TRIANGLES:
+    //          num_primitives = num_tris;
+    //          break;
+    //        case BVH_MODEL_POINTCLOUD:
+    //          num_primitives = num_vertices;
+    //          break;
+    //        default:
+    //          ;
+    //      }
+    //    }
+    //
+    //    if(num_primitives != other_num_primitives)
+    //      return false;
+    //
+    //    for(int k = 0; k < num_primitives; ++k)
+    //    {
+    //      if(primitive_indices[k] != other.primitive_indices[k])
+    //        return false;
+    //    }
+
+    if (num_bvs != other.num_bvs) return false;
+
+    if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
+      return false;
+    if (bvs.get() && other.bvs.get()) {
+      const bv_node_vector_t& bvs_ = *bvs;
+      const bv_node_vector_t& other_bvs_ = *(other.bvs);
+      for (unsigned int k = 0; k < num_bvs; ++k) {
+        if (bvs_[k] != other_bvs_[k]) return false;
+      }
+    }
+
+    return true;
+  }
+};
+
+/// @}
+
+template <>
+void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+                                              const Vec3s& parent_c);
+
+template <>
+void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+                                              const Vec3s& parent_c);
+
+template <>
+void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
+                                                 Matrix3s& parent_axes,
+                                                 const Vec3s& parent_c);
+
+/// @brief Specialization of getNodeType() for BVHModel with different BV types
+template <>
+NODE_TYPE BVHModel<AABB>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<OBB>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<RSS>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<kIOS>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const;
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/BVH/BVH_utility.h b/include/coal/BVH/BVH_utility.h
new file mode 100644
index 0000000000000000000000000000000000000000..daf0e478163da2351972cb1b129404467a2ba19b
--- /dev/null
+++ b/include/coal/BVH/BVH_utility.h
@@ -0,0 +1,115 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_BVH_UTILITY_H
+#define COAL_BVH_UTILITY_H
+
+#include "coal/BVH/BVH_model.h"
+
+namespace coal {
+/// @brief Extract the part of the BVHModel that is inside an AABB.
+/// A triangle in collision with the AABB is considered inside.
+template <typename BV>
+COAL_DLLAPI BVHModel<BV>* BVHExtract(const BVHModel<BV>& model,
+                                     const Transform3s& pose, const AABB& aabb);
+
+template <>
+COAL_DLLAPI BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model,
+                                      const Transform3s& pose,
+                                      const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model,
+                                       const Transform3s& pose,
+                                       const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model,
+                                      const Transform3s& pose,
+                                      const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model,
+                                       const Transform3s& pose,
+                                       const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model,
+                                         const Transform3s& pose,
+                                         const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model,
+                                            const Transform3s& pose,
+                                            const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model,
+                                            const Transform3s& pose,
+                                            const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
+                                            const Transform3s& pose,
+                                            const AABB& aabb);
+
+/// @brief Compute the covariance matrix for a set or subset of points. if ts =
+/// null, then indices refer to points directly; otherwise refer to triangles
+COAL_DLLAPI void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+                               unsigned int* indices, unsigned int n,
+                               Matrix3s& M);
+
+/// @brief Compute the RSS bounding volume parameters: radius, rectangle size
+/// and the origin, given the BV axises.
+COAL_DLLAPI void getRadiusAndOriginAndRectangleSize(
+    Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n,
+    const Matrix3s& axes, Vec3s& origin, CoalScalar l[2], CoalScalar& r);
+
+/// @brief Compute the bounding volume extent and center for a set or subset of
+/// points, given the BV axises.
+COAL_DLLAPI void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+                                    unsigned int* indices, unsigned int n,
+                                    Matrix3s& axes, Vec3s& center,
+                                    Vec3s& extent);
+
+/// @brief Compute the center and radius for a triangle's circumcircle
+COAL_DLLAPI void circumCircleComputation(const Vec3s& a, const Vec3s& b,
+                                         const Vec3s& c, Vec3s& center,
+                                         CoalScalar& radius);
+
+/// @brief Compute the maximum distance from a given center point to a point
+/// cloud
+COAL_DLLAPI CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+                                       unsigned int* indices, unsigned int n,
+                                       const Vec3s& query);
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase.h b/include/coal/broadphase/broadphase.h
new file mode 100644
index 0000000000000000000000000000000000000000..887dbb141156a7a8026b1058c537a628eb88fce9
--- /dev/null
+++ b/include/coal/broadphase/broadphase.h
@@ -0,0 +1,48 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, Inria
+ *  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.
+ */
+
+#ifndef COAL_BROADPHASE_BROADPHASE_H
+#define COAL_BROADPHASE_BROADPHASE_H
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
+#include "coal/broadphase/broadphase_bruteforce.h"
+#include "coal/broadphase/broadphase_SaP.h"
+#include "coal/broadphase/broadphase_SSaP.h"
+#include "coal/broadphase/broadphase_interval_tree.h"
+#include "coal/broadphase/broadphase_spatialhash.h"
+
+#include "coal/broadphase/default_broadphase_callbacks.h"
+
+#endif  // ifndef COAL_BROADPHASE_BROADPHASE_H
diff --git a/include/coal/broadphase/broadphase_SSaP.h b/include/coal/broadphase/broadphase_SSaP.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ca05050ca97441db3b419832e99e32950dbfaa8
--- /dev/null
+++ b/include/coal/broadphase/broadphase_SSaP.h
@@ -0,0 +1,146 @@
+/*
+ * 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 COAL_BROAD_PHASE_SSAP_H
+#define COAL_BROAD_PHASE_SSAP_H
+
+#include <vector>
+#include "coal/broadphase/broadphase_collision_manager.h"
+
+namespace coal {
+
+/// @brief Simple SAP collision manager
+class COAL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  SSaPCollisionManager();
+
+  /// @brief remove one object from the manager
+  void registerObject(CollisionObject* obj);
+
+  /// @brief add one object to the manager
+  void unregisterObject(CollisionObject* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  virtual void update();
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+ protected:
+  /// @brief check collision between one object and a list of objects, return
+  /// value is whether stop is possible
+  bool checkColl(
+      typename std::vector<CollisionObject*>::const_iterator pos_start,
+      typename std::vector<CollisionObject*>::const_iterator pos_end,
+      CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief check distance between one object and a list of objects, return
+  /// value is whether stop is possible
+  bool checkDis(
+      typename std::vector<CollisionObject*>::const_iterator pos_start,
+      typename std::vector<CollisionObject*>::const_iterator pos_end,
+      CollisionObject* obj, DistanceCallBackBase* callback,
+      CoalScalar& min_dist) const;
+
+  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
+                 CoalScalar& min_dist) const;
+
+  static int selectOptimalAxis(
+      const std::vector<CollisionObject*>& objs_x,
+      const std::vector<CollisionObject*>& objs_y,
+      const std::vector<CollisionObject*>& objs_z,
+      typename std::vector<CollisionObject*>::const_iterator& it_beg,
+      typename std::vector<CollisionObject*>::const_iterator& it_end);
+
+  /// @brief Objects sorted according to lower x value
+  std::vector<CollisionObject*> objs_x;
+
+  /// @brief Objects sorted according to lower y value
+  std::vector<CollisionObject*> objs_y;
+
+  /// @brief Objects sorted according to lower z value
+  std::vector<CollisionObject*> objs_z;
+
+  /// @brief tag about whether the environment is maintained suitably (i.e., the
+  /// objs_x, objs_y, objs_z are sorted correctly
+  bool setup_;
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_SaP.h b/include/coal/broadphase/broadphase_SaP.h
new file mode 100644
index 0000000000000000000000000000000000000000..118ee65a979227111d5a5cd0db0e95f861690a94
--- /dev/null
+++ b/include/coal/broadphase/broadphase_SaP.h
@@ -0,0 +1,224 @@
+/*
+ * 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 COAL_BROAD_PHASE_SAP_H
+#define COAL_BROAD_PHASE_SAP_H
+
+#include <map>
+#include <list>
+
+#include "coal/broadphase/broadphase_collision_manager.h"
+
+namespace coal {
+
+/// @brief Rigorous SAP collision manager
+class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  SaPCollisionManager();
+
+  ~SaPCollisionManager();
+
+  /// @brief add objects to the manager
+  void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+  /// @brief remove one object from the manager
+  void registerObject(CollisionObject* obj);
+
+  /// @brief add one object to the manager
+  void unregisterObject(CollisionObject* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  virtual void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+ protected:
+  struct EndPoint;
+
+  /// @brief SAP interval for one object
+  struct SaPAABB {
+    /// @brief object
+    CollisionObject* obj;
+
+    /// @brief lower bound end point of the interval
+    EndPoint* lo;
+
+    /// @brief higher bound end point of the interval
+    EndPoint* hi;
+
+    /// @brief cached AABB value
+    AABB cached;
+  };
+
+  /// @brief End point for an interval
+  struct EndPoint {
+    /// @brief tag for whether it is a lower bound or higher bound of an
+    /// interval, 0 for lo, and 1 for hi
+    char minmax;
+
+    /// @brief back pointer to SAP interval
+    SaPAABB* aabb;
+
+    /// @brief the previous end point in the end point list
+    EndPoint* prev[3];
+
+    /// @brief the next end point in the end point list
+    EndPoint* next[3];
+
+    /// @brief get the value of the end point
+    const Vec3s& getVal() const;
+
+    /// @brief set the value of the end point
+    Vec3s& getVal();
+
+    CoalScalar getVal(int i) const;
+
+    CoalScalar& getVal(int i);
+  };
+
+  /// @brief A pair of objects that are not culling away and should further
+  /// check collision
+  struct SaPPair {
+    SaPPair(CollisionObject* a, CollisionObject* b);
+
+    CollisionObject* obj1;
+    CollisionObject* obj2;
+
+    bool operator==(const SaPPair& other) const;
+  };
+
+  /// @brief Functor to help unregister one object
+  class COAL_DLLAPI isUnregistered {
+    CollisionObject* obj;
+
+   public:
+    isUnregistered(CollisionObject* obj_);
+
+    bool operator()(const SaPPair& pair) const;
+  };
+
+  /// @brief Functor to help remove collision pairs no longer valid (i.e.,
+  /// should be culled away)
+  class COAL_DLLAPI isNotValidPair {
+    CollisionObject* obj1;
+    CollisionObject* obj2;
+
+   public:
+    isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_);
+
+    bool operator()(const SaPPair& pair);
+  };
+
+  void update_(SaPAABB* updated_aabb);
+
+  void updateVelist();
+
+  /// @brief End point list for x, y, z coordinates
+  EndPoint* elist[3];
+
+  /// @brief vector version of elist, for acceleration
+  std::vector<EndPoint*> velist[3];
+
+  /// @brief SAP interval list
+  std::list<SaPAABB*> AABB_arr;
+
+  /// @brief The pair of objects that should further check for collision
+  std::list<SaPPair> overlap_pairs;
+
+  int optimal_axis;
+
+  std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
+
+  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
+                 CoalScalar& min_dist) const;
+
+  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  void addToOverlapPairs(const SaPPair& p);
+
+  void removeFromOverlapPairs(const SaPPair& p);
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_bruteforce.h b/include/coal/broadphase/broadphase_bruteforce.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab1f9d564ed9d2b40907b94d975b5e880ec22e01
--- /dev/null
+++ b/include/coal/broadphase/broadphase_bruteforce.h
@@ -0,0 +1,112 @@
+/*
+ * 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 COAL_BROAD_PHASE_BRUTE_FORCE_H
+#define COAL_BROAD_PHASE_BRUTE_FORCE_H
+
+#include <list>
+#include "coal/broadphase/broadphase_collision_manager.h"
+
+namespace coal {
+
+/// @brief Brute force N-body collision manager
+class COAL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  NaiveCollisionManager();
+
+  /// @brief add objects to the manager
+  void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+  /// @brief add one object to the manager
+  void registerObject(CollisionObject* obj);
+
+  /// @brief remove one object from the manager
+  void unregisterObject(CollisionObject* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  virtual void update();
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+ protected:
+  /// @brief objects belonging to the manager are stored in a list structure
+  std::list<CollisionObject*> objs;
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_callbacks.h b/include/coal/broadphase/broadphase_callbacks.h
new file mode 100644
index 0000000000000000000000000000000000000000..5a132d1065037ec190628ad532c1566bf9c4d8af
--- /dev/null
+++ b/include/coal/broadphase/broadphase_callbacks.h
@@ -0,0 +1,96 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, INRIA
+ *  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 the copyright holder 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 Justin Carpentier (justin.carpentier@inria.fr) */
+
+#ifndef COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
+#define COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+
+namespace coal {
+
+/// @brief Base callback class for collision queries.
+/// This class can be supersed by child classes to provide desired behaviors
+/// according to the application (e.g, only listing the potential
+/// CollisionObjects in collision).
+struct COAL_DLLAPI CollisionCallBackBase {
+  /// @brief Initialization of the callback before running the collision
+  /// broadphase manager.
+  virtual void init() {};
+
+  /// @brief Collision evaluation between two objects in collision.
+  ///        This callback will cause the broadphase evaluation to stop if it
+  ///        returns true.
+  ///
+  /// @param[in] o1 Collision object #1.
+  /// @param[in] o2 Collision object #2.
+  virtual bool collide(CollisionObject* o1, CollisionObject* o2) = 0;
+
+  /// @brief Functor call associated to the collide operation.
+  virtual bool operator()(CollisionObject* o1, CollisionObject* o2) {
+    return collide(o1, o2);
+  }
+};
+
+/// @brief Base callback class for distance queries.
+/// This class can be supersed by child classes to provide desired behaviors
+/// according to the application (e.g, only listing the potential
+/// CollisionObjects in collision).
+struct COAL_DLLAPI DistanceCallBackBase {
+  /// @brief Initialization of the callback before running the collision
+  /// broadphase manager.
+  virtual void init() {};
+
+  /// @brief Distance evaluation between two objects in collision.
+  ///        This callback will cause the broadphase evaluation to stop if it
+  ///        returns true.
+  ///
+  /// @param[in] o1 Collision object #1.
+  /// @param[in] o2 Collision object #2.
+  /// @param[out] dist Distance between the two collision geometries.
+  virtual bool distance(CollisionObject* o1, CollisionObject* o2,
+                        CoalScalar& dist) = 0;
+
+  /// @brief Functor call associated to the distance operation.
+  virtual bool operator()(CollisionObject* o1, CollisionObject* o2,
+                          CoalScalar& dist) {
+    return distance(o1, o2, dist);
+  }
+};
+
+}  // namespace coal
+
+#endif  // COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
diff --git a/include/coal/broadphase/broadphase_collision_manager.h b/include/coal/broadphase/broadphase_collision_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..a394878afc21776544d4c20520b071625f34d953
--- /dev/null
+++ b/include/coal/broadphase/broadphase_collision_manager.h
@@ -0,0 +1,139 @@
+/*
+ * 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 COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
+#define COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
+
+#include <set>
+#include <vector>
+#include <boost/function.hpp>
+
+#include "coal/collision_object.h"
+#include "coal/broadphase/broadphase_callbacks.h"
+
+namespace coal {
+
+/// @brief Base class for broad phase collision. It helps to accelerate the
+/// collision/distance between N objects. Also support self collision, self
+/// distance and collision/distance with another M objects.
+class COAL_DLLAPI BroadPhaseCollisionManager {
+ public:
+  BroadPhaseCollisionManager();
+
+  virtual ~BroadPhaseCollisionManager();
+
+  /// @brief add objects to the manager
+  virtual void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+  /// @brief add one object to the manager
+  virtual void registerObject(CollisionObject* obj) = 0;
+
+  /// @brief remove one object from the manager
+  virtual void unregisterObject(CollisionObject* obj) = 0;
+
+  /// @brief initialize the manager, related with the specific type of manager
+  virtual void setup() = 0;
+
+  /// @brief update the condition of manager
+  virtual void update() = 0;
+
+  /// @brief update the manager by explicitly given the object updated
+  virtual void update(CollisionObject* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  virtual void update(const std::vector<CollisionObject*>& updated_objs);
+
+  /// @brief clear the manager
+  virtual void clear() = 0;
+
+  /// @brief return the objects managed by the manager
+  virtual void getObjects(std::vector<CollisionObject*>& objs) const = 0;
+
+  /// @brief return the objects managed by the manager
+  virtual std::vector<CollisionObject*> getObjects() const {
+    std::vector<CollisionObject*> res(size());
+    getObjects(res);
+    return res;
+  };
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  virtual void collide(CollisionObject* obj,
+                       CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  virtual void distance(CollisionObject* obj,
+                        DistanceCallBackBase* callback) const = 0;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  virtual void collide(CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  virtual void distance(DistanceCallBackBase* callback) const = 0;
+
+  /// @brief perform collision test with objects belonging to another manager
+  virtual void collide(BroadPhaseCollisionManager* other_manager,
+                       CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance test with objects belonging to another manager
+  virtual void distance(BroadPhaseCollisionManager* other_manager,
+                        DistanceCallBackBase* callback) const = 0;
+
+  /// @brief whether the manager is empty
+  virtual bool empty() const = 0;
+
+  /// @brief the number of objects managed by the manager
+  virtual size_t size() const = 0;
+
+ protected:
+  /// @brief tools help to avoid repeating collision or distance callback for
+  /// the pairs of objects tested before. It can be useful for some of the
+  /// broadphase algorithms.
+  mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
+  mutable bool enable_tested_set_;
+
+  bool inTestedSet(CollisionObject* a, CollisionObject* b) const;
+
+  void insertTestedSet(CollisionObject* a, CollisionObject* b) const;
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..b3e7a4aa8bf9f8b90554430acb15207614ac60ed
--- /dev/null
+++ b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h
@@ -0,0 +1,84 @@
+/*
+ * 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 COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
+#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
+
+#include "coal/broadphase/broadphase_continuous_collision_manager.h"
+#include <algorithm>
+
+namespace coal {
+
+//==============================================================================
+BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() {
+  // Do nothing
+}
+
+//==============================================================================
+
+BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() {
+  // Do nothing
+}
+
+//==============================================================================
+
+void BroadPhaseContinuousCollisionManager::registerObjects(
+    const std::vector<ContinuousCollisionObject*>& other_objs) {
+  for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]);
+}
+
+//==============================================================================
+
+void BroadPhaseContinuousCollisionManager::update(
+    ContinuousCollisionObject* updated_obj) {
+  COAL_UNUSED_VARIABLE(updated_obj);
+
+  update();
+}
+
+//==============================================================================
+
+void BroadPhaseContinuousCollisionManager::update(
+    const std::vector<ContinuousCollisionObject*>& updated_objs) {
+  COAL_UNUSED_VARIABLE(updated_objs);
+
+  update();
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_continuous_collision_manager.h b/include/coal/broadphase/broadphase_continuous_collision_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..c2e36e746b6ff8738c515cbf33f8cf497cef382d
--- /dev/null
+++ b/include/coal/broadphase/broadphase_continuous_collision_manager.h
@@ -0,0 +1,143 @@
+/*
+ * 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 COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
+#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
+
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/collision_object.h"
+#include "coal/narrowphase/continuous_collision_object.h"
+
+namespace coal {
+
+/// @brief Callback for continuous collision between two objects. Return value
+/// is whether can stop now.
+template <typename S>
+using ContinuousCollisionCallBack = bool (*)(ContinuousCollisionObject* o1,
+                                             ContinuousCollisionObject* o2,
+                                             void* cdata);
+
+/// @brief Callback for continuous distance between two objects, Return value is
+/// whether can stop now, also return the minimum distance till now.
+template <typename S>
+using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1,
+                                            ContinuousCollisionObject* o2,
+                                            S& dist);
+
+/// @brief Base class for broad phase continuous collision. It helps to
+/// accelerate the continuous collision/distance between N objects. Also support
+/// self collision, self distance and collision/distance with another M objects.
+template <typename S>
+class COAL_DLLAPI BroadPhaseContinuousCollisionManager {
+ public:
+  BroadPhaseContinuousCollisionManager();
+
+  virtual ~BroadPhaseContinuousCollisionManager();
+
+  /// @brief add objects to the manager
+  virtual void registerObjects(
+      const std::vector<ContinuousCollisionObject*>& other_objs);
+
+  /// @brief add one object to the manager
+  virtual void registerObject(ContinuousCollisionObject* obj) = 0;
+
+  /// @brief remove one object from the manager
+  virtual void unregisterObject(ContinuousCollisionObject* obj) = 0;
+
+  /// @brief initialize the manager, related with the specific type of manager
+  virtual void setup() = 0;
+
+  /// @brief update the condition of manager
+  virtual void update() = 0;
+
+  /// @brief update the manager by explicitly given the object updated
+  virtual void update(ContinuousCollisionObject* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  virtual void update(
+      const std::vector<ContinuousCollisionObject*>& updated_objs);
+
+  /// @brief clear the manager
+  virtual void clear() = 0;
+
+  /// @brief return the objects managed by the manager
+  virtual void getObjects(
+      std::vector<ContinuousCollisionObject*>& objs) const = 0;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  virtual void collide(ContinuousCollisionObject* obj,
+                       CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  virtual void distance(ContinuousCollisionObject* obj,
+                        DistanceCallBackBase* callback) const = 0;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  virtual void collide(CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  virtual void distance(DistanceCallBackBase* callback) const = 0;
+
+  /// @brief perform collision test with objects belonging to another manager
+  virtual void collide(BroadPhaseContinuousCollisionManager* other_manager,
+                       CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance test with objects belonging to another manager
+  virtual void distance(BroadPhaseContinuousCollisionManager* other_manager,
+                        DistanceCallBackBase* callback) const = 0;
+
+  /// @brief whether the manager is empty
+  virtual bool empty() const = 0;
+
+  /// @brief the number of objects managed by the manager
+  virtual size_t size() const = 0;
+};
+
+using BroadPhaseContinuousCollisionManagerf =
+    BroadPhaseContinuousCollisionManager<float>;
+using BroadPhaseContinuousCollisionManagerd =
+    BroadPhaseContinuousCollisionManager<CoalScalar>;
+
+}  // namespace coal
+
+#include "coal/broadphase/broadphase_continuous_collision_manager-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e1314f6e88fc725ede812f3a4de3a126ee825e1
--- /dev/null
+++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h
@@ -0,0 +1,234 @@
+/*
+ * 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 COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
+#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
+
+#include <limits>
+
+#if COAL_HAVE_OCTOMAP
+#include "coal/octree.h"
+#endif
+
+#include "coal/BV/BV.h"
+#include "coal/shape/geometric_shapes_utility.h"
+
+namespace coal {
+namespace detail {
+
+namespace dynamic_AABB_tree {
+
+#if COAL_HAVE_OCTOMAP
+
+//==============================================================================
+template <typename Derived>
+bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
+                       const OcTree* tree2, const OcTree::OcTreeNode* root2,
+                       const AABB& root2_bv,
+                       const Eigen::MatrixBase<Derived>& translation2,
+                       CollisionCallBackBase* callback) {
+  if (!root2) {
+    if (root1->isLeaf()) {
+      CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+      if (!obj1->collisionGeometry()->isFree()) {
+        const AABB& root2_bv_t = translate(root2_bv, translation2);
+        if (root1->bv.overlap(root2_bv_t)) {
+          Box* box = new Box();
+          Transform3s box_tf;
+          Transform3s tf2 = Transform3s::Identity();
+          tf2.translation() = translation2;
+          constructBox(root2_bv, tf2, *box, box_tf);
+
+          box->cost_density =
+              tree2->getOccupancyThres();  // thresholds are 0, 1, so uncertain
+
+          CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
+          return (*callback)(obj1, &obj2);
+        }
+      }
+    } else {
+      if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv,
+                            translation2, callback))
+        return true;
+      if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv,
+                            translation2, callback))
+        return true;
+    }
+
+    return false;
+  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
+    CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+    if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
+      const AABB& root2_bv_t = translate(root2_bv, translation2);
+      if (root1->bv.overlap(root2_bv_t)) {
+        Box* box = new Box();
+        Transform3s box_tf;
+        Transform3s tf2 = Transform3s::Identity();
+        tf2.translation() = translation2;
+        constructBox(root2_bv, tf2, *box, box_tf);
+
+        box->cost_density = root2->getOccupancy();
+        box->threshold_occupied = tree2->getOccupancyThres();
+
+        CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
+        return (*callback)(obj1, &obj2);
+      } else
+        return false;
+    } else
+      return false;
+  }
+
+  const AABB& root2_bv_t = translate(root2_bv, translation2);
+  if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
+
+  if (!tree2->nodeHasChildren(root2) ||
+      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
+    if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv,
+                          translation2, callback))
+      return true;
+    if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv,
+                          translation2, callback))
+      return true;
+  } else {
+    for (unsigned int i = 0; i < 8; ++i) {
+      if (tree2->nodeChildExists(root2, i)) {
+        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
+        AABB child_bv;
+        computeChildBV(root2_bv, i, child_bv);
+
+        if (collisionRecurse_(root1, tree2, child, child_bv, translation2,
+                              callback))
+          return true;
+      } else {
+        AABB child_bv;
+        computeChildBV(root2_bv, i, child_bv);
+        if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2,
+                              callback))
+          return true;
+      }
+    }
+  }
+  return false;
+}
+
+//==============================================================================
+template <typename Derived>
+bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
+                      const OcTree* tree2, const OcTree::OcTreeNode* root2,
+                      const AABB& root2_bv,
+                      const Eigen::MatrixBase<Derived>& translation2,
+                      DistanceCallBackBase* callback, CoalScalar& min_dist) {
+  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
+    if (tree2->isNodeOccupied(root2)) {
+      Box* box = new Box();
+      Transform3s box_tf;
+      Transform3s tf2 = Transform3s::Identity();
+      tf2.translation() = translation2;
+      constructBox(root2_bv, tf2, *box, box_tf);
+      CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
+      return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
+                         min_dist);
+    } else
+      return false;
+  }
+
+  if (!tree2->isNodeOccupied(root2)) return false;
+
+  if (!tree2->nodeHasChildren(root2) ||
+      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
+    const AABB& aabb2 = translate(root2_bv, translation2);
+    CoalScalar d1 = aabb2.distance(root1->children[0]->bv);
+    CoalScalar d2 = aabb2.distance(root1->children[1]->bv);
+
+    if (d2 < d1) {
+      if (d2 < min_dist) {
+        if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+
+      if (d1 < min_dist) {
+        if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+    } else {
+      if (d1 < min_dist) {
+        if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+
+      if (d2 < min_dist) {
+        if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+    }
+  } else {
+    for (unsigned int i = 0; i < 8; ++i) {
+      if (tree2->nodeChildExists(root2, i)) {
+        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
+        AABB child_bv;
+        computeChildBV(root2_bv, i, child_bv);
+        const AABB& aabb2 = translate(child_bv, translation2);
+
+        CoalScalar d = root1->bv.distance(aabb2);
+
+        if (d < min_dist) {
+          if (distanceRecurse_(root1, tree2, child, child_bv, translation2,
+                               callback, min_dist))
+            return true;
+        }
+      }
+    }
+  }
+
+  return false;
+}
+
+#endif
+
+}  // namespace dynamic_AABB_tree
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree.h
new file mode 100644
index 0000000000000000000000000000000000000000..d3ecd549787ab510a0e3bcfda9fae90228464f08
--- /dev/null
+++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree.h
@@ -0,0 +1,150 @@
+/*
+ * 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 COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
+#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
+
+#include <unordered_map>
+#include <functional>
+
+#include "coal/fwd.hh"
+// #include "coal/BV/utility.h"
+#include "coal/shape/geometric_shapes.h"
+// #include "coal/geometry/shape/utility.h"
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/detail/hierarchy_tree.h"
+
+namespace coal {
+
+class COAL_DLLAPI DynamicAABBTreeCollisionManager
+    : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  using DynamicAABBNode = detail::NodeBase<AABB>;
+  using DynamicAABBTable =
+      std::unordered_map<CollisionObject*, DynamicAABBNode*>;
+
+  int max_tree_nonbalanced_level;
+  int tree_incremental_balance_pass;
+  int* tree_topdown_balance_threshold{nullptr};
+  int* tree_topdown_level{nullptr};
+  int tree_init_level;
+
+  bool octree_as_geometry_collide;
+  bool octree_as_geometry_distance;
+
+  DynamicAABBTreeCollisionManager();
+
+  /// @brief add objects to the manager
+  void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+  /// @brief add one object to the manager
+  void registerObject(CollisionObject* obj);
+
+  /// @brief remove one object from the manager
+  void unregisterObject(CollisionObject* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  virtual void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager_,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager_,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+  /// @brief returns the AABB tree structure.
+  const detail::HierarchyTree<AABB>& getTree() const;
+
+  /// @brief returns the AABB tree structure.
+  detail::HierarchyTree<AABB>& getTree();
+
+ private:
+  detail::HierarchyTree<AABB> dtree{};
+  std::unordered_map<CollisionObject*, DynamicAABBNode*> table;
+
+  bool setup_;
+
+  void update_(CollisionObject* updated_obj);
+};
+
+}  // namespace coal
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..730db5782ffc0d1a49aff397e64c59b3afcca145
--- /dev/null
+++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
@@ -0,0 +1,233 @@
+/*
+ * 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 COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
+#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
+#include "coal/shape/geometric_shapes_utility.h"
+
+#if COAL_HAVE_OCTOMAP
+#include "coal/octree.h"
+#endif
+
+namespace coal {
+namespace detail {
+namespace dynamic_AABB_tree_array {
+
+#if COAL_HAVE_OCTOMAP
+
+//==============================================================================
+template <typename Derived>
+bool collisionRecurse_(
+    DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
+    size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
+    const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
+    CollisionCallBackBase* callback) {
+  DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
+      nodes1 + root1_id;
+  if (!root2) {
+    if (root1->isLeaf()) {
+      CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+      if (!obj1->collisionGeometry()->isFree()) {
+        const AABB& root_bv_t = translate(root2_bv, translation2);
+        if (root1->bv.overlap(root_bv_t)) {
+          Box* box = new Box();
+          Transform3s box_tf;
+          Transform3s tf2 = Transform3s::Identity();
+          tf2.translation() = translation2;
+          constructBox(root2_bv, tf2, *box, box_tf);
+
+          box->cost_density = tree2->getDefaultOccupancy();
+
+          CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
+          return (*callback)(obj1, &obj2);
+        }
+      }
+    } else {
+      if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr,
+                            root2_bv, translation2, callback))
+        return true;
+      if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr,
+                            root2_bv, translation2, callback))
+        return true;
+    }
+
+    return false;
+  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
+    CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+    if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
+      const AABB& root_bv_t = translate(root2_bv, translation2);
+      if (root1->bv.overlap(root_bv_t)) {
+        Box* box = new Box();
+        Transform3s box_tf;
+        Transform3s tf2 = Transform3s::Identity();
+        tf2.translation() = translation2;
+        constructBox(root2_bv, tf2, *box, box_tf);
+
+        box->cost_density = root2->getOccupancy();
+        box->threshold_occupied = tree2->getOccupancyThres();
+
+        CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
+        return (*callback)(obj1, &obj2);
+      } else
+        return false;
+    } else
+      return false;
+  }
+
+  const AABB& root_bv_t = translate(root2_bv, translation2);
+  if (tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false;
+
+  if (!tree2->nodeHasChildren(root2) ||
+      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
+    if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
+                          translation2, callback))
+      return true;
+    if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
+                          translation2, callback))
+      return true;
+  } else {
+    for (unsigned int i = 0; i < 8; ++i) {
+      if (tree2->nodeChildExists(root2, i)) {
+        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
+        AABB child_bv;
+        computeChildBV(root2_bv, i, child_bv);
+
+        if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv,
+                              translation2, callback))
+          return true;
+      } else {
+        AABB child_bv;
+        computeChildBV(root2_bv, i, child_bv);
+        if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv,
+                              translation2, callback))
+          return true;
+      }
+    }
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename Derived>
+bool distanceRecurse_(
+    DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
+    size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
+    const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
+    DistanceCallBackBase* callback, CoalScalar& min_dist) {
+  DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
+      nodes1 + root1_id;
+  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
+    if (tree2->isNodeOccupied(root2)) {
+      Box* box = new Box();
+      Transform3s box_tf;
+      Transform3s tf2 = Transform3s::Identity();
+      tf2.translation() = translation2;
+      constructBox(root2_bv, tf2, *box, box_tf);
+      CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
+      return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
+                         min_dist);
+    } else
+      return false;
+  }
+
+  if (!tree2->isNodeOccupied(root2)) return false;
+
+  if (!tree2->nodeHasChildren(root2) ||
+      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
+    const AABB& aabb2 = translate(root2_bv, translation2);
+
+    CoalScalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
+    CoalScalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
+
+    if (d2 < d1) {
+      if (d2 < min_dist) {
+        if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+
+      if (d1 < min_dist) {
+        if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+    } else {
+      if (d1 < min_dist) {
+        if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+
+      if (d2 < min_dist) {
+        if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+    }
+  } else {
+    for (unsigned int i = 0; i < 8; ++i) {
+      if (tree2->nodeChildExists(root2, i)) {
+        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
+        AABB child_bv;
+        computeChildBV(root2_bv, i, child_bv);
+
+        const AABB& aabb2 = translate(child_bv, translation2);
+        CoalScalar d = root1->bv.distance(aabb2);
+
+        if (d < min_dist) {
+          if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv,
+                               translation2, callback, min_dist))
+            return true;
+        }
+      }
+    }
+  }
+
+  return false;
+}
+
+#endif
+
+}  // namespace dynamic_AABB_tree_array
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h
new file mode 100644
index 0000000000000000000000000000000000000000..35f17cd8216a24c5df84881ef285fc88801bffb1
--- /dev/null
+++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h
@@ -0,0 +1,146 @@
+/*
+ * 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 COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
+#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
+
+#include <unordered_map>
+#include <functional>
+#include <limits>
+
+#include "coal/fwd.hh"
+// #include "coal/BV/utility.h"
+#include "coal/shape/geometric_shapes.h"
+// #include "coal/geometry/shape/utility.h"
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/detail/hierarchy_tree_array.h"
+
+namespace coal {
+
+class COAL_DLLAPI DynamicAABBTreeArrayCollisionManager
+    : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  using DynamicAABBNode = detail::implementation_array::NodeBase<AABB>;
+  using DynamicAABBTable = std::unordered_map<CollisionObject*, size_t>;
+
+  int max_tree_nonbalanced_level;
+  int tree_incremental_balance_pass;
+  int* tree_topdown_balance_threshold{nullptr};
+  int* tree_topdown_level{nullptr};
+  int tree_init_level;
+
+  bool octree_as_geometry_collide;
+  bool octree_as_geometry_distance;
+
+  DynamicAABBTreeArrayCollisionManager();
+
+  /// @brief add objects to the manager
+  void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+  /// @brief add one object to the manager
+  void registerObject(CollisionObject* obj);
+
+  /// @brief remove one object from the manager
+  void unregisterObject(CollisionObject* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  virtual void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager_,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager_,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+  const detail::implementation_array::HierarchyTree<AABB>& getTree() const;
+
+ private:
+  detail::implementation_array::HierarchyTree<AABB> dtree{};
+  std::unordered_map<CollisionObject*, size_t> table;
+
+  bool setup_;
+
+  void update_(CollisionObject* updated_obj);
+};
+
+}  // namespace coal
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/broadphase_interval_tree.h b/include/coal/broadphase/broadphase_interval_tree.h
new file mode 100644
index 0000000000000000000000000000000000000000..9950cf82e885d24cd1cc5158c284f29b1476dba2
--- /dev/null
+++ b/include/coal/broadphase/broadphase_interval_tree.h
@@ -0,0 +1,169 @@
+/*
+ * 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 COAL_BROAD_PHASE_INTERVAL_TREE_H
+#define COAL_BROAD_PHASE_INTERVAL_TREE_H
+
+#include <deque>
+#include <map>
+
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/detail/interval_tree.h"
+
+namespace coal {
+
+/// @brief Collision manager based on interval tree
+class COAL_DLLAPI IntervalTreeCollisionManager
+    : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  IntervalTreeCollisionManager();
+
+  ~IntervalTreeCollisionManager();
+
+  /// @brief remove one object from the manager
+  void registerObject(CollisionObject* obj);
+
+  /// @brief add one object to the manager
+  void unregisterObject(CollisionObject* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  virtual void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+ protected:
+  /// @brief SAP end point
+  /// @brief SAP end point
+  struct COAL_DLLAPI EndPoint {
+    /// @brief object related with the end point
+    CollisionObject* obj;
+
+    /// @brief end point value
+    CoalScalar value;
+
+    /// @brief tag for whether it is a lower bound or higher bound of an
+    /// interval, 0 for lo, and 1 for hi
+    char minmax;
+
+    bool operator<(const EndPoint& p) const;
+  };
+
+  /// @brief Extention interval tree's interval to SAP interval, adding more
+  /// information
+  struct COAL_DLLAPI SAPInterval : public detail::SimpleInterval {
+    CollisionObject* obj;
+
+    SAPInterval(CoalScalar low_, CoalScalar high_, CollisionObject* obj_);
+  };
+
+  bool checkColl(
+      typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
+      typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
+      CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  bool checkDist(
+      typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
+      typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
+      CollisionObject* obj, DistanceCallBackBase* callback,
+      CoalScalar& min_dist) const;
+
+  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
+                 CoalScalar& min_dist) const;
+
+  /// @brief vector stores all the end points
+  std::vector<EndPoint> endpoints[3];
+
+  /// @brief  interval tree manages the intervals
+  detail::IntervalTree* interval_trees[3];
+
+  std::map<CollisionObject*, SAPInterval*> obj_interval_maps[3];
+
+  /// @brief tag for whether the interval tree is maintained suitably
+  bool setup_;
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_spatialhash-inl.h b/include/coal/broadphase/broadphase_spatialhash-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f155a4e0fdd4fb444fe344445ce8ce54f11a64e
--- /dev/null
+++ b/include/coal/broadphase/broadphase_spatialhash-inl.h
@@ -0,0 +1,539 @@
+/*
+ * 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 COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
+#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
+
+#include "coal/broadphase/broadphase_spatialhash.h"
+
+namespace coal {
+
+//==============================================================================
+template <typename HashTable>
+SpatialHashingCollisionManager<HashTable>::SpatialHashingCollisionManager(
+    CoalScalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max,
+    unsigned int default_table_size)
+    : scene_limit(AABB(scene_min, scene_max)),
+      hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
+  hash_table->init(default_table_size);
+}
+
+//==============================================================================
+template <typename HashTable>
+SpatialHashingCollisionManager<HashTable>::~SpatialHashingCollisionManager() {
+  delete hash_table;
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::registerObject(
+    CollisionObject* obj) {
+  objs.push_back(obj);
+
+  const AABB& obj_aabb = obj->getAABB();
+  AABB overlap_aabb;
+
+  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
+    if (!scene_limit.contain(obj_aabb))
+      objs_partially_penetrating_scene_limit.push_back(obj);
+
+    hash_table->insert(overlap_aabb, obj);
+  } else {
+    objs_outside_scene_limit.push_back(obj);
+  }
+
+  obj_aabb_map[obj] = obj_aabb;
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::unregisterObject(
+    CollisionObject* obj) {
+  objs.remove(obj);
+
+  const AABB& obj_aabb = obj->getAABB();
+  AABB overlap_aabb;
+
+  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
+    if (!scene_limit.contain(obj_aabb))
+      objs_partially_penetrating_scene_limit.remove(obj);
+
+    hash_table->remove(overlap_aabb, obj);
+  } else {
+    objs_outside_scene_limit.remove(obj);
+  }
+
+  obj_aabb_map.erase(obj);
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::setup() {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update() {
+  hash_table->clear();
+  objs_partially_penetrating_scene_limit.clear();
+  objs_outside_scene_limit.clear();
+
+  for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) {
+    CollisionObject* obj = *it;
+    const AABB& obj_aabb = obj->getAABB();
+    AABB overlap_aabb;
+
+    if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
+      if (!scene_limit.contain(obj_aabb))
+        objs_partially_penetrating_scene_limit.push_back(obj);
+
+      hash_table->insert(overlap_aabb, obj);
+    } else {
+      objs_outside_scene_limit.push_back(obj);
+    }
+
+    obj_aabb_map[obj] = obj_aabb;
+  }
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update(
+    CollisionObject* updated_obj) {
+  const AABB& new_aabb = updated_obj->getAABB();
+  const AABB& old_aabb = obj_aabb_map[updated_obj];
+
+  AABB old_overlap_aabb;
+  const auto is_old_aabb_overlapping =
+      scene_limit.overlap(old_aabb, old_overlap_aabb);
+  if (is_old_aabb_overlapping)
+    hash_table->remove(old_overlap_aabb, updated_obj);
+
+  AABB new_overlap_aabb;
+  const auto is_new_aabb_overlapping =
+      scene_limit.overlap(new_aabb, new_overlap_aabb);
+  if (is_new_aabb_overlapping)
+    hash_table->insert(new_overlap_aabb, updated_obj);
+
+  ObjectStatus old_status;
+  if (is_old_aabb_overlapping) {
+    if (scene_limit.contain(old_aabb))
+      old_status = Inside;
+    else
+      old_status = PartiallyPenetrating;
+  } else {
+    old_status = Outside;
+  }
+
+  if (is_new_aabb_overlapping) {
+    if (scene_limit.contain(new_aabb)) {
+      if (old_status == PartiallyPenetrating) {
+        // Status change: PartiallyPenetrating --> Inside
+        // Required action(s):
+        // - remove object from "objs_partially_penetrating_scene_limit"
+
+        auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
+                                 objs_partially_penetrating_scene_limit.end(),
+                                 updated_obj);
+        objs_partially_penetrating_scene_limit.erase(find_it);
+      } else if (old_status == Outside) {
+        // Status change: Outside --> Inside
+        // Required action(s):
+        // - remove object from "objs_outside_scene_limit"
+
+        auto find_it = std::find(objs_outside_scene_limit.begin(),
+                                 objs_outside_scene_limit.end(), updated_obj);
+        objs_outside_scene_limit.erase(find_it);
+      }
+    } else {
+      if (old_status == Inside) {
+        // Status change: Inside --> PartiallyPenetrating
+        // Required action(s):
+        // - add object to "objs_partially_penetrating_scene_limit"
+
+        objs_partially_penetrating_scene_limit.push_back(updated_obj);
+      } else if (old_status == Outside) {
+        // Status change: Outside --> PartiallyPenetrating
+        // Required action(s):
+        // - remove object from "objs_outside_scene_limit"
+        // - add object to "objs_partially_penetrating_scene_limit"
+
+        auto find_it = std::find(objs_outside_scene_limit.begin(),
+                                 objs_outside_scene_limit.end(), updated_obj);
+        objs_outside_scene_limit.erase(find_it);
+
+        objs_partially_penetrating_scene_limit.push_back(updated_obj);
+      }
+    }
+  } else {
+    if (old_status == Inside) {
+      // Status change: Inside --> Outside
+      // Required action(s):
+      // - add object to "objs_outside_scene_limit"
+
+      objs_outside_scene_limit.push_back(updated_obj);
+    } else if (old_status == PartiallyPenetrating) {
+      // Status change: PartiallyPenetrating --> Outside
+      // Required action(s):
+      // - remove object from "objs_partially_penetrating_scene_limit"
+      // - add object to "objs_outside_scene_limit"
+
+      auto find_it =
+          std::find(objs_partially_penetrating_scene_limit.begin(),
+                    objs_partially_penetrating_scene_limit.end(), updated_obj);
+      objs_partially_penetrating_scene_limit.erase(find_it);
+
+      objs_outside_scene_limit.push_back(updated_obj);
+    }
+  }
+
+  obj_aabb_map[updated_obj] = new_aabb;
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update(
+    const std::vector<CollisionObject*>& updated_objs) {
+  for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::clear() {
+  objs.clear();
+  hash_table->clear();
+  objs_outside_scene_limit.clear();
+  obj_aabb_map.clear();
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::getObjects(
+    std::vector<CollisionObject*>& objs_) const {
+  objs_.resize(objs.size());
+  std::copy(objs.begin(), objs.end(), objs_.begin());
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(
+    CollisionObject* obj, CollisionCallBackBase* callback) const {
+  if (size() == 0) return;
+  collide_(obj, callback);
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(
+    CollisionObject* obj, DistanceCallBackBase* callback) const {
+  if (size() == 0) return;
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
+  distance_(obj, callback, min_dist);
+}
+
+//==============================================================================
+template <typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::collide_(
+    CollisionObject* obj, CollisionCallBackBase* callback) const {
+  const auto& obj_aabb = obj->getAABB();
+  AABB overlap_aabb;
+
+  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
+    const auto query_result = hash_table->query(overlap_aabb);
+    for (const auto& obj2 : query_result) {
+      if (obj == obj2) continue;
+
+      if ((*callback)(obj, obj2)) return true;
+    }
+
+    if (!scene_limit.contain(obj_aabb)) {
+      for (const auto& obj2 : objs_outside_scene_limit) {
+        if (obj == obj2) continue;
+
+        if ((*callback)(obj, obj2)) return true;
+      }
+    }
+  } else {
+    for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
+      if (obj == obj2) continue;
+
+      if ((*callback)(obj, obj2)) return true;
+    }
+
+    for (const auto& obj2 : objs_outside_scene_limit) {
+      if (obj == obj2) continue;
+
+      if ((*callback)(obj, obj2)) return true;
+    }
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::distance_(
+    CollisionObject* obj, DistanceCallBackBase* callback,
+    CoalScalar& min_dist) const {
+  auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  auto aabb = obj->getAABB();
+  if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
+    Vec3s min_dist_delta(min_dist, min_dist, min_dist);
+    aabb.expand(min_dist_delta);
+  }
+
+  AABB overlap_aabb;
+
+  auto status = 1;
+  CoalScalar old_min_distance;
+
+  while (1) {
+    old_min_distance = min_dist;
+
+    if (scene_limit.overlap(aabb, overlap_aabb)) {
+      if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
+                                  callback, min_dist)) {
+        return true;
+      }
+
+      if (!scene_limit.contain(aabb)) {
+        if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
+                                    min_dist)) {
+          return true;
+        }
+      }
+    } else {
+      if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
+                                  callback, min_dist)) {
+        return true;
+      }
+
+      if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
+                                  min_dist)) {
+        return true;
+      }
+    }
+
+    if (status == 1) {
+      if (old_min_distance < (std::numeric_limits<CoalScalar>::max)()) {
+        break;
+      } else {
+        if (min_dist < old_min_distance) {
+          Vec3s min_dist_delta(min_dist, min_dist, min_dist);
+          aabb = AABB(obj->getAABB(), min_dist_delta);
+          status = 0;
+        } else {
+          if (aabb == obj->getAABB())
+            aabb.expand(delta);
+          else
+            aabb.expand(obj->getAABB(), 2.0);
+        }
+      }
+    } else if (status == 0) {
+      break;
+    }
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(
+    CollisionCallBackBase* callback) const {
+  if (size() == 0) return;
+
+  for (const auto& obj1 : objs) {
+    const auto& obj_aabb = obj1->getAABB();
+    AABB overlap_aabb;
+
+    if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
+      auto query_result = hash_table->query(overlap_aabb);
+      for (const auto& obj2 : query_result) {
+        if (obj1 < obj2) {
+          if ((*callback)(obj1, obj2)) return;
+        }
+      }
+
+      if (!scene_limit.contain(obj_aabb)) {
+        for (const auto& obj2 : objs_outside_scene_limit) {
+          if (obj1 < obj2) {
+            if ((*callback)(obj1, obj2)) return;
+          }
+        }
+      }
+    } else {
+      for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
+        if (obj1 < obj2) {
+          if ((*callback)(obj1, obj2)) return;
+        }
+      }
+
+      for (const auto& obj2 : objs_outside_scene_limit) {
+        if (obj1 < obj2) {
+          if ((*callback)(obj1, obj2)) return;
+        }
+      }
+    }
+  }
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(
+    DistanceCallBackBase* callback) const {
+  if (size() == 0) return;
+
+  this->enable_tested_set_ = true;
+  this->tested_set.clear();
+
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
+
+  for (const auto& obj : objs) {
+    if (distance_(obj, callback, min_dist)) break;
+  }
+
+  this->enable_tested_set_ = false;
+  this->tested_set.clear();
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(
+    BroadPhaseCollisionManager* other_manager_,
+    CollisionCallBackBase* callback) const {
+  auto* other_manager =
+      static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
+
+  if ((size() == 0) || (other_manager->size() == 0)) return;
+
+  if (this == other_manager) {
+    collide(callback);
+    return;
+  }
+
+  if (this->size() < other_manager->size()) {
+    for (const auto& obj : objs) {
+      if (other_manager->collide_(obj, callback)) return;
+    }
+  } else {
+    for (const auto& obj : other_manager->objs) {
+      if (collide_(obj, callback)) return;
+    }
+  }
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(
+    BroadPhaseCollisionManager* other_manager_,
+    DistanceCallBackBase* callback) const {
+  auto* other_manager =
+      static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
+
+  if ((size() == 0) || (other_manager->size() == 0)) return;
+
+  if (this == other_manager) {
+    distance(callback);
+    return;
+  }
+
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
+
+  if (this->size() < other_manager->size()) {
+    for (const auto& obj : objs)
+      if (other_manager->distance_(obj, callback, min_dist)) return;
+  } else {
+    for (const auto& obj : other_manager->objs)
+      if (distance_(obj, callback, min_dist)) return;
+  }
+}
+
+//==============================================================================
+template <typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::empty() const {
+  return objs.empty();
+}
+
+//==============================================================================
+template <typename HashTable>
+size_t SpatialHashingCollisionManager<HashTable>::size() const {
+  return objs.size();
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::computeBound(
+    std::vector<CollisionObject*>& objs, Vec3s& l, Vec3s& u) {
+  AABB bound;
+  for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
+
+  l = bound.min_;
+  u = bound.max_;
+}
+
+//==============================================================================
+template <typename HashTable>
+template <typename Container>
+bool SpatialHashingCollisionManager<HashTable>::distanceObjectToObjects(
+    CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback,
+    CoalScalar& min_dist) const {
+  for (auto& obj2 : objs) {
+    if (obj == obj2) continue;
+
+    if (!this->enable_tested_set_) {
+      if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
+        if ((*callback)(obj, obj2, min_dist)) return true;
+      }
+    } else {
+      if (!this->inTestedSet(obj, obj2)) {
+        if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
+          if ((*callback)(obj, obj2, min_dist)) return true;
+        }
+
+        this->insertTestedSet(obj, obj2);
+      }
+    }
+  }
+
+  return false;
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/broadphase_spatialhash.h b/include/coal/broadphase/broadphase_spatialhash.h
new file mode 100644
index 0000000000000000000000000000000000000000..10c4830ceb21e1ae60aee5301e6a1afa5fbbb8c9
--- /dev/null
+++ b/include/coal/broadphase/broadphase_spatialhash.h
@@ -0,0 +1,167 @@
+/*
+ * 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 COAL_BROADPHASE_BROADPAHSESPATIALHASH_H
+#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_H
+
+#include <list>
+#include <map>
+#include "coal/BV/AABB.h"
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/detail/simple_hash_table.h"
+#include "coal/broadphase/detail/sparse_hash_table.h"
+#include "coal/broadphase/detail/spatial_hash.h"
+
+namespace coal {
+
+/// @brief spatial hashing collision mananger
+template <typename HashTable = detail::SimpleHashTable<AABB, CollisionObject*,
+                                                       detail::SpatialHash> >
+class SpatialHashingCollisionManager : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3s& scene_min,
+                                 const Vec3s& scene_max,
+                                 unsigned int default_table_size = 1000);
+
+  ~SpatialHashingCollisionManager();
+
+  /// @brief add one object to the manager
+  void registerObject(CollisionObject* obj);
+
+  /// @brief remove one object from the manager
+  void unregisterObject(CollisionObject* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  virtual void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging ot the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e, N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+  /// @brief compute the bound for the environent
+  static void computeBound(std::vector<CollisionObject*>& objs, Vec3s& l,
+                           Vec3s& u);
+
+ protected:
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging ot the manager
+  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
+                 CoalScalar& min_dist) const;
+
+  /// @brief all objects in the scene
+  std::list<CollisionObject*> objs;
+
+  /// @brief objects partially penetrating (not totally inside nor outside) the
+  /// scene limit are in another list
+  std::list<CollisionObject*> objs_partially_penetrating_scene_limit;
+
+  /// @brief objects outside the scene limit are in another list
+  std::list<CollisionObject*> objs_outside_scene_limit;
+
+  /// @brief the size of the scene
+  AABB scene_limit;
+
+  /// @brief store the map between objects and their aabbs. will make update
+  /// more convenient
+  std::map<CollisionObject*, AABB> obj_aabb_map;
+
+  /// @brief objects in the scene limit (given by scene_min and scene_max) are
+  /// in the spatial hash table
+  HashTable* hash_table;
+
+ private:
+  enum ObjectStatus { Inside, PartiallyPenetrating, Outside };
+
+  template <typename Container>
+  bool distanceObjectToObjects(CollisionObject* obj, const Container& objs,
+                               DistanceCallBackBase* callback,
+                               CoalScalar& min_dist) const;
+};
+
+}  // namespace coal
+
+#include "coal/broadphase/broadphase_spatialhash-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/default_broadphase_callbacks.h b/include/coal/broadphase/default_broadphase_callbacks.h
new file mode 100644
index 0000000000000000000000000000000000000000..711b6c7082763b2b095a98a7ed8905b5edcf5773
--- /dev/null
+++ b/include/coal/broadphase/default_broadphase_callbacks.h
@@ -0,0 +1,255 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Toyota Research Institute
+ *  Copyright (c) 2022-2023, INRIA
+ *  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 the copyright holder 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 Sean Curtis (sean@tri.global) */
+/** @author Justin Carpentier (justin.carpentier@inria.fr) */
+
+#ifndef COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
+#define COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
+
+#include "coal/broadphase/broadphase_callbacks.h"
+#include "coal/collision.h"
+#include "coal/distance.h"
+// #include "coal/narrowphase/continuous_collision.h"
+// #include "coal/narrowphase/continuous_collision_request.h"
+// #include "coal/narrowphase/continuous_collision_result.h"
+// #include "coal/narrowphase/distance_request.h"
+// #include "coal/narrowphase/distance_result.h"
+
+namespace coal {
+
+/// @brief Collision data stores the collision request and the result given by
+/// collision algorithm.
+struct CollisionData {
+  CollisionData() { done = false; }
+
+  /// @brief Collision request
+  CollisionRequest request;
+
+  /// @brief Collision result
+  CollisionResult result;
+
+  /// @brief Whether the collision iteration can stop
+  bool done;
+
+  /// @brief Clears the CollisionData
+  void clear() {
+    result.clear();
+    done = false;
+  }
+};
+
+/// @brief Distance data stores the distance request and the result given by
+/// distance algorithm.
+struct DistanceData {
+  DistanceData() { done = false; }
+
+  /// @brief Distance request
+  DistanceRequest request;
+
+  /// @brief Distance result
+  DistanceResult result;
+
+  /// @brief Whether the distance iteration can stop
+  bool done;
+
+  /// @brief Clears the DistanceData
+  void clear() {
+    result.clear();
+    done = false;
+  }
+};
+
+/// @brief Provides a simple callback for the collision query in the
+/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
+/// points to an instance of CollisionData. It simply invokes the
+/// `collide()` method on the culled pair of geometries and stores the results
+/// in the data's CollisionResult instance.
+///
+/// This callback will cause the broadphase evaluation to stop if:
+///   - the collision requests _disables_ cost _and_
+///   - the collide() reports a collision for the culled pair, _and_
+///   - we've reported the number of contacts requested in the CollisionRequest.
+///
+/// For a given instance of CollisionData, if broadphase evaluation has
+/// already terminated (i.e., defaultCollisionFunction() returned `true`),
+/// subsequent invocations with the same instance of CollisionData will
+/// return immediately, requesting termination of broadphase evaluation (i.e.,
+/// return `true`).
+///
+/// @param o1   The first object in the culled pair.
+/// @param o2   The second object in the culled pair.
+/// @param data A non-null pointer to a CollisionData instance.
+/// @return `true` if the broadphase evaluation should stop.
+/// @tparam S   The scalar type with which the computation will be performed.
+bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
+                              void* data);
+
+/// @brief Collision data for use with the DefaultContinuousCollisionFunction.
+/// It stores the collision request and the result given by the collision
+/// algorithm (and stores the conclusion of whether further evaluation of the
+/// broadphase collision manager has been deemed unnecessary).
+// struct DefaultContinuousCollisionData {
+//   ContinuousCollisionRequest request;
+//   ContinuousCollisionResult result;
+//
+//   /// If `true`, requests that the broadphase evaluation stop.
+//   bool done{false};
+// };
+
+/// @brief Provides a simple callback for the continuous collision query in the
+/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
+/// points to an instance of DefaultContinuousCollisionData. It simply invokes
+/// the `collide()` method on the culled pair of geometries and stores the
+/// results in the data's ContinuousCollisionResult instance.
+///
+/// This callback will never cause the broadphase evaluation to terminate early.
+/// However, if the `done` member of the DefaultContinuousCollisionData is set
+/// to true, this method will simply return without doing any computation.
+///
+/// For a given instance of DefaultContinuousCollisionData, if broadphase
+/// evaluation has already terminated (i.e.,
+/// DefaultContinuousCollisionFunction() returned `true`), subsequent
+/// invocations with the same instance of CollisionData will return
+/// immediately, requesting termination of broadphase evaluation (i.e., return
+/// `true`).
+///
+/// @param o1   The first object in the culled pair.
+/// @param o2   The second object in the culled pair.
+/// @param data A non-null pointer to a CollisionData instance.
+/// @return True if the broadphase evaluation should stop.
+/// @tparam S   The scalar type with which the computation will be performed.
+// bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1,
+//                                         ContinuousCollisionObject* o2,
+//                                         void* data) {
+//   assert(data != nullptr);
+//   auto* cdata = static_cast<DefaultContinuousCollisionData*>(data);
+//
+//   if (cdata->done) return true;
+//
+//   const ContinuousCollisionRequest& request = cdata->request;
+//   ContinuousCollisionResult& result = cdata->result;
+//   collide(o1, o2, request, result);
+//
+//   return cdata->done;
+// }
+
+/// @brief Provides a simple callback for the distance query in the
+/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
+/// points to an instance of DistanceData. It simply invokes the
+/// `distance()` method on the culled pair of geometries and stores the results
+/// in the data's DistanceResult instance.
+///
+/// This callback will cause the broadphase evaluation to stop if:
+///   - The distance is less than or equal to zero (i.e., the pair is in
+///     contact).
+///
+/// For a given instance of DistanceData, if broadphase evaluation has
+/// already terminated (i.e., defaultDistanceFunction() returned `true`),
+/// subsequent invocations with the same instance of DistanceData will
+/// simply report the previously reported minimum distance and request
+/// termination of broadphase evaluation (i.e., return `true`).
+///
+/// @param o1     The first object in the culled pair.
+/// @param o2     The second object in the culled pair.
+/// @param data   A non-null pointer to a DistanceData instance.
+/// @param dist   The distance computed by distance().
+/// @return `true` if the broadphase evaluation should stop.
+/// @tparam S   The scalar type with which the computation will be performed.
+bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
+                             void* data, CoalScalar& dist);
+
+/// @brief Default collision callback to check collision between collision
+/// objects.
+struct COAL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase {
+  /// @brief Initialize the callback.
+  /// Clears the collision result and sets the done boolean to false.
+  void init() { data.clear(); }
+
+  bool collide(CollisionObject* o1, CollisionObject* o2);
+
+  CollisionData data;
+
+  virtual ~CollisionCallBackDefault() {};
+};
+
+/// @brief Default distance callback to check collision between collision
+/// objects.
+struct COAL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase {
+  /// @brief Initialize the callback.
+  /// Clears the distance result and sets the done boolean to false.
+  void init() { data.clear(); }
+
+  bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist);
+
+  DistanceData data;
+
+  virtual ~DistanceCallBackDefault() {};
+};
+
+/// @brief Collision callback to collect collision pairs potentially in contacts
+struct COAL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase {
+  typedef std::pair<CollisionObject*, CollisionObject*> CollisionPair;
+
+  /// @brief Default constructor.
+  CollisionCallBackCollect(const size_t max_size);
+
+  bool collide(CollisionObject* o1, CollisionObject* o2);
+
+  /// @brief Returns the number of registered collision pairs
+  size_t numCollisionPairs() const;
+
+  /// @brief Returns a const reference to the active collision_pairs to check
+  const std::vector<CollisionPair>& getCollisionPairs() const;
+
+  /// @brief Reset the callback
+  void init();
+
+  /// @brief Check whether a collision pair exists
+  bool exist(const CollisionPair& pair) const;
+
+  /// @brief Check whether a collision pair exists
+  bool exist(CollisionObject* o1, CollisionObject* o2) const;
+
+  virtual ~CollisionCallBackCollect() {};
+
+ protected:
+  std::vector<CollisionPair> collision_pairs;
+  size_t max_size;
+};
+
+}  // namespace coal
+
+#endif  // COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
diff --git a/include/coal/broadphase/detail/hierarchy_tree-inl.h b/include/coal/broadphase/detail/hierarchy_tree-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..af29cf6a5a70384ad9c73fa34c441972b28b61ba
--- /dev/null
+++ b/include/coal/broadphase/detail/hierarchy_tree-inl.h
@@ -0,0 +1,998 @@
+/*
+ * 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 COAL_HIERARCHY_TREE_INL_H
+#define COAL_HIERARCHY_TREE_INL_H
+
+#include "coal/broadphase/detail/hierarchy_tree.h"
+
+namespace coal {
+
+namespace detail {
+
+//==============================================================================
+template <typename BV>
+HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
+  root_node = nullptr;
+  n_leaves = 0;
+  free_node = nullptr;
+  max_lookahead_level = -1;
+  opath = 0;
+  bu_threshold = bu_threshold_;
+  topdown_level = topdown_level_;
+}
+
+//==============================================================================
+template <typename BV>
+HierarchyTree<BV>::~HierarchyTree() {
+  clear();
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init(std::vector<Node*>& leaves, int level) {
+  switch (level) {
+    case 0:
+      init_0(leaves);
+      break;
+    case 1:
+      init_1(leaves);
+      break;
+    case 2:
+      init_2(leaves);
+      break;
+    case 3:
+      init_3(leaves);
+      break;
+    default:
+      init_0(leaves);
+  }
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::insert(const BV& bv,
+                                                            void* data) {
+  Node* leaf = createNode(nullptr, bv, data);
+  insertLeaf(root_node, leaf);
+  ++n_leaves;
+  return leaf;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::remove(Node* leaf) {
+  removeLeaf(leaf);
+  deleteNode(leaf);
+  --n_leaves;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::clear() {
+  if (root_node) recurseDeleteNode(root_node);
+  n_leaves = 0;
+  delete free_node;
+  free_node = nullptr;
+  max_lookahead_level = -1;
+  opath = 0;
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::empty() const {
+  return (nullptr == root_node);
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::update(Node* leaf, int lookahead_level) {
+  // TODO(DamrongGuoy): Since we update a leaf node by removing and
+  //  inserting the same leaf node, it is likely to change the structure of
+  //  the tree even if no object's pose has changed. In the future,
+  //  find a way to preserve the structure of the tree to solve this issue:
+  //  https://github.com/flexible-collision-library/fcl/issues/368
+
+  // First we remove the leaf node pointed by `leaf` variable from the tree.
+  // The `sub_root` variable is the root of the subtree containing nodes
+  // whose BVs were adjusted due to node removal.
+  typename HierarchyTree<BV>::Node* sub_root = removeLeaf(leaf);
+  if (sub_root) {
+    if (lookahead_level > 0) {
+      // For positive `lookahead_level`, we move the `sub_root` variable up.
+      for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i)
+        sub_root = sub_root->parent;
+    } else
+      // By default, lookahead_level = -1, and we reset the `sub_root` variable
+      // to the root of the entire tree.
+      sub_root = root_node;
+  }
+  // Then we insert the node pointed by `leaf` variable back into the
+  // subtree rooted at `sub_root` variable.
+  insertLeaf(sub_root, leaf);
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(Node* leaf, const BV& bv) {
+  if (leaf->bv.contain(bv)) return false;
+  update_(leaf, bv);
+  return true;
+}
+
+//==============================================================================
+template <typename S, typename BV>
+struct UpdateImpl {
+  static bool run(const HierarchyTree<BV>& tree,
+                  typename HierarchyTree<BV>::Node* leaf, const BV& bv,
+                  const Vec3s& /*vel*/, CoalScalar /*margin*/) {
+    if (leaf->bv.contain(bv)) return false;
+    tree.update_(leaf, bv);
+    return true;
+  }
+
+  static bool run(const HierarchyTree<BV>& tree,
+                  typename HierarchyTree<BV>::Node* leaf, const BV& bv,
+                  const Vec3s& /*vel*/) {
+    if (leaf->bv.contain(bv)) return false;
+    tree.update_(leaf, bv);
+    return true;
+  }
+};
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel,
+                               CoalScalar margin) {
+  return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel, margin);
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel) {
+  return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel);
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::getMaxHeight() const {
+  if (!root_node) return 0;
+  return getMaxHeight(root_node);
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::getMaxDepth() const {
+  if (!root_node) return 0;
+
+  size_t max_depth;
+  getMaxDepth(root_node, 0, max_depth);
+  return max_depth;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::balanceBottomup() {
+  if (root_node) {
+    std::vector<Node*> leaves;
+    leaves.reserve(n_leaves);
+    fetchLeaves(root_node, leaves);
+    bottomup(leaves.begin(), leaves.end());
+    root_node = leaves[0];
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::balanceTopdown() {
+  if (root_node) {
+    std::vector<Node*> leaves;
+    leaves.reserve(n_leaves);
+    fetchLeaves(root_node, leaves);
+    root_node = topdown(leaves.begin(), leaves.end());
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::balanceIncremental(int iterations) {
+  if (iterations < 0) iterations = (int)n_leaves;
+  if (root_node && (iterations > 0)) {
+    for (int i = 0; i < iterations; ++i) {
+      Node* node = root_node;
+      unsigned int bit = 0;
+      while (!node->isLeaf()) {
+        node = sort(node, root_node)->children[(opath >> bit) & 1];
+        bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
+      }
+      update(node);
+      ++opath;
+    }
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::refit() {
+  if (root_node) recurseRefit(root_node);
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::extractLeaves(const Node* root,
+                                      std::vector<Node*>& leaves) const {
+  if (!root->isLeaf()) {
+    extractLeaves(root->children[0], leaves);
+    extractLeaves(root->children[1], leaves);
+  } else
+    leaves.push_back(root);
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::size() const {
+  return n_leaves;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getRoot() const {
+  return root_node;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node*& HierarchyTree<BV>::getRoot() {
+  return root_node;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::print(Node* root, int depth) {
+  for (int i = 0; i < depth; ++i) std::cout << " ";
+  std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", "
+            << root->bv.min_[2] << "; " << root->bv.max_[0] << ", "
+            << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
+  if (root->isLeaf()) {
+  } else {
+    print(root->children[0], depth + 1);
+    print(root->children[1], depth + 1);
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::bottomup(const NodeVecIterator lbeg,
+                                 const NodeVecIterator lend) {
+  NodeVecIterator lcur_end = lend;
+  while (lbeg < lcur_end - 1) {
+    NodeVecIterator min_it1 = lbeg;
+    NodeVecIterator min_it2 = lbeg + 1;
+    CoalScalar min_size = (std::numeric_limits<CoalScalar>::max)();
+    for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) {
+      for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) {
+        CoalScalar cur_size = ((*it1)->bv + (*it2)->bv).size();
+        if (cur_size < min_size) {
+          min_size = cur_size;
+          min_it1 = it1;
+          min_it2 = it2;
+        }
+      }
+    }
+
+    Node* n[2] = {*min_it1, *min_it2};
+    Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr);
+    p->children[0] = n[0];
+    p->children[1] = n[1];
+    n[0]->parent = p;
+    n[1]->parent = p;
+    *min_it1 = p;
+    Node* tmp = *min_it2;
+    lcur_end--;
+    *min_it2 = *lcur_end;
+    *lcur_end = tmp;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown(
+    const NodeVecIterator lbeg, const NodeVecIterator lend) {
+  switch (topdown_level) {
+    case 0:
+      return topdown_0(lbeg, lend);
+      break;
+    case 1:
+      return topdown_1(lbeg, lend);
+      break;
+    default:
+      return topdown_0(lbeg, lend);
+  }
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::getMaxHeight(Node* node) const {
+  if (!node->isLeaf()) {
+    size_t height1 = getMaxHeight(node->children[0]);
+    size_t height2 = getMaxHeight(node->children[1]);
+    return std::max(height1, height2) + 1;
+  } else
+    return 0;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::getMaxDepth(Node* node, size_t depth,
+                                    size_t& max_depth) const {
+  if (!node->isLeaf()) {
+    getMaxDepth(node->children[0], depth + 1, max_depth);
+    getMaxDepth(node->children[1], depth + 1, max_depth);
+  } else
+    max_depth = std::max(max_depth, depth);
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_0(
+    const NodeVecIterator lbeg, const NodeVecIterator lend) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (num_leaves > bu_threshold) {
+      BV vol = (*lbeg)->bv;
+      for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv;
+
+      int best_axis = 0;
+      CoalScalar extent[3] = {vol.width(), vol.height(), vol.depth()};
+      if (extent[1] > extent[0]) best_axis = 1;
+      if (extent[2] > extent[best_axis]) best_axis = 2;
+
+      // compute median
+      NodeVecIterator lcenter = lbeg + num_leaves / 2;
+      std::nth_element(lbeg, lcenter, lend,
+                       std::bind(&nodeBaseLess<BV>, std::placeholders::_1,
+                                 std::placeholders::_2, std::ref(best_axis)));
+
+      Node* node = createNode(nullptr, vol, nullptr);
+      node->children[0] = topdown_0(lbeg, lcenter);
+      node->children[1] = topdown_0(lcenter, lend);
+      node->children[0]->parent = node;
+      node->children[1]->parent = node;
+      return node;
+    } else {
+      bottomup(lbeg, lend);
+      return *lbeg;
+    }
+  }
+  return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_1(
+    const NodeVecIterator lbeg, const NodeVecIterator lend) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (num_leaves > bu_threshold) {
+      Vec3s split_p = (*lbeg)->bv.center();
+      BV vol = (*lbeg)->bv;
+      NodeVecIterator it;
+      for (it = lbeg + 1; it < lend; ++it) {
+        split_p += (*it)->bv.center();
+        vol += (*it)->bv;
+      }
+      split_p /= static_cast<CoalScalar>(num_leaves);
+      int best_axis = -1;
+      long bestmidp = num_leaves;
+      int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
+      for (it = lbeg; it < lend; ++it) {
+        Vec3s x = (*it)->bv.center() - split_p;
+        for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
+      }
+
+      for (int i = 0; i < 3; ++i) {
+        if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
+          long midp = std::abs(splitcount[i][0] - splitcount[i][1]);
+          if (midp < bestmidp) {
+            best_axis = i;
+            bestmidp = midp;
+          }
+        }
+      }
+
+      if (best_axis < 0) best_axis = 0;
+
+      CoalScalar split_value = split_p[best_axis];
+      NodeVecIterator lcenter = lbeg;
+      for (it = lbeg; it < lend; ++it) {
+        if ((*it)->bv.center()[best_axis] < split_value) {
+          Node* temp = *it;
+          *it = *lcenter;
+          *lcenter = temp;
+          ++lcenter;
+        }
+      }
+
+      Node* node = createNode(nullptr, vol, nullptr);
+      node->children[0] = topdown_1(lbeg, lcenter);
+      node->children[1] = topdown_1(lcenter, lend);
+      node->children[0]->parent = node;
+      node->children[1]->parent = node;
+      return node;
+    } else {
+      bottomup(lbeg, lend);
+      return *lbeg;
+    }
+  }
+  return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init_0(std::vector<Node*>& leaves) {
+  clear();
+  root_node = topdown(leaves.begin(), leaves.end());
+  n_leaves = leaves.size();
+  max_lookahead_level = -1;
+  opath = 0;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init_1(std::vector<Node*>& leaves) {
+  clear();
+
+  BV bound_bv;
+  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
+  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
+
+  morton_functor<CoalScalar, uint32_t> coder(bound_bv);
+  for (size_t i = 0; i < leaves.size(); ++i)
+    leaves[i]->code = coder(leaves[i]->bv.center());
+
+  std::sort(leaves.begin(), leaves.end(), SortByMorton());
+
+  root_node = mortonRecurse_0(leaves.begin(), leaves.end(),
+                              (1 << (coder.bits() - 1)), coder.bits() - 1);
+
+  refit();
+  n_leaves = leaves.size();
+  max_lookahead_level = -1;
+  opath = 0;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init_2(std::vector<Node*>& leaves) {
+  clear();
+
+  BV bound_bv;
+  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
+  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
+
+  morton_functor<CoalScalar, uint32_t> coder(bound_bv);
+  for (size_t i = 0; i < leaves.size(); ++i)
+    leaves[i]->code = coder(leaves[i]->bv.center());
+
+  std::sort(leaves.begin(), leaves.end(), SortByMorton());
+
+  root_node = mortonRecurse_1(leaves.begin(), leaves.end(),
+                              (1 << (coder.bits() - 1)), coder.bits() - 1);
+
+  refit();
+  n_leaves = leaves.size();
+  max_lookahead_level = -1;
+  opath = 0;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init_3(std::vector<Node*>& leaves) {
+  clear();
+
+  BV bound_bv;
+  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
+  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
+
+  morton_functor<CoalScalar, uint32_t> coder(bound_bv);
+  for (size_t i = 0; i < leaves.size(); ++i)
+    leaves[i]->code = coder(leaves[i]->bv.center());
+
+  std::sort(leaves.begin(), leaves.end(), SortByMorton());
+
+  root_node = mortonRecurse_2(leaves.begin(), leaves.end());
+
+  refit();
+  n_leaves = leaves.size();
+  max_lookahead_level = -1;
+  opath = 0;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_0(
+    const NodeVecIterator lbeg, const NodeVecIterator lend,
+    const uint32_t& split, int bits) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (bits > 0) {
+      Node dummy;
+      dummy.code = split;
+      NodeVecIterator lcenter =
+          std::lower_bound(lbeg, lend, &dummy, SortByMorton());
+
+      if (lcenter == lbeg) {
+        uint32_t split2 = split | (1 << (bits - 1));
+        return mortonRecurse_0(lbeg, lend, split2, bits - 1);
+      } else if (lcenter == lend) {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        return mortonRecurse_0(lbeg, lend, split1, bits - 1);
+      } else {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        uint32_t split2 = split | (1 << (bits - 1));
+
+        Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
+        Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
+        Node* node = createNode(nullptr, nullptr);
+        node->children[0] = child1;
+        node->children[1] = child2;
+        child1->parent = node;
+        child2->parent = node;
+        return node;
+      }
+    } else {
+      Node* node = topdown(lbeg, lend);
+      return node;
+    }
+  } else
+    return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_1(
+    const NodeVecIterator lbeg, const NodeVecIterator lend,
+    const uint32_t& split, int bits) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (bits > 0) {
+      Node dummy;
+      dummy.code = split;
+      NodeVecIterator lcenter =
+          std::lower_bound(lbeg, lend, &dummy, SortByMorton());
+
+      if (lcenter == lbeg) {
+        uint32_t split2 = split | (1 << (bits - 1));
+        return mortonRecurse_1(lbeg, lend, split2, bits - 1);
+      } else if (lcenter == lend) {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        return mortonRecurse_1(lbeg, lend, split1, bits - 1);
+      } else {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        uint32_t split2 = split | (1 << (bits - 1));
+
+        Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
+        Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
+        Node* node = createNode(nullptr, nullptr);
+        node->children[0] = child1;
+        node->children[1] = child2;
+        child1->parent = node;
+        child2->parent = node;
+        return node;
+      }
+    } else {
+      Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
+      Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
+      Node* node = createNode(nullptr, nullptr);
+      node->children[0] = child1;
+      node->children[1] = child2;
+      child1->parent = node;
+      child2->parent = node;
+      return node;
+    }
+  } else
+    return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_2(
+    const NodeVecIterator lbeg, const NodeVecIterator lend) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
+    Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
+    Node* node = createNode(nullptr, nullptr);
+    node->children[0] = child1;
+    node->children[1] = child2;
+    child1->parent = node;
+    child2->parent = node;
+    return node;
+  } else
+    return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::update_(Node* leaf, const BV& bv) {
+  Node* root = removeLeaf(leaf);
+  if (root) {
+    if (max_lookahead_level >= 0) {
+      for (int i = 0; (i < max_lookahead_level) && root->parent; ++i)
+        root = root->parent;
+    } else
+      root = root_node;
+  }
+
+  leaf->bv = bv;
+  insertLeaf(root, leaf);
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::sort(Node* n, Node*& r) {
+  Node* p = n->parent;
+  if (p > n) {
+    size_t i = indexOf(n);
+    size_t j = 1 - i;
+    Node* s = p->children[j];
+    Node* q = p->parent;
+    if (q)
+      q->children[indexOf(p)] = n;
+    else
+      r = n;
+    s->parent = n;
+    p->parent = n;
+    n->parent = q;
+    p->children[0] = n->children[0];
+    p->children[1] = n->children[1];
+    n->children[0]->parent = p;
+    n->children[1]->parent = p;
+    n->children[i] = p;
+    n->children[j] = s;
+    std::swap(p->bv, n->bv);
+    return p;
+  }
+  return n;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::insertLeaf(Node* const sub_root, Node* const leaf)
+// Attempts to insert `leaf` into a subtree rooted at `sub_root`.
+// 1. If the whole tree is empty, then `leaf` simply becomes the tree.
+// 2. Otherwise, a leaf node called `sibling` of the subtree rooted at
+//    `sub_root` is selected (the selection criteria is a black box for this
+//    algorithm), and the `sibling` leaf is replaced by an internal node with
+//    two children, `sibling` and `leaf`. The bounding volumes are updated as
+//    necessary.
+{
+  if (!root_node) {
+    // If the entire tree is empty, the node pointed by `leaf` variable will
+    // become the root of the tree.
+    root_node = leaf;
+    leaf->parent = nullptr;
+    return;
+  }
+  // Traverse the tree from the given `sub_root` down to an existing leaf
+  // node that we call `sibling`. The `sibling` node will eventually become
+  // the sibling of the given `leaf` node.
+  Node* sibling = sub_root;
+  while (!sibling->isLeaf()) {
+    sibling = sibling->children[select(*leaf, *(sibling->children[0]),
+                                       *(sibling->children[1]))];
+  }
+  Node* prev = sibling->parent;
+  // Create a new `node` that later will become the new parent of both the
+  // `sibling` and the given `leaf`.
+  Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr);
+  if (prev)
+  // If the parent `prev` of the `sibling` is an interior node, we will
+  // replace the `sibling` with the subtree {node {`sibling`, leaf}} like
+  // this:
+  //        Before                After
+  //
+  //          ╱                     ╱
+  //        prev                  prev
+  //        ╱  ╲        ─>        ╱  ╲
+  //  sibling  ...             node  ...
+  //                           ╱  ╲
+  //                     sibling  leaf
+  {
+    prev->children[indexOf(sibling)] = node;
+    node->children[0] = sibling;
+    sibling->parent = node;
+    node->children[1] = leaf;
+    leaf->parent = node;
+    // Now that we've inserted `leaf` some of the existing bounding
+    // volumes might not fully enclose their children. Walk up the tree
+    // looking for parents that don't already enclose their children
+    // and create a new tight-fitting bounding volume for those.
+    do {
+      if (!prev->bv.contain(node->bv))
+        prev->bv = prev->children[0]->bv + prev->children[1]->bv;
+      else
+        break;
+      node = prev;
+    } while (nullptr != (prev = node->parent));
+  } else
+  // If the `sibling` has no parent, i.e., the tree is a singleton,
+  // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like
+  // this:
+  //
+  //        node
+  //        ╱  ╲
+  //  sibling  leaf
+  {
+    node->children[0] = sibling;
+    sibling->parent = node;
+    node->children[1] = leaf;
+    leaf->parent = node;
+    root_node = node;
+  }
+
+  // Note that the above algorithm always adds the new `leaf` node as the right
+  // child, i.e., children[1].  Calling removeLeaf(l) followed by calling
+  // this function insertLeaf(l) where l is a left child will result in
+  // switching l and its sibling even if no object's pose has changed.
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::removeLeaf(
+    Node* const leaf) {
+  // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling,
+  // and its parent with just its sibling. It then "tightens" the ancestor
+  // bounding volumes. Returns a pointer to the parent of the highest node whose
+  // BV changed due to the removal.
+  if (leaf == root_node) {
+    // If the `leaf` node is the only node in the tree, the tree becomes empty.
+    root_node = nullptr;
+    return nullptr;
+  }
+  Node* parent = leaf->parent;
+  Node* prev = parent->parent;
+  Node* sibling = parent->children[1 - indexOf(leaf)];
+  if (prev) {
+    // If the parent node is not the root node, the sibling node will
+    // replace the parent node like this:
+    //
+    //            Before              After
+    //             ...                 ...
+    //             ╱                   ╱
+    //           prev                prev
+    //          ╱   ╲               ╱   ╲
+    //     parent   ...    ─>  sibling  ...
+    //      ╱  ╲                 ╱╲
+    //  leaf  sibling           ...
+    //           ╱╲
+    //          ...
+    //
+    // Step 1: replace the subtree {parent {leaf, sibling {...}}} with
+    // {sibling {...}}.
+    prev->children[indexOf(parent)] = sibling;
+    sibling->parent = prev;
+    deleteNode(parent);
+    // Step 2: tighten up the BVs of the ancestor nodes.
+    while (prev) {
+      BV new_bv = prev->children[0]->bv + prev->children[1]->bv;
+      if (!(new_bv == prev->bv)) {
+        prev->bv = new_bv;
+        prev = prev->parent;
+      } else
+        break;
+    }
+
+    return prev ? prev : root_node;
+  } else {
+    // If the parent node is the root node, the sibling node will become the
+    // root of the whole tree like this:
+    //
+    //     Before                   After
+    //
+    //     parent
+    //      ╱  ╲
+    //  leaf  sibling     ─>       sibling
+    //           ╱╲                  ╱╲
+    //          ...                 ...
+    root_node = sibling;
+    sibling->parent = nullptr;
+    deleteNode(parent);
+    return root_node;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::fetchLeaves(Node* root, std::vector<Node*>& leaves,
+                                    int depth) {
+  if ((!root->isLeaf()) && depth) {
+    fetchLeaves(root->children[0], leaves, depth - 1);
+    fetchLeaves(root->children[1], leaves, depth - 1);
+    deleteNode(root);
+  } else {
+    leaves.push_back(root);
+  }
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::indexOf(Node* node) {
+  // node cannot be nullptr
+  return (node->parent->children[1] == node);
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
+                                                                const BV& bv,
+                                                                void* data) {
+  Node* node = createNode(parent, data);
+  node->bv = bv;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
+                                                                const BV& bv1,
+                                                                const BV& bv2,
+                                                                void* data) {
+  Node* node = createNode(parent, data);
+  node->bv = bv1 + bv2;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
+                                                                void* data) {
+  Node* node = nullptr;
+  if (free_node) {
+    node = free_node;
+    free_node = nullptr;
+  } else
+    node = new Node();
+  node->parent = parent;
+  node->data = data;
+  node->children[1] = 0;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::deleteNode(Node* node) {
+  if (free_node != node) {
+    delete free_node;
+    free_node = node;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::recurseDeleteNode(Node* node) {
+  if (!node->isLeaf()) {
+    recurseDeleteNode(node->children[0]);
+    recurseDeleteNode(node->children[1]);
+  }
+
+  if (node == root_node) root_node = nullptr;
+  deleteNode(node);
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::recurseRefit(Node* node) {
+  if (!node->isLeaf()) {
+    recurseRefit(node->children[0]);
+    recurseRefit(node->children[1]);
+    node->bv = node->children[0]->bv + node->children[1]->bv;
+  } else
+    return;
+}
+
+//==============================================================================
+template <typename BV>
+bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d) {
+  if (a->bv.center()[d] < b->bv.center()[d]) return true;
+  return false;
+}
+
+//==============================================================================
+template <typename S, typename BV>
+struct SelectImpl {
+  static std::size_t run(const NodeBase<BV>& /*query*/,
+                         const NodeBase<BV>& /*node1*/,
+                         const NodeBase<BV>& /*node2*/) {
+    return 0;
+  }
+
+  static std::size_t run(const BV& /*query*/, const NodeBase<BV>& /*node1*/,
+                         const NodeBase<BV>& /*node2*/) {
+    return 0;
+  }
+};
+
+//==============================================================================
+template <typename BV>
+size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
+              const NodeBase<BV>& node2) {
+  return SelectImpl<CoalScalar, BV>::run(query, node1, node2);
+}
+
+//==============================================================================
+template <typename BV>
+size_t select(const BV& query, const NodeBase<BV>& node1,
+              const NodeBase<BV>& node2) {
+  return SelectImpl<CoalScalar, BV>::run(query, node1, node2);
+}
+
+//==============================================================================
+template <typename S>
+struct SelectImpl<S, AABB> {
+  static std::size_t run(const NodeBase<AABB>& node,
+                         const NodeBase<AABB>& node1,
+                         const NodeBase<AABB>& node2) {
+    const AABB& bv = node.bv;
+    const AABB& bv1 = node1.bv;
+    const AABB& bv2 = node2.bv;
+    Vec3s v = bv.min_ + bv.max_;
+    Vec3s v1 = v - (bv1.min_ + bv1.max_);
+    Vec3s v2 = v - (bv2.min_ + bv2.max_);
+    CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+    CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+    return (d1 < d2) ? 0 : 1;
+  }
+
+  static std::size_t run(const AABB& query, const NodeBase<AABB>& node1,
+                         const NodeBase<AABB>& node2) {
+    const AABB& bv = query;
+    const AABB& bv1 = node1.bv;
+    const AABB& bv2 = node2.bv;
+    Vec3s v = bv.min_ + bv.max_;
+    Vec3s v1 = v - (bv1.min_ + bv1.max_);
+    Vec3s v2 = v - (bv2.min_ + bv2.max_);
+    CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+    CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+    return (d1 < d2) ? 0 : 1;
+  }
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/hierarchy_tree.h b/include/coal/broadphase/detail/hierarchy_tree.h
new file mode 100644
index 0000000000000000000000000000000000000000..6b5fe5261137fe7b0f4d48ff340e62e48974310e
--- /dev/null
+++ b/include/coal/broadphase/detail/hierarchy_tree.h
@@ -0,0 +1,292 @@
+/*
+ * 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 COAL_HIERARCHY_TREE_H
+#define COAL_HIERARCHY_TREE_H
+
+#include <vector>
+#include <map>
+#include <functional>
+#include <iostream>
+#include "coal/warning.hh"
+#include "coal/BV/AABB.h"
+#include "coal/broadphase/detail/morton.h"
+#include "coal/broadphase/detail/node_base.h"
+
+namespace coal {
+
+namespace detail {
+
+/// @brief Class for hierarchy tree structure
+template <typename BV>
+class HierarchyTree {
+ public:
+  typedef NodeBase<BV> Node;
+
+  /// @brief Create hierarchy tree with suitable setting.
+  /// bu_threshold decides the height of tree node to start bottom-up
+  /// construction / optimization; topdown_level decides different methods to
+  /// construct tree in topdown manner. lower level method constructs tree with
+  /// better quality but is slower.
+  HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
+
+  ~HierarchyTree();
+
+  /// @brief Initialize the tree by a set of leaves using algorithm with a given
+  /// level.
+  void init(std::vector<Node*>& leaves, int level = 0);
+
+  /// @brief Insest a node
+  Node* insert(const BV& bv, void* data);
+
+  /// @brief Remove a leaf node
+  void remove(Node* leaf);
+
+  /// @brief Clear the tree
+  void clear();
+
+  /// @brief Whether the tree is empty
+  bool empty() const;
+
+  /// @brief Updates a `leaf` node. A use case is when the bounding volume
+  /// of an object changes. Ensure every parent node has its bounding volume
+  /// expand or shrink to fit the bounding volumes of its children.
+  /// @note Strangely the structure of the tree may change even if the
+  ///       bounding volume of the `leaf` node does not change. It is just
+  ///       another valid tree of the same set of objects.  This is because
+  ///       update() works by first removing `leaf` and then inserting `leaf`
+  ///       back. The structural change could be as simple as switching the
+  ///       order of two leaves if the sibling of the `leaf` is also a leaf.
+  ///       Or it could be more complicated if the sibling is an internal
+  ///       node.
+  void update(Node* leaf, int lookahead_level = -1);
+
+  /// @brief update the tree when the bounding volume of a given leaf has
+  /// changed
+  bool update(Node* leaf, const BV& bv);
+
+  /// @brief update one leaf's bounding volume, with prediction
+  bool update(Node* leaf, const BV& bv, const Vec3s& vel, CoalScalar margin);
+
+  /// @brief update one leaf's bounding volume, with prediction
+  bool update(Node* leaf, const BV& bv, const Vec3s& vel);
+
+  /// @brief get the max height of the tree
+  size_t getMaxHeight() const;
+
+  /// @brief get the max depth of the tree
+  size_t getMaxDepth() const;
+
+  /// @brief balance the tree from bottom
+  void balanceBottomup();
+
+  /// @brief balance the tree from top
+  void balanceTopdown();
+
+  /// @brief balance the tree in an incremental way
+  void balanceIncremental(int iterations);
+
+  /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
+  /// update the entire tree in a bottom-up manner
+  void refit();
+
+  /// @brief extract all the leaves of the tree
+  void extractLeaves(const Node* root, std::vector<Node*>& leaves) const;
+
+  /// @brief number of leaves in the tree
+  size_t size() const;
+
+  /// @brief get the root of the tree
+  Node* getRoot() const;
+
+  Node*& getRoot();
+
+  /// @brief print the tree in a recursive way
+  void print(Node* root, int depth);
+
+ private:
+  typedef typename std::vector<NodeBase<BV>*>::iterator NodeVecIterator;
+  typedef
+      typename std::vector<NodeBase<BV>*>::const_iterator NodeVecConstIterator;
+
+  struct SortByMorton {
+    bool operator()(const Node* a, const Node* b) const {
+      return a->code < b->code;
+    }
+  };
+
+  /// @brief construct a tree for a set of leaves from bottom -- very heavy way
+  void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief construct a tree for a set of leaves from top
+  Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief compute the maximum height of a subtree rooted from a given node
+  size_t getMaxHeight(Node* node) const;
+
+  /// @brief compute the maximum depth of a subtree rooted from a given node
+  void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const;
+
+  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
+  /// topdown manner. During construction, first compute the best split axis as
+  /// the axis along with the longest AABB edge. Then compute the median of all
+  /// nodes' center projection onto the axis and using it as the split
+  /// threshold.
+  Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
+  /// topdown manner. During construction, first compute the best split
+  /// thresholds for different axes as the average of all nodes' center. Then
+  /// choose the split axis as the axis whose threshold can divide the nodes
+  /// into two parts with almost similar size. This construction is more
+  /// expensive then topdown_0, but also can provide tree with better quality.
+  Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief init tree from leaves in the topdown manner (topdown_0 or
+  /// topdown_1)
+  void init_0(std::vector<Node*>& leaves);
+
+  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
+  /// for nodes which is of depth more than the maximum bits of the morton code,
+  /// we use bottomup method to construct the subtree, which is slow but can
+  /// construct tree with high quality.
+  void init_1(std::vector<Node*>& leaves);
+
+  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
+  /// for nodes which is of depth more than the maximum bits of the morton code,
+  /// we split the leaves into two parts with the same size simply using the
+  /// node index.
+  void init_2(std::vector<Node*>& leaves);
+
+  /// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
+  /// for all nodes, we simply divide the leaves into parts with the same size
+  /// simply using the node index.
+  void init_3(std::vector<Node*>& leaves);
+
+  Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend,
+                        const uint32_t& split, int bits);
+
+  Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend,
+                        const uint32_t& split, int bits);
+
+  Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief update one leaf node's bounding volume
+  void update_(Node* leaf, const BV& bv);
+
+  /// @brief sort node n and its parent according to their memory position
+  Node* sort(Node* n, Node*& r);
+
+  /// @brief Insert a leaf node and also update its ancestors. Maintain the
+  /// tree as a full binary tree (every interior node has exactly two children).
+  /// Furthermore, adjust the BV of interior nodes so that each parent's BV
+  /// tightly fits its children's BVs.
+  /// @param sub_root The root of the subtree into which we will insert the
+  //                  leaf node.
+  void insertLeaf(Node* const sub_root, Node* const leaf);
+
+  /// @brief Remove a leaf. Maintain the tree as a full binary tree (every
+  /// interior node has exactly two children). Furthermore, adjust the BV of
+  /// interior nodes so that each parent's BV tightly fits its children's BVs.
+  /// @note The leaf node itself is not deleted yet, but all the unnecessary
+  ///       internal nodes are deleted.
+  /// @returns the root of the subtree containing the nodes whose BVs were
+  //           adjusted.
+  Node* removeLeaf(Node* const leaf);
+
+  /// @brief Delete all internal nodes and return all leaves nodes with given
+  /// depth from root
+  void fetchLeaves(Node* root, std::vector<Node*>& leaves, int depth = -1);
+
+  static size_t indexOf(Node* node);
+
+  /// @brief create one node (leaf or internal)
+  Node* createNode(Node* parent, const BV& bv, void* data);
+
+  Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data);
+
+  Node* createNode(Node* parent, void* data);
+
+  void deleteNode(Node* node);
+
+  void recurseDeleteNode(Node* node);
+
+  void recurseRefit(Node* node);
+
+ protected:
+  Node* root_node;
+
+  size_t n_leaves;
+
+  unsigned int opath;
+
+  /// This is a one Node cache, the reason is that we need to remove a node and
+  /// then add it again frequently.
+  Node* free_node;
+
+  int max_lookahead_level;
+
+ public:
+  /// @brief decide which topdown algorithm to use
+  int topdown_level;
+
+  /// @brief decide the depth to use expensive bottom-up algorithm
+  int bu_threshold;
+};
+
+/// @brief Compare two nodes accoording to the d-th dimension of node center
+template <typename BV>
+bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d);
+
+/// @brief select from node1 and node2 which is close to a given query. 0 for
+/// node1 and 1 for node2
+template <typename BV>
+size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
+              const NodeBase<BV>& node2);
+
+/// @brief select from node1 and node2 which is close to a given query bounding
+/// volume. 0 for node1 and 1 for node2
+template <typename BV>
+size_t select(const BV& query, const NodeBase<BV>& node1,
+              const NodeBase<BV>& node2);
+
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/hierarchy_tree-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..fa902e67cc9dccad1bc7b169c1e1908e127739c7
--- /dev/null
+++ b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h
@@ -0,0 +1,978 @@
+/*
+ * 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 COAL_HIERARCHY_TREE_ARRAY_INL_H
+#define COAL_HIERARCHY_TREE_ARRAY_INL_H
+
+#include "coal/broadphase/detail/hierarchy_tree_array.h"
+
+#include <algorithm>
+#include <iostream>
+
+namespace coal {
+
+namespace detail {
+
+namespace implementation_array {
+
+//==============================================================================
+template <typename BV>
+HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
+  root_node = NULL_NODE;
+  n_nodes = 0;
+  n_nodes_alloc = 16;
+  nodes = new Node[n_nodes_alloc];
+  for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
+  nodes[n_nodes_alloc - 1].next = NULL_NODE;
+  n_leaves = 0;
+  freelist = 0;
+  opath = 0;
+  max_lookahead_level = -1;
+  bu_threshold = bu_threshold_;
+  topdown_level = topdown_level_;
+}
+
+//==============================================================================
+template <typename BV>
+HierarchyTree<BV>::~HierarchyTree() {
+  delete[] nodes;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init(Node* leaves, int n_leaves_, int level) {
+  switch (level) {
+    case 0:
+      init_0(leaves, n_leaves_);
+      break;
+    case 1:
+      init_1(leaves, n_leaves_);
+      break;
+    case 2:
+      init_2(leaves, n_leaves_);
+      break;
+    case 3:
+      init_3(leaves, n_leaves_);
+      break;
+    default:
+      init_0(leaves, n_leaves_);
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init_0(Node* leaves, int n_leaves_) {
+  clear();
+
+  n_leaves = (size_t)n_leaves_;
+  root_node = NULL_NODE;
+  nodes = new Node[n_leaves * 2];
+  std::copy(leaves, leaves + n_leaves, nodes);
+  freelist = n_leaves;
+  n_nodes = n_leaves;
+  n_nodes_alloc = 2 * n_leaves;
+  for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
+  nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+  size_t* ids = new size_t[n_leaves];
+  for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
+
+  root_node = topdown(ids, ids + n_leaves);
+  delete[] ids;
+
+  opath = 0;
+  max_lookahead_level = -1;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init_1(Node* leaves, int n_leaves_) {
+  clear();
+
+  n_leaves = (size_t)n_leaves_;
+  root_node = NULL_NODE;
+  nodes = new Node[n_leaves * 2];
+  std::copy(leaves, leaves + n_leaves, nodes);
+  freelist = n_leaves;
+  n_nodes = n_leaves;
+  n_nodes_alloc = 2 * n_leaves;
+  for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
+  nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+  BV bound_bv;
+  if (n_leaves > 0) bound_bv = nodes[0].bv;
+  for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
+
+  morton_functor<CoalScalar, uint32_t> coder(bound_bv);
+  for (size_t i = 0; i < n_leaves; ++i)
+    nodes[i].code = coder(nodes[i].bv.center());
+
+  size_t* ids = new size_t[n_leaves];
+  for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
+
+  const SortByMorton comp{nodes};
+  std::sort(ids, ids + n_leaves, comp);
+  root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
+                              coder.bits() - 1);
+  delete[] ids;
+
+  refit();
+
+  opath = 0;
+  max_lookahead_level = -1;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init_2(Node* leaves, int n_leaves_) {
+  clear();
+
+  n_leaves = (size_t)n_leaves_;
+  root_node = NULL_NODE;
+  nodes = new Node[n_leaves * 2];
+  std::copy(leaves, leaves + n_leaves, nodes);
+  freelist = n_leaves;
+  n_nodes = n_leaves;
+  n_nodes_alloc = 2 * n_leaves;
+  for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
+  nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+  BV bound_bv;
+  if (n_leaves > 0) bound_bv = nodes[0].bv;
+  for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
+
+  morton_functor<CoalScalar, uint32_t> coder(bound_bv);
+  for (size_t i = 0; i < n_leaves; ++i)
+    nodes[i].code = coder(nodes[i].bv.center());
+
+  size_t* ids = new size_t[n_leaves];
+  for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
+
+  const SortByMorton comp{nodes};
+  std::sort(ids, ids + n_leaves, comp);
+  root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
+                              coder.bits() - 1);
+  delete[] ids;
+
+  refit();
+
+  opath = 0;
+  max_lookahead_level = -1;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::init_3(Node* leaves, int n_leaves_) {
+  clear();
+
+  n_leaves = (size_t)n_leaves_;
+  root_node = NULL_NODE;
+  nodes = new Node[n_leaves * 2];
+  std::copy(leaves, leaves + n_leaves, nodes);
+  freelist = n_leaves;
+  n_nodes = n_leaves;
+  n_nodes_alloc = 2 * n_leaves;
+  for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
+  nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+  BV bound_bv;
+  if (n_leaves > 0) bound_bv = nodes[0].bv;
+  for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
+
+  morton_functor<CoalScalar, uint32_t> coder(bound_bv);
+  for (size_t i = 0; i < n_leaves; ++i)
+    nodes[i].code = coder(nodes[i].bv.center());
+
+  size_t* ids = new size_t[n_leaves];
+  for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
+
+  const SortByMorton comp{nodes};
+  std::sort(ids, ids + n_leaves, comp);
+  root_node = mortonRecurse_2(ids, ids + n_leaves);
+  delete[] ids;
+
+  refit();
+
+  opath = 0;
+  max_lookahead_level = -1;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::insert(const BV& bv, void* data) {
+  size_t node = createNode(NULL_NODE, bv, data);
+  insertLeaf(root_node, node);
+  ++n_leaves;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::remove(size_t leaf) {
+  removeLeaf(leaf);
+  deleteNode(leaf);
+  --n_leaves;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::clear() {
+  delete[] nodes;
+  root_node = NULL_NODE;
+  n_nodes = 0;
+  n_nodes_alloc = 16;
+  nodes = new Node[n_nodes_alloc];
+  for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
+  nodes[n_nodes_alloc - 1].next = NULL_NODE;
+  n_leaves = 0;
+  freelist = 0;
+  opath = 0;
+  max_lookahead_level = -1;
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::empty() const {
+  return (n_nodes == 0);
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::update(size_t leaf, int lookahead_level) {
+  size_t root = removeLeaf(leaf);
+  if (root != NULL_NODE) {
+    if (lookahead_level > 0) {
+      for (int i = 0;
+           (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
+        root = nodes[root].parent;
+    } else
+      root = root_node;
+  }
+  insertLeaf(root, leaf);
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(size_t leaf, const BV& bv) {
+  if (nodes[leaf].bv.contain(bv)) return false;
+  update_(leaf, bv);
+  return true;
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3s& vel,
+                               CoalScalar margin) {
+  COAL_UNUSED_VARIABLE(bv);
+  COAL_UNUSED_VARIABLE(vel);
+  COAL_UNUSED_VARIABLE(margin);
+
+  if (nodes[leaf].bv.contain(bv)) return false;
+  update_(leaf, bv);
+  return true;
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3s& vel) {
+  COAL_UNUSED_VARIABLE(vel);
+
+  if (nodes[leaf].bv.contain(bv)) return false;
+  update_(leaf, bv);
+  return true;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::getMaxHeight() const {
+  if (root_node == NULL_NODE) return 0;
+
+  return getMaxHeight(root_node);
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::getMaxDepth() const {
+  if (root_node == NULL_NODE) return 0;
+
+  size_t max_depth;
+  getMaxDepth(root_node, 0, max_depth);
+  return max_depth;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::balanceBottomup() {
+  if (root_node != NULL_NODE) {
+    Node* leaves = new Node[n_leaves];
+    Node* leaves_ = leaves;
+    extractLeaves(root_node, leaves_);
+    root_node = NULL_NODE;
+    std::copy(leaves, leaves + n_leaves, nodes);
+    freelist = n_leaves;
+    n_nodes = n_leaves;
+    for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
+    nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+    size_t* ids = new size_t[n_leaves];
+    for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
+
+    bottomup(ids, ids + n_leaves);
+    root_node = *ids;
+
+    delete[] ids;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::balanceTopdown() {
+  if (root_node != NULL_NODE) {
+    Node* leaves = new Node[n_leaves];
+    Node* leaves_ = leaves;
+    extractLeaves(root_node, leaves_);
+    root_node = NULL_NODE;
+    std::copy(leaves, leaves + n_leaves, nodes);
+    freelist = n_leaves;
+    n_nodes = n_leaves;
+    for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
+    nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+    size_t* ids = new size_t[n_leaves];
+    for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
+
+    root_node = topdown(ids, ids + n_leaves);
+    delete[] ids;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::balanceIncremental(int iterations) {
+  if (iterations < 0) iterations = (int)n_leaves;
+  if ((root_node != NULL_NODE) && (iterations > 0)) {
+    for (int i = 0; i < iterations; ++i) {
+      size_t node = root_node;
+      unsigned int bit = 0;
+      while (!nodes[node].isLeaf()) {
+        node = nodes[node].children[(opath >> bit) & 1];
+        bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
+      }
+      update(node);
+      ++opath;
+    }
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::refit() {
+  if (root_node != NULL_NODE) recurseRefit(root_node);
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::extractLeaves(size_t root, Node*& leaves) const {
+  if (!nodes[root].isLeaf()) {
+    extractLeaves(nodes[root].children[0], leaves);
+    extractLeaves(nodes[root].children[1], leaves);
+  } else {
+    *leaves = nodes[root];
+    leaves++;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::size() const {
+  return n_leaves;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::getRoot() const {
+  return root_node;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getNodes() const {
+  return nodes;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::print(size_t root, int depth) {
+  for (int i = 0; i < depth; ++i) std::cout << " ";
+  Node* n = nodes + root;
+  std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", "
+            << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1]
+            << ", " << n->bv.max_[2] << ")" << std::endl;
+  if (n->isLeaf()) {
+  } else {
+    print(n->children[0], depth + 1);
+    print(n->children[1], depth + 1);
+  }
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::getMaxHeight(size_t node) const {
+  if (!nodes[node].isLeaf()) {
+    size_t height1 = getMaxHeight(nodes[node].children[0]);
+    size_t height2 = getMaxHeight(nodes[node].children[1]);
+    return std::max(height1, height2) + 1;
+  } else
+    return 0;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::getMaxDepth(size_t node, size_t depth,
+                                    size_t& max_depth) const {
+  if (!nodes[node].isLeaf()) {
+    getMaxDepth(nodes[node].children[0], depth + 1, max_depth);
+    getmaxDepth(nodes[node].children[1], depth + 1, max_depth);
+  } else
+    max_depth = std::max(max_depth, depth);
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend) {
+  size_t* lcur_end = lend;
+  while (lbeg < lcur_end - 1) {
+    size_t *min_it1 = nullptr, *min_it2 = nullptr;
+    CoalScalar min_size = (std::numeric_limits<CoalScalar>::max)();
+    for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) {
+      for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) {
+        CoalScalar cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
+        if (cur_size < min_size) {
+          min_size = cur_size;
+          min_it1 = it1;
+          min_it2 = it2;
+        }
+      }
+    }
+
+    size_t p =
+        createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr);
+    nodes[p].children[0] = *min_it1;
+    nodes[p].children[1] = *min_it2;
+    nodes[*min_it1].parent = p;
+    nodes[*min_it2].parent = p;
+    *min_it1 = p;
+    size_t tmp = *min_it2;
+    lcur_end--;
+    *min_it2 = *lcur_end;
+    *lcur_end = tmp;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::topdown(size_t* lbeg, size_t* lend) {
+  switch (topdown_level) {
+    case 0:
+      return topdown_0(lbeg, lend);
+      break;
+    case 1:
+      return topdown_1(lbeg, lend);
+      break;
+    default:
+      return topdown_0(lbeg, lend);
+  }
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::topdown_0(size_t* lbeg, size_t* lend) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (num_leaves > bu_threshold) {
+      BV vol = nodes[*lbeg].bv;
+      for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv;
+
+      size_t best_axis = 0;
+      CoalScalar extent[3] = {vol.width(), vol.height(), vol.depth()};
+      if (extent[1] > extent[0]) best_axis = 1;
+      if (extent[2] > extent[best_axis]) best_axis = 2;
+
+      nodeBaseLess<BV> comp(nodes, best_axis);
+      size_t* lcenter = lbeg + num_leaves / 2;
+      std::nth_element(lbeg, lcenter, lend, comp);
+
+      size_t node = createNode(NULL_NODE, vol, nullptr);
+      nodes[node].children[0] = topdown_0(lbeg, lcenter);
+      nodes[node].children[1] = topdown_0(lcenter, lend);
+      nodes[nodes[node].children[0]].parent = node;
+      nodes[nodes[node].children[1]].parent = node;
+      return node;
+    } else {
+      bottomup(lbeg, lend);
+      return *lbeg;
+    }
+  }
+  return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (num_leaves > bu_threshold) {
+      Vec3s split_p = nodes[*lbeg].bv.center();
+      BV vol = nodes[*lbeg].bv;
+      for (size_t* i = lbeg + 1; i < lend; ++i) {
+        split_p += nodes[*i].bv.center();
+        vol += nodes[*i].bv;
+      }
+      split_p /= static_cast<CoalScalar>(num_leaves);
+      int best_axis = -1;
+      int bestmidp = (int)num_leaves;
+      int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
+      for (size_t* i = lbeg; i < lend; ++i) {
+        Vec3s x = nodes[*i].bv.center() - split_p;
+        for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
+      }
+
+      for (size_t i = 0; i < 3; ++i) {
+        if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
+          int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
+          if (midp < bestmidp) {
+            best_axis = (int)i;
+            bestmidp = midp;
+          }
+        }
+      }
+
+      if (best_axis < 0) best_axis = 0;
+
+      CoalScalar split_value = split_p[best_axis];
+      size_t* lcenter = lbeg;
+      for (size_t* i = lbeg; i < lend; ++i) {
+        if (nodes[*i].bv.center()[best_axis] < split_value) {
+          size_t temp = *i;
+          *i = *lcenter;
+          *lcenter = temp;
+          ++lcenter;
+        }
+      }
+
+      size_t node = createNode(NULL_NODE, vol, nullptr);
+      nodes[node].children[0] = topdown_1(lbeg, lcenter);
+      nodes[node].children[1] = topdown_1(lcenter, lend);
+      nodes[nodes[node].children[0]].parent = node;
+      nodes[nodes[node].children[1]].parent = node;
+      return node;
+    } else {
+      bottomup(lbeg, lend);
+      return *lbeg;
+    }
+  }
+  return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::mortonRecurse_0(size_t* lbeg, size_t* lend,
+                                          const uint32_t& split, int bits) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (bits > 0) {
+      const SortByMorton comp{nodes, split};
+      size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
+
+      if (lcenter == lbeg) {
+        uint32_t split2 = split | (1 << (bits - 1));
+        return mortonRecurse_0(lbeg, lend, split2, bits - 1);
+      } else if (lcenter == lend) {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        return mortonRecurse_0(lbeg, lend, split1, bits - 1);
+      } else {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        uint32_t split2 = split | (1 << (bits - 1));
+
+        size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
+        size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
+        size_t node = createNode(NULL_NODE, nullptr);
+        nodes[node].children[0] = child1;
+        nodes[node].children[1] = child2;
+        nodes[child1].parent = node;
+        nodes[child2].parent = node;
+        return node;
+      }
+    } else {
+      size_t node = topdown(lbeg, lend);
+      return node;
+    }
+  } else
+    return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::mortonRecurse_1(size_t* lbeg, size_t* lend,
+                                          const uint32_t& split, int bits) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (bits > 0) {
+      const SortByMorton comp{nodes, split};
+      size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
+
+      if (lcenter == lbeg) {
+        uint32_t split2 = split | (1 << (bits - 1));
+        return mortonRecurse_1(lbeg, lend, split2, bits - 1);
+      } else if (lcenter == lend) {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        return mortonRecurse_1(lbeg, lend, split1, bits - 1);
+      } else {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        uint32_t split2 = split | (1 << (bits - 1));
+
+        size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
+        size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
+        size_t node = createNode(NULL_NODE, nullptr);
+        nodes[node].children[0] = child1;
+        nodes[node].children[1] = child2;
+        nodes[child1].parent = node;
+        nodes[child2].parent = node;
+        return node;
+      }
+    } else {
+      size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
+      size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
+      size_t node = createNode(NULL_NODE, nullptr);
+      nodes[node].children[0] = child1;
+      nodes[node].children[1] = child2;
+      nodes[child1].parent = node;
+      nodes[child2].parent = node;
+      return node;
+    }
+  } else
+    return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::mortonRecurse_2(size_t* lbeg, size_t* lend) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
+    size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
+    size_t node = createNode(NULL_NODE, nullptr);
+    nodes[node].children[0] = child1;
+    nodes[node].children[1] = child2;
+    nodes[child1].parent = node;
+    nodes[child2].parent = node;
+    return node;
+  } else
+    return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::insertLeaf(size_t root, size_t leaf) {
+  if (root_node == NULL_NODE) {
+    root_node = leaf;
+    nodes[leaf].parent = NULL_NODE;
+  } else {
+    if (!nodes[root].isLeaf()) {
+      do {
+        root = nodes[root].children[select(leaf, nodes[root].children[0],
+                                           nodes[root].children[1], nodes)];
+      } while (!nodes[root].isLeaf());
+    }
+
+    size_t prev = nodes[root].parent;
+    size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr);
+    if (prev != NULL_NODE) {
+      nodes[prev].children[indexOf(root)] = node;
+      nodes[node].children[0] = root;
+      nodes[root].parent = node;
+      nodes[node].children[1] = leaf;
+      nodes[leaf].parent = node;
+      do {
+        if (!nodes[prev].bv.contain(nodes[node].bv))
+          nodes[prev].bv = nodes[nodes[prev].children[0]].bv +
+                           nodes[nodes[prev].children[1]].bv;
+        else
+          break;
+        node = prev;
+      } while (NULL_NODE != (prev = nodes[node].parent));
+    } else {
+      nodes[node].children[0] = root;
+      nodes[root].parent = node;
+      nodes[node].children[1] = leaf;
+      nodes[leaf].parent = node;
+      root_node = node;
+    }
+  }
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::removeLeaf(size_t leaf) {
+  if (leaf == root_node) {
+    root_node = NULL_NODE;
+    return NULL_NODE;
+  } else {
+    size_t parent = nodes[leaf].parent;
+    size_t prev = nodes[parent].parent;
+    size_t sibling = nodes[parent].children[1 - indexOf(leaf)];
+
+    if (prev != NULL_NODE) {
+      nodes[prev].children[indexOf(parent)] = sibling;
+      nodes[sibling].parent = prev;
+      deleteNode(parent);
+      while (prev != NULL_NODE) {
+        BV new_bv = nodes[nodes[prev].children[0]].bv +
+                    nodes[nodes[prev].children[1]].bv;
+        if (!(new_bv == nodes[prev].bv)) {
+          nodes[prev].bv = new_bv;
+          prev = nodes[prev].parent;
+        } else
+          break;
+      }
+
+      return (prev != NULL_NODE) ? prev : root_node;
+    } else {
+      root_node = sibling;
+      nodes[sibling].parent = NULL_NODE;
+      deleteNode(parent);
+      return root_node;
+    }
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::update_(size_t leaf, const BV& bv) {
+  size_t root = removeLeaf(leaf);
+  if (root != NULL_NODE) {
+    if (max_lookahead_level >= 0) {
+      for (int i = 0;
+           (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
+        root = nodes[root].parent;
+    }
+
+    nodes[leaf].bv = bv;
+    insertLeaf(root, leaf);
+  }
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::indexOf(size_t node) {
+  return (nodes[nodes[node].parent].children[1] == node);
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::allocateNode() {
+  if (freelist == NULL_NODE) {
+    Node* old_nodes = nodes;
+    n_nodes_alloc *= 2;
+    nodes = new Node[n_nodes_alloc];
+    std::copy(old_nodes, old_nodes + n_nodes, nodes);
+    delete[] old_nodes;
+
+    for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
+    nodes[n_nodes_alloc - 1].next = NULL_NODE;
+    freelist = n_nodes;
+  }
+
+  size_t node_id = freelist;
+  freelist = nodes[node_id].next;
+  nodes[node_id].parent = NULL_NODE;
+  nodes[node_id].children[0] = NULL_NODE;
+  nodes[node_id].children[1] = NULL_NODE;
+  ++n_nodes;
+  return node_id;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv1,
+                                     const BV& bv2, void* data) {
+  size_t node = allocateNode();
+  nodes[node].parent = parent;
+  nodes[node].data = data;
+  nodes[node].bv = bv1 + bv2;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv, void* data) {
+  size_t node = allocateNode();
+  nodes[node].parent = parent;
+  nodes[node].data = data;
+  nodes[node].bv = bv;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+size_t HierarchyTree<BV>::createNode(size_t parent, void* data) {
+  size_t node = allocateNode();
+  nodes[node].parent = parent;
+  nodes[node].data = data;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::deleteNode(size_t node) {
+  nodes[node].next = freelist;
+  freelist = node;
+  --n_nodes;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::recurseRefit(size_t node) {
+  if (!nodes[node].isLeaf()) {
+    recurseRefit(nodes[node].children[0]);
+    recurseRefit(nodes[node].children[1]);
+    nodes[node].bv =
+        nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv;
+  } else
+    return;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::fetchLeaves(size_t root, Node*& leaves, int depth) {
+  if ((!nodes[root].isLeaf()) && depth) {
+    fetchLeaves(nodes[root].children[0], leaves, depth - 1);
+    fetchLeaves(nodes[root].children[1], leaves, depth - 1);
+    deleteNode(root);
+  } else {
+    *leaves = nodes[root];
+    leaves++;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+nodeBaseLess<BV>::nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_)
+    : nodes(nodes_), d(d_) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename BV>
+bool nodeBaseLess<BV>::operator()(size_t i, size_t j) const {
+  if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true;
+
+  return false;
+}
+
+//==============================================================================
+template <typename S, typename BV>
+struct SelectImpl {
+  static bool run(size_t query, size_t node1, size_t node2,
+                  NodeBase<BV>* nodes) {
+    COAL_UNUSED_VARIABLE(query);
+    COAL_UNUSED_VARIABLE(node1);
+    COAL_UNUSED_VARIABLE(node2);
+    COAL_UNUSED_VARIABLE(nodes);
+
+    return 0;
+  }
+
+  static bool run(const BV& query, size_t node1, size_t node2,
+                  NodeBase<BV>* nodes) {
+    COAL_UNUSED_VARIABLE(query);
+    COAL_UNUSED_VARIABLE(node1);
+    COAL_UNUSED_VARIABLE(node2);
+    COAL_UNUSED_VARIABLE(nodes);
+
+    return 0;
+  }
+};
+
+//==============================================================================
+template <typename BV>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes) {
+  return SelectImpl<CoalScalar, BV>::run(query, node1, node2, nodes);
+}
+
+//==============================================================================
+template <typename BV>
+size_t select(const BV& query, size_t node1, size_t node2,
+              NodeBase<BV>* nodes) {
+  return SelectImpl<CoalScalar, BV>::run(query, node1, node2, nodes);
+}
+
+//==============================================================================
+template <typename S>
+struct SelectImpl<S, AABB> {
+  static bool run(size_t query, size_t node1, size_t node2,
+                  NodeBase<AABB>* nodes) {
+    const AABB& bv = nodes[query].bv;
+    const AABB& bv1 = nodes[node1].bv;
+    const AABB& bv2 = nodes[node2].bv;
+    Vec3s v = bv.min_ + bv.max_;
+    Vec3s v1 = v - (bv1.min_ + bv1.max_);
+    Vec3s v2 = v - (bv2.min_ + bv2.max_);
+    CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+    CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+    return (d1 < d2) ? 0 : 1;
+  }
+
+  static bool run(const AABB& query, size_t node1, size_t node2,
+                  NodeBase<AABB>* nodes) {
+    const AABB& bv = query;
+    const AABB& bv1 = nodes[node1].bv;
+    const AABB& bv2 = nodes[node2].bv;
+    Vec3s v = bv.min_ + bv.max_;
+    Vec3s v1 = v - (bv1.min_ + bv1.max_);
+    Vec3s v2 = v - (bv2.min_ + bv2.max_);
+    CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+    CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+    return (d1 < d2) ? 0 : 1;
+  }
+};
+
+}  // namespace implementation_array
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/hierarchy_tree_array.h b/include/coal/broadphase/detail/hierarchy_tree_array.h
new file mode 100644
index 0000000000000000000000000000000000000000..e4f12d16f7255e659f3e6127365da01a9d10bd2a
--- /dev/null
+++ b/include/coal/broadphase/detail/hierarchy_tree_array.h
@@ -0,0 +1,300 @@
+/*
+ * 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 COAL_HIERARCHY_TREE_ARRAY_H
+#define COAL_HIERARCHY_TREE_ARRAY_H
+
+#include <vector>
+#include <map>
+#include <functional>
+
+#include "coal/fwd.hh"
+#include "coal/BV/AABB.h"
+#include "coal/broadphase/detail/morton.h"
+#include "coal/broadphase/detail/node_base_array.h"
+
+namespace coal {
+
+namespace detail {
+
+namespace implementation_array {
+
+/// @brief Class for hierarchy tree structure
+template <typename BV>
+class HierarchyTree {
+ public:
+  typedef NodeBase<BV> Node;
+
+ private:
+  struct SortByMorton {
+    SortByMorton(Node* nodes_in) : nodes(nodes_in) {}
+    SortByMorton(Node* nodes_in, uint32_t split_in)
+        : nodes(nodes_in), split(split_in) {}
+    bool operator()(size_t a, size_t b) const {
+      if ((a != NULL_NODE) && (b != NULL_NODE))
+        return nodes[a].code < nodes[b].code;
+      else if (a == NULL_NODE)
+        return split < nodes[b].code;
+      else if (b == NULL_NODE)
+        return nodes[a].code < split;
+
+      return false;
+    }
+
+    Node* nodes{};
+    uint32_t split{};
+  };
+
+ public:
+  /// @brief Create hierarchy tree with suitable setting.
+  /// bu_threshold decides the height of tree node to start bottom-up
+  /// construction / optimization; topdown_level decides different methods to
+  /// construct tree in topdown manner. lower level method constructs tree with
+  /// better quality but is slower.
+  HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
+
+  ~HierarchyTree();
+
+  /// @brief Initialize the tree by a set of leaves using algorithm with a given
+  /// level.
+  void init(Node* leaves, int n_leaves_, int level = 0);
+
+  /// @brief Initialize the tree by a set of leaves using algorithm with a given
+  /// level.
+  size_t insert(const BV& bv, void* data);
+
+  /// @brief Remove a leaf node
+  void remove(size_t leaf);
+
+  /// @brief Clear the tree
+  void clear();
+
+  /// @brief Whether the tree is empty
+  bool empty() const;
+
+  /// @brief update one leaf node
+  void update(size_t leaf, int lookahead_level = -1);
+
+  /// @brief update the tree when the bounding volume of a given leaf has
+  /// changed
+  bool update(size_t leaf, const BV& bv);
+
+  /// @brief update one leaf's bounding volume, with prediction
+  bool update(size_t leaf, const BV& bv, const Vec3s& vel, CoalScalar margin);
+
+  /// @brief update one leaf's bounding volume, with prediction
+  bool update(size_t leaf, const BV& bv, const Vec3s& vel);
+
+  /// @brief get the max height of the tree
+  size_t getMaxHeight() const;
+
+  /// @brief get the max depth of the tree
+  size_t getMaxDepth() const;
+
+  /// @brief balance the tree from bottom
+  void balanceBottomup();
+
+  /// @brief balance the tree from top
+  void balanceTopdown();
+
+  /// @brief balance the tree in an incremental way
+  void balanceIncremental(int iterations);
+
+  /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
+  /// update the entire tree in a bottom-up manner
+  void refit();
+
+  /// @brief extract all the leaves of the tree
+  void extractLeaves(size_t root, Node*& leaves) const;
+
+  /// @brief number of leaves in the tree
+  size_t size() const;
+
+  /// @brief get the root of the tree
+  size_t getRoot() const;
+
+  /// @brief get the pointer to the nodes array
+  Node* getNodes() const;
+
+  /// @brief print the tree in a recursive way
+  void print(size_t root, int depth);
+
+ private:
+  /// @brief construct a tree for a set of leaves from bottom -- very heavy way
+  void bottomup(size_t* lbeg, size_t* lend);
+
+  /// @brief construct a tree for a set of leaves from top
+  size_t topdown(size_t* lbeg, size_t* lend);
+
+  /// @brief compute the maximum height of a subtree rooted from a given node
+  size_t getMaxHeight(size_t node) const;
+
+  /// @brief compute the maximum depth of a subtree rooted from a given node
+  void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const;
+
+  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
+  /// topdown manner. During construction, first compute the best split axis as
+  /// the axis along with the longest AABB edge. Then compute the median of all
+  /// nodes' center projection onto the axis and using it as the split
+  /// threshold.
+  size_t topdown_0(size_t* lbeg, size_t* lend);
+
+  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
+  /// topdown manner. During construction, first compute the best split
+  /// thresholds for different axes as the average of all nodes' center. Then
+  /// choose the split axis as the axis whose threshold can divide the nodes
+  /// into two parts with almost similar size. This construction is more
+  /// expensive then topdown_0, but also can provide tree with better quality.
+  size_t topdown_1(size_t* lbeg, size_t* lend);
+
+  /// @brief init tree from leaves in the topdown manner (topdown_0 or
+  /// topdown_1)
+  void init_0(Node* leaves, int n_leaves_);
+
+  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
+  /// for nodes which is of depth more than the maximum bits of the morton code,
+  /// we use bottomup method to construct the subtree, which is slow but can
+  /// construct tree with high quality.
+  void init_1(Node* leaves, int n_leaves_);
+
+  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
+  /// for nodes which is of depth more than the maximum bits of the morton code,
+  /// we split the leaves into two parts with the same size simply using the
+  /// node index.
+  void init_2(Node* leaves, int n_leaves_);
+
+  /// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
+  /// for all nodes, we simply divide the leaves into parts with the same size
+  /// simply using the node index.
+  void init_3(Node* leaves, int n_leaves_);
+
+  size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split,
+                         int bits);
+
+  size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split,
+                         int bits);
+
+  size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
+
+  /// @brief update one leaf node's bounding volume
+  void update_(size_t leaf, const BV& bv);
+
+  /// @brief Insert a leaf node and also update its ancestors
+  void insertLeaf(size_t root, size_t leaf);
+
+  /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the
+  /// unnecessary internal nodes are deleted. return the node with the smallest
+  /// depth and is influenced by the remove operation
+  size_t removeLeaf(size_t leaf);
+
+  /// @brief Delete all internal nodes and return all leaves nodes with given
+  /// depth from root
+  void fetchLeaves(size_t root, Node*& leaves, int depth = -1);
+
+  size_t indexOf(size_t node);
+
+  size_t allocateNode();
+
+  /// @brief create one node (leaf or internal)
+  size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data);
+
+  size_t createNode(size_t parent, const BV& bv, void* data);
+
+  size_t createNode(size_t parent, void* data);
+
+  void deleteNode(size_t node);
+
+  void recurseRefit(size_t node);
+
+ protected:
+  size_t root_node;
+  Node* nodes;
+  size_t n_nodes;
+  size_t n_nodes_alloc;
+
+  size_t n_leaves;
+  size_t freelist;
+  unsigned int opath;
+
+  int max_lookahead_level;
+
+ public:
+  /// @brief decide which topdown algorithm to use
+  int topdown_level;
+
+  /// @brief decide the depth to use expensive bottom-up algorithm
+  int bu_threshold;
+
+ public:
+  static const size_t NULL_NODE = std::numeric_limits<size_t>::max();
+};
+
+template <typename BV>
+const size_t HierarchyTree<BV>::NULL_NODE;
+
+/// @brief Functor comparing two nodes
+template <typename BV>
+struct nodeBaseLess {
+  nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_);
+
+  bool operator()(size_t i, size_t j) const;
+
+ private:
+  /// @brief the nodes array
+  const NodeBase<BV>* nodes;
+
+  /// @brief the dimension to compare
+  size_t d;
+};
+
+/// @brief select the node from node1 and node2 which is close to the query-th
+/// node in the nodes. 0 for node1 and 1 for node2.
+template <typename BV>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes);
+
+/// @brief select the node from node1 and node2 which is close to the query
+/// AABB. 0 for node1 and 1 for node2.
+template <typename BV>
+size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes);
+
+}  // namespace implementation_array
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/hierarchy_tree_array-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/detail/interval_tree.h b/include/coal/broadphase/detail/interval_tree.h
new file mode 100644
index 0000000000000000000000000000000000000000..e2f8485c6ffd77d69a6fa0c763c3cdc5a769665d
--- /dev/null
+++ b/include/coal/broadphase/detail/interval_tree.h
@@ -0,0 +1,127 @@
+/*
+ * 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 COAL_INTERVAL_TREE_H
+#define COAL_INTERVAL_TREE_H
+
+#include <deque>
+#include <limits>
+#include <cstdlib>
+
+#include "coal/broadphase/detail/interval_tree_node.h"
+
+namespace coal {
+namespace detail {
+
+/// @brief Class describes the information needed when we take the
+/// right branch in searching for intervals but possibly come back
+/// and check the left branch as well.
+struct COAL_DLLAPI it_recursion_node {
+ public:
+  IntervalTreeNode* start_node;
+
+  unsigned int parent_index;
+
+  bool try_right_branch;
+};
+
+/// @brief Interval tree
+class COAL_DLLAPI IntervalTree {
+ public:
+  IntervalTree();
+
+  ~IntervalTree();
+
+  /// @brief Print the whole interval tree
+  void print() const;
+
+  /// @brief Delete one node of the interval tree
+  SimpleInterval* deleteNode(IntervalTreeNode* node);
+
+  /// @brief delete node stored a given interval
+  void deleteNode(SimpleInterval* ivl);
+
+  /// @brief Insert one node of the interval tree
+  IntervalTreeNode* insert(SimpleInterval* new_interval);
+
+  /// @brief get the predecessor of a given node
+  IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const;
+
+  /// @brief Get the successor of a given node
+  IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const;
+
+  /// @brief Return result for a given query
+  std::deque<SimpleInterval*> query(CoalScalar low, CoalScalar high);
+
+ protected:
+  IntervalTreeNode* root;
+
+  IntervalTreeNode* invalid_node;
+
+  /// @brief left rotation of tree node
+  void leftRotate(IntervalTreeNode* node);
+
+  /// @brief right rotation of tree node
+  void rightRotate(IntervalTreeNode* node);
+
+  /// @brief Inserts node into the tree as if it were a regular binary tree
+  void recursiveInsert(IntervalTreeNode* node);
+
+  /// @brief recursively print a subtree
+  void recursivePrint(IntervalTreeNode* node) const;
+
+  /// @brief recursively find the node corresponding to the interval
+  IntervalTreeNode* recursiveSearch(IntervalTreeNode* node,
+                                    SimpleInterval* ivl) const;
+
+  /// @brief Travels up to the root fixing the max_high fields after an
+  /// insertion or deletion
+  void fixupMaxHigh(IntervalTreeNode* node);
+
+  void deleteFixup(IntervalTreeNode* node);
+
+ private:
+  unsigned int recursion_node_stack_size;
+  it_recursion_node* recursion_node_stack;
+  unsigned int current_parent;
+  unsigned int recursion_node_stack_top;
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h b/include/coal/broadphase/detail/interval_tree_node-inl.h
similarity index 92%
rename from include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h
rename to include/coal/broadphase/detail/interval_tree_node-inl.h
index b9771ae18632077841e089acd2cda65fd3ff0da1..1edd7e6d226c7e4952c755f81b829093e0c1f088 100644
--- a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h
+++ b/include/coal/broadphase/detail/interval_tree_node-inl.h
@@ -35,16 +35,15 @@
 
 /** @author Jia Pan */
 
-#ifndef HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
-#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
+#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
+#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
 
-#include "hpp/fcl/broadphase/detail/interval_tree_node.h"
+#include "coal/broadphase/detail/interval_tree_node.h"
 
 #include <iostream>
 #include <algorithm>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace detail {
 
 //==============================================================================
@@ -90,7 +89,6 @@ void IntervalTreeNode::print(IntervalTreeNode* nil,
 }
 
 }  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
 #endif
diff --git a/include/coal/broadphase/detail/interval_tree_node.h b/include/coal/broadphase/detail/interval_tree_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..01c5de4bd3528ccac4f86276b67c68eecdd74936
--- /dev/null
+++ b/include/coal/broadphase/detail/interval_tree_node.h
@@ -0,0 +1,90 @@
+/*
+ * 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 COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H
+#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H
+
+#include "coal/broadphase/detail/simple_interval.h"
+#include "coal/fwd.hh"
+
+namespace coal {
+
+namespace detail {
+
+class COAL_DLLAPI IntervalTree;
+
+/// @brief The node for interval tree
+class COAL_DLLAPI IntervalTreeNode {
+ public:
+  friend class IntervalTree;
+
+  /// @brief Create an empty node
+  IntervalTreeNode();
+
+  /// @brief Create an node storing the interval
+  IntervalTreeNode(SimpleInterval* new_interval);
+
+  ~IntervalTreeNode();
+
+  /// @brief Print the interval node information: set left = invalid_node and
+  /// right = root
+  void print(IntervalTreeNode* left, IntervalTreeNode* right) const;
+
+ protected:
+  /// @brief interval stored in the node
+  SimpleInterval* stored_interval;
+
+  CoalScalar key;
+
+  CoalScalar high;
+
+  CoalScalar max_high;
+
+  /// @brief red or black node: if red = false then the node is black
+  bool red;
+
+  IntervalTreeNode* left;
+
+  IntervalTreeNode* right;
+
+  IntervalTreeNode* parent;
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/morton-inl.h b/include/coal/broadphase/detail/morton-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..b646f299e8e4f5220ed1ba4f1cdb680757d5bad6
--- /dev/null
+++ b/include/coal/broadphase/detail/morton-inl.h
@@ -0,0 +1,151 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  Copyright (c) 2016, Toyota Research Institute
+ *  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 COAL_MORTON_INL_H
+#define COAL_MORTON_INL_H
+
+#include "coal/broadphase/detail/morton.h"
+
+namespace coal {  /// @cond IGNORE
+namespace detail {
+
+//==============================================================================
+template <typename S>
+uint32_t quantize(S x, uint32_t n) {
+  return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0));
+}
+
+//==============================================================================
+template <typename S>
+morton_functor<S, uint32_t>::morton_functor(const AABB& bbox)
+    : base(bbox.min_),
+      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
+          1.0 / (bbox.max_[1] - bbox.min_[1]),
+          1.0 / (bbox.max_[2] - bbox.min_[2])) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+uint32_t morton_functor<S, uint32_t>::operator()(const Vec3s& point) const {
+  uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
+  uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
+  uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
+
+  return detail::morton_code(x, y, z);
+}
+
+//==============================================================================
+template <typename S>
+morton_functor<S, uint64_t>::morton_functor(const AABB& bbox)
+    : base(bbox.min_),
+      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
+          1.0 / (bbox.max_[1] - bbox.min_[1]),
+          1.0 / (bbox.max_[2] - bbox.min_[2])) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+uint64_t morton_functor<S, uint64_t>::operator()(const Vec3s& point) const {
+  uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
+  uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
+  uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20);
+
+  return detail::morton_code60(x, y, z);
+}
+
+//==============================================================================
+template <typename S>
+constexpr size_t morton_functor<S, uint64_t>::bits() {
+  return 60;
+}
+
+//==============================================================================
+template <typename S>
+constexpr size_t morton_functor<S, uint32_t>::bits() {
+  return 30;
+}
+
+//==============================================================================
+template <typename S, size_t N>
+morton_functor<S, std::bitset<N>>::morton_functor(const AABB& bbox)
+    : base(bbox.min_),
+      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
+          1.0 / (bbox.max_[1] - bbox.min_[1]),
+          1.0 / (bbox.max_[2] - bbox.min_[2])) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S, size_t N>
+std::bitset<N> morton_functor<S, std::bitset<N>>::operator()(
+    const Vec3s& point) const {
+  S x = (point[0] - base[0]) * inv[0];
+  S y = (point[1] - base[1]) * inv[1];
+  S z = (point[2] - base[2]) * inv[2];
+  int start_bit = bits() - 1;
+  std::bitset<N> bset;
+
+  x *= 2;
+  y *= 2;
+  z *= 2;
+
+  for (size_t i = 0; i < bits() / 3; ++i) {
+    bset[start_bit--] = ((z < 1) ? 0 : 1);
+    bset[start_bit--] = ((y < 1) ? 0 : 1);
+    bset[start_bit--] = ((x < 1) ? 0 : 1);
+    x = ((x >= 1) ? 2 * (x - 1) : 2 * x);
+    y = ((y >= 1) ? 2 * (y - 1) : 2 * y);
+    z = ((z >= 1) ? 2 * (z - 1) : 2 * z);
+  }
+
+  return bset;
+}
+
+//==============================================================================
+template <typename S, size_t N>
+constexpr size_t morton_functor<S, std::bitset<N>>::bits() {
+  return N;
+}
+
+}  // namespace detail
+/// @endcond
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/morton.h b/include/coal/broadphase/detail/morton.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7a9c947370649364fdd49535ae2b9c3c310ae03
--- /dev/null
+++ b/include/coal/broadphase/detail/morton.h
@@ -0,0 +1,120 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  Copyright (c) 2016, Toyota Research Institute
+ *  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 COAL_MORTON_H
+#define COAL_MORTON_H
+
+#include "coal/BV/AABB.h"
+#include <cstdint>
+#include <bitset>
+
+namespace coal {
+
+/// @cond IGNORE
+namespace detail {
+
+template <typename S>
+uint32_t quantize(S x, uint32_t n);
+
+/// @brief compute 30 bit morton code
+COAL_DLLAPI
+uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z);
+
+/// @brief compute 60 bit morton code
+COAL_DLLAPI
+uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z);
+
+/// @brief Functor to compute the morton code for a given AABB
+/// This is specialized for 32- and 64-bit unsigned integers giving
+/// a 30- or 60-bit code, respectively, and for `std::bitset<N>` where
+/// N is the length of the code and must be a multiple of 3.
+template <typename S, typename T>
+struct morton_functor {};
+
+/// @brief Functor to compute 30 bit morton code for a given AABB
+template <typename S>
+struct morton_functor<S, uint32_t> {
+  morton_functor(const AABB& bbox);
+
+  uint32_t operator()(const Vec3s& point) const;
+
+  const Vec3s base;
+  const Vec3s inv;
+
+  static constexpr size_t bits();
+};
+
+using morton_functoru32f = morton_functor<float, uint32_t>;
+using morton_functoru32d = morton_functor<CoalScalar, uint32_t>;
+
+/// @brief Functor to compute 60 bit morton code for a given AABB
+template <typename S>
+struct morton_functor<S, uint64_t> {
+  morton_functor(const AABB& bbox);
+
+  uint64_t operator()(const Vec3s& point) const;
+
+  const Vec3s base;
+  const Vec3s inv;
+
+  static constexpr size_t bits();
+};
+
+/// @brief Functor to compute N bit morton code for a given AABB
+/// N must be a multiple of 3.
+template <typename S, size_t N>
+struct morton_functor<S, std::bitset<N>> {
+  static_assert(N % 3 == 0, "Number of bits must be a multiple of 3");
+
+  morton_functor(const AABB& bbox);
+
+  std::bitset<N> operator()(const Vec3s& point) const;
+
+  const Vec3s base;
+  const Vec3s inv;
+
+  static constexpr size_t bits();
+};
+
+}  // namespace detail
+/// @endcond
+}  // namespace coal
+
+#include "coal/broadphase/detail/morton-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/detail/node_base-inl.h b/include/coal/broadphase/detail/node_base-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..db63bcee1de90e67ed056d5b8c2489e32bac0f52
--- /dev/null
+++ b/include/coal/broadphase/detail/node_base-inl.h
@@ -0,0 +1,75 @@
+/*
+ * 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 COAL_BROADPHASE_DETAIL_NODEBASE_INL_H
+#define COAL_BROADPHASE_DETAIL_NODEBASE_INL_H
+
+#include "coal/broadphase/detail/node_base.h"
+
+namespace coal {
+namespace detail {
+
+//============================================================================//
+//                                                                            //
+//                              Implementations                               //
+//                                                                            //
+//============================================================================//
+
+//==============================================================================
+template <typename BV>
+bool NodeBase<BV>::isLeaf() const {
+  return (children[1] == nullptr);
+}
+
+//==============================================================================
+template <typename BV>
+bool NodeBase<BV>::isInternal() const {
+  return !isLeaf();
+}
+
+//==============================================================================
+template <typename BV>
+NodeBase<BV>::NodeBase() {
+  parent = nullptr;
+  children[0] = nullptr;
+  children[1] = nullptr;
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/node_base.h b/include/coal/broadphase/detail/node_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..179495e6f15755822ba9ce64e90c6e14349dc1b9
--- /dev/null
+++ b/include/coal/broadphase/detail/node_base.h
@@ -0,0 +1,79 @@
+/*
+ * 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 COAL_BROADPHASE_DETAIL_NODEBASE_H
+#define COAL_BROADPHASE_DETAIL_NODEBASE_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+namespace detail {
+
+/// @brief dynamic AABB tree node
+template <typename BV>
+struct NodeBase {
+  /// @brief the bounding volume for the node
+  BV bv;
+
+  /// @brief pointer to parent node
+  NodeBase<BV>* parent;
+
+  /// @brief whether is a leaf
+  bool isLeaf() const;
+
+  /// @brief whether is internal node
+  bool isInternal() const;
+
+  union {
+    /// @brief for leaf node, children nodes
+    NodeBase<BV>* children[2];
+    void* data;
+  };
+
+  /// @brief morton code for current BV
+  uint32_t code;
+
+  NodeBase();
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/node_base-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/detail/node_base_array-inl.h b/include/coal/broadphase/detail/node_base_array-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..a48fceef939c4f3fbf479cd299f1684f58ca51d3
--- /dev/null
+++ b/include/coal/broadphase/detail/node_base_array-inl.h
@@ -0,0 +1,65 @@
+/*
+ * 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 COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
+#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
+
+#include "coal/broadphase/detail/node_base_array.h"
+
+namespace coal {
+
+namespace detail {
+
+namespace implementation_array {
+
+//==============================================================================
+template <typename BV>
+bool NodeBase<BV>::isLeaf() const {
+  return (children[1] == (size_t)(-1));
+}
+
+//==============================================================================
+template <typename BV>
+bool NodeBase<BV>::isInternal() const {
+  return !isLeaf();
+}
+
+}  // namespace implementation_array
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/node_base_array.h b/include/coal/broadphase/detail/node_base_array.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c18ef1f7523761ff0839e4b2eab40fde14872e2
--- /dev/null
+++ b/include/coal/broadphase/detail/node_base_array.h
@@ -0,0 +1,75 @@
+/*
+ * 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 COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H
+#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+namespace detail {
+
+namespace implementation_array {
+
+template <typename BV>
+struct NodeBase {
+  BV bv;
+
+  union {
+    size_t parent;
+    size_t next;
+  };
+
+  union {
+    size_t children[2];
+    void* data;
+  };
+
+  uint32_t code;
+
+  bool isLeaf() const;
+  bool isInternal() const;
+};
+
+}  // namespace implementation_array
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/node_base_array-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/detail/simple_hash_table-inl.h b/include/coal/broadphase/detail/simple_hash_table-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae534f0371ed460d3021c5351168b1fea821baaf
--- /dev/null
+++ b/include/coal/broadphase/detail/simple_hash_table-inl.h
@@ -0,0 +1,111 @@
+/*
+ * 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 COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H
+#define COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H
+
+#include "coal/broadphase/detail/simple_hash_table.h"
+
+#include <iterator>
+namespace coal {
+
+namespace detail {
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+SimpleHashTable<Key, Data, HashFnc>::SimpleHashTable(const HashFnc& h) : h_(h) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+void SimpleHashTable<Key, Data, HashFnc>::init(size_t size) {
+  if (size == 0) {
+    COAL_THROW_PRETTY("SimpleHashTable must have non-zero size.",
+                      std::logic_error);
+  }
+
+  table_.resize(size);
+  table_size_ = size;
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+void SimpleHashTable<Key, Data, HashFnc>::insert(Key key, Data value) {
+  std::vector<unsigned int> indices = h_(key);
+  size_t range = table_.size();
+  for (size_t i = 0; i < indices.size(); ++i)
+    table_[indices[i] % range].push_back(value);
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+std::vector<Data> SimpleHashTable<Key, Data, HashFnc>::query(Key key) const {
+  size_t range = table_.size();
+  std::vector<unsigned int> indices = h_(key);
+  std::set<Data> result;
+  for (size_t i = 0; i < indices.size(); ++i) {
+    size_t index = indices[i] % range;
+    std::copy(table_[index].begin(), table_[index].end(),
+              std::inserter(result, result.end()));
+  }
+
+  return std::vector<Data>(result.begin(), result.end());
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+void SimpleHashTable<Key, Data, HashFnc>::remove(Key key, Data value) {
+  size_t range = table_.size();
+  std::vector<unsigned int> indices = h_(key);
+  for (size_t i = 0; i < indices.size(); ++i) {
+    size_t index = indices[i] % range;
+    table_[index].remove(value);
+  }
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+void SimpleHashTable<Key, Data, HashFnc>::clear() {
+  table_.clear();
+  table_.resize(table_size_);
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/simple_hash_table.h b/include/coal/broadphase/detail/simple_hash_table.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b47490266d09d77bd384f2c942e986056617cc5
--- /dev/null
+++ b/include/coal/broadphase/detail/simple_hash_table.h
@@ -0,0 +1,87 @@
+/*
+ * 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 COAL_BROADPHASE_SIMPLEHASHTABLE_H
+#define COAL_BROADPHASE_SIMPLEHASHTABLE_H
+
+#include <set>
+#include <vector>
+#include <list>
+
+namespace coal {
+
+namespace detail {
+
+/// @brief A simple hash table implemented as multiple buckets. HashFnc is any
+/// extended hash function: HashFnc(key) = {index1, index2, ..., }
+template <typename Key, typename Data, typename HashFnc>
+class SimpleHashTable {
+ protected:
+  typedef std::list<Data> Bin;
+
+  std::vector<Bin> table_;
+
+  HashFnc h_;
+
+  size_t table_size_;
+
+ public:
+  SimpleHashTable(const HashFnc& h);
+
+  /// @brief Init the number of bins in the hash table
+  void init(size_t size);
+
+  //// @brief Insert a key-value pair into the table
+  void insert(Key key, Data value);
+
+  /// @brief Find the elements in the hash table whose key is the same as query
+  /// key.
+  std::vector<Data> query(Key key) const;
+
+  /// @brief remove the key-value pair from the table
+  void remove(Key key, Data value);
+
+  /// @brief clear the hash table
+  void clear();
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/simple_hash_table-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/detail/simple_interval-inl.h b/include/coal/broadphase/detail/simple_interval-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..95075cbc19426df136380ae47c42fef70914db8c
--- /dev/null
+++ b/include/coal/broadphase/detail/simple_interval-inl.h
@@ -0,0 +1,60 @@
+/*
+ * 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 COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
+#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
+
+#include "coal/broadphase/detail/simple_interval.h"
+#include <algorithm>
+
+namespace coal {
+namespace detail {
+
+//==============================================================================
+SimpleInterval::~SimpleInterval() {
+  // Do nothing
+}
+
+//==============================================================================
+void SimpleInterval::print() {
+  // Do nothing
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/simple_interval.h b/include/coal/broadphase/detail/simple_interval.h
new file mode 100644
index 0000000000000000000000000000000000000000..efd9cbe55adbb2ae919c417729554253dfdff76c
--- /dev/null
+++ b/include/coal/broadphase/detail/simple_interval.h
@@ -0,0 +1,62 @@
+/*
+ * 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 COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H
+#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+
+namespace coal {
+namespace detail {
+
+/// @brief Interval trees implemented using red-black-trees as described in
+/// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest.
+struct COAL_DLLAPI SimpleInterval {
+ public:
+  virtual ~SimpleInterval();
+
+  virtual void print();
+
+  /// @brief interval is defined as [low, high]
+  CoalScalar low, high;
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/sparse_hash_table-inl.h b/include/coal/broadphase/detail/sparse_hash_table-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..299f9871ab60c602acffcb654b97a90517216e42
--- /dev/null
+++ b/include/coal/broadphase/detail/sparse_hash_table-inl.h
@@ -0,0 +1,111 @@
+/*
+ * 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 COAL_BROADPHASE_SPARSEHASHTABLE_INL_H
+#define COAL_BROADPHASE_SPARSEHASHTABLE_INL_H
+
+#include "coal/broadphase/detail/sparse_hash_table.h"
+
+namespace coal {
+
+namespace detail {
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+SparseHashTable<Key, Data, HashFnc, TableT>::SparseHashTable(const HashFnc& h)
+    : h_(h) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+void SparseHashTable<Key, Data, HashFnc, TableT>::init(size_t) {
+  table_.clear();
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+void SparseHashTable<Key, Data, HashFnc, TableT>::insert(Key key, Data value) {
+  std::vector<unsigned int> indices = h_(key);
+  for (size_t i = 0; i < indices.size(); ++i)
+    table_[indices[i]].push_back(value);
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+std::vector<Data> SparseHashTable<Key, Data, HashFnc, TableT>::query(
+    Key key) const {
+  std::vector<unsigned int> indices = h_(key);
+  std::set<Data> result;
+  for (size_t i = 0; i < indices.size(); ++i) {
+    unsigned int index = indices[i];
+    typename Table::const_iterator p = table_.find(index);
+    if (p != table_.end()) {
+      std::copy((*p).second.begin(), (*p).second.end(),
+                std ::inserter(result, result.end()));
+    }
+  }
+
+  return std::vector<Data>(result.begin(), result.end());
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+void SparseHashTable<Key, Data, HashFnc, TableT>::remove(Key key, Data value) {
+  std::vector<unsigned int> indices = h_(key);
+  for (size_t i = 0; i < indices.size(); ++i) {
+    unsigned int index = indices[i];
+    table_[index].remove(value);
+  }
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+void SparseHashTable<Key, Data, HashFnc, TableT>::clear() {
+  table_.clear();
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/sparse_hash_table.h b/include/coal/broadphase/detail/sparse_hash_table.h
new file mode 100644
index 0000000000000000000000000000000000000000..13c4aeeca5b7f49bc8c4e235c63eaad9cc9e2cd8
--- /dev/null
+++ b/include/coal/broadphase/detail/sparse_hash_table.h
@@ -0,0 +1,93 @@
+/*
+ * 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 COAL_BROADPHASE_SPARSEHASHTABLE_H
+#define COAL_BROADPHASE_SPARSEHASHTABLE_H
+
+#include <set>
+#include <vector>
+#include <list>
+#include <unordered_map>
+
+namespace coal {
+
+namespace detail {
+
+template <typename U, typename V>
+class unordered_map_hash_table : public std::unordered_map<U, V> {
+  typedef std::unordered_map<U, V> Base;
+
+ public:
+  unordered_map_hash_table() : Base() {};
+};
+
+/// @brief A hash table implemented using unordered_map
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT = unordered_map_hash_table>
+class SparseHashTable {
+ protected:
+  HashFnc h_;
+  typedef std::list<Data> Bin;
+  typedef TableT<size_t, Bin> Table;
+
+  Table table_;
+
+ public:
+  SparseHashTable(const HashFnc& h);
+
+  /// @brief Init the hash table. The bucket size is dynamically decided.
+  void init(size_t);
+
+  /// @brief insert one key-value pair into the hash table
+  void insert(Key key, Data value);
+
+  /// @brief find the elements whose key is the same as the query
+  std::vector<Data> query(Key key) const;
+
+  /// @brief remove one key-value pair from the hash table
+  void remove(Key key, Data value);
+
+  /// @brief clear the hash table
+  void clear();
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/sparse_hash_table-inl.h"
+
+#endif
diff --git a/include/coal/broadphase/detail/spatial_hash-inl.h b/include/coal/broadphase/detail/spatial_hash-inl.h
new file mode 100644
index 0000000000000000000000000000000000000000..cb9d6bd9a5d2576386071ead19c3ae4ce3142ec0
--- /dev/null
+++ b/include/coal/broadphase/detail/spatial_hash-inl.h
@@ -0,0 +1,80 @@
+/*
+ * 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 COAL_BROADPHASE_SPATIALHASH_INL_H
+#define COAL_BROADPHASE_SPATIALHASH_INL_H
+
+#include "coal/broadphase/detail/spatial_hash.h"
+#include <algorithm>
+
+namespace coal {
+namespace detail {
+
+//==============================================================================
+SpatialHash::SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_)
+    : cell_size(cell_size_), scene_limit(scene_limit_) {
+  width[0] = std::ceil(scene_limit.width() / cell_size);
+  width[1] = std::ceil(scene_limit.height() / cell_size);
+  width[2] = std::ceil(scene_limit.depth() / cell_size);
+}
+
+//==============================================================================
+std::vector<unsigned int> SpatialHash::operator()(const AABB& aabb) const {
+  int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size);
+  int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size);
+  int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size);
+  int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size);
+  int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size);
+  int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size);
+
+  std::vector<unsigned int> keys((max_x - min_x) * (max_y - min_y) *
+                                 (max_z - min_z));
+  int id = 0;
+  for (int x = min_x; x < max_x; ++x) {
+    for (int y = min_y; y < max_y; ++y) {
+      for (int z = min_z; z < max_z; ++z) {
+        keys[id++] = x + y * width[0] + z * width[0] * width[1];
+      }
+    }
+  }
+  return keys;
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/broadphase/detail/spatial_hash.h b/include/coal/broadphase/detail/spatial_hash.h
new file mode 100644
index 0000000000000000000000000000000000000000..6d8646e38253aa5c9aa8ff4f3923af0c1ed1ac14
--- /dev/null
+++ b/include/coal/broadphase/detail/spatial_hash.h
@@ -0,0 +1,63 @@
+/*
+ * 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 COAL_BROADPHASE_SPATIALHASH_H
+#define COAL_BROADPHASE_SPATIALHASH_H
+
+#include "coal/BV/AABB.h"
+#include <vector>
+
+namespace coal {
+
+namespace detail {
+
+/// @brief Spatial hash function: hash an AABB to a set of integer values
+struct COAL_DLLAPI SpatialHash {
+  SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_);
+
+  std::vector<unsigned int> operator()(const AABB& aabb) const;
+
+ private:
+  CoalScalar cell_size;
+  AABB scene_limit;
+  unsigned int width[3];
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif
diff --git a/include/coal/collision.h b/include/coal/collision.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf9b9a1c01e55825fd769e5f920e05b59fbec83d
--- /dev/null
+++ b/include/coal/collision.h
@@ -0,0 +1,120 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2021, INRIA
+ *  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 COAL_COLLISION_H
+#define COAL_COLLISION_H
+
+#include "coal/data_types.h"
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/collision_func_matrix.h"
+#include "coal/timings.h"
+
+namespace coal {
+
+/// @brief Main collision interface: given two collision objects, and the
+/// requirements for contacts, including num of max contacts, whether perform
+/// exhaustive collision (i.e., returning returning all the contact points),
+/// whether return detailed contact information (i.e., normal, contact point,
+/// depth; otherwise only contact primitive id is returned), this function
+/// performs the collision between them.
+/// Return value is the number of contacts generated between the two objects.
+COAL_DLLAPI std::size_t collide(const CollisionObject* o1,
+                                const CollisionObject* o2,
+                                const CollisionRequest& request,
+                                CollisionResult& result);
+
+/// @copydoc collide(const CollisionObject*, const CollisionObject*, const
+/// CollisionRequest&, CollisionResult&)
+COAL_DLLAPI std::size_t collide(const CollisionGeometry* o1,
+                                const Transform3s& tf1,
+                                const CollisionGeometry* o2,
+                                const Transform3s& tf2,
+                                const CollisionRequest& request,
+                                CollisionResult& result);
+
+/// @brief This class reduces the cost of identifying the geometry pair.
+/// This is mostly useful for repeated shape-shape queries.
+///
+/// \code
+///   ComputeCollision calc_collision (o1, o2);
+///   std::size_t ncontacts = calc_collision(tf1, tf2, request, result);
+/// \endcode
+class COAL_DLLAPI ComputeCollision {
+ public:
+  /// @brief Default constructor from two Collision Geometries.
+  ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2);
+
+  std::size_t operator()(const Transform3s& tf1, const Transform3s& tf2,
+                         const CollisionRequest& request,
+                         CollisionResult& result) const;
+
+  bool operator==(const ComputeCollision& other) const {
+    return o1 == other.o1 && o2 == other.o2 && solver == other.solver;
+  }
+
+  bool operator!=(const ComputeCollision& other) const {
+    return !(*this == other);
+  }
+
+  virtual ~ComputeCollision() {};
+
+ protected:
+  // These pointers are made mutable to let the derived classes to update
+  // their values when updating the collision geometry (e.g. creating a new
+  // one). This feature should be used carefully to avoid any mis usage (e.g,
+  // changing the type of the collision geometry should be avoided).
+  mutable const CollisionGeometry* o1;
+  mutable const CollisionGeometry* o2;
+
+  mutable GJKSolver solver;
+
+  CollisionFunctionMatrix::CollisionFunc func;
+  bool swap_geoms;
+
+  virtual std::size_t run(const Transform3s& tf1, const Transform3s& tf2,
+                          const CollisionRequest& request,
+                          CollisionResult& result) const;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h
new file mode 100644
index 0000000000000000000000000000000000000000..e885e66a6280f70ef843b74adca968fcb03eea62
--- /dev/null
+++ b/include/coal/collision_data.h
@@ -0,0 +1,1237 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2024, INRIA
+ *  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 COAL_COLLISION_DATA_H
+#define COAL_COLLISION_DATA_H
+
+#include <vector>
+#include <array>
+#include <set>
+#include <limits>
+#include <numeric>
+
+#include "coal/collision_object.h"
+#include "coal/config.hh"
+#include "coal/data_types.h"
+#include "coal/timings.h"
+#include "coal/narrowphase/narrowphase_defaults.h"
+#include "coal/logging.h"
+
+namespace coal {
+
+/// @brief Contact information returned by collision
+struct COAL_DLLAPI Contact {
+  /// @brief collision object 1
+  const CollisionGeometry* o1;
+
+  /// @brief collision object 2
+  const CollisionGeometry* o2;
+
+  /// @brief contact primitive in object 1
+  /// if object 1 is mesh or point cloud, it is the triangle or point id
+  /// if object 1 is geometry shape, it is NONE (-1),
+  /// if object 1 is octree, it is the id of the cell
+  int b1;
+
+  /// @brief contact primitive in object 2
+  /// if object 2 is mesh or point cloud, it is the triangle or point id
+  /// if object 2 is geometry shape, it is NONE (-1),
+  /// if object 2 is octree, it is the id of the cell
+  int b2;
+
+  /// @brief contact normal, pointing from o1 to o2.
+  /// The normal defined as the normalized separation vector:
+  /// normal = (p2 - p1) / dist(o1, o2), where p1 = nearest_points[0]
+  /// belongs to o1 and p2 = nearest_points[1] belongs to o2 and dist(o1, o2) is
+  /// the **signed** distance between o1 and o2. The normal always points from
+  /// o1 to o2.
+  /// @note The separation vector is the smallest vector such that if o1 is
+  /// translated by it, o1 and o2 are in touching contact (they share at least
+  /// one contact point but have a zero intersection volume). If the shapes
+  /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) =
+  /// (p2-p1).norm().
+  Vec3s normal;
+
+  /// @brief nearest points associated to this contact.
+  /// @note Also referred as "witness points" in other collision libraries.
+  /// The points p1 = nearest_points[0] and p2 = nearest_points[1] verify the
+  /// property that dist(o1, o2) * (p1 - p2) is the separation vector between o1
+  /// and o2, with dist(o1, o2) being the **signed** distance separating o1 from
+  /// o2. See \ref DistanceResult::normal for the definition of the separation
+  /// vector. If o1 and o2 have multiple contacts, the nearest_points are
+  /// associated with the contact which has the greatest penetration depth.
+  /// TODO (louis): rename `nearest_points` to `witness_points`.
+  std::array<Vec3s, 2> nearest_points;
+
+  /// @brief contact position, in world space
+  Vec3s pos;
+
+  /// @brief penetration depth
+  CoalScalar penetration_depth;
+
+  /// @brief invalid contact primitive information
+  static const int NONE = -1;
+
+  /// @brief Default constructor
+  Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
+    penetration_depth = (std::numeric_limits<CoalScalar>::max)();
+    nearest_points[0] = nearest_points[1] = normal = pos =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
+          int b2_)
+      : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
+    penetration_depth = (std::numeric_limits<CoalScalar>::max)();
+    nearest_points[0] = nearest_points[1] = normal = pos =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
+          int b2_, const Vec3s& pos_, const Vec3s& normal_, CoalScalar depth_)
+      : o1(o1_),
+        o2(o2_),
+        b1(b1_),
+        b2(b2_),
+        normal(normal_),
+        nearest_points{pos_ - (depth_ * normal_ / 2),
+                       pos_ + (depth_ * normal_ / 2)},
+        pos(pos_),
+        penetration_depth(depth_) {}
+
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
+          int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_,
+          CoalScalar depth_)
+      : o1(o1_),
+        o2(o2_),
+        b1(b1_),
+        b2(b2_),
+        normal(normal_),
+        nearest_points{p1, p2},
+        pos((p1 + p2) / 2),
+        penetration_depth(depth_) {}
+
+  bool operator<(const Contact& other) const {
+    if (b1 == other.b1) return b2 < other.b2;
+    return b1 < other.b1;
+  }
+
+  bool operator==(const Contact& other) const {
+    return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 &&
+           b2 == other.b2 && normal == other.normal && pos == other.pos &&
+           nearest_points[0] == other.nearest_points[0] &&
+           nearest_points[1] == other.nearest_points[1] &&
+           penetration_depth == other.penetration_depth;
+  }
+
+  bool operator!=(const Contact& other) const { return !(*this == other); }
+
+  CoalScalar getDistanceToCollision(const CollisionRequest& request) const;
+};
+
+struct QueryResult;
+
+/// @brief base class for all query requests
+struct COAL_DLLAPI QueryRequest {
+  // @brief Initial guess to use for the GJK algorithm
+  GJKInitialGuess gjk_initial_guess;
+
+  /// @brief whether enable gjk initial guess
+  /// @Deprecated Use gjk_initial_guess instead
+  COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
+  bool enable_cached_gjk_guess;
+
+  /// @brief the gjk initial guess set by user
+  mutable Vec3s cached_gjk_guess;
+
+  /// @brief the support function initial guess set by user
+  mutable support_func_guess_t cached_support_func_guess;
+
+  /// @brief maximum iteration for the GJK algorithm
+  size_t gjk_max_iterations;
+
+  /// @brief tolerance for the GJK algorithm.
+  /// Note: This tolerance determines the precision on the estimated distance
+  /// between two geometries which are not in collision.
+  /// It is recommended to not set this tolerance to less than 1e-6.
+  CoalScalar gjk_tolerance;
+
+  /// @brief whether to enable the Nesterov accleration of GJK
+  GJKVariant gjk_variant;
+
+  /// @brief convergence criterion used to stop GJK
+  GJKConvergenceCriterion gjk_convergence_criterion;
+
+  /// @brief convergence criterion used to stop GJK
+  GJKConvergenceCriterionType gjk_convergence_criterion_type;
+
+  /// @brief max number of iterations for EPA
+  size_t epa_max_iterations;
+
+  /// @brief tolerance for EPA.
+  /// Note: This tolerance determines the precision on the estimated distance
+  /// between two geometries which are in collision.
+  /// It is recommended to not set this tolerance to less than 1e-6.
+  /// Also, setting EPA's tolerance to less than GJK's is not recommended.
+  CoalScalar epa_tolerance;
+
+  /// @brief enable timings when performing collision/distance request
+  bool enable_timings;
+
+  /// @brief threshold below which a collision is considered.
+  CoalScalar collision_distance_threshold;
+
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  /// @brief Default constructor.
+  QueryRequest()
+      : gjk_initial_guess(GJKInitialGuess::DefaultGuess),
+        enable_cached_gjk_guess(false),
+        cached_gjk_guess(1, 0, 0),
+        cached_support_func_guess(support_func_guess_t::Zero()),
+        gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
+        gjk_tolerance(GJK_DEFAULT_TOLERANCE),
+        gjk_variant(GJKVariant::DefaultGJK),
+        gjk_convergence_criterion(GJKConvergenceCriterion::Default),
+        gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
+        epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
+        epa_tolerance(EPA_DEFAULT_TOLERANCE),
+        enable_timings(false),
+        collision_distance_threshold(
+            Eigen::NumTraits<CoalScalar>::dummy_precision()) {}
+
+  /// @brief Copy  constructor.
+  QueryRequest(const QueryRequest& other) = default;
+
+  /// @brief Copy  assignment operator.
+  QueryRequest& operator=(const QueryRequest& other) = default;
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  /// @brief Updates the guess for the internal GJK algorithm in order to
+  /// warm-start it when reusing this collision request on the same collision
+  /// pair.
+  /// @note The option `gjk_initial_guess` must be set to
+  /// `GJKInitialGuess::CachedGuess` for this to work.
+  void updateGuess(const QueryResult& result) const;
+
+  /// @brief whether two QueryRequest are the same or not
+  inline bool operator==(const QueryRequest& other) const {
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    return gjk_initial_guess == other.gjk_initial_guess &&
+           enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
+           gjk_variant == other.gjk_variant &&
+           gjk_convergence_criterion == other.gjk_convergence_criterion &&
+           gjk_convergence_criterion_type ==
+               other.gjk_convergence_criterion_type &&
+           gjk_tolerance == other.gjk_tolerance &&
+           gjk_max_iterations == other.gjk_max_iterations &&
+           cached_gjk_guess == other.cached_gjk_guess &&
+           cached_support_func_guess == other.cached_support_func_guess &&
+           epa_max_iterations == other.epa_max_iterations &&
+           epa_tolerance == other.epa_tolerance &&
+           enable_timings == other.enable_timings &&
+           collision_distance_threshold == other.collision_distance_threshold;
+    COAL_COMPILER_DIAGNOSTIC_POP
+  }
+};
+
+/// @brief base class for all query results
+struct COAL_DLLAPI QueryResult {
+  /// @brief stores the last GJK ray when relevant.
+  Vec3s cached_gjk_guess;
+
+  /// @brief stores the last support function vertex index, when relevant.
+  support_func_guess_t cached_support_func_guess;
+
+  /// @brief timings for the given request
+  CPUTimes timings;
+
+  QueryResult()
+      : cached_gjk_guess(Vec3s::Zero()),
+        cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
+};
+
+inline void QueryRequest::updateGuess(const QueryResult& result) const {
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+      enable_cached_gjk_guess) {
+    cached_gjk_guess = result.cached_gjk_guess;
+    cached_support_func_guess = result.cached_support_func_guess;
+  }
+  COAL_COMPILER_DIAGNOSTIC_POP
+}
+
+struct CollisionResult;
+
+/// @brief flag declaration for specifying required params in CollisionResult
+enum CollisionRequestFlag {
+  CONTACT = 0x00001,
+  DISTANCE_LOWER_BOUND = 0x00002,
+  NO_REQUEST = 0x01000
+};
+
+/// @brief request to the collision algorithm
+struct COAL_DLLAPI CollisionRequest : QueryRequest {
+  /// @brief The maximum number of contacts that can be returned
+  size_t num_max_contacts;
+
+  /// @brief whether the contact information (normal, penetration depth and
+  /// contact position) will return.
+  bool enable_contact;
+
+  /// Whether a lower bound on distance is returned when objects are disjoint
+  COAL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.)
+  bool enable_distance_lower_bound;
+
+  /// @brief Distance below which objects are considered in collision.
+  /// See \ref coal_collision_and_distance_lower_bound_computation
+  /// @note If set to -inf, the objects tested for collision are considered
+  ///       as collision free and no test is actually performed by functions
+  ///       coal::collide of class coal::ComputeCollision.
+  CoalScalar security_margin;
+
+  /// @brief Distance below which bounding volumes are broken down.
+  /// See \ref coal_collision_and_distance_lower_bound_computation
+  CoalScalar break_distance;
+
+  /// @brief Distance above which GJK solver makes an early stopping.
+  /// GJK stops searching for the closest points when it proves that the
+  /// distance between two geometries is above this threshold.
+  ///
+  /// @remarks Consequently, the closest points might be incorrect, but allows
+  /// to save computational resources.
+  CoalScalar distance_upper_bound;
+
+  /// @brief Constructor from a flag and a maximal number of contacts.
+  ///
+  /// @param[in] flag Collision request flag
+  /// @param[in] num_max_contacts  Maximal number of allowed contacts
+  ///
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
+      : num_max_contacts(num_max_contacts_),
+        enable_contact(flag & CONTACT),
+        enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND),
+        security_margin(0),
+        break_distance(1e-3),
+        distance_upper_bound((std::numeric_limits<CoalScalar>::max)()) {}
+
+  /// @brief Default constructor.
+  CollisionRequest()
+      : num_max_contacts(1),
+        enable_contact(true),
+        enable_distance_lower_bound(false),
+        security_margin(0),
+        break_distance(1e-3),
+        distance_upper_bound((std::numeric_limits<CoalScalar>::max)()) {}
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  bool isSatisfied(const CollisionResult& result) const;
+
+  /// @brief whether two CollisionRequest are the same or not
+  inline bool operator==(const CollisionRequest& other) const {
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    return QueryRequest::operator==(other) &&
+           num_max_contacts == other.num_max_contacts &&
+           enable_contact == other.enable_contact &&
+           enable_distance_lower_bound == other.enable_distance_lower_bound &&
+           security_margin == other.security_margin &&
+           break_distance == other.break_distance &&
+           distance_upper_bound == other.distance_upper_bound;
+    COAL_COMPILER_DIAGNOSTIC_POP
+  }
+};
+
+inline CoalScalar Contact::getDistanceToCollision(
+    const CollisionRequest& request) const {
+  return penetration_depth - request.security_margin;
+}
+
+/// @brief collision result
+struct COAL_DLLAPI CollisionResult : QueryResult {
+ private:
+  /// @brief contact information
+  std::vector<Contact> contacts;
+
+ public:
+  /// Lower bound on distance between objects if they are disjoint.
+  /// See \ref coal_collision_and_distance_lower_bound_computation
+  /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is
+  /// set to infinity, distance_lower_bound is the actual distance between the
+  /// shapes.
+  CoalScalar distance_lower_bound;
+
+  /// @brief normal associated to nearest_points.
+  /// Same as `CollisionResult::nearest_points` but for the normal.
+  Vec3s normal;
+
+  /// @brief nearest points.
+  /// A `CollisionResult` can have multiple contacts.
+  /// The nearest points in `CollisionResults` correspond to the witness points
+  /// associated with the smallest distance i.e the `distance_lower_bound`.
+  /// For bounding volumes and BVHs, these nearest points are available
+  /// only when distance_lower_bound is inferior to
+  /// CollisionRequest::break_distance.
+  std::array<Vec3s, 2> nearest_points;
+
+ public:
+  CollisionResult()
+      : distance_lower_bound((std::numeric_limits<CoalScalar>::max)()) {
+    nearest_points[0] = nearest_points[1] = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  /// @brief Update the lower bound only if the distance is inferior.
+  inline void updateDistanceLowerBound(
+      const CoalScalar& distance_lower_bound_) {
+    if (distance_lower_bound_ < distance_lower_bound)
+      distance_lower_bound = distance_lower_bound_;
+  }
+
+  /// @brief add one contact into result structure
+  inline void addContact(const Contact& c) { contacts.push_back(c); }
+
+  /// @brief whether two CollisionResult are the same or not
+  inline bool operator==(const CollisionResult& other) const {
+    return contacts == other.contacts &&
+           distance_lower_bound == other.distance_lower_bound &&
+           nearest_points[0] == other.nearest_points[0] &&
+           nearest_points[1] == other.nearest_points[1] &&
+           normal == other.normal;
+  }
+
+  /// @brief return binary collision result
+  bool isCollision() const { return contacts.size() > 0; }
+
+  /// @brief number of contacts found
+  size_t numContacts() const { return contacts.size(); }
+
+  /// @brief get the i-th contact calculated
+  const Contact& getContact(size_t i) const {
+    if (contacts.size() == 0)
+      COAL_THROW_PRETTY(
+          "The number of contacts is zero. No Contact can be returned.",
+          std::invalid_argument);
+
+    if (i < contacts.size())
+      return contacts[i];
+    else
+      return contacts.back();
+  }
+
+  /// @brief set the i-th contact calculated
+  void setContact(size_t i, const Contact& c) {
+    if (contacts.size() == 0)
+      COAL_THROW_PRETTY(
+          "The number of contacts is zero. No Contact can be returned.",
+          std::invalid_argument);
+
+    if (i < contacts.size())
+      contacts[i] = c;
+    else
+      contacts.back() = c;
+  }
+
+  /// @brief get all the contacts
+  void getContacts(std::vector<Contact>& contacts_) const {
+    contacts_.resize(contacts.size());
+    std::copy(contacts.begin(), contacts.end(), contacts_.begin());
+  }
+
+  const std::vector<Contact>& getContacts() const { return contacts; }
+
+  /// @brief clear the results obtained
+  void clear() {
+    distance_lower_bound = (std::numeric_limits<CoalScalar>::max)();
+    contacts.clear();
+    timings.clear();
+    nearest_points[0] = nearest_points[1] = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  /// @brief reposition Contact objects when fcl inverts them
+  /// during their construction.
+  void swapObjects();
+};
+
+/// @brief This structure allows to encode contact patches.
+/// A contact patch is defined by a set of points belonging to a subset of a
+/// plane passing by `p` and supported by `n`, where `n = Contact::normal` and
+/// `p = Contact::pos`. If we denote by P this plane and by S1 and S2 the first
+/// and second shape of a collision pair, a contact patch is represented as a
+/// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the
+/// set-intersection. Since a contact patch is a subset of a plane supported by
+/// `n`, it has a preferred direction. In Coal, the `Contact::normal` points
+/// from S1 to S2. In the same way, a contact patch points by default from S1
+/// to S2.
+///
+/// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope),
+/// so the points of the set, forming the convex-hull of the polytope, are
+/// stored in a counter-clockwise fashion.
+/// @note If needed, the internal algorithms of Coal can easily be extended
+/// to compute a contact volume instead of a contact patch.
+struct COAL_DLLAPI ContactPatch {
+ public:
+  using Polygon = std::vector<Vec2s>;
+
+  /// @brief Frame of the set, expressed in the world coordinates.
+  /// The z-axis of the frame's rotation is the contact patch normal.
+  Transform3s tf;
+
+  /// @brief Direction of ContactPatch.
+  /// When doing collision detection, the convention of Coal is that the
+  /// normal always points from the first to the second shape of the collision
+  /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`.
+  /// The PatchDirection enum allows to identify if the patch points from
+  /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted
+  /// type). The Inverted type should only be used for internal Coal
+  /// computations (it allows to properly define two separate contact patches in
+  /// the same frame).
+  enum PatchDirection { DEFAULT = 0, INVERTED = 1 };
+
+  /// @brief Direction of this contact patch.
+  PatchDirection direction;
+
+  /// @brief Penetration depth of the contact patch. This value corresponds to
+  /// the signed distance `d` between the shapes.
+  /// @note For each contact point `p` in the patch of normal `n`, `p1 = p -
+  /// 0.5*d*n` and `p2 = p + 0.5*d*n` define a pair of witness points. `p1`
+  /// belongs to the surface of the first shape and `p2` belongs to the surface
+  /// of the second shape. For any pair of witness points, we always have `p2 -
+  /// p1 = d * n`. The vector `d * n` is called a minimum separation vector:
+  /// if S1 is translated by it, S1 and S2 are not in collision anymore.
+  /// @note Although there may exist multiple minimum separation vectors between
+  /// two shapes, the term "minimum" comes from the fact that it's impossible to
+  /// find a different separation vector which has a smaller norm than `d * n`.
+  CoalScalar penetration_depth;
+
+  /// @brief Default maximum size of the polygon representing the contact patch.
+  /// Used to pre-allocate memory for the patch.
+  static constexpr size_t default_preallocated_size = 12;
+
+ protected:
+  /// @brief Container for the vertices of the set.
+  Polygon m_points;
+
+ public:
+  /// @brief Default constructor.
+  /// Note: the preallocated size does not determine the maximum number of
+  /// points in the patch, it only serves as preallocation if the maximum size
+  /// of the patch is known in advance. Coal will automatically expand/shrink
+  /// the contact patch if needed.
+  explicit ContactPatch(size_t preallocated_size = default_preallocated_size)
+      : tf(Transform3s::Identity()),
+        direction(PatchDirection::DEFAULT),
+        penetration_depth(0) {
+    this->m_points.reserve(preallocated_size);
+  }
+
+  /// @brief Normal of the contact patch, expressed in the WORLD frame.
+  Vec3s getNormal() const {
+    if (this->direction == PatchDirection::INVERTED) {
+      return -this->tf.rotation().col(2);
+    }
+    return this->tf.rotation().col(2);
+  }
+
+  /// @brief Returns the number of points in the contact patch.
+  size_t size() const { return this->m_points.size(); }
+
+  /// @brief Add a 3D point to the set, expressed in the world frame.
+  /// @note This function takes a 3D point and expresses it in the local frame
+  /// of the set. It then takes only the x and y components of the vector,
+  /// effectively doing a projection onto the plane to which the set belongs.
+  /// TODO(louis): if necessary, we can store the offset to the plane (x, y).
+  void addPoint(const Vec3s& point_3d) {
+    const Vec3s point = this->tf.inverseTransform(point_3d);
+    this->m_points.emplace_back(point.template head<2>());
+  }
+
+  /// @brief Get the i-th point of the set, expressed in the 3D world frame.
+  Vec3s getPoint(const size_t i) const {
+    Vec3s point(0, 0, 0);
+    point.head<2>() = this->point(i);
+    point = tf.transform(point);
+    return point;
+  }
+
+  /// @brief Get the i-th point of the contact patch, projected back onto the
+  /// first shape of the collision pair. This point is expressed in the 3D
+  /// world frame.
+  Vec3s getPointShape1(const size_t i) const {
+    Vec3s point = this->getPoint(i);
+    point -= (this->penetration_depth / 2) * this->getNormal();
+    return point;
+  }
+
+  /// @brief Get the i-th point of the contact patch, projected back onto the
+  /// first shape of the collision pair. This 3D point is expressed in the world
+  /// frame.
+  Vec3s getPointShape2(const size_t i) const {
+    Vec3s point = this->getPoint(i);
+    point += (this->penetration_depth / 2) * this->getNormal();
+    return point;
+  }
+
+  /// @brief Getter for the 2D points in the set.
+  Polygon& points() { return this->m_points; }
+
+  /// @brief Const getter for the 2D points in the set.
+  const Polygon& points() const { return this->m_points; }
+
+  /// @brief Getter for the i-th 2D point in the set.
+  Vec2s& point(const size_t i) {
+    COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
+    if (i < this->m_points.size()) {
+      return this->m_points[i];
+    }
+    return this->m_points.back();
+  }
+
+  /// @brief Const getter for the i-th 2D point in the set.
+  const Vec2s& point(const size_t i) const {
+    COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
+    if (i < this->m_points.size()) {
+      return this->m_points[i];
+    }
+    return this->m_points.back();
+  }
+
+  /// @brief Clear the set.
+  void clear() {
+    this->m_points.clear();
+    this->tf.setIdentity();
+    this->penetration_depth = 0;
+  }
+
+  /// @brief Whether two contact patches are the same or not.
+  /// @note This compares the two sets terms by terms.
+  /// However, two contact patches can be identical, but have a different
+  /// order for their points. Use `isEqual` in this case.
+  bool operator==(const ContactPatch& other) const {
+    return this->tf == other.tf && this->direction == other.direction &&
+           this->penetration_depth == other.penetration_depth &&
+           this->points() == other.points();
+  }
+
+  /// @brief Whether two contact patches are the same or not.
+  /// Checks for different order of the points.
+  bool isSame(const ContactPatch& other,
+              const CoalScalar tol =
+                  Eigen::NumTraits<CoalScalar>::dummy_precision()) const {
+    // The x and y axis of the set are arbitrary, but the z axis is
+    // always the normal. The position of the origin of the frame is also
+    // arbitrary. So we only check if the normals are the same.
+    if (!this->getNormal().isApprox(other.getNormal(), tol)) {
+      return false;
+    }
+
+    if (std::abs(this->penetration_depth - other.penetration_depth) > tol) {
+      return false;
+    }
+
+    if (this->direction != other.direction) {
+      return false;
+    }
+
+    if (this->size() != other.size()) {
+      return false;
+    }
+
+    // Check all points of the contact patch.
+    for (size_t i = 0; i < this->size(); ++i) {
+      bool found = false;
+      const Vec3s pi = this->getPoint(i);
+      for (size_t j = 0; j < other.size(); ++j) {
+        const Vec3s other_pj = other.getPoint(j);
+        if (pi.isApprox(other_pj, tol)) {
+          found = true;
+        }
+      }
+      if (!found) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+};
+
+/// @brief Construct a frame from a `Contact`'s position and normal.
+/// Because both `Contact`'s position and normal are expressed in the world
+/// frame, this frame is also expressed w.r.t the world frame.
+/// The origin of the frame is `contact.pos` and the z-axis of the frame is
+/// `contact.normal`.
+inline void constructContactPatchFrameFromContact(const Contact& contact,
+                                                  ContactPatch& contact_patch) {
+  contact_patch.penetration_depth = contact.penetration_depth;
+  contact_patch.tf.translation() = contact.pos;
+  contact_patch.tf.rotation() =
+      constructOrthonormalBasisFromVector(contact.normal);
+  contact_patch.direction = ContactPatch::PatchDirection::DEFAULT;
+}
+
+/// @brief Structure used for internal computations. A support set and a
+/// contact patch can be represented by the same structure. In fact, a contact
+/// patch is the intersection of two support sets, one with
+/// `PatchDirection::DEFAULT` and one with `PatchDirection::INVERTED`.
+/// @note A support set with `DEFAULT` direction is the support set of a shape
+/// in the direction given by `+n`, where `n` is the z-axis of the frame's
+/// patch rotation. An `INVERTED` support set is the support set of a shape in
+/// the direction `-n`.
+using SupportSet = ContactPatch;
+
+/// @brief Request for a contact patch computation.
+struct COAL_DLLAPI ContactPatchRequest {
+  /// @brief Maximum number of contact patches that will be computed.
+  size_t max_num_patch;
+
+ protected:
+  /// @brief Maximum samples to compute the support sets of curved shapes,
+  /// i.e. when the normal is perpendicular to the base of a cylinder. For
+  /// now, only relevant for Cone and Cylinder. In the future this might be
+  /// extended to Sphere and Ellipsoid.
+  size_t m_num_samples_curved_shapes;
+
+  /// @brief Tolerance below which points are added to a contact patch.
+  /// In details, given two shapes S1 and S2, a contact patch is the triple
+  /// intersection between the separating plane (P) (passing by `Contact::pos`
+  /// and supported by `Contact::normal`), S1 and S2; i.e. a contact patch is
+  /// `P & S1 & S2` if we denote `&` the set intersection operator. If a point
+  /// p1 of S1 is at a distance below `patch_tolerance` from the separating
+  /// plane, it is taken into account in the computation of the contact patch.
+  /// Otherwise, it is not used for the computation.
+  /// @note Needs to be positive.
+  CoalScalar m_patch_tolerance;
+
+ public:
+  /// @brief Default constructor.
+  /// @param max_num_patch maximum number of contact patches per collision pair.
+  /// @param max_sub_patch_size maximum size of each sub contact patch. Each
+  /// contact patch contains an internal representation for an inscribed sub
+  /// contact patch. This allows physics simulation to always work with a
+  /// predetermined maximum size for each contact patch. A sub contact patch is
+  /// simply a subset of the vertices of a contact patch.
+  /// @param num_samples_curved_shapes for shapes like cones and cylinders,
+  /// which have smooth basis (circles in this case), we need to sample a
+  /// certain amount of point of this basis.
+  /// @param patch_tolerance the tolerance below which a point of a shape is
+  /// considered to belong to the support set of this shape in the direction of
+  /// the normal. Said otherwise, `patch_tolerance` determines the "thickness"
+  /// of the separating plane between shapes of a collision pair.
+  explicit ContactPatchRequest(size_t max_num_patch = 1,
+                               size_t num_samples_curved_shapes =
+                                   ContactPatch::default_preallocated_size,
+                               CoalScalar patch_tolerance = 1e-3)
+      : max_num_patch(max_num_patch) {
+    this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
+    this->setPatchTolerance(patch_tolerance);
+  }
+
+  /// @brief Construct a contact patch request from a collision request.
+  explicit ContactPatchRequest(const CollisionRequest& collision_request,
+                               size_t num_samples_curved_shapes =
+                                   ContactPatch::default_preallocated_size,
+                               CoalScalar patch_tolerance = 1e-3)
+      : max_num_patch(collision_request.num_max_contacts) {
+    this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
+    this->setPatchTolerance(patch_tolerance);
+  }
+
+  /// @copydoc m_num_samples_curved_shapes
+  void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) {
+    if (num_samples_curved_shapes < 3) {
+      COAL_LOG_WARNING(
+          "`num_samples_curved_shapes` cannot be lower than 3. Setting it to "
+          "3 to prevent bugs.");
+      this->m_num_samples_curved_shapes = 3;
+    } else {
+      this->m_num_samples_curved_shapes = num_samples_curved_shapes;
+    }
+  }
+
+  /// @copydoc m_num_samples_curved_shapes
+  size_t getNumSamplesCurvedShapes() const {
+    return this->m_num_samples_curved_shapes;
+  }
+
+  /// @copydoc m_patch_tolerance
+  void setPatchTolerance(const CoalScalar patch_tolerance) {
+    if (patch_tolerance < 0) {
+      COAL_LOG_WARNING(
+          "`patch_tolerance` cannot be negative. Setting it to 0 to prevent "
+          "bugs.");
+      this->m_patch_tolerance = Eigen::NumTraits<CoalScalar>::dummy_precision();
+    } else {
+      this->m_patch_tolerance = patch_tolerance;
+    }
+  }
+
+  /// @copydoc m_patch_tolerance
+  CoalScalar getPatchTolerance() const { return this->m_patch_tolerance; }
+
+  /// @brief Whether two ContactPatchRequest are identical or not.
+  bool operator==(const ContactPatchRequest& other) const {
+    return this->max_num_patch == other.max_num_patch &&
+           this->getNumSamplesCurvedShapes() ==
+               other.getNumSamplesCurvedShapes() &&
+           this->getPatchTolerance() == other.getPatchTolerance();
+  }
+};
+
+/// @brief Result for a contact patch computation.
+struct COAL_DLLAPI ContactPatchResult {
+  using ContactPatchVector = std::vector<ContactPatch>;
+  using ContactPatchRef = std::reference_wrapper<ContactPatch>;
+  using ContactPatchRefVector = std::vector<ContactPatchRef>;
+
+ protected:
+  /// @brief Data container for the vector of contact patches.
+  /// @note Contrary to `CollisionResult` or `DistanceResult`, which have a
+  /// very small memory footprint, contact patches can contain relatively
+  /// large polytopes. In order to reuse a `ContactPatchResult` while avoiding
+  /// successive mallocs, we have a data container and a vector which points
+  /// to the currently active patches in this data container.
+  ContactPatchVector m_contact_patches_data;
+
+  /// @brief Contact patches in `m_contact_patches_data` can have two
+  /// statuses: used or unused. This index tracks the first unused patch in
+  /// the `m_contact_patches_data` vector.
+  size_t m_id_available_patch;
+
+  /// @brief Vector of contact patches of the result.
+  ContactPatchRefVector m_contact_patches;
+
+ public:
+  /// @brief Default constructor.
+  ContactPatchResult() : m_id_available_patch(0) {
+    const size_t max_num_patch = 1;
+    const ContactPatchRequest request(max_num_patch);
+    this->set(request);
+  }
+
+  /// @brief Constructor using a `ContactPatchRequest`.
+  explicit ContactPatchResult(const ContactPatchRequest& request)
+      : m_id_available_patch(0) {
+    this->set(request);
+  };
+
+  /// @brief Number of contact patches in the result.
+  size_t numContactPatches() const { return this->m_contact_patches.size(); }
+
+  /// @brief Returns a new unused contact patch from the internal data vector.
+  ContactPatchRef getUnusedContactPatch() {
+    if (this->m_id_available_patch >= this->m_contact_patches_data.size()) {
+      COAL_LOG_WARNING(
+          "Trying to get an unused contact patch but all contact patches are "
+          "used. Increasing size of contact patches vector, at the cost of a "
+          "copy. You should increase `max_num_patch` in the "
+          "`ContactPatchRequest`.");
+      this->m_contact_patches_data.emplace_back(
+          this->m_contact_patches_data.back());
+      this->m_contact_patches_data.back().clear();
+    }
+    ContactPatch& contact_patch =
+        this->m_contact_patches_data[this->m_id_available_patch];
+    contact_patch.clear();
+    this->m_contact_patches.emplace_back(contact_patch);
+    ++(this->m_id_available_patch);
+    return this->m_contact_patches.back();
+  }
+
+  /// @brief Const getter for the i-th contact patch of the result.
+  const ContactPatch& getContactPatch(const size_t i) const {
+    if (this->m_contact_patches.empty()) {
+      COAL_THROW_PRETTY(
+          "The number of contact patches is zero. No ContactPatch can be "
+          "returned.",
+          std::invalid_argument);
+    }
+    if (i < this->m_contact_patches.size()) {
+      return this->m_contact_patches[i];
+    }
+    return this->m_contact_patches.back();
+  }
+
+  /// @brief Getter for the i-th contact patch of the result.
+  ContactPatch& contactPatch(const size_t i) {
+    if (this->m_contact_patches.empty()) {
+      COAL_THROW_PRETTY(
+          "The number of contact patches is zero. No ContactPatch can be "
+          "returned.",
+          std::invalid_argument);
+    }
+    if (i < this->m_contact_patches.size()) {
+      return this->m_contact_patches[i];
+    }
+    return this->m_contact_patches.back();
+  }
+
+  /// @brief Clears the contact patch result.
+  void clear() {
+    this->m_contact_patches.clear();
+    this->m_id_available_patch = 0;
+    for (ContactPatch& patch : this->m_contact_patches_data) {
+      patch.clear();
+    }
+  }
+
+  /// @brief Set up a `ContactPatchResult` from a `ContactPatchRequest`
+  void set(const ContactPatchRequest& request) {
+    if (this->m_contact_patches_data.size() < request.max_num_patch) {
+      this->m_contact_patches_data.resize(request.max_num_patch);
+    }
+    for (ContactPatch& patch : this->m_contact_patches_data) {
+      patch.points().reserve(request.getNumSamplesCurvedShapes());
+    }
+    this->clear();
+  }
+
+  /// @brief Return true if this `ContactPatchResult` is aligned with the
+  /// `ContactPatchRequest` given as input.
+  bool check(const ContactPatchRequest& request) const {
+    assert(this->m_contact_patches_data.size() >= request.max_num_patch);
+    if (this->m_contact_patches_data.size() < request.max_num_patch) {
+      return false;
+    }
+
+    for (const ContactPatch& patch : this->m_contact_patches_data) {
+      if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) {
+        assert(patch.points().capacity() >=
+               request.getNumSamplesCurvedShapes());
+        return false;
+      }
+    }
+    return true;
+  }
+
+  /// @brief Whether two ContactPatchResult are identical or not.
+  bool operator==(const ContactPatchResult& other) const {
+    if (this->numContactPatches() != other.numContactPatches()) {
+      return false;
+    }
+
+    for (size_t i = 0; i < this->numContactPatches(); ++i) {
+      const ContactPatch& patch = this->getContactPatch(i);
+      const ContactPatch& other_patch = other.getContactPatch(i);
+      if (!(patch == other_patch)) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  /// @brief Repositions the ContactPatch when they get inverted during their
+  /// construction.
+  void swapObjects() {
+    // Create new transform: it's the reflection of `tf` along the z-axis.
+    // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis
+    // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis.
+    for (size_t i = 0; i < this->numContactPatches(); ++i) {
+      ContactPatch& patch = this->contactPatch(i);
+      patch.tf.rotation().col(0) *= -1.0;
+      patch.tf.rotation().col(2) *= -1.0;
+
+      for (size_t j = 0; j < patch.size(); ++j) {
+        patch.point(i)(0) *= -1.0;  // only invert the x-axis
+      }
+    }
+  }
+};
+
+struct DistanceResult;
+
+/// @brief request to the distance computation
+struct COAL_DLLAPI DistanceRequest : QueryRequest {
+  /// @brief whether to return the nearest points.
+  /// Nearest points are always computed and are the points of the shapes that
+  /// achieve a distance of `DistanceResult::min_distance`.
+  COAL_DEPRECATED_MESSAGE(
+      Nearest points are always computed : they are the points of the shapes
+          that achieve a distance of
+      `DistanceResult::min_distance`
+              .\n Use `enable_signed_distance` if you want to compute a signed
+                  minimum distance(and thus its corresponding nearest points)
+              .)
+  bool enable_nearest_points;
+
+  /// @brief whether to compute the penetration depth when objects are in
+  /// collision.
+  /// Turning this off can save computation time if only the distance
+  /// when objects are disjoint is needed.
+  /// @note The minimum distance between the shapes is stored in
+  /// `DistanceResult::min_distance`.
+  /// If `enable_signed_distance` is off, `DistanceResult::min_distance`
+  /// is always positive.
+  /// If `enable_signed_distance` is on, `DistanceResult::min_distance`
+  /// can be positive or negative.
+  /// The nearest points are the points of the shapes that achieve
+  /// a distance of `DistanceResult::min_distance`.
+  bool enable_signed_distance;
+
+  /// @brief error threshold for approximate distance
+  CoalScalar rel_err;  // relative error, between 0 and 1
+  CoalScalar abs_err;  // absolute error
+
+  /// \param enable_nearest_points_ enables the nearest points computation.
+  /// \param enable_signed_distance_ allows to compute the penetration depth
+  /// \param rel_err_
+  /// \param abs_err_
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  DistanceRequest(bool enable_nearest_points_ = true,
+                  bool enable_signed_distance_ = true,
+                  CoalScalar rel_err_ = 0.0, CoalScalar abs_err_ = 0.0)
+      : enable_nearest_points(enable_nearest_points_),
+        enable_signed_distance(enable_signed_distance_),
+        rel_err(rel_err_),
+        abs_err(abs_err_) {}
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  bool isSatisfied(const DistanceResult& result) const;
+
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  DistanceRequest& operator=(const DistanceRequest& other) = default;
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  /// @brief whether two DistanceRequest are the same or not
+  inline bool operator==(const DistanceRequest& other) const {
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    return QueryRequest::operator==(other) &&
+           enable_nearest_points == other.enable_nearest_points &&
+           enable_signed_distance == other.enable_signed_distance &&
+           rel_err == other.rel_err && abs_err == other.abs_err;
+    COAL_COMPILER_DIAGNOSTIC_POP
+  }
+};
+
+/// @brief distance result
+struct COAL_DLLAPI DistanceResult : QueryResult {
+ public:
+  /// @brief minimum distance between two objects.
+  /// If two objects are in collision and
+  /// DistanceRequest::enable_signed_distance is activated, min_distance <= 0.
+  /// @note The nearest points are the points of the shapes that achieve a
+  /// distance of `DistanceResult::min_distance`.
+  CoalScalar min_distance;
+
+  /// @brief normal.
+  Vec3s normal;
+
+  /// @brief nearest points.
+  /// See CollisionResult::nearest_points.
+  std::array<Vec3s, 2> nearest_points;
+
+  /// @brief collision object 1
+  const CollisionGeometry* o1;
+
+  /// @brief collision object 2
+  const CollisionGeometry* o2;
+
+  /// @brief information about the nearest point in object 1
+  /// if object 1 is mesh or point cloud, it is the triangle or point id
+  /// if object 1 is geometry shape, it is NONE (-1),
+  /// if object 1 is octree, it is the id of the cell
+  int b1;
+
+  /// @brief information about the nearest point in object 2
+  /// if object 2 is mesh or point cloud, it is the triangle or point id
+  /// if object 2 is geometry shape, it is NONE (-1),
+  /// if object 2 is octree, it is the id of the cell
+  int b2;
+
+  /// @brief invalid contact primitive information
+  static const int NONE = -1;
+
+  DistanceResult(
+      CoalScalar min_distance_ = (std::numeric_limits<CoalScalar>::max)())
+      : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
+    const Vec3s nan(
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+    nearest_points[0] = nearest_points[1] = normal = nan;
+  }
+
+  /// @brief add distance information into the result
+  void update(CoalScalar distance, const CollisionGeometry* o1_,
+              const CollisionGeometry* o2_, int b1_, int b2_) {
+    if (min_distance > distance) {
+      min_distance = distance;
+      o1 = o1_;
+      o2 = o2_;
+      b1 = b1_;
+      b2 = b2_;
+    }
+  }
+
+  /// @brief add distance information into the result
+  void update(CoalScalar distance, const CollisionGeometry* o1_,
+              const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1,
+              const Vec3s& p2, const Vec3s& normal_) {
+    if (min_distance > distance) {
+      min_distance = distance;
+      o1 = o1_;
+      o2 = o2_;
+      b1 = b1_;
+      b2 = b2_;
+      nearest_points[0] = p1;
+      nearest_points[1] = p2;
+      normal = normal_;
+    }
+  }
+
+  /// @brief add distance information into the result
+  void update(const DistanceResult& other_result) {
+    if (min_distance > other_result.min_distance) {
+      min_distance = other_result.min_distance;
+      o1 = other_result.o1;
+      o2 = other_result.o2;
+      b1 = other_result.b1;
+      b2 = other_result.b2;
+      nearest_points[0] = other_result.nearest_points[0];
+      nearest_points[1] = other_result.nearest_points[1];
+      normal = other_result.normal;
+    }
+  }
+
+  /// @brief clear the result
+  void clear() {
+    const Vec3s nan(
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+    min_distance = (std::numeric_limits<CoalScalar>::max)();
+    o1 = NULL;
+    o2 = NULL;
+    b1 = NONE;
+    b2 = NONE;
+    nearest_points[0] = nearest_points[1] = normal = nan;
+    timings.clear();
+  }
+
+  /// @brief whether two DistanceResult are the same or not
+  inline bool operator==(const DistanceResult& other) const {
+    bool is_same = min_distance == other.min_distance &&
+                   nearest_points[0] == other.nearest_points[0] &&
+                   nearest_points[1] == other.nearest_points[1] &&
+                   normal == other.normal && o1 == other.o1 && o2 == other.o2 &&
+                   b1 == other.b1 && b2 == other.b2;
+
+    // TODO: check also that two GeometryObject are indeed equal.
+    if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
+    is_same &= (o1 == other.o1);
+    //    else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 ==
+    //    *other.o1;
+
+    if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
+    is_same &= (o2 == other.o2);
+    //    else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 ==
+    //    *other.o2;
+
+    return is_same;
+  }
+};
+
+namespace internal {
+inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
+                                           CollisionResult& res,
+                                           const CoalScalar sqrDistLowerBound) {
+  // BV cannot find negative distance.
+  if (res.distance_lower_bound <= 0) return;
+  CoalScalar new_dlb = std::sqrt(sqrDistLowerBound);  // - req.security_margin;
+  if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
+}
+
+inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
+                                             CollisionResult& res,
+                                             const CoalScalar& distance,
+                                             const Vec3s& p0, const Vec3s& p1,
+                                             const Vec3s& normal) {
+  if (distance < res.distance_lower_bound) {
+    res.distance_lower_bound = distance;
+    res.nearest_points[0] = p0;
+    res.nearest_points[1] = p1;
+    res.normal = normal;
+  }
+}
+}  // namespace internal
+
+inline CollisionRequestFlag operator~(CollisionRequestFlag a) {
+  return static_cast<CollisionRequestFlag>(~static_cast<int>(a));
+}
+
+inline CollisionRequestFlag operator|(CollisionRequestFlag a,
+                                      CollisionRequestFlag b) {
+  return static_cast<CollisionRequestFlag>(static_cast<int>(a) |
+                                           static_cast<int>(b));
+}
+
+inline CollisionRequestFlag operator&(CollisionRequestFlag a,
+                                      CollisionRequestFlag b) {
+  return static_cast<CollisionRequestFlag>(static_cast<int>(a) &
+                                           static_cast<int>(b));
+}
+
+inline CollisionRequestFlag operator^(CollisionRequestFlag a,
+                                      CollisionRequestFlag b) {
+  return static_cast<CollisionRequestFlag>(static_cast<int>(a) ^
+                                           static_cast<int>(b));
+}
+
+inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a,
+                                        CollisionRequestFlag b) {
+  return (CollisionRequestFlag&)((int&)(a) |= static_cast<int>(b));
+}
+
+inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a,
+                                        CollisionRequestFlag b) {
+  return (CollisionRequestFlag&)((int&)(a) &= static_cast<int>(b));
+}
+
+inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a,
+                                        CollisionRequestFlag b) {
+  return (CollisionRequestFlag&)((int&)(a) ^= static_cast<int>(b));
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/collision_func_matrix.h b/include/coal/collision_func_matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..e9aeca48f6a4c6a2c33e14074487ee9961afe616
--- /dev/null
+++ b/include/coal/collision_func_matrix.h
@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_COLLISION_FUNC_MATRIX_H
+#define COAL_COLLISION_FUNC_MATRIX_H
+
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/narrowphase/narrowphase.h"
+
+namespace coal {
+
+/// @brief collision matrix stores the functions for collision between different
+/// types of objects and provides a uniform call interface
+
+struct COAL_DLLAPI CollisionFunctionMatrix {
+  /// @brief the uniform call interface for collision: for collision, we need
+  /// know
+  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
+  /// and tf2;
+  /// 2. the solver for narrow phase collision, this is for the collision
+  /// between geometric shapes;
+  /// 3. the request setting for collision (e.g., whether need to return normal
+  /// information, whether need to compute cost);
+  /// 4. the structure to return collision result
+  typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1,
+                                       const Transform3s& tf1,
+                                       const CollisionGeometry* o2,
+                                       const Transform3s& tf2,
+                                       const GJKSolver* nsolver,
+                                       const CollisionRequest& request,
+                                       CollisionResult& result);
+
+  /// @brief each item in the collision matrix is a function to handle collision
+  /// between objects of type1 and type2
+  CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
+
+  CollisionFunctionMatrix();
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/collision_object.h b/include/coal/collision_object.h
new file mode 100644
index 0000000000000000000000000000000000000000..ccc5cf13618a388c97431eab41fa62c176164543
--- /dev/null
+++ b/include/coal/collision_object.h
@@ -0,0 +1,360 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_COLLISION_OBJECT_BASE_H
+#define COAL_COLLISION_OBJECT_BASE_H
+
+#include <limits>
+#include <typeinfo>
+
+#include "coal/deprecated.hh"
+#include "coal/fwd.hh"
+#include "coal/BV/AABB.h"
+#include "coal/math/transform.h"
+
+namespace coal {
+
+/// @brief object type: BVH (mesh, points), basic geometry, octree
+enum OBJECT_TYPE {
+  OT_UNKNOWN,
+  OT_BVH,
+  OT_GEOM,
+  OT_OCTREE,
+  OT_HFIELD,
+  OT_COUNT
+};
+
+/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS,
+/// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone,
+/// cylinder, convex, plane, triangle), and octree
+enum NODE_TYPE {
+  BV_UNKNOWN,
+  BV_AABB,
+  BV_OBB,
+  BV_RSS,
+  BV_kIOS,
+  BV_OBBRSS,
+  BV_KDOP16,
+  BV_KDOP18,
+  BV_KDOP24,
+  GEOM_BOX,
+  GEOM_SPHERE,
+  GEOM_CAPSULE,
+  GEOM_CONE,
+  GEOM_CYLINDER,
+  GEOM_CONVEX,
+  GEOM_PLANE,
+  GEOM_HALFSPACE,
+  GEOM_TRIANGLE,
+  GEOM_OCTREE,
+  GEOM_ELLIPSOID,
+  HF_AABB,
+  HF_OBBRSS,
+  NODE_COUNT
+};
+
+/// @addtogroup Construction_Of_BVH
+/// @{
+
+/// @brief The geometry for the object for collision or distance computation
+class COAL_DLLAPI CollisionGeometry {
+ public:
+  CollisionGeometry()
+      : aabb_center(Vec3s::Constant((std::numeric_limits<CoalScalar>::max)())),
+        aabb_radius(-1),
+        cost_density(1),
+        threshold_occupied(1),
+        threshold_free(0) {}
+
+  /// \brief Copy constructor
+  CollisionGeometry(const CollisionGeometry& other) = default;
+
+  virtual ~CollisionGeometry() {}
+
+  /// @brief Clone *this into a new CollisionGeometry
+  virtual CollisionGeometry* clone() const = 0;
+
+  /// \brief Equality operator
+  bool operator==(const CollisionGeometry& other) const {
+    return cost_density == other.cost_density &&
+           threshold_occupied == other.threshold_occupied &&
+           threshold_free == other.threshold_free &&
+           aabb_center == other.aabb_center &&
+           aabb_radius == other.aabb_radius && aabb_local == other.aabb_local &&
+           isEqual(other);
+  }
+
+  /// \brief Difference operator
+  bool operator!=(const CollisionGeometry& other) const {
+    return isNotEqual(other);
+  }
+
+  /// @brief get the type of the object
+  virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
+
+  /// @brief get the node type
+  virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+  /// @brief compute the AABB for object in local coordinate
+  virtual void computeLocalAABB() = 0;
+
+  /// @brief get user data in geometry
+  void* getUserData() const { return user_data; }
+
+  /// @brief set user data in geometry
+  void setUserData(void* data) { user_data = data; }
+
+  /// @brief whether the object is completely occupied
+  inline bool isOccupied() const { return cost_density >= threshold_occupied; }
+
+  /// @brief whether the object is completely free
+  inline bool isFree() const { return cost_density <= threshold_free; }
+
+  /// @brief whether the object has some uncertainty
+  bool isUncertain() const;
+
+  /// @brief AABB center in local coordinate
+  Vec3s aabb_center;
+
+  /// @brief AABB radius
+  CoalScalar aabb_radius;
+
+  /// @brief AABB in local coordinate, used for tight AABB when only translation
+  /// transform
+  AABB aabb_local;
+
+  /// @brief pointer to user defined data specific to this object
+  void* user_data;
+
+  /// @brief collision cost for unit volume
+  CoalScalar cost_density;
+
+  /// @brief threshold for occupied ( >= is occupied)
+  CoalScalar threshold_occupied;
+
+  /// @brief threshold for free (<= is free)
+  CoalScalar threshold_free;
+
+  /// @brief compute center of mass
+  virtual Vec3s computeCOM() const { return Vec3s::Zero(); }
+
+  /// @brief compute the inertia matrix, related to the origin
+  virtual Matrix3s computeMomentofInertia() const {
+    return Matrix3s::Constant(NAN);
+  }
+
+  /// @brief compute the volume
+  virtual CoalScalar computeVolume() const { return 0; }
+
+  /// @brief compute the inertia matrix, related to the com
+  virtual Matrix3s computeMomentofInertiaRelatedToCOM() const {
+    Matrix3s C = computeMomentofInertia();
+    Vec3s com = computeCOM();
+    CoalScalar V = computeVolume();
+
+    return (Matrix3s() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
+            C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2],
+            C(1, 0) + V * com[1] * com[0],
+            C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
+            C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0],
+            C(2, 1) + V * com[2] * com[1],
+            C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]))
+        .finished();
+  }
+
+ private:
+  /// @brief equal operator with another object of derived type.
+  virtual bool isEqual(const CollisionGeometry& other) const = 0;
+
+  /// @brief not equal operator with another object of derived type.
+  virtual bool isNotEqual(const CollisionGeometry& other) const {
+    return !(*this == other);
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief the object for collision or distance computation, contains the
+/// geometry and the transform information
+class COAL_DLLAPI CollisionObject {
+ public:
+  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
+                  bool compute_local_aabb = true)
+      : cgeom(cgeom_), user_data(nullptr) {
+    init(compute_local_aabb);
+  }
+
+  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
+                  const Transform3s& tf, bool compute_local_aabb = true)
+      : cgeom(cgeom_), t(tf), user_data(nullptr) {
+    init(compute_local_aabb);
+  }
+
+  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
+                  const Matrix3s& R, const Vec3s& T,
+                  bool compute_local_aabb = true)
+      : cgeom(cgeom_), t(R, T), user_data(nullptr) {
+    init(compute_local_aabb);
+  }
+
+  bool operator==(const CollisionObject& other) const {
+    return cgeom == other.cgeom && t == other.t && user_data == other.user_data;
+  }
+
+  bool operator!=(const CollisionObject& other) const {
+    return !(*this == other);
+  }
+
+  ~CollisionObject() {}
+
+  /// @brief get the type of the object
+  OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); }
+
+  /// @brief get the node type
+  NODE_TYPE getNodeType() const { return cgeom->getNodeType(); }
+
+  /// @brief get the AABB in world space
+  const AABB& getAABB() const { return aabb; }
+
+  /// @brief get the AABB in world space
+  AABB& getAABB() { return aabb; }
+
+  /// @brief compute the AABB in world space
+  void computeAABB() {
+    if (t.getRotation().isIdentity()) {
+      aabb = translate(cgeom->aabb_local, t.getTranslation());
+    } else {
+      aabb.min_ = aabb.max_ = t.getTranslation();
+
+      Vec3s min_world, max_world;
+      for (int k = 0; k < 3; ++k) {
+        min_world.array() = t.getRotation().row(k).array() *
+                            cgeom->aabb_local.min_.transpose().array();
+        max_world.array() = t.getRotation().row(k).array() *
+                            cgeom->aabb_local.max_.transpose().array();
+
+        aabb.min_[k] += (min_world.array().min)(max_world.array()).sum();
+        aabb.max_[k] += (min_world.array().max)(max_world.array()).sum();
+      }
+    }
+  }
+
+  /// @brief get user data in object
+  void* getUserData() const { return user_data; }
+
+  /// @brief set user data in object
+  void setUserData(void* data) { user_data = data; }
+
+  /// @brief get translation of the object
+  inline const Vec3s& getTranslation() const { return t.getTranslation(); }
+
+  /// @brief get matrix rotation of the object
+  inline const Matrix3s& getRotation() const { return t.getRotation(); }
+
+  /// @brief get object's transform
+  inline const Transform3s& getTransform() const { return t; }
+
+  /// @brief set object's rotation matrix
+  void setRotation(const Matrix3s& R) { t.setRotation(R); }
+
+  /// @brief set object's translation
+  void setTranslation(const Vec3s& T) { t.setTranslation(T); }
+
+  /// @brief set object's transform
+  void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); }
+
+  /// @brief set object's transform
+  void setTransform(const Transform3s& tf) { t = tf; }
+
+  /// @brief whether the object is in local coordinate
+  bool isIdentityTransform() const { return t.isIdentity(); }
+
+  /// @brief set the object in local coordinate
+  void setIdentityTransform() { t.setIdentity(); }
+
+  /// @brief get shared pointer to collision geometry of the object instance
+  const shared_ptr<const CollisionGeometry> collisionGeometry() const {
+    return cgeom;
+  }
+
+  /// @brief get shared pointer to collision geometry of the object instance
+  const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; }
+
+  /// @brief get raw pointer to collision geometry of the object instance
+  const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); }
+
+  /// @brief get raw pointer to collision geometry of the object instance
+  CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); }
+
+  /// @brief Associate a new CollisionGeometry
+  ///
+  /// @param[in] collision_geometry The new CollisionGeometry
+  /// @param[in] compute_local_aabb Whether the local aabb of the input new has
+  /// to be computed.
+  ///
+  void setCollisionGeometry(
+      const shared_ptr<CollisionGeometry>& collision_geometry,
+      bool compute_local_aabb = true) {
+    if (collision_geometry.get() != cgeom.get()) {
+      cgeom = collision_geometry;
+      init(compute_local_aabb);
+    }
+  }
+
+ protected:
+  void init(bool compute_local_aabb = true) {
+    if (cgeom) {
+      if (compute_local_aabb) cgeom->computeLocalAABB();
+      computeAABB();
+    }
+  }
+
+  shared_ptr<CollisionGeometry> cgeom;
+
+  Transform3s t;
+
+  /// @brief AABB in global coordinate
+  mutable AABB aabb;
+
+  /// @brief pointer to user defined data specific to this object
+  void* user_data;
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/collision_utility.h b/include/coal/collision_utility.h
new file mode 100644
index 0000000000000000000000000000000000000000..44a597d2693bb462eb2001c22f35baa03f103d56
--- /dev/null
+++ b/include/coal/collision_utility.h
@@ -0,0 +1,56 @@
+// Copyright (c) 2017 CNRS
+// Copyright (c) 2022 INRIA
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of Coal.
+// Coal is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Coal is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// Coal. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef COAL_COLLISION_UTILITY_H
+#define COAL_COLLISION_UTILITY_H
+
+#include "coal/collision_object.h"
+
+namespace coal {
+
+COAL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model,
+                                       const Transform3s& pose,
+                                       const AABB& aabb);
+
+/**
+ * \brief Returns the name associated to a NODE_TYPE
+ */
+inline const char* get_node_type_name(NODE_TYPE node_type) {
+  static const char* node_type_name_all[] = {
+      "BV_UNKNOWN",     "BV_AABB",       "BV_OBB",      "BV_RSS",
+      "BV_kIOS",        "BV_OBBRSS",     "BV_KDOP16",   "BV_KDOP18",
+      "BV_KDOP24",      "GEOM_BOX",      "GEOM_SPHERE", "GEOM_CAPSULE",
+      "GEOM_CONE",      "GEOM_CYLINDER", "GEOM_CONVEX", "GEOM_PLANE",
+      "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE", "GEOM_ELLIPSOID",
+      "HF_AABB",        "HF_OBBRSS",     "NODE_COUNT"};
+
+  return node_type_name_all[node_type];
+}
+
+/**
+ * \brief Returns the name associated to a OBJECT_TYPE
+ */
+inline const char* get_object_type_name(OBJECT_TYPE object_type) {
+  static const char* object_type_name_all[] = {
+      "OT_UNKNOWN", "OT_BVH", "OT_GEOM", "OT_OCTREE", "OT_HFIELD", "OT_COUNT"};
+
+  return object_type_name_all[object_type];
+}
+
+}  // namespace coal
+
+#endif  // COAL_COLLISION_UTILITY_H
diff --git a/include/coal/contact_patch.h b/include/coal/contact_patch.h
new file mode 100644
index 0000000000000000000000000000000000000000..a942fa0b9f5d8b75c0cde0f7a141777fc6df2512
--- /dev/null
+++ b/include/coal/contact_patch.h
@@ -0,0 +1,122 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, INRIA
+ *  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 INRIA 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 Louis Montaut */
+
+#ifndef COAL_CONTACT_PATCH_H
+#define COAL_CONTACT_PATCH_H
+
+#include "coal/data_types.h"
+#include "coal/collision_data.h"
+#include "coal/contact_patch/contact_patch_solver.h"
+#include "coal/contact_patch_func_matrix.h"
+
+namespace coal {
+
+/// @brief Main contact patch computation interface.
+/// @note Please see @ref ContactPatchRequest and @ref ContactPatchResult for
+/// more info on the content of the input/output of this function. Also, please
+/// read @ref ContactPatch if you want to fully understand what is meant by
+/// "contact patch".
+COAL_DLLAPI void computeContactPatch(const CollisionGeometry* o1,
+                                     const Transform3s& tf1,
+                                     const CollisionGeometry* o2,
+                                     const Transform3s& tf2,
+                                     const CollisionResult& collision_result,
+                                     const ContactPatchRequest& request,
+                                     ContactPatchResult& result);
+
+/// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3s&,
+// const CollisionGeometry*, const Transform3s&, const CollisionResult&, const
+// ContactPatchRequest&, ContactPatchResult&);
+COAL_DLLAPI void computeContactPatch(const CollisionObject* o1,
+                                     const CollisionObject* o2,
+                                     const CollisionResult& collision_result,
+                                     const ContactPatchRequest& request,
+                                     ContactPatchResult& result);
+
+/// @brief This class reduces the cost of identifying the geometry pair.
+/// This is usefull for repeated shape-shape queries.
+/// @note This needs to be called after `collide` or after `ComputeCollision`.
+///
+/// \code
+///   ComputeContactPatch calc_patch (o1, o2);
+///   calc_patch(tf1, tf2, collision_result, patch_request, patch_result);
+/// \endcode
+class COAL_DLLAPI ComputeContactPatch {
+ public:
+  /// @brief Default constructor from two Collision Geometries.
+  ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2);
+
+  void operator()(const Transform3s& tf1, const Transform3s& tf2,
+                  const CollisionResult& collision_result,
+                  const ContactPatchRequest& request,
+                  ContactPatchResult& result) const;
+
+  bool operator==(const ComputeContactPatch& other) const {
+    return this->o1 == other.o1 && this->o2 == other.o2 &&
+           this->csolver == other.csolver;
+  }
+
+  bool operator!=(const ComputeContactPatch& other) const {
+    return !(*this == other);
+  }
+
+  virtual ~ComputeContactPatch() = default;
+
+ protected:
+  // These pointers are made mutable to let the derived classes to update
+  // their values when updating the collision geometry (e.g. creating a new
+  // one). This feature should be used carefully to avoid any mis usage (e.g,
+  // changing the type of the collision geometry should be avoided).
+  mutable const CollisionGeometry* o1;
+  mutable const CollisionGeometry* o2;
+
+  mutable ContactPatchSolver csolver;
+
+  ContactPatchFunctionMatrix::ContactPatchFunc func;
+  bool swap_geoms;
+
+  virtual void run(const Transform3s& tf1, const Transform3s& tf2,
+                   const CollisionResult& collision_result,
+                   const ContactPatchRequest& request,
+                   ContactPatchResult& result) const;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/contact_patch/contact_patch_solver.h b/include/coal/contact_patch/contact_patch_solver.h
new file mode 100644
index 0000000000000000000000000000000000000000..cce436ea107f93fd9fb9411451ef2f05c618f4be
--- /dev/null
+++ b/include/coal/contact_patch/contact_patch_solver.h
@@ -0,0 +1,209 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, INRIA
+ *  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 INRIA 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 Louis Montaut */
+
+#ifndef COAL_CONTACT_PATCH_SOLVER_H
+#define COAL_CONTACT_PATCH_SOLVER_H
+
+#include "coal/collision_data.h"
+#include "coal/logging.h"
+#include "coal/narrowphase/gjk.h"
+
+namespace coal {
+
+/// @brief Solver to compute contact patches, i.e. the intersection between two
+/// contact surfaces projected onto the shapes' separating plane.
+/// Otherwise said, a contact patch is simply the intersection between two
+/// support sets: the support set of shape S1 in direction `n` and the support
+/// set of shape S2 in direction `-n`, where `n` is the contact normal
+/// (satisfying the optimality conditions of GJK/EPA).
+/// @note A contact patch is **not** the support set of the Minkowski Difference
+/// in the direction of the normal.
+/// A contact patch is actually the support set of the Minkowski difference in
+/// the direction of the normal, i.e. the instersection of the shapes support
+/// sets as mentioned above.
+///
+/// TODO(louis): algo improvement:
+/// - The clipping algo is currently n1 * n2; it can be done in n1 + n2.
+struct COAL_DLLAPI ContactPatchSolver {
+  // Note: `ContactPatch` is an alias for `SupportSet`.
+  // The two can be used interchangeably.
+  using ShapeSupportData = details::ShapeSupportData;
+  using SupportSetDirection = SupportSet::PatchDirection;
+
+  /// @brief Support set function for shape si.
+  /// @param[in] shape the shape.
+  /// @param[in/out] support_set a support set of the shape. A support set is
+  /// attached to a frame. All the points of the set computed by this function
+  /// will be expressed in the local frame of the support set. The support set
+  /// is computed in the direction of the positive z-axis if its direction is
+  /// DEFAULT, negative z-axis if its direction is INVERTED.
+  /// @param[in/out] hint for the support computation of ConvexBase shapes. Gets
+  /// updated after calling the function onto ConvexBase shapes.
+  /// @param[in/out] support_data for the support computation of ConvexBase
+  /// shapes. Gets updated with visited vertices after calling the function onto
+  /// ConvexBase shapes.
+  /// @param[in] num_sampled_supports for shapes like cone or cylinders which
+  /// have smooth non-strictly convex sides (their bases are circles), we need
+  /// to know how many supports we sample from these sides. For any other shape,
+  /// this parameter is not used.
+  /// @param[in] tol the "thickness" of the support plane. Any point v which
+  /// satisfies `max_{x in shape}(x.dot(dir)) - v.dot(dir) <= tol` is tol
+  /// distant from the support plane and is added to the support set.
+  typedef void (*SupportSetFunction)(const ShapeBase* shape,
+                                     SupportSet& support_set, int& hint,
+                                     ShapeSupportData& support_data,
+                                     size_t num_sampled_supports,
+                                     CoalScalar tol);
+
+  /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors.
+  static constexpr size_t default_num_preallocated_supports = 16;
+
+  /// @brief Number of points sampled for Cone and Cylinder when the normal is
+  /// orthogonal to the shapes' basis.
+  /// See @ref ContactPatchRequest::m_num_samples_curved_shapes for more
+  /// details.
+  size_t num_samples_curved_shapes;
+
+  /// @brief Tolerance below which points are added to the shapes support sets.
+  /// See @ref ContactPatchRequest::m_patch_tolerance for more details.
+  CoalScalar patch_tolerance;
+
+  /// @brief Support set function for shape s1.
+  mutable SupportSetFunction supportFuncShape1;
+
+  /// @brief Support set function for shape s2.
+  mutable SupportSetFunction supportFuncShape2;
+
+  /// @brief Temporary data to compute the support sets on each shape.
+  mutable std::array<ShapeSupportData, 2> supports_data;
+
+  /// @brief Guess for the support sets computation.
+  mutable support_func_guess_t support_guess;
+
+  /// @brief Holder for support set of shape 1, used for internal computation.
+  /// After `computePatch` has been called, this support set is no longer valid.
+  mutable SupportSet support_set_shape1;
+
+  /// @brief Holder for support set of shape 2, used for internal computation.
+  /// After `computePatch` has been called, this support set is no longer valid.
+  mutable SupportSet support_set_shape2;
+
+  /// @brief Temporary support set used for the Sutherland-Hodgman algorithm.
+  mutable SupportSet support_set_buffer;
+
+  /// @brief Tracks which point of the Sutherland-Hodgman result have been added
+  /// to the contact patch. Only used if the post-processing step occurs, i.e.
+  /// if the result of Sutherland-Hodgman has a size bigger than
+  /// `max_patch_size`.
+  mutable std::vector<bool> added_to_patch;
+
+  /// @brief Default constructor.
+  explicit ContactPatchSolver() {
+    const size_t num_contact_patch = 1;
+    const size_t preallocated_patch_size =
+        ContactPatch::default_preallocated_size;
+    const CoalScalar patch_tolerance = 1e-3;
+    const ContactPatchRequest request(num_contact_patch,
+                                      preallocated_patch_size, patch_tolerance);
+    this->set(request);
+  }
+
+  /// @brief Construct the solver with a `ContactPatchRequest`.
+  explicit ContactPatchSolver(const ContactPatchRequest& request) {
+    this->set(request);
+  }
+
+  /// @brief Set up the solver using a `ContactPatchRequest`.
+  void set(const ContactPatchRequest& request);
+
+  /// @brief Sets the support guess used during support set computation of
+  /// shapes s1 and s2.
+  void setSupportGuess(const support_func_guess_t guess) const {
+    this->support_guess = guess;
+  }
+
+  /// @brief Main API of the solver: compute a contact patch from a contact
+  /// between shapes s1 and s2.
+  /// The contact patch is the (triple) intersection between the separating
+  /// plane passing (by `contact.pos` and supported by `contact.normal`) and the
+  /// shapes s1 and s2.
+  template <typename ShapeType1, typename ShapeType2>
+  void computePatch(const ShapeType1& s1, const Transform3s& tf1,
+                    const ShapeType2& s2, const Transform3s& tf2,
+                    const Contact& contact, ContactPatch& contact_patch) const;
+
+  /// @brief Reset the internal quantities of the solver.
+  template <typename ShapeType1, typename ShapeType2>
+  void reset(const ShapeType1& shape1, const Transform3s& tf1,
+             const ShapeType2& shape2, const Transform3s& tf2,
+             const ContactPatch& contact_patch) const;
+
+  /// @brief Retrieve result, adds a post-processing step if result has bigger
+  /// size than `this->max_patch_size`.
+  void getResult(const Contact& contact, const ContactPatch::Polygon* result,
+                 ContactPatch& contact_patch) const;
+
+  /// @return the intersecting point between line defined by ray (a, b) and
+  /// the segment [c, d].
+  /// @note we make the following hypothesis:
+  /// 1) c != d (should be when creating initial polytopes)
+  /// 2) (c, d) is not parallel to ray -> if so, we return d.
+  static Vec2s computeLineSegmentIntersection(const Vec2s& a, const Vec2s& b,
+                                              const Vec2s& c, const Vec2s& d);
+
+  /// @brief Construct support set function for shape.
+  static SupportSetFunction makeSupportSetFunction(
+      const ShapeBase* shape, ShapeSupportData& support_data);
+
+  bool operator==(const ContactPatchSolver& other) const {
+    return this->num_samples_curved_shapes == other.num_samples_curved_shapes &&
+           this->patch_tolerance == other.patch_tolerance &&
+           this->support_guess == other.support_guess &&
+           this->support_set_shape1 == other.support_set_shape1 &&
+           this->support_set_shape2 == other.support_set_shape2 &&
+           this->support_set_buffer == other.support_set_buffer &&
+           this->added_to_patch == other.added_to_patch &&
+           this->supportFuncShape1 == other.supportFuncShape1 &&
+           this->supportFuncShape2 == other.supportFuncShape2;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#include "coal/contact_patch/contact_patch_solver.hxx"
+
+#endif  // COAL_CONTACT_PATCH_SOLVER_H
diff --git a/include/coal/contact_patch/contact_patch_solver.hxx b/include/coal/contact_patch/contact_patch_solver.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..f4f97b02cfb6eacf3196e272dc3e8310da65dfb1
--- /dev/null
+++ b/include/coal/contact_patch/contact_patch_solver.hxx
@@ -0,0 +1,425 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, INRIA
+ *  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 INRIA 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 Louis Montaut */
+
+#ifndef COAL_CONTACT_PATCH_SOLVER_HXX
+#define COAL_CONTACT_PATCH_SOLVER_HXX
+
+#include "coal/data_types.h"
+#include "coal/shape/geometric_shapes_traits.h"
+
+namespace coal {
+
+// ============================================================================
+inline void ContactPatchSolver::set(const ContactPatchRequest& request) {
+  // Note: it's important for the number of pre-allocated Vec3s in
+  // `m_clipping_sets` to be larger than `request.max_size_patch`
+  // because we don't know in advance how many supports will be discarded to
+  // form the convex-hulls of the shapes supports which will serve as the
+  // input of the Sutherland-Hodgman algorithm.
+  size_t num_preallocated_supports = default_num_preallocated_supports;
+  if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) {
+    num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes();
+  }
+
+  // Used for support set computation of shape1 and for the first iterate of the
+  // Sutherland-Hodgman algo.
+  this->support_set_shape1.points().reserve(num_preallocated_supports);
+  this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
+
+  // Used for computing the next iterate of the Sutherland-Hodgman algo.
+  this->support_set_buffer.points().reserve(num_preallocated_supports);
+
+  // Used for support set computation of shape2 and acts as the "clipper" set in
+  // the Sutherland-Hodgman algo.
+  this->support_set_shape2.points().reserve(num_preallocated_supports);
+  this->support_set_shape2.direction = SupportSetDirection::INVERTED;
+
+  this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes();
+  this->patch_tolerance = request.getPatchTolerance();
+}
+
+// ============================================================================
+template <typename ShapeType1, typename ShapeType2>
+void ContactPatchSolver::computePatch(const ShapeType1& s1,
+                                      const Transform3s& tf1,
+                                      const ShapeType2& s2,
+                                      const Transform3s& tf2,
+                                      const Contact& contact,
+                                      ContactPatch& contact_patch) const {
+  // Note: `ContactPatch` is an alias for `SupportSet`.
+  // Step 1
+  constructContactPatchFrameFromContact(contact, contact_patch);
+  contact_patch.points().clear();
+  if ((bool)(shape_traits<ShapeType1>::IsStrictlyConvex) ||
+      (bool)(shape_traits<ShapeType2>::IsStrictlyConvex)) {
+    // If a shape is strictly convex, the support set in any direction is
+    // reduced to a single point. Thus, the contact point `contact.pos` is the
+    // only point belonging to the contact patch, and it has already been
+    // computed.
+    // TODO(louis): even for strictly convex shapes, we can sample the support
+    // function around the normal and return a pseudo support set. This would
+    // allow spheres and ellipsoids to have a contact surface, which does make
+    // sense in certain physics simulation cases.
+    // Do the same for strictly convex regions of non-strictly convex shapes
+    // like the ends of capsules.
+    contact_patch.addPoint(contact.pos);
+    return;
+  }
+
+  // Step 2 - Compute support set of each shape, in the direction of
+  // the contact's normal.
+  // The first shape's support set is called "current"; it will be the first
+  // iterate of the Sutherland-Hodgman algorithm. The second shape's support set
+  // is called "clipper"; it will be used to clip "current". The support set
+  // computation step computes a convex polygon; its vertices are ordered
+  // counter-clockwise. This is important as the Sutherland-Hodgman algorithm
+  // expects points to be ranked counter-clockwise.
+  this->reset(s1, tf1, s2, tf2, contact_patch);
+  assert(this->num_samples_curved_shapes > 3);
+
+  this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0],
+                          this->supports_data[0],
+                          this->num_samples_curved_shapes,
+                          this->patch_tolerance);
+
+  this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1],
+                          this->supports_data[1],
+                          this->num_samples_curved_shapes,
+                          this->patch_tolerance);
+
+  // We can immediatly return if one of the support set has only
+  // one point.
+  if (this->support_set_shape1.size() <= 1 ||
+      this->support_set_shape2.size() <= 1) {
+    contact_patch.addPoint(contact.pos);
+    return;
+  }
+
+  // `eps` is be used to check strict positivity of determinants.
+  const CoalScalar eps = Eigen::NumTraits<CoalScalar>::dummy_precision();
+  using Polygon = SupportSet::Polygon;
+
+  if ((this->support_set_shape1.size() == 2) &&
+      (this->support_set_shape2.size() == 2)) {
+    // Segment-Segment case
+    // We compute the determinant; if it is non-zero, the intersection
+    // has already been computed: it's `Contact::pos`.
+    const Polygon& pts1 = this->support_set_shape1.points();
+    const Vec2s& a = pts1[0];
+    const Vec2s& b = pts1[1];
+
+    const Polygon& pts2 = this->support_set_shape2.points();
+    const Vec2s& c = pts2[0];
+    const Vec2s& d = pts2[1];
+
+    const CoalScalar det =
+        (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0));
+    if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) ||
+        ((b - a).squaredNorm() < eps)) {
+      contact_patch.addPoint(contact.pos);
+      return;
+    }
+
+    const Vec2s cd = (d - c);
+    const CoalScalar l = cd.squaredNorm();
+    Polygon& patch = contact_patch.points();
+
+    // Project a onto [c, d]
+    CoalScalar t1 = (a - c).dot(cd);
+    t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l));
+    const Vec2s p1 = c + t1 * cd;
+    patch.emplace_back(p1);
+
+    // Project b onto [c, d]
+    CoalScalar t2 = (b - c).dot(cd);
+    t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l));
+    const Vec2s p2 = c + t2 * cd;
+    if ((p1 - p2).squaredNorm() >= eps) {
+      patch.emplace_back(p2);
+    }
+    return;
+  }
+
+  //
+  // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the
+  // "current" polygon. The resulting intersection is the contact patch of the
+  // contact between s1 and s2. "clipper" and "current" are the support sets of
+  // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to
+  // shape1 and current to shape2, depending on which case we are). Currently,
+  // to clip one polygon with the other, we use the Sutherland-Hodgman
+  // algorithm:
+  // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm
+  // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using
+  // another polygon of size >=3. However, it can be easily extended to handle
+  // the segment-polygon case.
+  //
+  // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 +
+  // n2 where n1 and n2 are the sizes of the first and second polygon.
+  const size_t max_result_size =
+      this->support_set_shape1.size() + this->support_set_shape2.size();
+  if (this->added_to_patch.size() < max_result_size) {
+    this->added_to_patch.assign(max_result_size, false);
+  }
+
+  const Polygon* clipper_ptr = nullptr;
+  Polygon* current_ptr = nullptr;
+  Polygon* previous_ptr = &(this->support_set_buffer.points());
+
+  // Let the clipper set be the one with the most vertices, to make sure it is
+  // at least a triangle.
+  if (this->support_set_shape1.size() < this->support_set_shape2.size()) {
+    current_ptr = &(this->support_set_shape1.points());
+    clipper_ptr = &(this->support_set_shape2.points());
+  } else {
+    current_ptr = &(this->support_set_shape2.points());
+    clipper_ptr = &(this->support_set_shape1.points());
+  }
+
+  const Polygon& clipper = *(clipper_ptr);
+  const size_t clipper_size = clipper.size();
+  for (size_t i = 0; i < clipper_size; ++i) {
+    // Swap `current` and `previous`.
+    // `previous` tracks the last iteration of the algorithm; `current` is
+    // filled by clipping `current` using `clipper`.
+    Polygon* tmp_ptr = previous_ptr;
+    previous_ptr = current_ptr;
+    current_ptr = tmp_ptr;
+
+    const Polygon& previous = *(previous_ptr);
+    Polygon& current = *(current_ptr);
+    current.clear();
+
+    const Vec2s& a = clipper[i];
+    const Vec2s& b = clipper[(i + 1) % clipper_size];
+    const Vec2s ab = b - a;
+
+    if (previous.size() == 2) {
+      //
+      // Segment-Polygon case
+      //
+      const Vec2s& p1 = previous[0];
+      const Vec2s& p2 = previous[1];
+
+      const Vec2s ap1 = p1 - a;
+      const Vec2s ap2 = p2 - a;
+
+      const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
+      const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
+
+      if (det1 < 0 && det2 < 0) {
+        // Both p1 and p2 are outside the clipping polygon, i.e. there is no
+        // intersection. The algorithm can stop.
+        break;
+      }
+
+      if (det1 >= 0 && det2 >= 0) {
+        // Both p1 and p2 are inside the clipping polygon, there is nothing to
+        // do; move to the next iteration.
+        current = previous;
+        continue;
+      }
+
+      // Compute the intersection between the line (a, b) and the segment
+      // [p1, p2].
+      if (det1 >= 0) {
+        if (det1 > eps) {
+          const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
+          current.emplace_back(p1);
+          current.emplace_back(p);
+          continue;
+        } else {
+          // p1 is the only point of current which is also a point of the
+          // clipper. We can exit.
+          current.emplace_back(p1);
+          break;
+        }
+      } else {
+        if (det2 > eps) {
+          const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
+          current.emplace_back(p2);
+          current.emplace_back(p);
+          continue;
+        } else {
+          // p2 is the only point of current which is also a point of the
+          // clipper. We can exit.
+          current.emplace_back(p2);
+          break;
+        }
+      }
+    } else {
+      //
+      // Polygon-Polygon case.
+      //
+      std::fill(this->added_to_patch.begin(),  //
+                this->added_to_patch.end(),    //
+                false);
+
+      const size_t previous_size = previous.size();
+      for (size_t j = 0; j < previous_size; ++j) {
+        const Vec2s& p1 = previous[j];
+        const Vec2s& p2 = previous[(j + 1) % previous_size];
+
+        const Vec2s ap1 = p1 - a;
+        const Vec2s ap2 = p2 - a;
+
+        const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
+        const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
+
+        if (det1 < 0 && det2 < 0) {
+          // No intersection. Continue to next segment of previous.
+          continue;
+        }
+
+        if (det1 >= 0 && det2 >= 0) {
+          // Both p1 and p2 are inside the clipping polygon, add p1 to current
+          // (only if it has not already been added).
+          if (!this->added_to_patch[j]) {
+            current.emplace_back(p1);
+            this->added_to_patch[j] = true;
+          }
+          // Continue to next segment of previous.
+          continue;
+        }
+
+        if (det1 >= 0) {
+          if (det1 > eps) {
+            if (!this->added_to_patch[j]) {
+              current.emplace_back(p1);
+              this->added_to_patch[j] = true;
+            }
+            const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
+            current.emplace_back(p);
+          } else {
+            // a, b and p1 are colinear; we add only p1.
+            if (!this->added_to_patch[j]) {
+              current.emplace_back(p1);
+              this->added_to_patch[j] = true;
+            }
+          }
+        } else {
+          if (det2 > eps) {
+            const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
+            current.emplace_back(p);
+          } else {
+            if (!this->added_to_patch[(j + 1) % previous.size()]) {
+              current.emplace_back(p2);
+              this->added_to_patch[(j + 1) % previous.size()] = true;
+            }
+          }
+        }
+      }
+    }
+    //
+    // End of iteration i of Sutherland-Hodgman.
+    if (current.size() <= 1) {
+      // No intersection or one point found, the algo can early stop.
+      break;
+    }
+  }
+
+  // Transfer the result of the Sutherland-Hodgman algorithm to the contact
+  // patch.
+  this->getResult(contact, current_ptr, contact_patch);
+}
+
+// ============================================================================
+inline void ContactPatchSolver::getResult(
+    const Contact& contact, const ContactPatch::Polygon* result_ptr,
+    ContactPatch& contact_patch) const {
+  if (result_ptr->size() <= 1) {
+    contact_patch.addPoint(contact.pos);
+    return;
+  }
+
+  const ContactPatch::Polygon& result = *(result_ptr);
+  ContactPatch::Polygon& patch = contact_patch.points();
+  patch = result;
+}
+
+// ============================================================================
+template <typename ShapeType1, typename ShapeType2>
+inline void ContactPatchSolver::reset(const ShapeType1& shape1,
+                                      const Transform3s& tf1,
+                                      const ShapeType2& shape2,
+                                      const Transform3s& tf2,
+                                      const ContactPatch& contact_patch) const {
+  // Reset internal quantities
+  this->support_set_shape1.clear();
+  this->support_set_shape2.clear();
+  this->support_set_buffer.clear();
+
+  // Get the support function of each shape
+  const Transform3s& tfc = contact_patch.tf;
+
+  this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
+  // Set the reference frame of the support set of the first shape to be the
+  // local frame of shape 1.
+  Transform3s& tf1c = this->support_set_shape1.tf;
+  tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation();
+  tf1c.translation().noalias() =
+      tf1.rotation().transpose() * (tfc.translation() - tf1.translation());
+  this->supportFuncShape1 =
+      this->makeSupportSetFunction(&shape1, this->supports_data[0]);
+
+  this->support_set_shape2.direction = SupportSetDirection::INVERTED;
+  // Set the reference frame of the support set of the second shape to be the
+  // local frame of shape 2.
+  Transform3s& tf2c = this->support_set_shape2.tf;
+  tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation();
+  tf2c.translation().noalias() =
+      tf2.rotation().transpose() * (tfc.translation() - tf2.translation());
+  this->supportFuncShape2 =
+      this->makeSupportSetFunction(&shape2, this->supports_data[1]);
+}
+
+// ==========================================================================
+inline Vec2s ContactPatchSolver::computeLineSegmentIntersection(
+    const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) {
+  const Vec2s ab = b - a;
+  const Vec2s n(-ab(1), ab(0));
+  const CoalScalar denominator = n.dot(c - d);
+  if (std::abs(denominator) < std::numeric_limits<double>::epsilon()) {
+    return d;
+  }
+  const CoalScalar nominator = n.dot(a - d);
+  CoalScalar alpha = nominator / denominator;
+  alpha = std::min<double>(1.0, std::max<CoalScalar>(0.0, alpha));
+  return alpha * c + (1 - alpha) * d;
+}
+
+}  // namespace coal
+
+#endif  // COAL_CONTACT_PATCH_SOLVER_HXX
diff --git a/include/coal/contact_patch_func_matrix.h b/include/coal/contact_patch_func_matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..f95d73c4eb6341ab7be3cffe2608c1cdc708a032
--- /dev/null
+++ b/include/coal/contact_patch_func_matrix.h
@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, INRIA
+ *  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 INRIA 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 Louis Montaut */
+
+#ifndef COAL_CONTACT_PATCH_FUNC_MATRIX_H
+#define COAL_CONTACT_PATCH_FUNC_MATRIX_H
+
+#include "coal/collision_data.h"
+#include "coal/contact_patch/contact_patch_solver.h"
+#include "coal/narrowphase/narrowphase.h"
+
+namespace coal {
+
+/// @brief The contact patch matrix stores the functions for contact patches
+/// computation between different types of objects and provides a uniform call
+/// interface
+struct COAL_DLLAPI ContactPatchFunctionMatrix {
+  /// @brief the uniform call interface for computing contact patches: we need
+  /// know
+  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
+  ///    and tf2;
+  /// 2. the collision result that generated contact patches candidates
+  ///    (`coal::Contact`), from which contact patches will be expanded;
+  /// 3. the solver for computation of contact patches;
+  /// 4. the request setting for contact patches (e.g. maximum amount of
+  ///    patches, patch tolerance etc.)
+  /// 5. the structure to return contact patches
+  ///    (`coal::ContactPatchResult`).
+  ///
+  /// Note: we pass a GJKSolver, because it allows to reuse internal computation
+  /// that was made during the narrow phase. It also allows to experiment with
+  /// different ways to compute contact patches. We could, for example, perturb
+  /// tf1 and tf2 and make multiple calls to the GJKSolver (although this is not
+  /// the approach done by default).
+  typedef void (*ContactPatchFunc)(const CollisionGeometry* o1,
+                                   const Transform3s& tf1,
+                                   const CollisionGeometry* o2,
+                                   const Transform3s& tf2,
+                                   const CollisionResult& collision_result,
+                                   const ContactPatchSolver* csolver,
+                                   const ContactPatchRequest& request,
+                                   ContactPatchResult& result);
+
+  /// @brief Each item in the contact patch matrix is a function to handle
+  /// contact patch computation between objects of type1 and type2.
+  ContactPatchFunc contact_patch_matrix[NODE_COUNT][NODE_COUNT];
+
+  ContactPatchFunctionMatrix();
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/data_types.h b/include/coal/data_types.h
new file mode 100644
index 0000000000000000000000000000000000000000..11d34a98178ec57347216a8af86d1333f8ab4d95
--- /dev/null
+++ b/include/coal/data_types.h
@@ -0,0 +1,197 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2023, Inria
+ *  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 COAL_DATA_TYPES_H
+#define COAL_DATA_TYPES_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "coal/config.hh"
+
+#ifdef COAL_HAS_OCTOMAP
+#define OCTOMAP_VERSION_AT_LEAST(x, y, z) \
+  (OCTOMAP_MAJOR_VERSION > x ||           \
+   (OCTOMAP_MAJOR_VERSION >= x &&         \
+    (OCTOMAP_MINOR_VERSION > y ||         \
+     (OCTOMAP_MINOR_VERSION >= y && OCTOMAP_PATCH_VERSION >= z))))
+
+#define OCTOMAP_VERSION_AT_MOST(x, y, z) \
+  (OCTOMAP_MAJOR_VERSION < x ||          \
+   (OCTOMAP_MAJOR_VERSION <= x &&        \
+    (OCTOMAP_MINOR_VERSION < y ||        \
+     (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z))))
+#endif  // COAL_HAS_OCTOMAP
+
+namespace coal {
+#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL
+// We keep the FCL_REAL typedef and the Vec[..]f typedefs for backward
+// compatibility.
+typedef double FCL_REAL;
+typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
+typedef Eigen::Matrix<FCL_REAL, 2, 1> Vec2f;
+typedef Eigen::Matrix<FCL_REAL, 6, 1> Vec6f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> VecXf;
+typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3, Eigen::RowMajor> Matrixx3f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 2, Eigen::RowMajor> Matrixx2f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
+#endif
+
+typedef double CoalScalar;
+typedef Eigen::Matrix<CoalScalar, 3, 1> Vec3s;
+typedef Eigen::Matrix<CoalScalar, 2, 1> Vec2s;
+typedef Eigen::Matrix<CoalScalar, 6, 1> Vec6s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 1> VecXs;
+typedef Eigen::Matrix<CoalScalar, 3, 3> Matrix3s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 2, Eigen::RowMajor> MatrixX2s;
+typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor>
+    Matrixx3i;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+typedef Eigen::Vector2i support_func_guess_t;
+
+/// @brief Initial guess to use for the GJK algorithm
+/// DefaultGuess: Vec3s(1, 0, 0)
+/// CachedGuess: previous vector found by GJK or guess cached by the user
+/// BoundingVolumeGuess: guess using the centers of the shapes' AABB
+/// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called
+/// on the two shapes.
+enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess };
+
+/// @brief Variant to use for the GJK algorithm
+enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration };
+
+/// @brief Which convergence criterion is used to stop the algorithm (when the
+/// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and
+/// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe
+/// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG.
+enum GJKConvergenceCriterion { Default, DualityGap, Hybrid };
+
+/// @brief Wether the convergence criterion is scaled on the norm of the
+/// solution or not
+enum GJKConvergenceCriterionType { Relative, Absolute };
+
+/// @brief Triangle with 3 indices for points
+class COAL_DLLAPI Triangle {
+ public:
+  typedef std::size_t index_type;
+  typedef int size_type;
+
+  /// @brief Default constructor
+  Triangle() {}
+
+  /// @brief Create a triangle with given vertex indices
+  Triangle(index_type p1, index_type p2, index_type p3) { set(p1, p2, p3); }
+
+  /// @brief Set the vertex indices of the triangle
+  inline void set(index_type p1, index_type p2, index_type p3) {
+    vids[0] = p1;
+    vids[1] = p2;
+    vids[2] = p3;
+  }
+
+  /// @brief Access the triangle index
+  inline index_type operator[](index_type i) const { return vids[i]; }
+
+  inline index_type& operator[](index_type i) { return vids[i]; }
+
+  static inline size_type size() { return 3; }
+
+  bool operator==(const Triangle& other) const {
+    return vids[0] == other.vids[0] && vids[1] == other.vids[1] &&
+           vids[2] == other.vids[2];
+  }
+
+  bool operator!=(const Triangle& other) const { return !(*this == other); }
+
+  bool isValid() const {
+    return vids[0] != (std::numeric_limits<index_type>::max)() &&
+           vids[1] != (std::numeric_limits<index_type>::max)() &&
+           vids[2] != (std::numeric_limits<index_type>::max)();
+  }
+
+ private:
+  /// @brief indices for each vertex of triangle
+  index_type vids[3] = {(std::numeric_limits<index_type>::max)(),
+                        (std::numeric_limits<index_type>::max)(),
+                        (std::numeric_limits<index_type>::max)()};
+};
+
+/// @brief Quadrilateral with 4 indices for points
+struct COAL_DLLAPI Quadrilateral {
+  typedef std::size_t index_type;
+  typedef int size_type;
+
+  Quadrilateral() {}
+
+  Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3) {
+    set(p0, p1, p2, p3);
+  }
+
+  /// @brief Set the vertex indices of the quadrilateral
+  inline void set(index_type p0, index_type p1, index_type p2, index_type p3) {
+    vids[0] = p0;
+    vids[1] = p1;
+    vids[2] = p2;
+    vids[3] = p3;
+  }
+
+  /// @access the quadrilateral index
+  inline index_type operator[](index_type i) const { return vids[i]; }
+
+  inline index_type& operator[](index_type i) { return vids[i]; }
+
+  static inline size_type size() { return 4; }
+
+  bool operator==(const Quadrilateral& other) const {
+    return vids[0] == other.vids[0] && vids[1] == other.vids[1] &&
+           vids[2] == other.vids[2] && vids[3] == other.vids[3];
+  }
+
+  bool operator!=(const Quadrilateral& other) const {
+    return !(*this == other);
+  }
+
+ private:
+  index_type vids[4];
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/distance.h b/include/coal/distance.h
new file mode 100644
index 0000000000000000000000000000000000000000..7dd4a3e7d544a23f96332d84051a095a258f1d35
--- /dev/null
+++ b/include/coal/distance.h
@@ -0,0 +1,115 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_DISTANCE_H
+#define COAL_DISTANCE_H
+
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/distance_func_matrix.h"
+#include "coal/timings.h"
+
+namespace coal {
+
+/// @brief Main distance interface: given two collision objects, and the
+/// requirements for contacts, including whether return the nearest points, this
+/// function performs the distance between them. Return value is the minimum
+/// distance generated between the two objects.
+COAL_DLLAPI CoalScalar distance(const CollisionObject* o1,
+                                const CollisionObject* o2,
+                                const DistanceRequest& request,
+                                DistanceResult& result);
+
+/// @copydoc distance(const CollisionObject*, const CollisionObject*, const
+/// DistanceRequest&, DistanceResult&)
+COAL_DLLAPI CoalScalar distance(const CollisionGeometry* o1,
+                                const Transform3s& tf1,
+                                const CollisionGeometry* o2,
+                                const Transform3s& tf2,
+                                const DistanceRequest& request,
+                                DistanceResult& result);
+
+/// This class reduces the cost of identifying the geometry pair.
+/// This is mostly useful for repeated shape-shape queries.
+///
+/// \code
+///   ComputeDistance calc_distance (o1, o2);
+///   CoalScalar distance = calc_distance(tf1, tf2, request, result);
+/// \endcode
+class COAL_DLLAPI ComputeDistance {
+ public:
+  ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2);
+
+  CoalScalar operator()(const Transform3s& tf1, const Transform3s& tf2,
+                        const DistanceRequest& request,
+                        DistanceResult& result) const;
+
+  bool operator==(const ComputeDistance& other) const {
+    return o1 == other.o1 && o2 == other.o2 && swap_geoms == other.swap_geoms &&
+           solver == other.solver && func == other.func;
+  }
+
+  bool operator!=(const ComputeDistance& other) const {
+    return !(*this == other);
+  }
+
+  virtual ~ComputeDistance() {};
+
+ protected:
+  // These pointers are made mutable to let the derived classes to update
+  // their values when updating the collision geometry (e.g. creating a new
+  // one). This feature should be used carefully to avoid any mis usage (e.g,
+  // changing the type of the collision geometry should be avoided).
+  mutable const CollisionGeometry* o1;
+  mutable const CollisionGeometry* o2;
+
+  mutable GJKSolver solver;
+
+  DistanceFunctionMatrix::DistanceFunc func;
+  bool swap_geoms;
+
+  virtual CoalScalar run(const Transform3s& tf1, const Transform3s& tf2,
+                         const DistanceRequest& request,
+                         DistanceResult& result) const;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/distance_func_matrix.h b/include/coal/distance_func_matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..a37e1df51cba80f5be2f1ada872a8af8e6c574d6
--- /dev/null
+++ b/include/coal/distance_func_matrix.h
@@ -0,0 +1,74 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_DISTANCE_FUNC_MATRIX_H
+#define COAL_DISTANCE_FUNC_MATRIX_H
+
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/narrowphase/narrowphase.h"
+
+namespace coal {
+
+/// @brief distance matrix stores the functions for distance between different
+/// types of objects and provides a uniform call interface
+struct COAL_DLLAPI DistanceFunctionMatrix {
+  /// @brief the uniform call interface for distance: for distance, we need know
+  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
+  /// and tf2;
+  /// 2. the solver for narrow phase collision, this is for distance computation
+  /// between geometric shapes;
+  /// 3. the request setting for distance (e.g., whether need to return nearest
+  /// points);
+  typedef CoalScalar (*DistanceFunc)(const CollisionGeometry* o1,
+                                     const Transform3s& tf1,
+                                     const CollisionGeometry* o2,
+                                     const Transform3s& tf2,
+                                     const GJKSolver* nsolver,
+                                     const DistanceRequest& request,
+                                     DistanceResult& result);
+
+  /// @brief each item in the distance matrix is a function to handle distance
+  /// between objects of type1 and type2
+  DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
+
+  DistanceFunctionMatrix();
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/hpp/fcl/doc.hh b/include/coal/doc.hh
similarity index 79%
rename from include/hpp/fcl/doc.hh
rename to include/coal/doc.hh
index 4d645bf23522706bb16a508427708d2cf7242321..7b86b9d730e2a823db4620ec7c791592e7b21be5 100644
--- a/include/hpp/fcl/doc.hh
+++ b/include/coal/doc.hh
@@ -32,30 +32,29 @@
 //  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 //  POSSIBILITY OF SUCH DAMAGE.
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 /// \mainpage
-/// \anchor hpp_fcl_documentation
+/// \anchor coal_documentation
 ///
-/// \section hpp_fcl_introduction Introduction
+/// \section coal_introduction Introduction
 ///
-/// hpp-fcl is a modified version the FCL libraries.
+/// Coal is a modified version the FCL libraries.
 ///
 /// It is a library for collision detection and distance computation between
 /// various types of geometric shapes reprensented either by
-/// \li basic shapes (hpp::fcl::ShapeBase) like box, sphere, cylinders, ...
-/// \li or by bounding volume hierarchies of various types (hpp::fcl::BVHModel)
+/// \li basic shapes (coal::ShapeBase) like box, sphere, cylinders, ...
+/// \li or by bounding volume hierarchies of various types (coal::BVHModel)
 ///
-/// \par Using hpp-fcl
+/// \par Using Coal
 ///
 /// The main entry points to the library are functions
-/// \li hpp::fcl::collide(const CollisionObject*, const CollisionObject*, const
-/// CollisionRequest&, CollisionResult&) \li hpp::fcl::distance(const
+/// \li coal::collide(const CollisionObject*, const CollisionObject*, const
+/// CollisionRequest&, CollisionResult&) \li coal::distance(const
 /// CollisionObject*, const CollisionObject*, const DistanceRequest&,
 /// DistanceResult&)
 ///
-/// \section hpp_fcl_collision_and_distance_lower_bound_computation Collision
+/// \section coal_collision_and_distance_lower_bound_computation Collision
 /// detection and distance lower bound
 ///
 /// Collision queries can return a distance lower bound between the two objects,
@@ -72,9 +71,8 @@ namespace fcl {
 /// is only guaranted that it will be inferior to
 /// <em>distance - security_margin</em> and superior to \em break_distance.
 /// \note If CollisionRequest::security_margin is set to -inf, no collision test
-/// is performed by function hpp::fcl::collide or class
-/// hpp::fcl::ComputeCollision and the objects are considered as not
+/// is performed by function coal::collide or class
+/// coal::ComputeCollision and the objects are considered as not
 /// colliding.
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh
new file mode 100644
index 0000000000000000000000000000000000000000..c2408880a9b11f8271a6b1444395792ec6b1d417
--- /dev/null
+++ b/include/coal/fwd.hh
@@ -0,0 +1,158 @@
+//
+// Software License Agreement (BSD License)
+//
+//  Copyright (c) 2014, CNRS-LAAS
+//  Copyright (c) 2022-2024, Inria
+//  Author: Florent Lamiraux
+//
+//  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 CNRS-LAAS 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.
+//
+
+#ifndef COAL_FWD_HH
+#define COAL_FWD_HH
+
+#include <cassert>
+#include <memory>
+#include <sstream>
+#include <stdexcept>
+
+#include "coal/config.hh"
+#include "coal/deprecated.hh"
+#include "coal/warning.hh"
+
+#if _WIN32
+#define COAL_PRETTY_FUNCTION __FUNCSIG__
+#else
+#define COAL_PRETTY_FUNCTION __PRETTY_FUNCTION__
+#endif
+
+#define COAL_UNUSED_VARIABLE(var) (void)(var)
+
+#ifdef NDEBUG
+#define COAL_ONLY_USED_FOR_DEBUG(var) COAL_UNUSED_VARIABLE(var)
+#else
+#define COAL_ONLY_USED_FOR_DEBUG(var)
+#endif
+
+#define COAL_THROW_PRETTY(message, exception)              \
+  {                                                        \
+    std::stringstream ss;                                  \
+    ss << "From file: " << __FILE__ << "\n";               \
+    ss << "in function: " << COAL_PRETTY_FUNCTION << "\n"; \
+    ss << "at line: " << __LINE__ << "\n";                 \
+    ss << "message: " << message << "\n";                  \
+    throw exception(ss.str());                             \
+  }
+
+#ifdef COAL_TURN_ASSERT_INTO_EXCEPTION
+#define COAL_ASSERT(check, message, exception) \
+  do {                                         \
+    if (!(check)) {                            \
+      COAL_THROW_PRETTY(message, exception);   \
+    }                                          \
+  } while (0)
+#else
+#define COAL_ASSERT(check, message, exception) \
+  {                                            \
+    COAL_UNUSED_VARIABLE(exception(message));  \
+    assert((check) && message);                \
+  }
+#endif
+
+#if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
+#define COAL_WITH_CXX11_SUPPORT
+#endif
+
+#if defined(__GNUC__) || defined(__clang__)
+#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push")
+#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
+  _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"")
+
+// GCC version 4.6 and higher supports -Wmaybe-uninitialized
+#if (defined(__GNUC__) && \
+     ((__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)))
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"")
+// Use __has_warning with clang. Clang introduced it in 2024 (3.5+)
+#elif (defined(__clang__) && defined(__has_warning) && \
+       __has_warning("-Wmaybe-uninitialized"))
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"")
+#else
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
+#endif
+#elif defined(WIN32)
+#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)")
+#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
+  _Pragma("warning(disable : 4996)")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("warning(disable : 4700)")
+#else
+#define COAL_COMPILER_DIAGNOSTIC_PUSH
+#define COAL_COMPILER_DIAGNOSTIC_POP
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
+#endif  // __GNUC__
+
+namespace coal {
+using std::dynamic_pointer_cast;
+using std::make_shared;
+using std::shared_ptr;
+
+class CollisionObject;
+typedef shared_ptr<CollisionObject> CollisionObjectPtr_t;
+typedef shared_ptr<const CollisionObject> CollisionObjectConstPtr_t;
+class CollisionGeometry;
+typedef shared_ptr<CollisionGeometry> CollisionGeometryPtr_t;
+typedef shared_ptr<const CollisionGeometry> CollisionGeometryConstPtr_t;
+class Transform3s;
+
+class AABB;
+
+class BVHModelBase;
+typedef shared_ptr<BVHModelBase> BVHModelPtr_t;
+
+class OcTree;
+typedef shared_ptr<OcTree> OcTreePtr_t;
+typedef shared_ptr<const OcTree> OcTreeConstPtr_t;
+}  // namespace coal
+
+#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL
+namespace hpp {
+namespace fcl {
+using namespace coal;
+using Transform3f = Transform3s;  // For backward compatibility
+}  // namespace fcl
+}  // namespace hpp
+#endif
+
+#endif  // COAL_FWD_HH
diff --git a/include/coal/hfield.h b/include/coal/hfield.h
new file mode 100644
index 0000000000000000000000000000000000000000..9949296ed5a97b0436ba9a5e7c6acc3e39b9891c
--- /dev/null
+++ b/include/coal/hfield.h
@@ -0,0 +1,543 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021-2024, INRIA
+ *  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 Justin Carpentier */
+
+#ifndef COAL_HEIGHT_FIELD_H
+#define COAL_HEIGHT_FIELD_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+#include "coal/collision_object.h"
+#include "coal/BV/BV_node.h"
+#include "coal/BVH/BVH_internal.h"
+
+#include <vector>
+
+namespace coal {
+
+/// @addtogroup Construction_Of_HeightField
+/// @{
+
+struct COAL_DLLAPI HFNodeBase {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum class FaceOrientation {
+    TOP = 1,
+    BOTTOM = 1,
+    NORTH = 2,
+    EAST = 4,
+    SOUTH = 8,
+    WEST = 16
+  };
+
+  /// @brief An index for first child node or primitive
+  /// If the value is positive, it is the index of the first child bv node
+  /// If the value is negative, it is -(primitive index + 1)
+  /// Zero is not used.
+  size_t first_child;
+
+  Eigen::DenseIndex x_id, x_size;
+  Eigen::DenseIndex y_id, y_size;
+
+  CoalScalar max_height;
+  int contact_active_faces;
+
+  /// @brief Default constructor
+  HFNodeBase()
+      : first_child(0),
+        x_id(-1),
+        x_size(0),
+        y_id(-1),
+        y_size(0),
+        max_height(std::numeric_limits<CoalScalar>::lowest()),
+        contact_active_faces(0) {}
+
+  /// @brief Comparison operator
+  bool operator==(const HFNodeBase& other) const {
+    return first_child == other.first_child && x_id == other.x_id &&
+           x_size == other.x_size && y_id == other.y_id &&
+           y_size == other.y_size && max_height == other.max_height &&
+           contact_active_faces == other.contact_active_faces;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const HFNodeBase& other) const { return !(*this == other); }
+
+  /// @brief Whether current node is a leaf node (i.e. contains a primitive
+  /// index)
+  inline bool isLeaf() const { return x_size == 1 && y_size == 1; }
+
+  /// @brief Return the index of the first child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline size_t leftChild() const { return first_child; }
+
+  /// @brief Return the index of the second child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline size_t rightChild() const { return first_child + 1; }
+
+  inline Eigen::Vector2i leftChildIndexes() const {
+    return Eigen::Vector2i(x_id, y_id);
+  }
+  inline Eigen::Vector2i rightChildIndexes() const {
+    return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2);
+  }
+};
+
+inline HFNodeBase::FaceOrientation operator&(HFNodeBase::FaceOrientation a,
+                                             HFNodeBase::FaceOrientation b) {
+  return HFNodeBase::FaceOrientation(int(a) & int(b));
+}
+
+inline int operator&(int a, HFNodeBase::FaceOrientation b) {
+  return a & int(b);
+}
+
+template <typename BV>
+struct COAL_DLLAPI HFNode : public HFNodeBase {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef HFNodeBase Base;
+
+  /// @brief bounding volume storing the geometry
+  BV bv;
+
+  /// @brief Equality operator
+  bool operator==(const HFNode& other) const {
+    return Base::operator==(other) && bv == other.bv;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const HFNode& other) const { return !(*this == other); }
+
+  /// @brief Check whether two BVNode collide
+  bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }
+  /// @brief Check whether two BVNode collide
+  bool overlap(const HFNode& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const {
+    return bv.overlap(other.bv, request, sqrDistLowerBound);
+  }
+
+  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
+  /// the underlying BV supports distance, return the nearest points.
+  CoalScalar distance(const HFNode& other, Vec3s* P1 = NULL,
+                      Vec3s* P2 = NULL) const {
+    return bv.distance(other.bv, P1, P2);
+  }
+
+  /// @brief Access to the center of the BV
+  Vec3s getCenter() const { return bv.center(); }
+
+  /// @brief Access to the orientation of the BV
+  coal::Matrix3s::IdentityReturnType getOrientation() const {
+    return Matrix3s::Identity();
+  }
+
+  virtual ~HFNode() {}
+};
+
+namespace details {
+
+template <typename BV>
+struct UpdateBoundingVolume {
+  static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) {
+    AABB bv_aabb(pointA, pointB);
+    //      AABB bv_aabb;
+    //      bv_aabb.update(pointA,pointB);
+    convertBV(bv_aabb, bv);
+  }
+};
+
+template <>
+struct UpdateBoundingVolume<AABB> {
+  static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) {
+    AABB bv_aabb(pointA, pointB);
+    convertBV(bv_aabb, bv);
+    //      bv.update(pointA,pointB);
+  }
+};
+
+}  // namespace details
+
+/// @brief Data structure depicting a height field given by the base grid
+/// dimensions and the elevation along the grid. \tparam BV one of the bounding
+/// volume class in \ref Bounding_Volume.
+///
+/// An height field is defined by its base dimensions along the X and Y axes and
+/// a set ofpoints defined by their altitude, regularly dispatched on the grid.
+/// The height field is centered at the origin and the corners of the geometry
+/// correspond to the following coordinates [± x_dim/2; ± y_dim/2].
+template <typename BV>
+class COAL_DLLAPI HeightField : public CollisionGeometry {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef CollisionGeometry Base;
+
+  typedef HFNode<BV> Node;
+  typedef std::vector<Node, Eigen::aligned_allocator<Node>> BVS;
+
+  /// @brief Constructing an empty HeightField
+  HeightField()
+      : CollisionGeometry(),
+        min_height((std::numeric_limits<CoalScalar>::min)()),
+        max_height((std::numeric_limits<CoalScalar>::max)()) {}
+
+  /// @brief Constructing an HeightField from its base dimensions and the set of
+  /// heights points.
+  ///        The granularity of the height field along X and Y direction is
+  ///        extraded from the Z grid.
+  ///
+  /// \param[in] x_dim Dimension along the X axis
+  /// \param[in] y_dim Dimension along the Y axis
+  /// \param[in] heights Matrix containing the altitude of each point compositng
+  /// the height field
+  /// \param[in] min_height Minimal height of the height field
+  ///
+  HeightField(const CoalScalar x_dim, const CoalScalar y_dim,
+              const MatrixXs& heights,
+              const CoalScalar min_height = (CoalScalar)0)
+      : CollisionGeometry() {
+    init(x_dim, y_dim, heights, min_height);
+  }
+
+  /// @brief Copy contructor from another HeightField
+  ///
+  /// \param[in] other to copy.
+  ///
+  HeightField(const HeightField& other)
+      : CollisionGeometry(other),
+        x_dim(other.x_dim),
+        y_dim(other.y_dim),
+        heights(other.heights),
+        min_height(other.min_height),
+        max_height(other.max_height),
+        x_grid(other.x_grid),
+        y_grid(other.y_grid),
+        bvs(other.bvs),
+        num_bvs(other.num_bvs) {}
+
+  /// @brief Returns a const reference of the grid along the X direction.
+  const VecXs& getXGrid() const { return x_grid; }
+  /// @brief Returns a const reference of the grid along the Y direction.
+  const VecXs& getYGrid() const { return y_grid; }
+
+  /// @brief Returns a const reference of the heights
+  const MatrixXs& getHeights() const { return heights; }
+
+  /// @brief Returns the dimension of the Height Field along the X direction.
+  CoalScalar getXDim() const { return x_dim; }
+  /// @brief Returns the dimension of the Height Field along the Y direction.
+  CoalScalar getYDim() const { return y_dim; }
+
+  /// @brief Returns the minimal height value of the Height Field.
+  CoalScalar getMinHeight() const { return min_height; }
+  /// @brief Returns the maximal height value of the Height Field.
+  CoalScalar getMaxHeight() const { return max_height; }
+
+  virtual HeightField<BV>* clone() const { return new HeightField(*this); }
+
+  const BVS& getNodes() const { return bvs; }
+
+  /// @brief deconstruction, delete mesh data related.
+  virtual ~HeightField() {}
+
+  /// @brief Compute the AABB for the HeightField, used for broad-phase
+  /// collision
+  void computeLocalAABB() {
+    const Vec3s A(x_grid[0], y_grid[0], min_height);
+    const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
+                  max_height);
+    const AABB aabb_(A, B);
+
+    aabb_radius = (A - B).norm() / 2.;
+    aabb_local = aabb_;
+    aabb_center = aabb_.center();
+  }
+
+  /// @brief Update Height Field height
+  void updateHeights(const MatrixXs& new_heights) {
+    if (new_heights.rows() != heights.rows() ||
+        new_heights.cols() != heights.cols())
+      COAL_THROW_PRETTY(
+          "The matrix containing the new heights values does not have the same "
+          "matrix size as the original one.\n"
+          "\tinput values - rows: "
+              << new_heights.rows() << " - cols: " << new_heights.cols() << "\n"
+              << "\texpected values - rows: " << heights.rows()
+              << " - cols: " << heights.cols() << "\n",
+          std::invalid_argument);
+
+    heights = new_heights.cwiseMax(min_height);
+    this->max_height = recursiveUpdateHeight(0);
+    assert(this->max_height == heights.maxCoeff());
+  }
+
+ protected:
+  void init(const CoalScalar x_dim, const CoalScalar y_dim,
+            const MatrixXs& heights, const CoalScalar min_height) {
+    this->x_dim = x_dim;
+    this->y_dim = y_dim;
+    this->heights = heights.cwiseMax(min_height);
+    this->min_height = min_height;
+    this->max_height = heights.maxCoeff();
+
+    const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows();
+    assert(NX >= 2 && "The number of columns is too small.");
+    assert(NY >= 2 && "The number of rows is too small.");
+
+    x_grid = VecXs::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim);
+    y_grid = VecXs::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim);
+
+    // Allocate BVS
+    const size_t num_tot_bvs =
+        (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1));
+    bvs.resize(num_tot_bvs);
+    num_bvs = 0;
+
+    // Build tree
+    buildTree();
+  }
+
+  /// @brief Get the object type: it is a HFIELD
+  OBJECT_TYPE getObjectType() const { return OT_HFIELD; }
+
+  Vec3s computeCOM() const { return Vec3s::Zero(); }
+
+  CoalScalar computeVolume() const { return 0; }
+
+  Matrix3s computeMomentofInertia() const { return Matrix3s::Zero(); }
+
+ protected:
+  /// @brief Dimensions in meters along X and Y directions
+  CoalScalar x_dim, y_dim;
+
+  /// @brief Elevation values in meters of the Height Field
+  MatrixXs heights;
+
+  /// @brief Minimal height of the Height Field: all values bellow min_height
+  /// will be discarded.
+  CoalScalar min_height, max_height;
+
+  /// @brief Grids along the X and Y directions. Useful for plotting or other
+  /// related things.
+  VecXs x_grid, y_grid;
+
+  /// @brief Bounding volume hierarchy
+  BVS bvs;
+  unsigned int num_bvs;
+
+  /// @brief Build the bounding volume hierarchy
+  int buildTree() {
+    num_bvs = 1;
+    const CoalScalar max_recursive_height =
+        recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1);
+    assert(max_recursive_height == max_height &&
+           "the maximal height is not correct");
+    COAL_UNUSED_VARIABLE(max_recursive_height);
+
+    bvs.resize(num_bvs);
+    return BVH_OK;
+  }
+
+  CoalScalar recursiveUpdateHeight(const size_t bv_id) {
+    HFNode<BV>& bv_node = bvs[bv_id];
+
+    CoalScalar max_height;
+    if (bv_node.isLeaf()) {
+      max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff();
+    } else {
+      CoalScalar max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
+                 max_right_height = recursiveUpdateHeight(bv_node.rightChild());
+
+      max_height = (std::max)(max_left_height, max_right_height);
+    }
+
+    bv_node.max_height = max_height;
+
+    const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
+    const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size],
+                       y_grid[bv_node.y_id + bv_node.y_size], max_height);
+
+    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
+
+    return max_height;
+  }
+
+  CoalScalar recursiveBuildTree(const size_t bv_id,
+                                const Eigen::DenseIndex x_id,
+                                const Eigen::DenseIndex x_size,
+                                const Eigen::DenseIndex y_id,
+                                const Eigen::DenseIndex y_size) {
+    assert(x_id < heights.cols() && "x_id is out of bounds");
+    assert(y_id < heights.rows() && "y_id is out of bounds");
+    assert(x_size >= 0 && y_size >= 0 &&
+           "x_size or y_size are not of correct value");
+    assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
+
+    HFNode<BV>& bv_node = bvs[bv_id];
+    CoalScalar max_height;
+    if (x_size == 1 &&
+        y_size == 1)  // don't build any BV for the current child node
+    {
+      max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
+    } else {
+      bv_node.first_child = num_bvs;
+      num_bvs += 2;
+
+      CoalScalar max_left_height = min_height, max_right_height = min_height;
+      if (x_size >= y_size)  // splitting along the X axis
+      {
+        Eigen::DenseIndex x_size_half = x_size / 2;
+        if (x_size == 1) x_size_half = 1;
+        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
+                                             x_size_half, y_id, y_size);
+
+        max_right_height =
+            recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,
+                               x_size - x_size_half, y_id, y_size);
+      } else  // splitting along the Y axis
+      {
+        Eigen::DenseIndex y_size_half = y_size / 2;
+        if (y_size == 1) y_size_half = 1;
+        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
+                                             y_id, y_size_half);
+
+        max_right_height =
+            recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
+                               y_id + y_size_half, y_size - y_size_half);
+      }
+
+      max_height = (std::max)(max_left_height, max_right_height);
+    }
+
+    bv_node.max_height = max_height;
+    //    max_height = std::max(max_height,min_height);
+
+    const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height);
+    assert(x_id + x_size < x_grid.size());
+    assert(y_id + y_size < y_grid.size());
+    const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
+                       max_height);
+
+    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
+    bv_node.x_id = x_id;
+    bv_node.y_id = y_id;
+    bv_node.x_size = x_size;
+    bv_node.y_size = y_size;
+
+    if (bv_node.isLeaf()) {
+      int& contact_active_faces = bv_node.contact_active_faces;
+      contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP);
+      contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM);
+
+      if (bv_node.x_id == 0)  // first col
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST);
+
+      if (bv_node.y_id == 0)  // first row (TOP)
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH);
+
+      if (bv_node.x_id + 1 == heights.cols() - 1)  // last col
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST);
+
+      if (bv_node.y_id + 1 == heights.rows() - 1)  // last row (BOTTOM)
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH);
+    }
+
+    return max_height;
+  }
+
+ public:
+  /// @brief Access the bv giving the its index
+  const HFNode<BV>& getBV(unsigned int i) const {
+    if (i >= num_bvs)
+      COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
+    return bvs[i];
+  }
+
+  /// @brief Access the bv giving the its index
+  HFNode<BV>& getBV(unsigned int i) {
+    if (i >= num_bvs)
+      COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
+    return bvs[i];
+  }
+
+  /// @brief Get the BV type: default is unknown
+  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const HeightField& other = *other_ptr;
+
+    return x_dim == other.x_dim && y_dim == other.y_dim &&
+           heights == other.heights && min_height == other.min_height &&
+           max_height == other.max_height && x_grid == other.x_grid &&
+           y_grid == other.y_grid && bvs == other.bvs &&
+           num_bvs == other.num_bvs;
+  }
+};
+
+/// @brief Specialization of getNodeType() for HeightField with different BV
+/// types
+template <>
+NODE_TYPE HeightField<AABB>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<OBB>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<RSS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<kIOS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<OBBRSS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<16>>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<18>>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<24>>::getNodeType() const;
+
+/// @}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/internal/BV_fitter.h b/include/coal/internal/BV_fitter.h
new file mode 100644
index 0000000000000000000000000000000000000000..a115d914f2297e20b6f4a718929200a9418800ba
--- /dev/null
+++ b/include/coal/internal/BV_fitter.h
@@ -0,0 +1,220 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_BV_FITTER_H
+#define COAL_BV_FITTER_H
+
+#include "coal/BVH/BVH_internal.h"
+#include "coal/BV/kIOS.h"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BV/AABB.h"
+#include <iostream>
+
+namespace coal {
+
+/// @brief Compute a bounding volume that fits a set of n points.
+template <typename BV>
+void fit(Vec3s* ps, unsigned int n, BV& bv) {
+  for (unsigned int i = 0; i < n; ++i)  // TODO(jcarpent): vectorize
+  {
+    bv += ps[i];
+  }
+}
+
+template <>
+void fit<OBB>(Vec3s* ps, unsigned int n, OBB& bv);
+
+template <>
+void fit<RSS>(Vec3s* ps, unsigned int n, RSS& bv);
+
+template <>
+void fit<kIOS>(Vec3s* ps, unsigned int n, kIOS& bv);
+
+template <>
+void fit<OBBRSS>(Vec3s* ps, unsigned int n, OBBRSS& bv);
+
+template <>
+void fit<AABB>(Vec3s* ps, unsigned int n, AABB& bv);
+
+/// @brief The class for the default algorithm fitting a bounding volume to a
+/// set of points
+template <typename BV>
+class COAL_DLLAPI BVFitterTpl {
+ public:
+  /// @brief default deconstructor
+  virtual ~BVFitterTpl() {}
+
+  /// @brief Prepare the geometry primitive data for fitting
+  void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) {
+    vertices = vertices_;
+    prev_vertices = NULL;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /// @brief Prepare the geometry primitive data for fitting, for deformable
+  /// mesh
+  void set(Vec3s* vertices_, Vec3s* prev_vertices_, Triangle* tri_indices_,
+           BVHModelType type_) {
+    vertices = vertices_;
+    prev_vertices = prev_vertices_;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /// @brief Compute the fitting BV
+  virtual BV fit(unsigned int* primitive_indices,
+                 unsigned int num_primitives) = 0;
+
+  /// @brief Clear the geometry primitive data
+  void clear() {
+    vertices = NULL;
+    prev_vertices = NULL;
+    tri_indices = NULL;
+    type = BVH_MODEL_UNKNOWN;
+  }
+
+ protected:
+  Vec3s* vertices;
+  Vec3s* prev_vertices;
+  Triangle* tri_indices;
+  BVHModelType type;
+};
+
+/// @brief The class for the default algorithm fitting a bounding volume to a
+/// set of points
+template <typename BV>
+class COAL_DLLAPI BVFitter : public BVFitterTpl<BV> {
+  typedef BVFitterTpl<BV> Base;
+
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data
+  BV fit(unsigned int* primitive_indices, unsigned int num_primitives) {
+    BV bv;
+
+    if (type == BVH_MODEL_TRIANGLES)  /// The primitive is triangle
+    {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
+        Triangle t = tri_indices[primitive_indices[i]];
+        bv += vertices[t[0]];
+        bv += vertices[t[1]];
+        bv += vertices[t[2]];
+
+        if (prev_vertices)  /// can fitting both current and previous frame
+        {
+          bv += prev_vertices[t[0]];
+          bv += prev_vertices[t[1]];
+          bv += prev_vertices[t[2]];
+        }
+      }
+    } else if (type == BVH_MODEL_POINTCLOUD)  /// The primitive is point
+    {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
+        bv += vertices[primitive_indices[i]];
+
+        if (prev_vertices)  /// can fitting both current and previous frame
+        {
+          bv += prev_vertices[primitive_indices[i]];
+        }
+      }
+    }
+
+    return bv;
+  }
+
+ protected:
+  using Base::prev_vertices;
+  using Base::tri_indices;
+  using Base::type;
+  using Base::vertices;
+};
+
+/// @brief Specification of BVFitter for OBB bounding volume
+template <>
+class COAL_DLLAPI BVFitter<OBB> : public BVFitterTpl<OBB> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  OBB fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+/// @brief Specification of BVFitter for RSS bounding volume
+template <>
+class COAL_DLLAPI BVFitter<RSS> : public BVFitterTpl<RSS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  RSS fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+/// @brief Specification of BVFitter for kIOS bounding volume
+template <>
+class COAL_DLLAPI BVFitter<kIOS> : public BVFitterTpl<kIOS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  kIOS fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+/// @brief Specification of BVFitter for OBBRSS bounding volume
+template <>
+class COAL_DLLAPI BVFitter<OBBRSS> : public BVFitterTpl<OBBRSS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  OBBRSS fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+/// @brief Specification of BVFitter for AABB bounding volume
+template <>
+class COAL_DLLAPI BVFitter<AABB> : public BVFitterTpl<AABB> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  AABB fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/internal/BV_splitter.h b/include/coal/internal/BV_splitter.h
new file mode 100644
index 0000000000000000000000000000000000000000..9bfdf459e29a402e186238beeb2644767314e184
--- /dev/null
+++ b/include/coal/internal/BV_splitter.h
@@ -0,0 +1,283 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_BV_SPLITTER_H
+#define COAL_BV_SPLITTER_H
+
+#include "coal/BVH/BVH_internal.h"
+#include "coal/BV/kIOS.h"
+#include "coal/BV/OBBRSS.h"
+#include <vector>
+#include <iostream>
+
+namespace coal {
+
+/// @brief Three types of split algorithms are provided in FCL as default
+enum SplitMethodType {
+  SPLIT_METHOD_MEAN,
+  SPLIT_METHOD_MEDIAN,
+  SPLIT_METHOD_BV_CENTER
+};
+
+/// @brief A class describing the split rule that splits each BV node
+template <typename BV>
+class BVSplitter {
+ public:
+  BVSplitter(SplitMethodType method)
+      : split_vector(0, 0, 0), split_method(method) {}
+
+  /// @brief Default deconstructor
+  virtual ~BVSplitter() {}
+
+  /// @brief Set the geometry data needed by the split rule
+  void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) {
+    vertices = vertices_;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /// @brief Compute the split rule according to a subset of geometry and the
+  /// corresponding BV node
+  void computeRule(const BV& bv, unsigned int* primitive_indices,
+                   unsigned int num_primitives) {
+    switch (split_method) {
+      case SPLIT_METHOD_MEAN:
+        computeRule_mean(bv, primitive_indices, num_primitives);
+        break;
+      case SPLIT_METHOD_MEDIAN:
+        computeRule_median(bv, primitive_indices, num_primitives);
+        break;
+      case SPLIT_METHOD_BV_CENTER:
+        computeRule_bvcenter(bv, primitive_indices, num_primitives);
+        break;
+      default:
+        std::cerr << "Split method not supported" << std::endl;
+    }
+  }
+
+  /// @brief Apply the split rule on a given point
+  bool apply(const Vec3s& q) const { return q[split_axis] > split_value; }
+
+  /// @brief Clear the geometry data set before
+  void clear() {
+    vertices = NULL;
+    tri_indices = NULL;
+    type = BVH_MODEL_UNKNOWN;
+  }
+
+ protected:
+  /// @brief The axis based on which the split decision is made. For most BV,
+  /// the axis is aligned with one of the world coordinate, so only split_axis
+  /// is needed. For oriented node, we can use a vector to make a better split
+  /// decision.
+  int split_axis;
+  Vec3s split_vector;
+
+  /// @brief The split threshold, different primitives are splitted according
+  /// whether their projection on the split_axis is larger or smaller than the
+  /// threshold
+  CoalScalar split_value;
+
+  /// @brief The mesh vertices or points handled by the splitter
+  Vec3s* vertices;
+
+  /// @brief The triangles handled by the splitter
+  Triangle* tri_indices;
+
+  /// @brief Whether the geometry is mesh or point cloud
+  BVHModelType type;
+
+  /// @brief The split algorithm used
+  SplitMethodType split_method;
+
+  /// @brief Split algorithm 1: Split the node from center
+  void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) {
+    Vec3s center = bv.center();
+    int axis = 2;
+
+    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
+      axis = 0;
+    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
+      axis = 1;
+
+    split_axis = axis;
+    split_value = center[axis];
+  }
+
+  /// @brief Split algorithm 2: Split the node according to the mean of the data
+  /// contained
+  void computeRule_mean(const BV& bv, unsigned int* primitive_indices,
+                        unsigned int num_primitives) {
+    int axis = 2;
+
+    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
+      axis = 0;
+    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
+      axis = 1;
+
+    split_axis = axis;
+    CoalScalar sum = 0;
+
+    if (type == BVH_MODEL_TRIANGLES) {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
+        const Triangle& t = tri_indices[primitive_indices[i]];
+        sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] +
+                vertices[t[2]][split_axis]);
+      }
+
+      sum /= 3;
+    } else if (type == BVH_MODEL_POINTCLOUD) {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
+        sum += vertices[primitive_indices[i]][split_axis];
+      }
+    }
+
+    split_value = sum / num_primitives;
+  }
+
+  /// @brief Split algorithm 3: Split the node according to the median of the
+  /// data contained
+  void computeRule_median(const BV& bv, unsigned int* primitive_indices,
+                          unsigned int num_primitives) {
+    int axis = 2;
+
+    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
+      axis = 0;
+    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
+      axis = 1;
+
+    split_axis = axis;
+    std::vector<CoalScalar> proj((size_t)num_primitives);
+
+    if (type == BVH_MODEL_TRIANGLES) {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
+        const Triangle& t = tri_indices[primitive_indices[i]];
+        proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] +
+                   vertices[t[2]][split_axis]) /
+                  3;
+      }
+    } else if (type == BVH_MODEL_POINTCLOUD) {
+      for (unsigned int i = 0; i < num_primitives; ++i)
+        proj[i] = vertices[primitive_indices[i]][split_axis];
+    }
+
+    std::sort(proj.begin(), proj.end());
+
+    if (num_primitives % 2 == 1) {
+      split_value = proj[(num_primitives - 1) / 2];
+    } else {
+      split_value =
+          (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
+    }
+  }
+};
+
+template <>
+bool COAL_DLLAPI BVSplitter<OBB>::apply(const Vec3s& q) const;
+
+template <>
+bool COAL_DLLAPI BVSplitter<RSS>::apply(const Vec3s& q) const;
+
+template <>
+bool COAL_DLLAPI BVSplitter<kIOS>::apply(const Vec3s& q) const;
+
+template <>
+bool COAL_DLLAPI BVSplitter<OBBRSS>::apply(const Vec3s& q) const;
+
+template <>
+void COAL_DLLAPI BVSplitter<OBB>::computeRule_bvcenter(
+    const OBB& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBB>::computeRule_mean(
+    const OBB& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBB>::computeRule_median(
+    const OBB& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<RSS>::computeRule_bvcenter(
+    const RSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<RSS>::computeRule_mean(
+    const RSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<RSS>::computeRule_median(
+    const RSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<kIOS>::computeRule_bvcenter(
+    const kIOS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<kIOS>::computeRule_mean(
+    const kIOS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<kIOS>::computeRule_median(
+    const kIOS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_bvcenter(
+    const OBBRSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_mean(
+    const OBBRSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_median(
+    const OBBRSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/internal/intersect.h b/include/coal/internal/intersect.h
new file mode 100644
index 0000000000000000000000000000000000000000..350101cffea84175220d2bf705539549c559de7c
--- /dev/null
+++ b/include/coal/internal/intersect.h
@@ -0,0 +1,187 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_INTERSECT_H
+#define COAL_INTERSECT_H
+
+/// @cond INTERNAL
+
+#include "coal/math/transform.h"
+
+namespace coal {
+
+/// @brief CCD intersect kernel among primitives
+class COAL_DLLAPI Intersect {
+ public:
+  static bool buildTrianglePlane(const Vec3s& v1, const Vec3s& v2,
+                                 const Vec3s& v3, Vec3s* n, CoalScalar* t);
+};  // class Intersect
+
+/// @brief Project functions
+class COAL_DLLAPI Project {
+ public:
+  struct COAL_DLLAPI ProjectResult {
+    /// @brief Parameterization of the projected point (based on the simplex to
+    /// be projected, use 2 or 3 or 4 of the array)
+    CoalScalar parameterization[4];
+
+    /// @brief square distance from the query point to the projected simplex
+    CoalScalar sqr_distance;
+
+    /// @brief the code of the projection type
+    unsigned int encode;
+
+    ProjectResult() : sqr_distance(-1), encode(0) {}
+  };
+
+  /// @brief Project point p onto line a-b
+  static ProjectResult projectLine(const Vec3s& a, const Vec3s& b,
+                                   const Vec3s& p);
+
+  /// @brief Project point p onto triangle a-b-c
+  static ProjectResult projectTriangle(const Vec3s& a, const Vec3s& b,
+                                       const Vec3s& c, const Vec3s& p);
+
+  /// @brief Project point p onto tetrahedra a-b-c-d
+  static ProjectResult projectTetrahedra(const Vec3s& a, const Vec3s& b,
+                                         const Vec3s& c, const Vec3s& d,
+                                         const Vec3s& p);
+
+  /// @brief Project origin (0) onto line a-b
+  static ProjectResult projectLineOrigin(const Vec3s& a, const Vec3s& b);
+
+  /// @brief Project origin (0) onto triangle a-b-c
+  static ProjectResult projectTriangleOrigin(const Vec3s& a, const Vec3s& b,
+                                             const Vec3s& c);
+
+  /// @brief Project origin (0) onto tetrahedran a-b-c-d
+  static ProjectResult projectTetrahedraOrigin(const Vec3s& a, const Vec3s& b,
+                                               const Vec3s& c, const Vec3s& d);
+};
+
+/// @brief Triangle distance functions
+class COAL_DLLAPI TriangleDistance {
+ public:
+  /// @brief Returns closest points between an segment pair.
+  /// The first segment is P + t * A
+  /// The second segment is Q + t * B
+  /// X, Y are the closest points on the two segments
+  /// VEC is the vector between X and Y
+  static void segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q,
+                        const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P,
+                                   Vec3s& Q);
+
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @param R, Tl, rotation and translation applied to T,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                   const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @param tf, rotation and translation applied to T,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                   const Transform3s& tf, Vec3s& P, Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
+  /// @param R, Tl, rotation and translation applied to T1, T2, T3,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3,
+                                   const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
+  /// @param tf, rotation and translation applied to T1, T2, T3,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3,
+                                   const Transform3s& tf, Vec3s& P, Vec3s& Q);
+};
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/shape_shape_contact_patch_func.h b/include/coal/internal/shape_shape_contact_patch_func.h
new file mode 100644
index 0000000000000000000000000000000000000000..3eb28b539fda0246da672c4231a4bb122960cee2
--- /dev/null
+++ b/include/coal/internal/shape_shape_contact_patch_func.h
@@ -0,0 +1,264 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, INRIA
+ *  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 Louis Montaut */
+
+#ifndef COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
+#define COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
+
+#include "coal/collision_data.h"
+#include "coal/collision_utility.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/contact_patch/contact_patch_solver.h"
+#include "coal/shape/geometric_shapes_traits.h"
+
+namespace coal {
+
+/// @brief Shape-shape contact patch computation.
+/// Assumes that `csolver` and the `ContactPatchResult` have already been set up
+/// by the `ContactPatchRequest`.
+template <typename ShapeType1, typename ShapeType2>
+struct ComputeShapeShapeContactPatch {
+  static void run(const CollisionGeometry* o1, const Transform3s& tf1,
+                  const CollisionGeometry* o2, const Transform3s& tf2,
+                  const CollisionResult& collision_result,
+                  const ContactPatchSolver* csolver,
+                  const ContactPatchRequest& request,
+                  ContactPatchResult& result) {
+    // Note: see specializations for Plane and Halfspace below.
+    if (!collision_result.isCollision()) {
+      return;
+    }
+    COAL_ASSERT(
+        result.check(request),
+        "The contact patch result and request are incompatible (issue of "
+        "contact patch size or maximum number of contact patches). Make sure "
+        "result is initialized with request.",
+        std::logic_error);
+
+    const ShapeType1& s1 = static_cast<const ShapeType1&>(*o1);
+    const ShapeType2& s2 = static_cast<const ShapeType2&>(*o2);
+    for (size_t i = 0; i < collision_result.numContacts(); ++i) {
+      if (i >= request.max_num_patch) {
+        break;
+      }
+      csolver->setSupportGuess(collision_result.cached_support_func_guess);
+      const Contact& contact = collision_result.getContact(i);
+      ContactPatch& contact_patch = result.getUnusedContactPatch();
+      csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch);
+    }
+  }
+};
+
+/// @brief Computes the contact patch between a Plane/Halfspace and another
+/// shape.
+/// @tparam InvertShapes set to true if the first shape of the collision pair
+/// is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling
+/// this function).
+template <bool InvertShapes, typename OtherShapeType, typename PlaneOrHalfspace>
+void computePatchPlaneOrHalfspace(const OtherShapeType& s1,
+                                  const Transform3s& tf1,
+                                  const PlaneOrHalfspace& s2,
+                                  const Transform3s& tf2,
+                                  const ContactPatchSolver* csolver,
+                                  const Contact& contact,
+                                  ContactPatch& contact_patch) {
+  COAL_UNUSED_VARIABLE(s2);
+  COAL_UNUSED_VARIABLE(tf2);
+  constructContactPatchFrameFromContact(contact, contact_patch);
+  if ((bool)(shape_traits<OtherShapeType>::IsStrictlyConvex)) {
+    // Only one point of contact; it has already been computed.
+    contact_patch.addPoint(contact.pos);
+    return;
+  }
+
+  // We only need to compute the support set in the direction of the normal.
+  // We need to temporarily express the patch in the local frame of shape1.
+  SupportSet& support_set = csolver->support_set_shape1;
+  support_set.tf.rotation().noalias() =
+      tf1.rotation().transpose() * contact_patch.tf.rotation();
+  support_set.tf.translation().noalias() =
+      tf1.rotation().transpose() *
+      (contact_patch.tf.translation() - tf1.translation());
+
+  // Note: for now, taking into account swept-sphere radius does not change
+  // anything to the support set computations. However it will be used in the
+  // future if we want to store the offsets to the support plane for each point
+  // in a support set.
+  using SupportOptions = details::SupportOptions;
+  if (InvertShapes) {
+    support_set.direction = ContactPatch::PatchDirection::INVERTED;
+    details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
+        &s1, support_set, csolver->support_guess[1], csolver->supports_data[1],
+        csolver->num_samples_curved_shapes, csolver->patch_tolerance);
+  } else {
+    support_set.direction = ContactPatch::PatchDirection::DEFAULT;
+    details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
+        &s1, support_set, csolver->support_guess[0], csolver->supports_data[0],
+        csolver->num_samples_curved_shapes, csolver->patch_tolerance);
+  }
+  csolver->getResult(contact, &(support_set.points()), contact_patch);
+}
+
+#define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace)          \
+  template <typename OtherShapeType>                                          \
+  struct ComputeShapeShapeContactPatch<OtherShapeType, PlaneOrHspace> {       \
+    static void run(const CollisionGeometry* o1, const Transform3s& tf1,      \
+                    const CollisionGeometry* o2, const Transform3s& tf2,      \
+                    const CollisionResult& collision_result,                  \
+                    const ContactPatchSolver* csolver,                        \
+                    const ContactPatchRequest& request,                       \
+                    ContactPatchResult& result) {                             \
+      if (!collision_result.isCollision()) {                                  \
+        return;                                                               \
+      }                                                                       \
+      COAL_ASSERT(                                                            \
+          result.check(request),                                              \
+          "The contact patch result and request are incompatible (issue of "  \
+          "contact patch size or maximum number of contact patches). Make "   \
+          "sure "                                                             \
+          "result is initialized with request.",                              \
+          std::logic_error);                                                  \
+                                                                              \
+      const OtherShapeType& s1 = static_cast<const OtherShapeType&>(*o1);     \
+      const PlaneOrHspace& s2 = static_cast<const PlaneOrHspace&>(*o2);       \
+      for (size_t i = 0; i < collision_result.numContacts(); ++i) {           \
+        if (i >= request.max_num_patch) {                                     \
+          break;                                                              \
+        }                                                                     \
+        csolver->setSupportGuess(collision_result.cached_support_func_guess); \
+        const Contact& contact = collision_result.getContact(i);              \
+        ContactPatch& contact_patch = result.getUnusedContactPatch();         \
+        computePatchPlaneOrHalfspace<false, OtherShapeType, PlaneOrHspace>(   \
+            s1, tf1, s2, tf2, csolver, contact, contact_patch);               \
+      }                                                                       \
+    }                                                                         \
+  };                                                                          \
+                                                                              \
+  template <typename OtherShapeType>                                          \
+  struct ComputeShapeShapeContactPatch<PlaneOrHspace, OtherShapeType> {       \
+    static void run(const CollisionGeometry* o1, const Transform3s& tf1,      \
+                    const CollisionGeometry* o2, const Transform3s& tf2,      \
+                    const CollisionResult& collision_result,                  \
+                    const ContactPatchSolver* csolver,                        \
+                    const ContactPatchRequest& request,                       \
+                    ContactPatchResult& result) {                             \
+      if (!collision_result.isCollision()) {                                  \
+        return;                                                               \
+      }                                                                       \
+      COAL_ASSERT(                                                            \
+          result.check(request),                                              \
+          "The contact patch result and request are incompatible (issue of "  \
+          "contact patch size or maximum number of contact patches). Make "   \
+          "sure "                                                             \
+          "result is initialized with request.",                              \
+          std::logic_error);                                                  \
+                                                                              \
+      const PlaneOrHspace& s1 = static_cast<const PlaneOrHspace&>(*o1);       \
+      const OtherShapeType& s2 = static_cast<const OtherShapeType&>(*o2);     \
+      for (size_t i = 0; i < collision_result.numContacts(); ++i) {           \
+        if (i >= request.max_num_patch) {                                     \
+          break;                                                              \
+        }                                                                     \
+        csolver->setSupportGuess(collision_result.cached_support_func_guess); \
+        const Contact& contact = collision_result.getContact(i);              \
+        ContactPatch& contact_patch = result.getUnusedContactPatch();         \
+        computePatchPlaneOrHalfspace<true, OtherShapeType, PlaneOrHspace>(    \
+            s2, tf2, s1, tf1, csolver, contact, contact_patch);               \
+      }                                                                       \
+    }                                                                         \
+  };
+
+PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane)
+PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace)
+
+#define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2)           \
+  template <>                                                                \
+  struct ComputeShapeShapeContactPatch<PlaneOrHspace1, PlaneOrHspace2> {     \
+    static void run(const CollisionGeometry* o1, const Transform3s& tf1,     \
+                    const CollisionGeometry* o2, const Transform3s& tf2,     \
+                    const CollisionResult& collision_result,                 \
+                    const ContactPatchSolver* csolver,                       \
+                    const ContactPatchRequest& request,                      \
+                    ContactPatchResult& result) {                            \
+      COAL_UNUSED_VARIABLE(o1);                                              \
+      COAL_UNUSED_VARIABLE(tf1);                                             \
+      COAL_UNUSED_VARIABLE(o2);                                              \
+      COAL_UNUSED_VARIABLE(tf2);                                             \
+      COAL_UNUSED_VARIABLE(csolver);                                         \
+      if (!collision_result.isCollision()) {                                 \
+        return;                                                              \
+      }                                                                      \
+      COAL_ASSERT(                                                           \
+          result.check(request),                                             \
+          "The contact patch result and request are incompatible (issue of " \
+          "contact patch size or maximum number of contact patches). Make "  \
+          "sure "                                                            \
+          "result is initialized with request.",                             \
+          std::logic_error);                                                 \
+                                                                             \
+      for (size_t i = 0; i < collision_result.numContacts(); ++i) {          \
+        if (i >= request.max_num_patch) {                                    \
+          break;                                                             \
+        }                                                                    \
+        const Contact& contact = collision_result.getContact(i);             \
+        ContactPatch& contact_patch = result.getUnusedContactPatch();        \
+        constructContactPatchFrameFromContact(contact, contact_patch);       \
+        contact_patch.addPoint(contact.pos);                                 \
+      }                                                                      \
+    }                                                                        \
+  };
+
+PLANE_HSPACE_CONTACT_PATCH(Plane, Plane)
+PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace)
+PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane)
+PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace)
+
+#undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH
+#undef PLANE_HSPACE_CONTACT_PATCH
+
+template <typename ShapeType1, typename ShapeType2>
+void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1,
+                            const CollisionGeometry* o2, const Transform3s& tf2,
+                            const CollisionResult& collision_result,
+                            const ContactPatchSolver* csolver,
+                            const ContactPatchRequest& request,
+                            ContactPatchResult& result) {
+  return ComputeShapeShapeContactPatch<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, collision_result, csolver, request, result);
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/internal/shape_shape_func.h b/include/coal/internal/shape_shape_func.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb18b3ae9be03910a53286519e90b0abb7a289b7
--- /dev/null
+++ b/include/coal/internal/shape_shape_func.h
@@ -0,0 +1,318 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, CNRS-LAAS
+ *  Copyright (c) 2024, INRIA
+ *  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 Florent Lamiraux */
+
+#ifndef COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
+#define COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/collision_utility.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes_traits.h"
+
+namespace coal {
+
+template <typename ShapeType1, typename ShapeType2>
+struct ShapeShapeDistancer {
+  static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1,
+                        const CollisionGeometry* o2, const Transform3s& tf2,
+                        const GJKSolver* nsolver,
+                        const DistanceRequest& request,
+                        DistanceResult& result) {
+    if (request.isSatisfied(result)) return result.min_distance;
+
+    // Witness points on shape1 and shape2, normal pointing from shape1 to
+    // shape2.
+    Vec3s p1, p2, normal;
+    const CoalScalar distance =
+        ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
+            o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
+            normal);
+
+    result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE,
+                  p1, p2, normal);
+
+    return distance;
+  }
+
+  static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1,
+                        const CollisionGeometry* o2, const Transform3s& tf2,
+                        const GJKSolver* nsolver,
+                        const bool compute_signed_distance, Vec3s& p1,
+                        Vec3s& p2, Vec3s& normal) {
+    const ShapeType1* obj1 = static_cast<const ShapeType1*>(o1);
+    const ShapeType2* obj2 = static_cast<const ShapeType2*>(o2);
+    return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2,
+                                  compute_signed_distance, p1, p2, normal);
+  }
+};
+
+/// @brief Shape-shape distance computation.
+/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
+/// a `CollisionRequest`.
+///
+/// @note This function is typically used for collision pairs containing two
+/// primitive shapes.
+/// @note This function might be specialized for some pairs of shapes.
+template <typename ShapeType1, typename ShapeType2>
+CoalScalar ShapeShapeDistance(const CollisionGeometry* o1,
+                              const Transform3s& tf1,
+                              const CollisionGeometry* o2,
+                              const Transform3s& tf2, const GJKSolver* nsolver,
+                              const DistanceRequest& request,
+                              DistanceResult& result) {
+  return ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, nsolver, request, result);
+}
+
+namespace internal {
+/// @brief Shape-shape distance computation.
+/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
+/// a `CollisionRequest`.
+///
+/// @note This function is typically used for collision pairs complex structures
+/// like BVHs, HeightFields or Octrees. These structures contain sets of
+/// primitive shapes.
+/// This function is meant to be called on the pairs of primitive shapes of
+/// these structures.
+/// @note This function might be specialized for some pairs of shapes.
+template <typename ShapeType1, typename ShapeType2>
+CoalScalar ShapeShapeDistance(const CollisionGeometry* o1,
+                              const Transform3s& tf1,
+                              const CollisionGeometry* o2,
+                              const Transform3s& tf2, const GJKSolver* nsolver,
+                              const bool compute_signed_distance, Vec3s& p1,
+                              Vec3s& p2, Vec3s& normal) {
+  return ::coal::ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal);
+}
+}  // namespace internal
+
+/// @brief Shape-shape collision detection.
+/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
+/// a `CollisionRequest`.
+///
+/// @note This function is typically used for collision pairs containing two
+/// primitive shapes.
+/// Complex structures like BVHs, HeightFields or Octrees contain sets of
+/// primitive shapes should use the `ShapeShapeDistance` function to do their
+/// internal collision detection checks.
+template <typename ShapeType1, typename ShapeType2>
+struct ShapeShapeCollider {
+  static std::size_t run(const CollisionGeometry* o1, const Transform3s& tf1,
+                         const CollisionGeometry* o2, const Transform3s& tf2,
+                         const GJKSolver* nsolver,
+                         const CollisionRequest& request,
+                         CollisionResult& result) {
+    if (request.isSatisfied(result)) return result.numContacts();
+
+    const bool compute_penetration =
+        request.enable_contact || (request.security_margin < 0);
+    Vec3s p1, p2, normal;
+    CoalScalar distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
+        o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal);
+
+    size_t num_contacts = 0;
+    const CoalScalar distToCollision = distance - request.security_margin;
+
+    internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
+                                               p1, p2, normal);
+    if (distToCollision <= request.collision_distance_threshold &&
+        result.numContacts() < request.num_max_contacts) {
+      if (result.numContacts() < request.num_max_contacts) {
+        Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal,
+                        distance);
+        result.addContact(contact);
+      }
+      num_contacts = result.numContacts();
+    }
+
+    return num_contacts;
+  }
+};
+
+template <typename ShapeType1, typename ShapeType2>
+std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
+                              const Transform3s& tf1,
+                              const CollisionGeometry* o2,
+                              const Transform3s& tf2, const GJKSolver* nsolver,
+                              const CollisionRequest& request,
+                              CollisionResult& result) {
+  return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, nsolver, request, result);
+}
+
+// clang-format off
+// ==============================================================================================================
+// ==============================================================================================================
+// ==============================================================================================================
+// Shape distance algorithms based on:
+// - built-in function: 0
+// - GJK:               1
+//
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | box        |  1  |   0    |    1    |   1  |    1     |   0   |      0     |    1     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | sphere     |/////|   0    |    0    |   1  |    0     |   0   |      0     |    0     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | capsule    |/////|////////|    0    |   1  |    1     |   0   |      0     |    1     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | cone       |/////|////////|/////////|   1  |    1     |   0   |      0     |    1     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | cylinder   |/////|////////|/////////|//////|    1     |   0   |      0     |    1     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | plane      |/////|////////|/////////|//////|//////////|   ?   |      ?     |    0     |    0      |    0   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | half-space |/////|////////|/////////|//////|//////////|///////|      ?     |    0     |    0      |    0   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|    0     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | ellipsoid  |/////|////////|/////////|//////|//////////|///////|////////////|//////////|    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | convex     |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////|    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+//
+// Number of pairs: 55
+//   - Specialized: 26
+//   - GJK:         29
+// clang-format on
+
+#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2)                            \
+  template <>                                                                  \
+  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T1, T2>(                 \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+      Vec3s& p2, Vec3s& normal);                                               \
+  template <>                                                                  \
+  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T2, T1>(                 \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+      Vec3s& p2, Vec3s& normal);                                               \
+  template <>                                                                  \
+  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T1, T2>(                    \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const DistanceRequest& request,                \
+      DistanceResult& result) {                                                \
+    result.o1 = o1;                                                            \
+    result.o2 = o2;                                                            \
+    result.b1 = DistanceResult::NONE;                                          \
+    result.b2 = DistanceResult::NONE;                                          \
+    result.min_distance = internal::ShapeShapeDistance<T1, T2>(                \
+        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
+        result.nearest_points[0], result.nearest_points[1], result.normal);    \
+    return result.min_distance;                                                \
+  }                                                                            \
+  template <>                                                                  \
+  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T2, T1>(                    \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const DistanceRequest& request,                \
+      DistanceResult& result) {                                                \
+    result.o1 = o1;                                                            \
+    result.o2 = o2;                                                            \
+    result.b1 = DistanceResult::NONE;                                          \
+    result.b2 = DistanceResult::NONE;                                          \
+    result.min_distance = internal::ShapeShapeDistance<T2, T1>(                \
+        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
+        result.nearest_points[0], result.nearest_points[1], result.normal);    \
+    return result.min_distance;                                                \
+  }
+
+#define SHAPE_SELF_DISTANCE_SPECIALIZATION(T)                                  \
+  template <>                                                                  \
+  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T, T>(                   \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+      Vec3s& p2, Vec3s& normal);                                               \
+  template <>                                                                  \
+  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T, T>(                      \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const DistanceRequest& request,                \
+      DistanceResult& result) {                                                \
+    result.o1 = o1;                                                            \
+    result.o2 = o2;                                                            \
+    result.b1 = DistanceResult::NONE;                                          \
+    result.b2 = DistanceResult::NONE;                                          \
+    result.min_distance = internal::ShapeShapeDistance<T, T>(                  \
+        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
+        result.nearest_points[0], result.nearest_points[1], result.normal);    \
+    return result.min_distance;                                                \
+  }
+
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace)
+
+#undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
+#undef SHAPE_SELF_DISTANCE_SPECIALIZATION
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/tools.h b/include/coal/internal/tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab26633d1437fa9680f0994200c7deb286fad831
--- /dev/null
+++ b/include/coal/internal/tools.h
@@ -0,0 +1,213 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 Joseph Mirabel */
+
+#ifndef COAL_INTERNAL_TOOLS_H
+#define COAL_INTERNAL_TOOLS_H
+
+#include "coal/fwd.hh"
+
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+template <typename Derived>
+static inline typename Derived::Scalar triple(
+    const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y,
+    const Eigen::MatrixBase<Derived>& z) {
+  return x.derived().dot(y.derived().cross(z.derived()));
+}
+
+template <typename Derived1, typename Derived2, typename Derived3>
+void generateCoordinateSystem(const Eigen::MatrixBase<Derived1>& _w,
+                              const Eigen::MatrixBase<Derived2>& _u,
+                              const Eigen::MatrixBase<Derived3>& _v) {
+  typedef typename Derived1::Scalar T;
+
+  Eigen::MatrixBase<Derived1>& w = const_cast<Eigen::MatrixBase<Derived1>&>(_w);
+  Eigen::MatrixBase<Derived2>& u = const_cast<Eigen::MatrixBase<Derived2>&>(_u);
+  Eigen::MatrixBase<Derived3>& v = const_cast<Eigen::MatrixBase<Derived3>&>(_v);
+
+  T inv_length;
+  if (std::abs(w[0]) >= std::abs(w[1])) {
+    inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+    u[0] = -w[2] * inv_length;
+    u[1] = (T)0;
+    u[2] = w[0] * inv_length;
+    v[0] = w[1] * u[2];
+    v[1] = w[2] * u[0] - w[0] * u[2];
+    v[2] = -w[1] * u[0];
+  } else {
+    inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+    u[0] = (T)0;
+    u[1] = w[2] * inv_length;
+    u[2] = -w[1] * inv_length;
+    v[0] = w[1] * u[2] - w[2] * u[1];
+    v[1] = -w[0] * u[2];
+    v[2] = w[0] * u[1];
+  }
+}
+
+/* ----- Start Matrices ------ */
+template <typename Derived, typename OtherDerived>
+void relativeTransform(const Eigen::MatrixBase<Derived>& R1,
+                       const Eigen::MatrixBase<OtherDerived>& t1,
+                       const Eigen::MatrixBase<Derived>& R2,
+                       const Eigen::MatrixBase<OtherDerived>& t2,
+                       const Eigen::MatrixBase<Derived>& R,
+                       const Eigen::MatrixBase<OtherDerived>& t) {
+  const_cast<Eigen::MatrixBase<Derived>&>(R) = R1.transpose() * R2;
+  const_cast<Eigen::MatrixBase<OtherDerived>&>(t) = R1.transpose() * (t2 - t1);
+}
+
+/// @brief compute the eigen vector and eigen vector of a matrix. dout is the
+/// eigen values, vout is the eigen vectors
+template <typename Derived, typename Vector>
+void eigen(const Eigen::MatrixBase<Derived>& m,
+           typename Derived::Scalar dout[3], Vector* vout) {
+  typedef typename Derived::Scalar S;
+  Derived R(m.derived());
+  int n = 3;
+  int j, iq, ip, i;
+  S tresh, theta, tau, t, sm, s, h, g, c;
+
+  S b[3];
+  S z[3];
+  S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+  S d[3];
+
+  for (ip = 0; ip < n; ++ip) {
+    b[ip] = d[ip] = R(ip, ip);
+    z[ip] = 0;
+  }
+
+  for (i = 0; i < 50; ++i) {
+    sm = 0;
+    for (ip = 0; ip < n; ++ip)
+      for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq));
+    if (sm == 0.0) {
+      vout[0] << v[0][0], v[0][1], v[0][2];
+      vout[1] << v[1][0], v[1][1], v[1][2];
+      vout[2] << v[2][0], v[2][1], v[2][2];
+      dout[0] = d[0];
+      dout[1] = d[1];
+      dout[2] = d[2];
+      return;
+    }
+
+    if (i < 3)
+      tresh = 0.2 * sm / (n * n);
+    else
+      tresh = 0.0;
+
+    for (ip = 0; ip < n; ++ip) {
+      for (iq = ip + 1; iq < n; ++iq) {
+        g = 100.0 * std::abs(R(ip, iq));
+        if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) &&
+            std::abs(d[iq]) + g == std::abs(d[iq]))
+          R(ip, iq) = 0.0;
+        else if (std::abs(R(ip, iq)) > tresh) {
+          h = d[iq] - d[ip];
+          if (std::abs(h) + g == std::abs(h))
+            t = (R(ip, iq)) / h;
+          else {
+            theta = 0.5 * h / (R(ip, iq));
+            t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta));
+            if (theta < 0.0) t = -t;
+          }
+          c = 1.0 / std::sqrt(1 + t * t);
+          s = t * c;
+          tau = s / (1.0 + c);
+          h = t * R(ip, iq);
+          z[ip] -= h;
+          z[iq] += h;
+          d[ip] -= h;
+          d[iq] += h;
+          R(ip, iq) = 0.0;
+          for (j = 0; j < ip; ++j) {
+            g = R(j, ip);
+            h = R(j, iq);
+            R(j, ip) = g - s * (h + g * tau);
+            R(j, iq) = h + s * (g - h * tau);
+          }
+          for (j = ip + 1; j < iq; ++j) {
+            g = R(ip, j);
+            h = R(j, iq);
+            R(ip, j) = g - s * (h + g * tau);
+            R(j, iq) = h + s * (g - h * tau);
+          }
+          for (j = iq + 1; j < n; ++j) {
+            g = R(ip, j);
+            h = R(iq, j);
+            R(ip, j) = g - s * (h + g * tau);
+            R(iq, j) = h + s * (g - h * tau);
+          }
+          for (j = 0; j < n; ++j) {
+            g = v[j][ip];
+            h = v[j][iq];
+            v[j][ip] = g - s * (h + g * tau);
+            v[j][iq] = h + s * (g - h * tau);
+          }
+        }
+      }
+    }
+    for (ip = 0; ip < n; ++ip) {
+      b[ip] += z[ip];
+      d[ip] = b[ip];
+      z[ip] = 0.0;
+    }
+  }
+
+  std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
+
+  return;
+}
+
+template <typename Derived, typename OtherDerived>
+bool isEqual(const Eigen::MatrixBase<Derived>& lhs,
+             const Eigen::MatrixBase<OtherDerived>& rhs,
+             const CoalScalar tol = std::numeric_limits<CoalScalar>::epsilon() *
+                                    100) {
+  return ((lhs - rhs).array().abs() < tol).all();
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/internal/traversal.h b/include/coal/internal/traversal.h
new file mode 100644
index 0000000000000000000000000000000000000000..32fcac457c12416f6c3741cc986a8d28f78cbf34
--- /dev/null
+++ b/include/coal/internal/traversal.h
@@ -0,0 +1,73 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, LAAS CNRS
+ *  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 Joseph Mirabel */
+
+#ifndef COAL_TRAVERSAL_DETAILS_TRAVERSAL_H
+#define COAL_TRAVERSAL_DETAILS_TRAVERSAL_H
+
+/// @cond INTERNAL
+
+namespace coal {
+
+enum { RelativeTransformationIsIdentity = 1 };
+
+namespace details {
+template <bool enabled>
+struct COAL_DLLAPI RelativeTransformation {
+  RelativeTransformation() : R(Matrix3s::Identity()) {}
+
+  const Matrix3s& _R() const { return R; }
+  const Vec3s& _T() const { return T; }
+
+  Matrix3s R;
+  Vec3s T;
+};
+
+template <>
+struct COAL_DLLAPI RelativeTransformation<false> {
+  static const Matrix3s& _R() {
+    COAL_THROW_PRETTY("should never reach this point", std::logic_error);
+  }
+  static const Vec3s& _T() {
+    COAL_THROW_PRETTY("should never reach this point", std::logic_error);
+  }
+};
+}  // namespace details
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/traversal_node_base.h b/include/coal/internal/traversal_node_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..edd252985a75c988655c74a061495acb1a8947b7
--- /dev/null
+++ b/include/coal/internal/traversal_node_base.h
@@ -0,0 +1,172 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_TRAVERSAL_NODE_BASE_H
+#define COAL_TRAVERSAL_NODE_BASE_H
+
+/// @cond INTERNAL
+
+#include "coal/data_types.h"
+#include "coal/math/transform.h"
+#include "coal/collision_data.h"
+
+namespace coal {
+
+/// @brief Node structure encoding the information required for traversal.
+
+class TraversalNodeBase {
+ public:
+  TraversalNodeBase() : enable_statistics(false) {}
+
+  virtual ~TraversalNodeBase() {}
+
+  virtual void preprocess() {}
+
+  virtual void postprocess() {}
+
+  /// @brief Whether b is a leaf node in the first BVH tree
+  virtual bool isFirstNodeLeaf(unsigned int /*b*/) const { return true; }
+
+  /// @brief Whether b is a leaf node in the second BVH tree
+  virtual bool isSecondNodeLeaf(unsigned int /*b*/) const { return true; }
+
+  /// @brief Traverse the subtree of the node in the first tree first
+  virtual bool firstOverSecond(unsigned int /*b1*/, unsigned int /*b2*/) const {
+    return true;
+  }
+
+  /// @brief Get the left child of the node b in the first tree
+  virtual int getFirstLeftChild(unsigned int b) const { return (int)b; }
+
+  /// @brief Get the right child of the node b in the first tree
+  virtual int getFirstRightChild(unsigned int b) const { return (int)b; }
+
+  /// @brief Get the left child of the node b in the second tree
+  virtual int getSecondLeftChild(unsigned int b) const { return (int)b; }
+
+  /// @brief Get the right child of the node b in the second tree
+  virtual int getSecondRightChild(unsigned int b) const { return (int)b; }
+
+  /// @brief Whether store some statistics information during traversal
+  void enableStatistics(bool enable) { enable_statistics = enable; }
+
+  /// @brief configuation of first object
+  Transform3s tf1;
+
+  /// @brief configuration of second object
+  Transform3s tf2;
+
+  /// @brief Whether stores statistics
+  bool enable_statistics;
+};
+
+/// @defgroup Traversal_For_Collision
+/// regroup class about traversal for distance.
+/// @{
+
+/// @brief Node structure encoding the information required for collision
+/// traversal.
+class CollisionTraversalNodeBase : public TraversalNodeBase {
+ public:
+  CollisionTraversalNodeBase(const CollisionRequest& request_)
+      : request(request_), result(NULL) {}
+
+  virtual ~CollisionTraversalNodeBase() {}
+
+  /// BV test between b1 and b2
+  /// @param b1, b2 Bounding volumes to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  virtual bool BVDisjoints(unsigned int b1, unsigned int b2,
+                           CoalScalar& sqrDistLowerBound) const = 0;
+
+  /// @brief Leaf test between node b1 and b2, if they are both leafs
+  virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/,
+                            CoalScalar& /*sqrDistLowerBound*/) const = 0;
+
+  /// @brief Check whether the traversal can stop
+  bool canStop() const { return this->request.isSatisfied(*(this->result)); }
+
+  /// @brief request setting for collision
+  const CollisionRequest& request;
+
+  /// @brief collision result kept during the traversal iteration
+  CollisionResult* result;
+};
+
+/// @}
+
+/// @defgroup Traversal_For_Distance
+/// regroup class about traversal for distance.
+/// @{
+
+/// @brief Node structure encoding the information required for distance
+/// traversal.
+class DistanceTraversalNodeBase : public TraversalNodeBase {
+ public:
+  DistanceTraversalNodeBase() : result(NULL) {}
+
+  virtual ~DistanceTraversalNodeBase() {}
+
+  /// @brief BV test between b1 and b2
+  /// @return a lower bound of the distance between the two BV.
+  /// @note except for OBB, this method returns the distance.
+  virtual CoalScalar BVDistanceLowerBound(unsigned int /*b1*/,
+                                          unsigned int /*b2*/) const {
+    return (std::numeric_limits<CoalScalar>::max)();
+  }
+
+  /// @brief Leaf test between node b1 and b2, if they are both leafs
+  virtual void leafComputeDistance(unsigned int b1, unsigned int b2) const = 0;
+
+  /// @brief Check whether the traversal can stop
+  virtual bool canStop(CoalScalar /*c*/) const { return false; }
+
+  /// @brief request setting for distance
+  DistanceRequest request;
+
+  /// @brief distance result kept during the traversal iteration
+  DistanceResult* result;
+};
+
+///@}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h b/include/coal/internal/traversal_node_bvh_hfield.h
similarity index 84%
rename from include/hpp/fcl/internal/traversal_node_bvh_hfield.h
rename to include/coal/internal/traversal_node_bvh_hfield.h
index 27a0caed5d6f83835dcf0af6594a80e846658145..fc550eb21dac39bfbfffafe72bcf7d9ce2acfff7 100644
--- a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h
+++ b/include/coal/internal/traversal_node_bvh_hfield.h
@@ -34,29 +34,28 @@
 
 /** \author Justin Carpentier */
 
-#ifndef HPP_FCL_TRAVERSAL_NODE_BVH_HFIELD_H
-#define HPP_FCL_TRAVERSAL_NODE_BVH_HFIELD_H
+#ifndef COAL_TRAVERSAL_NODE_BVH_HFIELD_H
+#define COAL_TRAVERSAL_NODE_BVH_HFIELD_H
 
 /// @cond INTERNAL
 
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/internal/traversal_node_base.h>
-#include <hpp/fcl/internal/traversal_node_hfield_shape.h>
-#include <hpp/fcl/BV/BV_node.h>
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/hfield.h>
-#include <hpp/fcl/internal/intersect.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/internal/traversal.h>
+#include "coal/collision_data.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal_node_hfield_shape.h"
+#include "coal/BV/BV_node.h"
+#include "coal/BV/BV.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/hfield.h"
+#include "coal/internal/intersect.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/internal/traversal.h"
 
 #include <limits>
 #include <vector>
 #include <cassert>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 /// @addtogroup Traversal_For_Collision
 /// @{
@@ -100,8 +99,8 @@ class MeshHeightFieldCollisionTraversalNode
 
   /// @brief Determine the traversal order, is the first BVTT subtree better
   bool firstOverSecond(unsigned int b1, unsigned int b2) const {
-    FCL_REAL sz1 = model1->getBV(b1).bv.size();
-    FCL_REAL sz2 = model2->getBV(b2).bv.size();
+    CoalScalar sz1 = model1->getBV(b1).bv.size();
+    CoalScalar sz2 = model2->getBV(b2).bv.size();
 
     bool l1 = model1->getBV(b1).isLeaf();
     bool l2 = model2->getBV(b2).isLeaf();
@@ -145,7 +144,7 @@ class MeshHeightFieldCollisionTraversalNode
   /// @retval sqrDistLowerBound square of a lower bound of the minimal
   ///         distance between bounding volumes.
   bool BVDisjoints(unsigned int b1, unsigned int b2,
-                   FCL_REAL& sqrDistLowerBound) const {
+                   CoalScalar& sqrDistLowerBound) const {
     if (this->enable_statistics) this->num_bv_tests++;
     if (RTIsIdentity)
       return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
@@ -174,7 +173,7 @@ class MeshHeightFieldCollisionTraversalNode
   ///       and the object are not colliding, the penetration depth is
   ///       negative.
   void leafCollides(unsigned int b1, unsigned int b2,
-                    FCL_REAL& sqrDistLowerBound) const {
+                    CoalScalar& sqrDistLowerBound) const {
     if (this->enable_statistics) this->num_leaf_tests++;
 
     const BVNode<BV1>& node1 = this->model1->getBV(b1);
@@ -183,9 +182,9 @@ class MeshHeightFieldCollisionTraversalNode
     int primitive_id1 = node1.primitiveId();
     const Triangle& tri_id1 = tri_indices1[primitive_id1];
 
-    const Vec3f& P1 = vertices1[tri_id1[0]];
-    const Vec3f& P2 = vertices1[tri_id1[1]];
-    const Vec3f& P3 = vertices1[tri_id1[2]];
+    const Vec3s& P1 = vertices1[tri_id1[0]];
+    const Vec3s& P2 = vertices1[tri_id1[1]];
+    const Vec3s& P3 = vertices1[tri_id1[2]];
 
     TriangleP tri1(P1, P2, P3);
 
@@ -194,17 +193,17 @@ class MeshHeightFieldCollisionTraversalNode
     details::buildConvexTriangles(node2, *this->model2, convex2, convex2);
 
     GJKSolver solver;
-    Vec3f p1,
+    Vec3s p1,
         p2;  // closest points if no collision contact points if collision.
-    Vec3f normal;
-    FCL_REAL distance;
+    Vec3s normal;
+    CoalScalar distance;
     solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
                          normal);
-    FCL_REAL distToCollision = distance - this->request.security_margin;
+    CoalScalar distToCollision = distance - this->request.security_margin;
     sqrDistLowerBound = distance * distance;
     if (distToCollision <= 0) {  // collision
-      Vec3f p(p1);               // contact point
-      FCL_REAL penetrationDepth(0);
+      Vec3s p(p1);               // contact point
+      CoalScalar penetrationDepth(0);
       if (this->result->numContacts() < this->request.num_max_contacts) {
         // How much (Q1, Q2, Q3) should be moved so that all vertices are
         // above (P1, P2, P3).
@@ -228,9 +227,9 @@ class MeshHeightFieldCollisionTraversalNode
   /// @brief statistical information
   mutable int num_bv_tests;
   mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
+  mutable CoalScalar query_time_seconds;
 
-  Vec3f* vertices1 Triangle* tri_indices1;
+  Vec3s* vertices1 Triangle* tri_indices1;
 
   details::RelativeTransformation<!bool(RTIsIdentity)> RT;
 };
@@ -251,19 +250,19 @@ typedef MeshHeightFieldCollisionTraversalNode<OBBRSS, 0>
 namespace details {
 template <typename BV>
 struct DistanceTraversalBVDistanceLowerBound_impl {
-  static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
+  static CoalScalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
     return b1.distance(b2);
   }
-  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
-                      const BVNode<BV>& b2) {
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode<BV>& b1,
+                        const BVNode<BV>& b2) {
     return distance(R, T, b1.bv, b2.bv);
   }
 };
 
 template <>
 struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
-  static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
-    FCL_REAL sqrDistLowerBound;
+  static CoalScalar run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
+    CoalScalar sqrDistLowerBound;
     CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
     // request.break_distance = ?
     if (b1.overlap(b2, request, sqrDistLowerBound)) {
@@ -272,9 +271,9 @@ struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
     }
     return sqrt(sqrDistLowerBound);
   }
-  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1,
-                      const BVNode<OBB>& b2) {
-    FCL_REAL sqrDistLowerBound;
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T,
+                        const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
+    CoalScalar sqrDistLowerBound;
     CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
     // request.break_distance = ?
     if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
@@ -287,8 +286,8 @@ struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
 
 template <>
 struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
-  static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
-    FCL_REAL sqrDistLowerBound;
+  static CoalScalar run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
+    CoalScalar sqrDistLowerBound;
     CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
     // request.break_distance = ?
     if (b1.overlap(b2, request, sqrDistLowerBound)) {
@@ -297,9 +296,9 @@ struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
     }
     return sqrt(sqrDistLowerBound);
   }
-  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1,
-                      const BVNode<AABB>& b2) {
-    FCL_REAL sqrDistLowerBound;
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T,
+                        const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
+    CoalScalar sqrDistLowerBound;
     CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
     // request.break_distance = ?
     if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
@@ -339,8 +338,8 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
 
   /// @brief Determine the traversal order, is the first BVTT subtree better
   bool firstOverSecond(unsigned int b1, unsigned int b2) const {
-    FCL_REAL sz1 = model1->getBV(b1).bv.size();
-    FCL_REAL sz2 = model2->getBV(b2).bv.size();
+    CoalScalar sz1 = model1->getBV(b1).bv.size();
+    CoalScalar sz2 = model2->getBV(b2).bv.size();
 
     bool l1 = model1->getBV(b1).isLeaf();
     bool l2 = model2->getBV(b2).isLeaf();
@@ -377,7 +376,7 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
   /// @brief statistical information
   mutable int num_bv_tests;
   mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
+  mutable CoalScalar query_time_seconds;
 };
 
 /// @brief Traversal node for distance computation between two meshes
@@ -417,7 +416,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
   }
 
   /// @brief BV culling test in one BVTT node
-  FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
     if (enable_statistics) num_bv_tests++;
     if (RTIsIdentity)
       return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
@@ -440,47 +439,47 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
     const Triangle& tri_id1 = tri_indices1[primitive_id1];
     const Triangle& tri_id2 = tri_indices2[primitive_id2];
 
-    const Vec3f& t11 = vertices1[tri_id1[0]];
-    const Vec3f& t12 = vertices1[tri_id1[1]];
-    const Vec3f& t13 = vertices1[tri_id1[2]];
+    const Vec3s& t11 = vertices1[tri_id1[0]];
+    const Vec3s& t12 = vertices1[tri_id1[1]];
+    const Vec3s& t13 = vertices1[tri_id1[2]];
 
-    const Vec3f& t21 = vertices2[tri_id2[0]];
-    const Vec3f& t22 = vertices2[tri_id2[1]];
-    const Vec3f& t23 = vertices2[tri_id2[2]];
+    const Vec3s& t21 = vertices2[tri_id2[0]];
+    const Vec3s& t22 = vertices2[tri_id2[1]];
+    const Vec3s& t23 = vertices2[tri_id2[2]];
 
     // nearest point pair
-    Vec3f P1, P2, normal;
+    Vec3s P1, P2, normal;
 
-    FCL_REAL d2;
+    CoalScalar d2;
     if (RTIsIdentity)
       d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
                                             P2);
     else
       d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
                                             RT._R(), RT._T(), P1, P2);
-    FCL_REAL d = sqrt(d2);
+    CoalScalar d = sqrt(d2);
 
     this->result->update(d, this->model1, this->model2, primitive_id1,
                          primitive_id2, P1, P2, normal);
   }
 
   /// @brief Whether the traversal process can stop early
-  bool canStop(FCL_REAL c) const {
+  bool canStop(CoalScalar c) const {
     if ((c >= this->result->min_distance - abs_err) &&
         (c * (1 + rel_err) >= this->result->min_distance))
       return true;
     return false;
   }
 
-  Vec3f* vertices1;
-  Vec3f* vertices2;
+  Vec3s* vertices1;
+  Vec3s* vertices2;
 
   Triangle* tri_indices1;
   Triangle* tri_indices2;
 
   /// @brief relative and absolute error, default value is 0.01 for both terms
-  FCL_REAL rel_err;
-  FCL_REAL abs_err;
+  CoalScalar rel_err;
+  CoalScalar abs_err;
 
   details::RelativeTransformation<!bool(RTIsIdentity)> RT;
 
@@ -490,8 +489,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
     const Triangle& init_tri1 = tri_indices1[init_tri_id1];
     const Triangle& init_tri2 = tri_indices2[init_tri_id2];
 
-    Vec3f init_tri1_points[3];
-    Vec3f init_tri2_points[3];
+    Vec3s init_tri1_points[3];
+    Vec3s init_tri2_points[3];
 
     init_tri1_points[0] = vertices1[init_tri1[0]];
     init_tri1_points[1] = vertices1[init_tri1[1]];
@@ -501,8 +500,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
     init_tri2_points[1] = vertices2[init_tri2[1]];
     init_tri2_points[2] = vertices2[init_tri2[2]];
 
-    Vec3f p1, p2, normal;
-    FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance(
+    Vec3s p1, p2, normal;
+    CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance(
         init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
         init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
         RT._T(), p1, p2));
@@ -535,20 +534,18 @@ typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
 namespace details {
 
 template <typename BV>
-inline const Matrix3f& getBVAxes(const BV& bv) {
+inline const Matrix3s& getBVAxes(const BV& bv) {
   return bv.axes;
 }
 
 template <>
-inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
+inline const Matrix3s& getBVAxes<OBBRSS>(const OBBRSS& bv) {
   return bv.obb.axes;
 }
 
 }  // namespace details
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
 
 /// @endcond
 
diff --git a/include/coal/internal/traversal_node_bvh_shape.h b/include/coal/internal/traversal_node_bvh_shape.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b85879a550703d8302d266efa78145507843f8c
--- /dev/null
+++ b/include/coal/internal/traversal_node_bvh_shape.h
@@ -0,0 +1,485 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_TRAVERSAL_NODE_MESH_SHAPE_H
+#define COAL_TRAVERSAL_NODE_MESH_SHAPE_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/internal/shape_shape_func.h"
+
+namespace coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @brief Traversal node for collision between BVH and shape
+template <typename BV, typename S>
+class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase {
+ public:
+  BVHShapeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  const BVHModel<BV>* model1;
+  const S* model2;
+  BV model2_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for collision between mesh and shape
+template <typename BV, typename S,
+          int _Options = RelativeTransformationIsIdentity>
+class MeshShapeCollisionTraversalNode
+    : public BVHShapeCollisionTraversalNode<BV, S> {
+ public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  MeshShapeCollisionTraversalNode(const CollisionRequest& request)
+      : BVHShapeCollisionTraversalNode<BV, S>(request) {
+    vertices = NULL;
+    tri_indices = NULL;
+
+    nsolver = NULL;
+  }
+
+  /// test between BV b1 and shape
+  /// @param b1 BV to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  /// @brief BV culling test in one BVTT node
+  bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
+                   CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    bool disjoint;
+    if (RTIsIdentity)
+      disjoint = !this->model1->getBV(b1).bv.overlap(
+          this->model2_bv, this->request, sqrDistLowerBound);
+    else
+      disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
+                          this->model1->getBV(b1).bv, this->model2_bv,
+                          this->request, sqrDistLowerBound);
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+    assert(!disjoint || sqrDistLowerBound > 0);
+    return disjoint;
+  }
+
+  /// @brief Intersection testing between leaves (one triangle and one shape)
+  void leafCollides(unsigned int b1, unsigned int /*b2*/,
+                    CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+    const BVNode<BV>& node = this->model1->getBV(b1);
+
+    int primitive_id = node.primitiveId();
+
+    const Triangle& tri_id = this->tri_indices[primitive_id];
+    const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
+                        this->vertices[tri_id[2]]);
+
+    // When reaching this point, `this->solver` has already been set up
+    // by the CollisionRequest `this->request`.
+    // The only thing we need to (and can) pass to `ShapeShapeDistance` is
+    // whether or not penetration information is should be computed in case of
+    // collision.
+    const bool compute_penetration =
+        this->request.enable_contact || (this->request.security_margin < 0);
+    Vec3s c1, c2, normal;
+    CoalScalar distance;
+
+    if (RTIsIdentity) {
+      static const Transform3s Id;
+      distance = internal::ShapeShapeDistance<TriangleP, S>(
+          &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration,
+          c1, c2, normal);
+    } else {
+      distance = internal::ShapeShapeDistance<TriangleP, S>(
+          &tri, this->tf1, this->model2, this->tf2, this->nsolver,
+          compute_penetration, c1, c2, normal);
+    }
+    const CoalScalar distToCollision = distance - this->request.security_margin;
+
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result),
+                                               distToCollision, c1, c2, normal);
+
+    if (distToCollision <= this->request.collision_distance_threshold) {
+      sqrDistLowerBound = 0;
+      if (this->result->numContacts() < this->request.num_max_contacts) {
+        this->result->addContact(Contact(this->model1, this->model2,
+                                         primitive_id, Contact::NONE, c1, c2,
+                                         normal, distance));
+        assert(this->result->isCollision());
+      }
+    } else {
+      sqrDistLowerBound = distToCollision * distToCollision;
+    }
+
+    assert(this->result->isCollision() || sqrDistLowerBound > 0);
+  }  // leafCollides
+
+  Vec3s* vertices;
+  Triangle* tri_indices;
+
+  const GJKSolver* nsolver;
+};
+
+/// @}
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for distance computation between BVH and shape
+template <typename BV, typename S>
+class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    return model1->getBV(b1).bv.distance(model2_bv);
+  }
+
+  const BVHModel<BV>* model1;
+  const S* model2;
+  BV model2_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for distance computation between shape and BVH
+template <typename S, typename BV>
+class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the second BVH tree is leaf
+  bool isSecondNodeLeaf(unsigned int b) const {
+    return model2->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the second BVH
+  int getSecondLeftChild(unsigned int b) const {
+    return model2->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the second BVH
+  int getSecondRightChild(unsigned int b) const {
+    return model2->getBV(b).rightChild();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const {
+    return model1_bv.distance(model2->getBV(b2).bv);
+  }
+
+  const S* model1;
+  const BVHModel<BV>* model2;
+  BV model1_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for distance between mesh and shape
+template <typename BV, typename S>
+class MeshShapeDistanceTraversalNode
+    : public BVHShapeDistanceTraversalNode<BV, S> {
+ public:
+  MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>() {
+    vertices = NULL;
+    tri_indices = NULL;
+
+    rel_err = 0;
+    abs_err = 0;
+
+    nsolver = NULL;
+  }
+
+  /// @brief Distance testing between leaves (one triangle and one shape)
+  void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node = this->model1->getBV(b1);
+
+    int primitive_id = node.primitiveId();
+
+    const Triangle& tri_id = tri_indices[primitive_id];
+    const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
+                        this->vertices[tri_id[2]]);
+
+    Vec3s p1, p2, normal;
+    const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
+        &tri, this->tf1, this->model2, this->tf2, this->nsolver,
+        this->request.enable_signed_distance, p1, p2, normal);
+
+    this->result->update(distance, this->model1, this->model2, primitive_id,
+                         DistanceResult::NONE, p1, p2, normal);
+  }
+
+  /// @brief Whether the traversal process can stop early
+  bool canStop(CoalScalar c) const {
+    if ((c >= this->result->min_distance - abs_err) &&
+        (c * (1 + rel_err) >= this->result->min_distance))
+      return true;
+    return false;
+  }
+
+  Vec3s* vertices;
+  Triangle* tri_indices;
+
+  CoalScalar rel_err;
+  CoalScalar abs_err;
+
+  const GJKSolver* nsolver;
+};
+
+/// @cond IGNORE
+namespace details {
+
+template <typename BV, typename S>
+void meshShapeDistanceOrientedNodeleafComputeDistance(
+    unsigned int b1, unsigned int /* b2 */, const BVHModel<BV>* model1,
+    const S& model2, Vec3s* vertices, Triangle* tri_indices,
+    const Transform3s& tf1, const Transform3s& tf2, const GJKSolver* nsolver,
+    bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request,
+    DistanceResult& result) {
+  if (enable_statistics) num_leaf_tests++;
+
+  const BVNode<BV>& node = model1->getBV(b1);
+  int primitive_id = node.primitiveId();
+
+  const Triangle& tri_id = tri_indices[primitive_id];
+  const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
+                      vertices[tri_id[2]]);
+
+  Vec3s p1, p2, normal;
+  const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
+      &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
+      normal);
+
+  result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE,
+                p1, p2, normal);
+}
+
+template <typename BV, typename S>
+static inline void distancePreprocessOrientedNode(
+    const BVHModel<BV>* model1, Vec3s* vertices, Triangle* tri_indices,
+    int init_tri_id, const S& model2, const Transform3s& tf1,
+    const Transform3s& tf2, const GJKSolver* nsolver,
+    const DistanceRequest& request, DistanceResult& result) {
+  const Triangle& tri_id = tri_indices[init_tri_id];
+  const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
+                      vertices[tri_id[2]]);
+
+  Vec3s p1, p2, normal;
+  const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
+      &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
+      normal);
+  result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE,
+                p1, p2, normal);
+}
+
+}  // namespace details
+
+/// @endcond
+
+/// @brief Traversal node for distance between mesh and shape, when mesh BVH is
+/// one of the oriented node (RSS, kIOS, OBBRSS)
+template <typename S>
+class MeshShapeDistanceTraversalNodeRSS
+    : public MeshShapeDistanceTraversalNode<RSS, S> {
+ public:
+  MeshShapeDistanceTraversalNodeRSS()
+      : MeshShapeDistanceTraversalNode<RSS, S>() {}
+
+  void preprocess() {
+    details::distancePreprocessOrientedNode(
+        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
+        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
+  }
+
+  void postprocess() {}
+
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
+                    this->model2_bv, this->model1->getBV(b1).bv);
+  }
+
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    details::meshShapeDistanceOrientedNodeleafComputeDistance(
+        b1, b2, this->model1, *(this->model2), this->vertices,
+        this->tri_indices, this->tf1, this->tf2, this->nsolver,
+        this->enable_statistics, this->num_leaf_tests, this->request,
+        *(this->result));
+  }
+};
+
+template <typename S>
+class MeshShapeDistanceTraversalNodekIOS
+    : public MeshShapeDistanceTraversalNode<kIOS, S> {
+ public:
+  MeshShapeDistanceTraversalNodekIOS()
+      : MeshShapeDistanceTraversalNode<kIOS, S>() {}
+
+  void preprocess() {
+    details::distancePreprocessOrientedNode(
+        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
+        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
+  }
+
+  void postprocess() {}
+
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
+                    this->model2_bv, this->model1->getBV(b1).bv);
+  }
+
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    details::meshShapeDistanceOrientedNodeleafComputeDistance(
+        b1, b2, this->model1, *(this->model2), this->vertices,
+        this->tri_indices, this->tf1, this->tf2, this->nsolver,
+        this->enable_statistics, this->num_leaf_tests, this->request,
+        *(this->result));
+  }
+};
+
+template <typename S>
+class MeshShapeDistanceTraversalNodeOBBRSS
+    : public MeshShapeDistanceTraversalNode<OBBRSS, S> {
+ public:
+  MeshShapeDistanceTraversalNodeOBBRSS()
+      : MeshShapeDistanceTraversalNode<OBBRSS, S>() {}
+
+  void preprocess() {
+    details::distancePreprocessOrientedNode(
+        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
+        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
+  }
+
+  void postprocess() {}
+
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
+                    this->model2_bv, this->model1->getBV(b1).bv);
+  }
+
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    details::meshShapeDistanceOrientedNodeleafComputeDistance(
+        b1, b2, this->model1, *(this->model2), this->vertices,
+        this->tri_indices, this->tf1, this->tf2, this->nsolver,
+        this->enable_statistics, this->num_leaf_tests, this->request,
+        *(this->result));
+  }
+};
+
+/// @}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/traversal_node_bvhs.h b/include/coal/internal/traversal_node_bvhs.h
new file mode 100644
index 0000000000000000000000000000000000000000..a7b2446f41af7aab8bf9574e113d8f74c8894742
--- /dev/null
+++ b/include/coal/internal/traversal_node_bvhs.h
@@ -0,0 +1,554 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_TRAVERSAL_NODE_MESHES_H
+#define COAL_TRAVERSAL_NODE_MESHES_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/BV/BV_node.h"
+#include "coal/BV/BV.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/internal/intersect.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/internal/traversal.h"
+#include "coal/internal/shape_shape_func.h"
+
+#include <cassert>
+
+namespace coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @brief Traversal node for collision between BVH models
+template <typename BV>
+class BVHCollisionTraversalNode : public CollisionTraversalNodeBase {
+ public:
+  BVHCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    assert(model1 != NULL && "model1 is NULL");
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Whether the BV node in the second BVH tree is leaf
+  bool isSecondNodeLeaf(unsigned int b) const {
+    assert(model2 != NULL && "model2 is NULL");
+    return model2->getBV(b).isLeaf();
+  }
+
+  /// @brief Determine the traversal order, is the first BVTT subtree better
+  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
+    CoalScalar sz1 = model1->getBV(b1).bv.size();
+    CoalScalar sz2 = model2->getBV(b2).bv.size();
+
+    bool l1 = model1->getBV(b1).isLeaf();
+    bool l2 = model2->getBV(b2).isLeaf();
+
+    if (l2 || (!l1 && (sz1 > sz2))) return true;
+    return false;
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief Obtain the left child of BV node in the second BVH
+  int getSecondLeftChild(unsigned int b) const {
+    return model2->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the second BVH
+  int getSecondRightChild(unsigned int b) const {
+    return model2->getBV(b).rightChild();
+  }
+
+  /// @brief The first BVH model
+  const BVHModel<BV>* model1;
+  /// @brief The second BVH model
+  const BVHModel<BV>* model2;
+
+  /// @brief statistical information
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for collision between two meshes
+template <typename BV, int _Options = RelativeTransformationIsIdentity>
+class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
+ public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  MeshCollisionTraversalNode(const CollisionRequest& request)
+      : BVHCollisionTraversalNode<BV>(request) {
+    vertices1 = NULL;
+    vertices2 = NULL;
+    tri_indices1 = NULL;
+    tri_indices2 = NULL;
+  }
+
+  /// BV test between b1 and b2
+  /// @param b1, b2 Bounding volumes to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  bool BVDisjoints(unsigned int b1, unsigned int b2,
+                   CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    bool disjoint;
+    if (RTIsIdentity)
+      disjoint = !this->model1->getBV(b1).overlap(
+          this->model2->getBV(b2), this->request, sqrDistLowerBound);
+    else {
+      disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
+                          this->model1->getBV(b1).bv, this->request,
+                          sqrDistLowerBound);
+    }
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+    return disjoint;
+  }
+
+  /// Intersection testing between leaves (two triangles)
+  ///
+  /// @param b1, b2 id of primitive in bounding volume hierarchy
+  /// @retval sqrDistLowerBound squared lower bound of distance between
+  ///         primitives if they are not in collision.
+  ///
+  /// This method supports a security margin. If the distance between
+  /// the primitives is less than the security margin, the objects are
+  /// considered as in collision. in this case a contact point is
+  /// returned in the CollisionResult.
+  ///
+  /// @note If the distance between objects is less than the security margin,
+  ///       and the object are not colliding, the penetration depth is
+  ///       negative.
+  void leafCollides(unsigned int b1, unsigned int b2,
+                    CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node1 = this->model1->getBV(b1);
+    const BVNode<BV>& node2 = this->model2->getBV(b2);
+
+    int primitive_id1 = node1.primitiveId();
+    int primitive_id2 = node2.primitiveId();
+
+    const Triangle& tri_id1 = tri_indices1[primitive_id1];
+    const Triangle& tri_id2 = tri_indices2[primitive_id2];
+
+    const Vec3s& P1 = vertices1[tri_id1[0]];
+    const Vec3s& P2 = vertices1[tri_id1[1]];
+    const Vec3s& P3 = vertices1[tri_id1[2]];
+    const Vec3s& Q1 = vertices2[tri_id2[0]];
+    const Vec3s& Q2 = vertices2[tri_id2[1]];
+    const Vec3s& Q3 = vertices2[tri_id2[2]];
+
+    TriangleP tri1(P1, P2, P3);
+    TriangleP tri2(Q1, Q2, Q3);
+
+    // TODO(louis): MeshCollisionTraversalNode should have its own GJKSolver.
+    GJKSolver solver(this->request);
+
+    const bool compute_penetration =
+        this->request.enable_contact || (this->request.security_margin < 0);
+    Vec3s p1, p2, normal;
+    DistanceResult distanceResult;
+    CoalScalar distance = internal::ShapeShapeDistance<TriangleP, TriangleP>(
+        &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1,
+        p2, normal);
+
+    const CoalScalar distToCollision = distance - this->request.security_margin;
+
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result),
+                                               distToCollision, p1, p2, normal);
+
+    if (distToCollision <=
+        this->request.collision_distance_threshold) {  // collision
+      sqrDistLowerBound = 0;
+      if (this->result->numContacts() < this->request.num_max_contacts) {
+        this->result->addContact(Contact(this->model1, this->model2,
+                                         primitive_id1, primitive_id2, p1, p2,
+                                         normal, distance));
+      }
+    } else
+      sqrDistLowerBound = distToCollision * distToCollision;
+  }
+
+  Vec3s* vertices1;
+  Vec3s* vertices2;
+
+  Triangle* tri_indices1;
+  Triangle* tri_indices2;
+
+  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
+};
+
+/// @brief Traversal node for collision between two meshes if their underlying
+/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
+typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB;
+typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS;
+typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS;
+typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
+
+/// @}
+
+namespace details {
+template <typename BV>
+struct DistanceTraversalBVDistanceLowerBound_impl {
+  static CoalScalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
+    return b1.distance(b2);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode<BV>& b1,
+                        const BVNode<BV>& b2) {
+    return distance(R, T, b1.bv, b2.bv);
+  }
+};
+
+template <>
+struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
+  static CoalScalar run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (b1.overlap(b2, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T,
+                        const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+};
+
+template <>
+struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
+  static CoalScalar run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (b1.overlap(b2, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T,
+                        const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+};
+}  // namespace details
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for distance computation between BVH models
+template <typename BV>
+class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Whether the BV node in the second BVH tree is leaf
+  bool isSecondNodeLeaf(unsigned int b) const {
+    return model2->getBV(b).isLeaf();
+  }
+
+  /// @brief Determine the traversal order, is the first BVTT subtree better
+  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
+    CoalScalar sz1 = model1->getBV(b1).bv.size();
+    CoalScalar sz2 = model2->getBV(b2).bv.size();
+
+    bool l1 = model1->getBV(b1).isLeaf();
+    bool l2 = model2->getBV(b2).isLeaf();
+
+    if (l2 || (!l1 && (sz1 > sz2))) return true;
+    return false;
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief Obtain the left child of BV node in the second BVH
+  int getSecondLeftChild(unsigned int b) const {
+    return model2->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the second BVH
+  int getSecondRightChild(unsigned int b) const {
+    return model2->getBV(b).rightChild();
+  }
+
+  /// @brief The first BVH model
+  const BVHModel<BV>* model1;
+  /// @brief The second BVH model
+  const BVHModel<BV>* model2;
+
+  /// @brief statistical information
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for distance computation between two meshes
+template <typename BV, int _Options = RelativeTransformationIsIdentity>
+class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
+ public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  using BVHDistanceTraversalNode<BV>::enable_statistics;
+  using BVHDistanceTraversalNode<BV>::request;
+  using BVHDistanceTraversalNode<BV>::result;
+  using BVHDistanceTraversalNode<BV>::tf1;
+  using BVHDistanceTraversalNode<BV>::model1;
+  using BVHDistanceTraversalNode<BV>::model2;
+  using BVHDistanceTraversalNode<BV>::num_bv_tests;
+  using BVHDistanceTraversalNode<BV>::num_leaf_tests;
+
+  MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
+    vertices1 = NULL;
+    vertices2 = NULL;
+    tri_indices1 = NULL;
+    tri_indices2 = NULL;
+
+    rel_err = this->request.rel_err;
+    abs_err = this->request.abs_err;
+  }
+
+  void preprocess() {
+    if (!RTIsIdentity) preprocessOrientedNode();
+  }
+
+  void postprocess() {
+    if (!RTIsIdentity) postprocessOrientedNode();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
+    if (enable_statistics) num_bv_tests++;
+    if (RTIsIdentity)
+      return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
+          model1->getBV(b1), model2->getBV(b2));
+    else
+      return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
+          RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
+  }
+
+  /// @brief Distance testing between leaves (two triangles)
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node1 = this->model1->getBV(b1);
+    const BVNode<BV>& node2 = this->model2->getBV(b2);
+
+    int primitive_id1 = node1.primitiveId();
+    int primitive_id2 = node2.primitiveId();
+
+    const Triangle& tri_id1 = tri_indices1[primitive_id1];
+    const Triangle& tri_id2 = tri_indices2[primitive_id2];
+
+    const Vec3s& t11 = vertices1[tri_id1[0]];
+    const Vec3s& t12 = vertices1[tri_id1[1]];
+    const Vec3s& t13 = vertices1[tri_id1[2]];
+
+    const Vec3s& t21 = vertices2[tri_id2[0]];
+    const Vec3s& t22 = vertices2[tri_id2[1]];
+    const Vec3s& t23 = vertices2[tri_id2[2]];
+
+    // nearest point pair
+    Vec3s P1, P2, normal;
+
+    CoalScalar d2;
+    if (RTIsIdentity)
+      d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
+                                            P2);
+    else
+      d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
+                                            RT._R(), RT._T(), P1, P2);
+    CoalScalar d = sqrt(d2);
+
+    this->result->update(d, this->model1, this->model2, primitive_id1,
+                         primitive_id2, P1, P2, normal);
+  }
+
+  /// @brief Whether the traversal process can stop early
+  bool canStop(CoalScalar c) const {
+    if ((c >= this->result->min_distance - abs_err) &&
+        (c * (1 + rel_err) >= this->result->min_distance))
+      return true;
+    return false;
+  }
+
+  Vec3s* vertices1;
+  Vec3s* vertices2;
+
+  Triangle* tri_indices1;
+  Triangle* tri_indices2;
+
+  /// @brief relative and absolute error, default value is 0.01 for both terms
+  CoalScalar rel_err;
+  CoalScalar abs_err;
+
+  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
+
+ private:
+  void preprocessOrientedNode() {
+    const int init_tri_id1 = 0, init_tri_id2 = 0;
+    const Triangle& init_tri1 = tri_indices1[init_tri_id1];
+    const Triangle& init_tri2 = tri_indices2[init_tri_id2];
+
+    Vec3s init_tri1_points[3];
+    Vec3s init_tri2_points[3];
+
+    init_tri1_points[0] = vertices1[init_tri1[0]];
+    init_tri1_points[1] = vertices1[init_tri1[1]];
+    init_tri1_points[2] = vertices1[init_tri1[2]];
+
+    init_tri2_points[0] = vertices2[init_tri2[0]];
+    init_tri2_points[1] = vertices2[init_tri2[1]];
+    init_tri2_points[2] = vertices2[init_tri2[2]];
+
+    Vec3s p1, p2, normal;
+    CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance(
+        init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
+        init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
+        RT._T(), p1, p2));
+
+    result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
+                   normal);
+  }
+  void postprocessOrientedNode() {
+    /// the points obtained by triDistance are not in world space: both are in
+    /// object1's local coordinate system, so we need to convert them into the
+    /// world space.
+    if (request.enable_nearest_points && (result->o1 == model1) &&
+        (result->o2 == model2)) {
+      result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
+      result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
+    }
+  }
+};
+
+/// @brief Traversal node for distance computation between two meshes if their
+/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
+typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
+typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
+typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
+
+/// @}
+
+/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to
+/// be transformed
+namespace details {
+
+template <typename BV>
+inline const Matrix3s& getBVAxes(const BV& bv) {
+  return bv.axes;
+}
+
+template <>
+inline const Matrix3s& getBVAxes<OBBRSS>(const OBBRSS& bv) {
+  return bv.obb.axes;
+}
+
+}  // namespace details
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h
new file mode 100644
index 0000000000000000000000000000000000000000..20e6202c26959c60677d3822cd8c5ac413af6766
--- /dev/null
+++ b/include/coal/internal/traversal_node_hfield_shape.h
@@ -0,0 +1,756 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021-2024, INRIA.
+ *  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 COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H
+#define COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/shape_shape_func.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal.h"
+#include "coal/internal/intersect.h"
+#include "coal/hfield.h"
+#include "coal/shape/convex.h"
+
+namespace coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+namespace details {
+template <typename BV>
+Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
+                                               const HeightField<BV>& model) {
+  const MatrixXs& heights = model.getHeights();
+  const VecXs& x_grid = model.getXGrid();
+  const VecXs& y_grid = model.getYGrid();
+
+  const CoalScalar min_height = model.getMinHeight();
+
+  const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
+                   y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
+  const Eigen::Block<const MatrixXs, 2, 2> cell =
+      heights.block<2, 2>(node.y_id, node.x_id);
+
+  assert(cell.maxCoeff() > min_height &&
+         "max_height is lower than min_height");  // Check whether the geometry
+                                                  // is degenerated
+
+  std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+      Vec3s(x0, y0, min_height),
+      Vec3s(x0, y1, min_height),
+      Vec3s(x1, y1, min_height),
+      Vec3s(x1, y0, min_height),
+      Vec3s(x0, y0, cell(0, 0)),
+      Vec3s(x0, y1, cell(1, 0)),
+      Vec3s(x1, y1, cell(1, 1)),
+      Vec3s(x1, y0, cell(0, 1)),
+  }));
+
+  std::shared_ptr<std::vector<Quadrilateral>> polygons(
+      new std::vector<Quadrilateral>(6));
+  (*polygons)[0].set(0, 3, 2, 1);  // x+ side
+  (*polygons)[1].set(0, 1, 5, 4);  // y- side
+  (*polygons)[2].set(1, 2, 6, 5);  // x- side
+  (*polygons)[3].set(2, 3, 7, 6);  // y+ side
+  (*polygons)[4].set(3, 0, 4, 7);  // z- side
+  (*polygons)[5].set(4, 5, 6, 7);  // z+ side
+
+  return Convex<Quadrilateral>(pts,  // points
+                               8,    // num points
+                               polygons,
+                               6  // number of polygons
+  );
+}
+
+enum class FaceOrientationConvexPart1 {
+  BOTTOM = 0,
+  TOP = 1,
+  WEST = 2,
+  SOUTH_EAST = 4,
+  NORTH = 8,
+};
+
+enum class FaceOrientationConvexPart2 {
+  BOTTOM = 0,
+  TOP = 1,
+  SOUTH = 2,
+  NORTH_WEST = 4,
+  EAST = 8,
+};
+
+template <typename BV>
+void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
+                          Convex<Triangle>& convex1, int& convex1_active_faces,
+                          Convex<Triangle>& convex2,
+                          int& convex2_active_faces) {
+  const MatrixXs& heights = model.getHeights();
+  const VecXs& x_grid = model.getXGrid();
+  const VecXs& y_grid = model.getYGrid();
+
+  const CoalScalar min_height = model.getMinHeight();
+
+  const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
+                   y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
+  const CoalScalar max_height = node.max_height;
+  const Eigen::Block<const MatrixXs, 2, 2> cell =
+      heights.block<2, 2>(node.y_id, node.x_id);
+
+  const int contact_active_faces = node.contact_active_faces;
+  convex1_active_faces = 0;
+  convex2_active_faces = 0;
+
+  typedef HFNodeBase::FaceOrientation FaceOrientation;
+
+  if (contact_active_faces & FaceOrientation::TOP) {
+    convex1_active_faces |= int(details::FaceOrientationConvexPart1::TOP);
+    convex2_active_faces |= int(details::FaceOrientationConvexPart2::TOP);
+  }
+
+  if (contact_active_faces & FaceOrientation::BOTTOM) {
+    convex1_active_faces |= int(details::FaceOrientationConvexPart1::BOTTOM);
+    convex2_active_faces |= int(details::FaceOrientationConvexPart2::BOTTOM);
+  }
+
+  // Specific orientation for Convex1
+  if (contact_active_faces & FaceOrientation::WEST) {
+    convex1_active_faces |= int(details::FaceOrientationConvexPart1::WEST);
+  }
+
+  if (contact_active_faces & FaceOrientation::NORTH) {
+    convex1_active_faces |= int(details::FaceOrientationConvexPart1::NORTH);
+  }
+
+  // Specific orientation for Convex2
+  if (contact_active_faces & FaceOrientation::EAST) {
+    convex2_active_faces |= int(details::FaceOrientationConvexPart2::EAST);
+  }
+
+  if (contact_active_faces & FaceOrientation::SOUTH) {
+    convex2_active_faces |= int(details::FaceOrientationConvexPart2::SOUTH);
+  }
+
+  assert(max_height > min_height &&
+         "max_height is lower than min_height");  // Check whether the geometry
+                                                  // is degenerated
+  COAL_UNUSED_VARIABLE(max_height);
+
+  {
+    std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+        Vec3s(x0, y0, min_height),  // A
+        Vec3s(x0, y1, min_height),  // B
+        Vec3s(x1, y0, min_height),  // C
+        Vec3s(x0, y0, cell(0, 0)),  // D
+        Vec3s(x0, y1, cell(1, 0)),  // E
+        Vec3s(x1, y0, cell(0, 1)),  // F
+    }));
+
+    std::shared_ptr<std::vector<Triangle>> triangles(
+        new std::vector<Triangle>(8));
+    (*triangles)[0].set(0, 2, 1);  // bottom
+    (*triangles)[1].set(3, 4, 5);  // top
+    (*triangles)[2].set(0, 1, 3);  // West 1
+    (*triangles)[3].set(3, 1, 4);  // West 2
+    (*triangles)[4].set(1, 2, 5);  // South-East 1
+    (*triangles)[5].set(1, 5, 4);  // South-East 1
+    (*triangles)[6].set(0, 5, 2);  // North 1
+    (*triangles)[7].set(5, 0, 3);  // North 2
+
+    convex1.set(pts,  // points
+                6,    // num points
+                triangles,
+                8  // number of polygons
+    );
+  }
+
+  {
+    std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+        Vec3s(x0, y1, min_height),  // A
+        Vec3s(x1, y1, min_height),  // B
+        Vec3s(x1, y0, min_height),  // C
+        Vec3s(x0, y1, cell(1, 0)),  // D
+        Vec3s(x1, y1, cell(1, 1)),  // E
+        Vec3s(x1, y0, cell(0, 1)),  // F
+    }));
+
+    std::shared_ptr<std::vector<Triangle>> triangles(
+        new std::vector<Triangle>(8));
+    (*triangles)[0].set(2, 1, 0);  // bottom
+    (*triangles)[1].set(3, 4, 5);  // top
+    (*triangles)[2].set(0, 1, 3);  // South 1
+    (*triangles)[3].set(3, 1, 4);  // South 2
+    (*triangles)[4].set(0, 5, 2);  // North West 1
+    (*triangles)[5].set(0, 3, 5);  // North West 2
+    (*triangles)[6].set(1, 2, 5);  // East 1
+    (*triangles)[7].set(4, 1, 2);  // East 2
+
+    convex2.set(pts,  // points
+                6,    // num points
+                triangles,
+                8  // number of polygons
+    );
+  }
+}
+
+inline Vec3s projectTriangle(const Vec3s& pointA, const Vec3s& pointB,
+                             const Vec3s& pointC, const Vec3s& point) {
+  const Project::ProjectResult result =
+      Project::projectTriangle(pointA, pointB, pointC, point);
+  Vec3s res = result.parameterization[0] * pointA +
+              result.parameterization[1] * pointB +
+              result.parameterization[2] * pointC;
+
+  return res;
+}
+
+inline Vec3s projectTetrahedra(const Vec3s& pointA, const Vec3s& pointB,
+                               const Vec3s& pointC, const Vec3s& pointD,
+                               const Vec3s& point) {
+  const Project::ProjectResult result =
+      Project::projectTetrahedra(pointA, pointB, pointC, pointD, point);
+  Vec3s res = result.parameterization[0] * pointA +
+              result.parameterization[1] * pointB +
+              result.parameterization[2] * pointC +
+              result.parameterization[3] * pointD;
+
+  return res;
+}
+
+inline Vec3s computeTriangleNormal(const Triangle& triangle,
+                                   const std::vector<Vec3s>& points) {
+  const Vec3s pointA = points[triangle[0]];
+  const Vec3s pointB = points[triangle[1]];
+  const Vec3s pointC = points[triangle[2]];
+
+  const Vec3s normal = (pointB - pointA).cross(pointC - pointA).normalized();
+  assert(!normal.array().isNaN().any() && "normal is ill-defined");
+
+  return normal;
+}
+
+inline Vec3s projectPointOnTriangle(const Vec3s& contact_point,
+                                    const Triangle& triangle,
+                                    const std::vector<Vec3s>& points) {
+  const Vec3s pointA = points[triangle[0]];
+  const Vec3s pointB = points[triangle[1]];
+  const Vec3s pointC = points[triangle[2]];
+
+  const Vec3s contact_point_projected =
+      projectTriangle(pointA, pointB, pointC, contact_point);
+
+  return contact_point_projected;
+}
+
+inline CoalScalar distanceContactPointToTriangle(
+    const Vec3s& contact_point, const Triangle& triangle,
+    const std::vector<Vec3s>& points) {
+  const Vec3s contact_point_projected =
+      projectPointOnTriangle(contact_point, triangle, points);
+  return (contact_point_projected - contact_point).norm();
+}
+
+inline CoalScalar distanceContactPointToFace(const size_t face_id,
+                                             const Vec3s& contact_point,
+                                             const Convex<Triangle>& convex,
+                                             size_t& closest_face_id) {
+  assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]");
+
+  const std::vector<Vec3s>& points = *(convex.points);
+  if (face_id <= 1) {
+    const Triangle& triangle = (*(convex.polygons))[face_id];
+    closest_face_id = face_id;
+    return distanceContactPointToTriangle(contact_point, triangle, points);
+  } else {
+    const Triangle& triangle1 = (*(convex.polygons))[face_id];
+    const CoalScalar distance_to_triangle1 =
+        distanceContactPointToTriangle(contact_point, triangle1, points);
+
+    const Triangle& triangle2 = (*(convex.polygons))[face_id + 1];
+    const CoalScalar distance_to_triangle2 =
+        distanceContactPointToTriangle(contact_point, triangle2, points);
+
+    if (distance_to_triangle1 > distance_to_triangle2) {
+      closest_face_id = face_id + 1;
+      return distance_to_triangle2;
+    } else {
+      closest_face_id = face_id;
+      return distance_to_triangle1;
+    }
+  }
+}
+
+template <typename Polygone, typename Shape>
+bool binCorrection(const Convex<Polygone>& convex,
+                   const int convex_active_faces, const Shape& shape,
+                   const Transform3s& shape_pose, CoalScalar& distance,
+                   Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal,
+                   Vec3s& face_normal, const bool is_collision) {
+  const CoalScalar prec = 1e-12;
+  const std::vector<Vec3s>& points = *(convex.points);
+
+  bool hfield_witness_is_on_bin_side = true;
+
+  //  int closest_face_id_bottom_face = -1;
+  //  int closest_face_id_top_face = -1;
+
+  std::vector<size_t> active_faces;
+  active_faces.reserve(5);
+  active_faces.push_back(0);
+  active_faces.push_back(1);
+
+  if (convex_active_faces & 2) active_faces.push_back(2);
+  if (convex_active_faces & 4) active_faces.push_back(4);
+  if (convex_active_faces & 8) active_faces.push_back(6);
+
+  Triangle face_triangle;
+  CoalScalar shortest_distance_to_face =
+      (std::numeric_limits<CoalScalar>::max)();
+  face_normal = normal;
+  for (const size_t active_face : active_faces) {
+    size_t closest_face_id;
+    const CoalScalar distance_to_face = distanceContactPointToFace(
+        active_face, contact_1, convex, closest_face_id);
+
+    const bool contact_point_is_on_face = distance_to_face <= prec;
+    if (contact_point_is_on_face) {
+      hfield_witness_is_on_bin_side = false;
+      face_triangle = (*(convex.polygons))[closest_face_id];
+      shortest_distance_to_face = distance_to_face;
+      break;
+    } else if (distance_to_face < shortest_distance_to_face) {
+      face_triangle = (*(convex.polygons))[closest_face_id];
+      shortest_distance_to_face = distance_to_face;
+    }
+  }
+
+  // We correct only if there is a collision with the bin
+  if (is_collision) {
+    if (!face_triangle.isValid())
+      COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error);
+
+    const Vec3s face_pointA = points[face_triangle[0]];
+    face_normal = computeTriangleNormal(face_triangle, points);
+
+    int hint = 0;
+    // Since we compute the support manually, we need to take into account the
+    // sphere swept radius of the shape.
+    // TODO: take into account the swept-sphere radius of the bin.
+    const Vec3s _support = getSupport<details::SupportOptions::WithSweptSphere>(
+        &shape, -shape_pose.rotation().transpose() * face_normal, hint);
+    const Vec3s support =
+        shape_pose.rotation() * _support + shape_pose.translation();
+
+    // Project support into the inclined bin having triangle
+    const CoalScalar offset_plane = face_normal.dot(face_pointA);
+    const Plane projection_plane(face_normal, offset_plane);
+    const CoalScalar distance_support_projection_plane =
+        projection_plane.signedDistance(support);
+
+    const Vec3s projected_support =
+        support - distance_support_projection_plane * face_normal;
+
+    // We need now to project the projected in the triangle shape
+    contact_1 =
+        projectPointOnTriangle(projected_support, face_triangle, points);
+    contact_2 = contact_1 + distance_support_projection_plane * face_normal;
+    normal = face_normal;
+    distance = -std::fabs(distance_support_projection_plane);
+  }
+
+  return hfield_witness_is_on_bin_side;
+}
+
+template <typename Polygone, typename Shape, int Options>
+bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
+                   const Convex<Polygone>& convex1,
+                   const int convex1_active_faces,
+                   const Convex<Polygone>& convex2,
+                   const int convex2_active_faces, const Transform3s& tf1,
+                   const Shape& shape, const Transform3s& tf2,
+                   CoalScalar& distance, Vec3s& c1, Vec3s& c2, Vec3s& normal,
+                   Vec3s& normal_top, bool& hfield_witness_is_on_bin_side) {
+  enum { RTIsIdentity = Options & RelativeTransformationIsIdentity };
+
+  const Transform3s Id;
+  // The solver `nsolver` has already been set up by the collision request
+  // `request`. If GJK early stopping is enabled through `request`, it will be
+  // used.
+  // The only thing we need to make sure is that in case of collision, the
+  // penetration information is computed (as we do bins comparison).
+  const bool compute_penetration = true;
+  Vec3s contact1_1, contact1_2, contact2_1, contact2_2;
+  Vec3s normal1, normal1_top, normal2, normal2_top;
+  CoalScalar distance1, distance2;
+
+  if (RTIsIdentity) {
+    distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
+        &convex1, Id, &shape, tf2, nsolver, compute_penetration, contact1_1,
+        contact1_2, normal1);
+  } else {
+    distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
+        &convex1, tf1, &shape, tf2, nsolver, compute_penetration, contact1_1,
+        contact1_2, normal1);
+  }
+  bool collision1 = (distance1 - request.security_margin <=
+                     request.collision_distance_threshold);
+
+  bool hfield_witness_is_on_bin_side1 =
+      binCorrection(convex1, convex1_active_faces, shape, tf2, distance1,
+                    contact1_1, contact1_2, normal1, normal1_top, collision1);
+
+  if (RTIsIdentity) {
+    distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
+        &convex2, Id, &shape, tf2, nsolver, compute_penetration, contact2_1,
+        contact2_2, normal2);
+  } else {
+    distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
+        &convex2, tf1, &shape, tf2, nsolver, compute_penetration, contact2_1,
+        contact2_2, normal2);
+  }
+  bool collision2 = (distance2 - request.security_margin <=
+                     request.collision_distance_threshold);
+
+  bool hfield_witness_is_on_bin_side2 =
+      binCorrection(convex2, convex2_active_faces, shape, tf2, distance2,
+                    contact2_1, contact2_2, normal2, normal2_top, collision2);
+
+  if (collision1 && collision2) {
+    if (distance1 > distance2)  // switch values
+    {
+      distance = distance2;
+      c1 = contact2_1;
+      c2 = contact2_2;
+      normal = normal2;
+      normal_top = normal2_top;
+      hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
+    } else {
+      distance = distance1;
+      c1 = contact1_1;
+      c2 = contact1_2;
+      normal = normal1;
+      normal_top = normal1_top;
+      hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
+    }
+    return true;
+  } else if (collision1) {
+    distance = distance1;
+    c1 = contact1_1;
+    c2 = contact1_2;
+    normal = normal1;
+    normal_top = normal1_top;
+    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
+    return true;
+  } else if (collision2) {
+    distance = distance2;
+    c1 = contact2_1;
+    c2 = contact2_2;
+    normal = normal2;
+    normal_top = normal2_top;
+    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
+    return true;
+  }
+
+  if (distance1 > distance2)  // switch values
+  {
+    distance = distance2;
+    c1 = contact2_1;
+    c2 = contact2_2;
+    normal = normal2;
+    normal_top = normal2_top;
+    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
+  } else {
+    distance = distance1;
+    c1 = contact1_1;
+    c2 = contact1_2;
+    normal = normal1;
+    normal_top = normal1_top;
+    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
+  }
+  return false;
+}
+
+}  // namespace details
+
+/// @brief Traversal node for collision between height field and shape
+template <typename BV, typename S,
+          int _Options = RelativeTransformationIsIdentity>
+class HeightFieldShapeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  typedef CollisionTraversalNodeBase Base;
+  typedef Eigen::Array<CoalScalar, 1, 2> Array2d;
+
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+
+    nsolver = NULL;
+    count = 0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return static_cast<int>(model1->getBV(b).leftChild());
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return static_cast<int>(model1->getBV(b).rightChild());
+  }
+
+  /// test between BV b1 and shape
+  /// @param b1 BV to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  /// @brief BV culling test in one BVTT node
+  bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
+                   CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+
+    bool disjoint;
+    if (RTIsIdentity) {
+      assert(false && "must never happened");
+      disjoint = !this->model1->getBV(b1).bv.overlap(
+          this->model2_bv, this->request, sqrDistLowerBound);
+    } else {
+      disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
+                          this->model1->getBV(b1).bv, this->model2_bv,
+                          this->request, sqrDistLowerBound);
+    }
+
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+
+    assert(!disjoint || sqrDistLowerBound > 0);
+    return disjoint;
+  }
+
+  /// @brief Intersection testing between leaves (one Convex and one shape)
+  void leafCollides(unsigned int b1, unsigned int /*b2*/,
+                    CoalScalar& sqrDistLowerBound) const {
+    count++;
+    if (this->enable_statistics) this->num_leaf_tests++;
+    const HFNode<BV>& node = this->model1->getBV(b1);
+
+    // Split quadrilateral primitives into two convex shapes corresponding to
+    // polyhedron with triangular bases. This is essential to keep the convexity
+
+    //    typedef Convex<Quadrilateral> ConvexQuadrilateral;
+    //    const ConvexQuadrilateral convex =
+    //    details::buildConvexQuadrilateral(node,*this->model1);
+
+    typedef Convex<Triangle> ConvexTriangle;
+    ConvexTriangle convex1, convex2;
+    int convex1_active_faces, convex2_active_faces;
+    // TODO: inherit from hfield's inflation here
+    details::buildConvexTriangles(node, *this->model1, convex1,
+                                  convex1_active_faces, convex2,
+                                  convex2_active_faces);
+
+    // Compute aabb_local for BoundingVolumeGuess case in the GJK solver
+    if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+      convex1.computeLocalAABB();
+      convex2.computeLocalAABB();
+    }
+
+    CoalScalar distance;
+    //    Vec3s contact_point, normal;
+    Vec3s c1, c2, normal, normal_face;
+    bool hfield_witness_is_on_bin_side;
+
+    bool collision = details::shapeDistance<Triangle, S, Options>(
+        nsolver, this->request, convex1, convex1_active_faces, convex2,
+        convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance,
+        c1, c2, normal, normal_face, hfield_witness_is_on_bin_side);
+
+    CoalScalar distToCollision = distance - this->request.security_margin;
+    if (distToCollision <= this->request.collision_distance_threshold) {
+      sqrDistLowerBound = 0;
+      if (this->result->numContacts() < this->request.num_max_contacts) {
+        if (normal_face.isApprox(normal) &&
+            (collision || !hfield_witness_is_on_bin_side)) {
+          this->result->addContact(Contact(this->model1, this->model2, (int)b1,
+                                           (int)Contact::NONE, c1, c2, normal,
+                                           distance));
+          assert(this->result->isCollision());
+        }
+      }
+    } else
+      sqrDistLowerBound = distToCollision * distToCollision;
+
+    //    const Vec3s c1 = contact_point - distance * 0.5 * normal;
+    //    const Vec3s c2 = contact_point + distance * 0.5 * normal;
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
+                                               distToCollision, c1, c2, normal);
+
+    assert(this->result->isCollision() || sqrDistLowerBound > 0);
+  }
+
+  const GJKSolver* nsolver;
+
+  const HeightField<BV>* model1;
+  const S* model2;
+  BV model2_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+  mutable int count;
+};
+
+/// @}
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for distance between height field and shape
+template <typename BV, typename S,
+          int _Options = RelativeTransformationIsIdentity>
+class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  typedef DistanceTraversalNodeBase Base;
+
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+
+    rel_err = 0;
+    abs_err = 0;
+    nsolver = NULL;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    return model1->getBV(b1).bv.distance(
+        model2_bv);  // TODO(jcarpent): tf1 is not taken into account here.
+  }
+
+  /// @brief Distance testing between leaves (one bin of the height field and
+  /// one shape)
+  /// TODO(louis): deal with Hfield-Shape distance just like in Hfield-Shape
+  /// collision (bin correction etc).
+  void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node = this->model1->getBV(b1);
+
+    typedef Convex<Quadrilateral> ConvexQuadrilateral;
+    const ConvexQuadrilateral convex =
+        details::buildConvexQuadrilateral(node, *this->model1);
+
+    Vec3s p1, p2, normal;
+    const CoalScalar distance =
+        internal::ShapeShapeDistance<ConvexQuadrilateral, S>(
+            &convex, this->tf1, this->model2, this->tf2, this->nsolver,
+            this->request.enable_signed_distance, p1, p2, normal);
+
+    this->result->update(distance, this->model1, this->model2, b1,
+                         DistanceResult::NONE, p1, p2, normal);
+  }
+
+  /// @brief Whether the traversal process can stop early
+  bool canStop(CoalScalar c) const {
+    if ((c >= this->result->min_distance - abs_err) &&
+        (c * (1 + rel_err) >= this->result->min_distance))
+      return true;
+    return false;
+  }
+
+  CoalScalar rel_err;
+  CoalScalar abs_err;
+
+  const GJKSolver* nsolver;
+
+  const HeightField<BV>* model1;
+  const S* model2;
+  BV model2_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/traversal_node_octree.h b/include/coal/internal/traversal_node_octree.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f9da5f074c1a3d84b44ccdbd3b0b9df26d3523c
--- /dev/null
+++ b/include/coal/internal/traversal_node_octree.h
@@ -0,0 +1,1369 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2022-2023, INRIA
+ *  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 COAL_TRAVERSAL_NODE_OCTREE_H
+#define COAL_TRAVERSAL_NODE_OCTREE_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal_node_hfield_shape.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/hfield.h"
+#include "coal/octree.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/shape_shape_func.h"
+
+namespace coal {
+
+/// @brief Algorithms for collision related with octree
+class COAL_DLLAPI OcTreeSolver {
+ private:
+  const GJKSolver* solver;
+
+  mutable const CollisionRequest* crequest;
+  mutable const DistanceRequest* drequest;
+
+  mutable CollisionResult* cresult;
+  mutable DistanceResult* dresult;
+
+ public:
+  OcTreeSolver(const GJKSolver* solver_)
+      : solver(solver_),
+        crequest(NULL),
+        drequest(NULL),
+        cresult(NULL),
+        dresult(NULL) {}
+
+  /// @brief collision between two octrees
+  void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
+                       const Transform3s& tf1, const Transform3s& tf2,
+                       const CollisionRequest& request_,
+                       CollisionResult& result_) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
+                           tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
+  }
+
+  /// @brief distance between two octrees
+  void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
+                      const Transform3s& tf1, const Transform3s& tf2,
+                      const DistanceRequest& request_,
+                      DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
+                          tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
+  }
+
+  /// @brief collision between octree and mesh
+  template <typename BV>
+  void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
+                           const Transform3s& tf1, const Transform3s& tf2,
+                           const CollisionRequest& request_,
+                           CollisionResult& result_) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
+                               tree2, 0, tf1, tf2);
+  }
+
+  /// @brief distance between octree and mesh
+  template <typename BV>
+  void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
+                          const Transform3s& tf1, const Transform3s& tf2,
+                          const DistanceRequest& request_,
+                          DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
+                              tree2, 0, tf1, tf2);
+  }
+
+  /// @brief collision between mesh and octree
+  template <typename BV>
+  void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
+                           const Transform3s& tf1, const Transform3s& tf2,
+                           const CollisionRequest& request_,
+                           CollisionResult& result_) const
+
+  {
+    crequest = &request_;
+    cresult = &result_;
+
+    OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
+                               tree1, 0, tf2, tf1);
+  }
+
+  /// @brief distance between mesh and octree
+  template <typename BV>
+  void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
+                          const Transform3s& tf1, const Transform3s& tf2,
+                          const DistanceRequest& request_,
+                          DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(),
+                              tree2->getRootBV(), tf1, tf2);
+  }
+
+  template <typename BV>
+  void OcTreeHeightFieldIntersect(
+      const OcTree* tree1, const HeightField<BV>* tree2, const Transform3s& tf1,
+      const Transform3s& tf2, const CollisionRequest& request_,
+      CollisionResult& result_, CoalScalar& sqrDistLowerBound) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(),
+                                      tree1->getRootBV(), tree2, 0, tf1, tf2,
+                                      sqrDistLowerBound);
+  }
+
+  template <typename BV>
+  void HeightFieldOcTreeIntersect(const HeightField<BV>* tree1,
+                                  const OcTree* tree2, const Transform3s& tf1,
+                                  const Transform3s& tf2,
+                                  const CollisionRequest& request_,
+                                  CollisionResult& result_,
+                                  CoalScalar& sqrDistLowerBound) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(),
+                                      tree2->getRootBV(), tf1, tf2,
+                                      sqrDistLowerBound);
+  }
+
+  /// @brief collision between octree and shape
+  template <typename S>
+  void OcTreeShapeIntersect(const OcTree* tree, const S& s,
+                            const Transform3s& tf1, const Transform3s& tf2,
+                            const CollisionRequest& request_,
+                            CollisionResult& result_) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    AABB bv2;
+    computeBV<AABB>(s, Transform3s(), bv2);
+    OBB obb2;
+    convertBV(bv2, tf2, obb2);
+    OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
+                                obb2, tf1, tf2);
+  }
+
+  /// @brief collision between shape and octree
+  template <typename S>
+  void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
+                            const Transform3s& tf1, const Transform3s& tf2,
+                            const CollisionRequest& request_,
+                            CollisionResult& result_) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    AABB bv1;
+    computeBV<AABB>(s, Transform3s(), bv1);
+    OBB obb1;
+    convertBV(bv1, tf1, obb1);
+    OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
+                                obb1, tf2, tf1);
+  }
+
+  /// @brief distance between octree and shape
+  template <typename S>
+  void OcTreeShapeDistance(const OcTree* tree, const S& s,
+                           const Transform3s& tf1, const Transform3s& tf2,
+                           const DistanceRequest& request_,
+                           DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    AABB aabb2;
+    computeBV<AABB>(s, tf2, aabb2);
+    OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
+                               aabb2, tf1, tf2);
+  }
+
+  /// @brief distance between shape and octree
+  template <typename S>
+  void ShapeOcTreeDistance(const S& s, const OcTree* tree,
+                           const Transform3s& tf1, const Transform3s& tf2,
+                           const DistanceRequest& request_,
+                           DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    AABB aabb1;
+    computeBV<AABB>(s, tf1, aabb1);
+    OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
+                               aabb1, tf2, tf1);
+  }
+
+ private:
+  template <typename S>
+  bool OcTreeShapeDistanceRecurse(const OcTree* tree1,
+                                  const OcTree::OcTreeNode* root1,
+                                  const AABB& bv1, const S& s,
+                                  const AABB& aabb2, const Transform3s& tf1,
+                                  const Transform3s& tf2) const {
+    if (!tree1->nodeHasChildren(root1)) {
+      if (tree1->isNodeOccupied(root1)) {
+        Box box;
+        Transform3s box_tf;
+        constructBox(bv1, tf1, box, box_tf);
+
+        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+          box.computeLocalAABB();
+        }
+
+        Vec3s p1, p2, normal;
+        const CoalScalar distance = internal::ShapeShapeDistance<Box, S>(
+            &box, box_tf, &s, tf2, this->solver,
+            this->drequest->enable_signed_distance, p1, p2, normal);
+
+        this->dresult->update(distance, tree1, &s,
+                              (int)(root1 - tree1->getRoot()),
+                              DistanceResult::NONE, p1, p2, normal);
+
+        return drequest->isSatisfied(*dresult);
+      } else
+        return false;
+    }
+
+    if (!tree1->isNodeOccupied(root1)) return false;
+
+    for (unsigned int i = 0; i < 8; ++i) {
+      if (tree1->nodeChildExists(root1, i)) {
+        const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+        AABB child_bv;
+        computeChildBV(bv1, i, child_bv);
+
+        AABB aabb1;
+        convertBV(child_bv, tf1, aabb1);
+        CoalScalar d = aabb1.distance(aabb2);
+        if (d < dresult->min_distance) {
+          if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1,
+                                         tf2))
+            return true;
+        }
+      }
+    }
+
+    return false;
+  }
+
+  template <typename S>
+  bool OcTreeShapeIntersectRecurse(const OcTree* tree1,
+                                   const OcTree::OcTreeNode* root1,
+                                   const AABB& bv1, const S& s, const OBB& obb2,
+                                   const Transform3s& tf1,
+                                   const Transform3s& tf2) const {
+    // Empty OcTree is considered free.
+    if (!root1) return false;
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least of one the nodes is free; OR
+    ///           2) (two uncertain nodes or one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree1->isNodeFree(root1))
+      return false;
+    else if ((tree1->isNodeUncertain(root1) || s.isUncertain()))
+      return false;
+    else {
+      OBB obb1;
+      convertBV(bv1, tf1, obb1);
+      CoalScalar sqrDistLowerBound;
+      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
+        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
+                                                 sqrDistLowerBound);
+        return false;
+      }
+    }
+
+    if (!tree1->nodeHasChildren(root1)) {
+      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
+
+      Box box;
+      Transform3s box_tf;
+      constructBox(bv1, tf1, box, box_tf);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        box.computeLocalAABB();
+      }
+
+      bool contactNotAdded =
+          (cresult->numContacts() >= crequest->num_max_contacts);
+      std::size_t ncontact = ShapeShapeCollide<Box, S>(
+          &box, box_tf, &s, tf2, solver, *crequest, *cresult);
+      assert(ncontact == 0 || ncontact == 1);
+      if (!contactNotAdded && ncontact == 1) {
+        // Update contact information.
+        const Contact& c = cresult->getContact(cresult->numContacts() - 1);
+        cresult->setContact(
+            cresult->numContacts() - 1,
+            Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()),
+                    c.b2, c.pos, c.normal, c.penetration_depth));
+      }
+
+      // no need to call `internal::updateDistanceLowerBoundFromLeaf` here
+      // as it is already done internally in `ShapeShapeCollide` above.
+      return crequest->isSatisfied(*cresult);
+    }
+
+    for (unsigned int i = 0; i < 8; ++i) {
+      if (tree1->nodeChildExists(root1, i)) {
+        const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+        AABB child_bv;
+        computeChildBV(bv1, i, child_bv);
+
+        if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1,
+                                        tf2))
+          return true;
+      }
+    }
+
+    return false;
+  }
+
+  template <typename BV>
+  bool OcTreeMeshDistanceRecurse(const OcTree* tree1,
+                                 const OcTree::OcTreeNode* root1,
+                                 const AABB& bv1, const BVHModel<BV>* tree2,
+                                 unsigned int root2, const Transform3s& tf1,
+                                 const Transform3s& tf2) const {
+    if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) {
+      if (tree1->isNodeOccupied(root1)) {
+        Box box;
+        Transform3s box_tf;
+        constructBox(bv1, tf1, box, box_tf);
+
+        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+          box.computeLocalAABB();
+        }
+
+        size_t primitive_id =
+            static_cast<size_t>(tree2->getBV(root2).primitiveId());
+        const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
+        const TriangleP tri((*(tree2->vertices))[tri_id[0]],
+                            (*(tree2->vertices))[tri_id[1]],
+                            (*(tree2->vertices))[tri_id[2]]);
+
+        Vec3s p1, p2, normal;
+        const CoalScalar distance =
+            internal::ShapeShapeDistance<Box, TriangleP>(
+                &box, box_tf, &tri, tf2, this->solver,
+                this->drequest->enable_signed_distance, p1, p2, normal);
+
+        this->dresult->update(distance, tree1, tree2,
+                              (int)(root1 - tree1->getRoot()),
+                              static_cast<int>(primitive_id), p1, p2, normal);
+
+        return this->drequest->isSatisfied(*dresult);
+      } else
+        return false;
+    }
+
+    if (!tree1->isNodeOccupied(root1)) return false;
+
+    if (tree2->getBV(root2).isLeaf() ||
+        (tree1->nodeHasChildren(root1) &&
+         (bv1.size() > tree2->getBV(root2).bv.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          CoalScalar d;
+          AABB aabb1, aabb2;
+          convertBV(child_bv, tf1, aabb1);
+          convertBV(tree2->getBV(root2).bv, tf2, aabb2);
+          d = aabb1.distance(aabb2);
+
+          if (d < dresult->min_distance) {
+            if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
+                                          tf1, tf2))
+              return true;
+          }
+        }
+      }
+    } else {
+      CoalScalar d;
+      AABB aabb1, aabb2;
+      convertBV(bv1, tf1, aabb1);
+      unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
+      convertBV(tree2->getBV(child).bv, tf2, aabb2);
+      d = aabb1.distance(aabb2);
+
+      if (d < dresult->min_distance) {
+        if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
+                                      tf2))
+          return true;
+      }
+
+      child = (unsigned int)tree2->getBV(root2).rightChild();
+      convertBV(tree2->getBV(child).bv, tf2, aabb2);
+      d = aabb1.distance(aabb2);
+
+      if (d < dresult->min_distance) {
+        if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
+                                      tf2))
+          return true;
+      }
+    }
+
+    return false;
+  }
+
+  /// \return True if the request is satisfied.
+  template <typename BV>
+  bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
+                                  const OcTree::OcTreeNode* root1,
+                                  const AABB& bv1, const BVHModel<BV>* tree2,
+                                  unsigned int root2, const Transform3s& tf1,
+                                  const Transform3s& tf2) const {
+    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
+    // code in this if(!root1) did not output anything so the empty OcTree is
+    // considered free. Should an empty OcTree be considered free ?
+
+    // Empty OcTree is considered free.
+    if (!root1) return false;
+    BVNode<BV> const& bvn2 = tree2->getBV(root2);
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least one of the nodes is free; OR
+    ///           2) (two uncertain nodes OR one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree1->isNodeFree(root1))
+      return false;
+    else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
+      return false;
+    else {
+      OBB obb1, obb2;
+      convertBV(bv1, tf1, obb1);
+      convertBV(bvn2.bv, tf2, obb2);
+      CoalScalar sqrDistLowerBound;
+      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
+        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
+                                                 sqrDistLowerBound);
+        return false;
+      }
+    }
+
+    // Check if leaf collides.
+    if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
+      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
+      Box box;
+      Transform3s box_tf;
+      constructBox(bv1, tf1, box, box_tf);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        box.computeLocalAABB();
+      }
+
+      size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
+      const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
+      const TriangleP tri((*(tree2->vertices))[tri_id[0]],
+                          (*(tree2->vertices))[tri_id[1]],
+                          (*(tree2->vertices))[tri_id[2]]);
+
+      // When reaching this point, `this->solver` has already been set up
+      // by the CollisionRequest `this->crequest`.
+      // The only thing we need to (and can) pass to `ShapeShapeDistance` is
+      // whether or not penetration information is should be computed in case of
+      // collision.
+      const bool compute_penetration = this->crequest->enable_contact ||
+                                       (this->crequest->security_margin < 0);
+      Vec3s c1, c2, normal;
+      const CoalScalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
+          &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2,
+          normal);
+      const CoalScalar distToCollision =
+          distance - this->crequest->security_margin;
+
+      internal::updateDistanceLowerBoundFromLeaf(
+          *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
+
+      if (cresult->numContacts() < crequest->num_max_contacts) {
+        if (distToCollision <= crequest->collision_distance_threshold) {
+          cresult->addContact(Contact(
+              tree1, tree2, (int)(root1 - tree1->getRoot()),
+              static_cast<int>(primitive_id), c1, c2, normal, distance));
+        }
+      }
+      return crequest->isSatisfied(*cresult);
+    }
+
+    // Determine which tree to traverse first.
+    if (bvn2.isLeaf() ||
+        (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2,
+                                         tf1, tf2))
+            return true;
+        }
+      }
+    } else {
+      if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
+                                     (unsigned int)bvn2.leftChild(), tf1, tf2))
+        return true;
+
+      if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
+                                     (unsigned int)bvn2.rightChild(), tf1, tf2))
+        return true;
+    }
+
+    return false;
+  }
+
+  /// \return True if the request is satisfied.
+  template <typename BV>
+  bool OcTreeHeightFieldIntersectRecurse(
+      const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
+      const HeightField<BV>* tree2, unsigned int root2, const Transform3s& tf1,
+      const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const {
+    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
+    // code in this if(!root1) did not output anything so the empty OcTree is
+    // considered free. Should an empty OcTree be considered free ?
+
+    // Empty OcTree is considered free.
+    if (!root1) return false;
+    HFNode<BV> const& bvn2 = tree2->getBV(root2);
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least one of the nodes is free; OR
+    ///           2) (two uncertain nodes OR one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree1->isNodeFree(root1))
+      return false;
+    else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
+      return false;
+    else {
+      OBB obb1, obb2;
+      convertBV(bv1, tf1, obb1);
+      convertBV(bvn2.bv, tf2, obb2);
+      CoalScalar sqrDistLowerBound_;
+      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) {
+        if (sqrDistLowerBound_ < sqrDistLowerBound)
+          sqrDistLowerBound = sqrDistLowerBound_;
+        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
+                                                 sqrDistLowerBound);
+        return false;
+      }
+    }
+
+    // Check if leaf collides.
+    if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
+      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
+      Box box;
+      Transform3s box_tf;
+      constructBox(bv1, tf1, box, box_tf);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        box.computeLocalAABB();
+      }
+
+      typedef Convex<Triangle> ConvexTriangle;
+      ConvexTriangle convex1, convex2;
+      int convex1_active_faces, convex2_active_faces;
+      details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces,
+                                    convex2, convex2_active_faces);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        convex1.computeLocalAABB();
+        convex2.computeLocalAABB();
+      }
+
+      Vec3s c1, c2, normal, normal_top;
+      CoalScalar distance;
+      bool hfield_witness_is_on_bin_side;
+
+      bool collision = details::shapeDistance<Triangle, Box, 0>(
+          solver, *crequest, convex1, convex1_active_faces, convex2,
+          convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal,
+          normal_top, hfield_witness_is_on_bin_side);
+
+      CoalScalar distToCollision =
+          distance - crequest->security_margin * (normal_top.dot(normal));
+
+      if (distToCollision <= crequest->collision_distance_threshold) {
+        sqrDistLowerBound = 0;
+        if (crequest->num_max_contacts > cresult->numContacts()) {
+          if (normal_top.isApprox(normal) &&
+              (collision || !hfield_witness_is_on_bin_side)) {
+            cresult->addContact(
+                Contact(tree1, tree2, (int)(root1 - tree1->getRoot()),
+                        (int)Contact::NONE, c1, c2, -normal, distance));
+          }
+        }
+      } else
+        sqrDistLowerBound = distToCollision * distToCollision;
+
+      //    const Vec3s c1 = contact_point - distance * 0.5 * normal;
+      //    const Vec3s c2 = contact_point + distance * 0.5 * normal;
+      internal::updateDistanceLowerBoundFromLeaf(
+          *crequest, *cresult, distToCollision, c1, c2, -normal);
+
+      assert(cresult->isCollision() || sqrDistLowerBound > 0);
+      return crequest->isSatisfied(*cresult);
+    }
+
+    // Determine which tree to traverse first.
+    if (bvn2.isLeaf() ||
+        (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2,
+                                                root2, tf1, tf2,
+                                                sqrDistLowerBound))
+            return true;
+        }
+      }
+    } else {
+      if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
+                                            (unsigned int)bvn2.leftChild(), tf1,
+                                            tf2, sqrDistLowerBound))
+        return true;
+
+      if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
+                                            (unsigned int)bvn2.rightChild(),
+                                            tf1, tf2, sqrDistLowerBound))
+        return true;
+    }
+
+    return false;
+  }
+
+  /// \return True if the request is satisfied.
+  template <typename BV>
+  bool HeightFieldOcTreeIntersectRecurse(
+      const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2,
+      const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1,
+      const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const {
+    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
+    // code in this if(!root1) did not output anything so the empty OcTree is
+    // considered free. Should an empty OcTree be considered free ?
+
+    // Empty OcTree is considered free.
+    if (!root2) return false;
+    HFNode<BV> const& bvn1 = tree1->getBV(root1);
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least one of the nodes is free; OR
+    ///           2) (two uncertain nodes OR one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree2->isNodeFree(root2))
+      return false;
+    else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain()))
+      return false;
+    else {
+      OBB obb1, obb2;
+      convertBV(bvn1.bv, tf1, obb1);
+      convertBV(bv2, tf2, obb2);
+      CoalScalar sqrDistLowerBound_;
+      if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) {
+        if (sqrDistLowerBound_ < sqrDistLowerBound)
+          sqrDistLowerBound = sqrDistLowerBound_;
+        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
+                                                 sqrDistLowerBound);
+        return false;
+      }
+    }
+
+    // Check if leaf collides.
+    if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) {
+      assert(tree2->isNodeOccupied(root2));  // it isn't free nor uncertain.
+      Box box;
+      Transform3s box_tf;
+      constructBox(bv2, tf2, box, box_tf);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        box.computeLocalAABB();
+      }
+
+      typedef Convex<Triangle> ConvexTriangle;
+      ConvexTriangle convex1, convex2;
+      int convex1_active_faces, convex2_active_faces;
+      details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces,
+                                    convex2, convex2_active_faces);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        convex1.computeLocalAABB();
+        convex2.computeLocalAABB();
+      }
+
+      Vec3s c1, c2, normal, normal_top;
+      CoalScalar distance;
+      bool hfield_witness_is_on_bin_side;
+
+      bool collision = details::shapeDistance<Triangle, Box, 0>(
+          solver, *crequest, convex1, convex1_active_faces, convex2,
+          convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal,
+          normal_top, hfield_witness_is_on_bin_side);
+
+      CoalScalar distToCollision =
+          distance - crequest->security_margin * (normal_top.dot(normal));
+
+      if (distToCollision <= crequest->collision_distance_threshold) {
+        sqrDistLowerBound = 0;
+        if (crequest->num_max_contacts > cresult->numContacts()) {
+          if (normal_top.isApprox(normal) &&
+              (collision || !hfield_witness_is_on_bin_side)) {
+            cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE,
+                                        (int)(root2 - tree2->getRoot()), c1, c2,
+                                        normal, distance));
+          }
+        }
+      } else
+        sqrDistLowerBound = distToCollision * distToCollision;
+
+      //    const Vec3s c1 = contact_point - distance * 0.5 * normal;
+      //    const Vec3s c2 = contact_point + distance * 0.5 * normal;
+      internal::updateDistanceLowerBoundFromLeaf(
+          *crequest, *cresult, distToCollision, c1, c2, normal);
+
+      assert(cresult->isCollision() || sqrDistLowerBound > 0);
+      return crequest->isSatisfied(*cresult);
+    }
+
+    // Determine which tree to traverse first.
+    if (bvn1.isLeaf() ||
+        (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree2->nodeChildExists(root2, i)) {
+          const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
+          AABB child_bv;
+          computeChildBV(bv2, i, child_bv);
+
+          if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child,
+                                                child_bv, tf1, tf2,
+                                                sqrDistLowerBound))
+            return true;
+        }
+      }
+    } else {
+      if (HeightFieldOcTreeIntersectRecurse(
+              tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1,
+              tf2, sqrDistLowerBound))
+        return true;
+
+      if (HeightFieldOcTreeIntersectRecurse(
+              tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1,
+              tf2, sqrDistLowerBound))
+        return true;
+    }
+
+    return false;
+  }
+
+  bool OcTreeDistanceRecurse(const OcTree* tree1,
+                             const OcTree::OcTreeNode* root1, const AABB& bv1,
+                             const OcTree* tree2,
+                             const OcTree::OcTreeNode* root2, const AABB& bv2,
+                             const Transform3s& tf1,
+                             const Transform3s& tf2) const {
+    if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
+      if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
+        Box box1, box2;
+        Transform3s box1_tf, box2_tf;
+        constructBox(bv1, tf1, box1, box1_tf);
+        constructBox(bv2, tf2, box2, box2_tf);
+
+        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+          box1.computeLocalAABB();
+          box2.computeLocalAABB();
+        }
+
+        Vec3s p1, p2, normal;
+        const CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
+            &box1, box1_tf, &box2, box2_tf, this->solver,
+            this->drequest->enable_signed_distance, p1, p2, normal);
+
+        this->dresult->update(distance, tree1, tree2,
+                              (int)(root1 - tree1->getRoot()),
+                              (int)(root2 - tree2->getRoot()), p1, p2, normal);
+
+        return drequest->isSatisfied(*dresult);
+      } else
+        return false;
+    }
+
+    if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
+      return false;
+
+    if (!tree2->nodeHasChildren(root2) ||
+        (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          CoalScalar d;
+          AABB aabb1, aabb2;
+          convertBV(bv1, tf1, aabb1);
+          convertBV(bv2, tf2, aabb2);
+          d = aabb1.distance(aabb2);
+
+          if (d < dresult->min_distance) {
+            if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
+                                      tf1, tf2))
+              return true;
+          }
+        }
+      }
+    } else {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree2->nodeChildExists(root2, i)) {
+          const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
+          AABB child_bv;
+          computeChildBV(bv2, i, child_bv);
+
+          CoalScalar d;
+          AABB aabb1, aabb2;
+          convertBV(bv1, tf1, aabb1);
+          convertBV(bv2, tf2, aabb2);
+          d = aabb1.distance(aabb2);
+
+          if (d < dresult->min_distance) {
+            if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
+                                      tf1, tf2))
+              return true;
+          }
+        }
+      }
+    }
+
+    return false;
+  }
+
+  bool OcTreeIntersectRecurse(const OcTree* tree1,
+                              const OcTree::OcTreeNode* root1, const AABB& bv1,
+                              const OcTree* tree2,
+                              const OcTree::OcTreeNode* root2, const AABB& bv2,
+                              const Transform3s& tf1,
+                              const Transform3s& tf2) const {
+    // Empty OcTree is considered free.
+    if (!root1) return false;
+    if (!root2) return false;
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least one of the nodes is free; OR
+    ///           2) (two uncertain nodes OR one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
+      return false;
+    else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
+      return false;
+
+    bool bothAreLeaves =
+        (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
+    if (!bothAreLeaves || !crequest->enable_contact) {
+      OBB obb1, obb2;
+      convertBV(bv1, tf1, obb1);
+      convertBV(bv2, tf2, obb2);
+      CoalScalar sqrDistLowerBound;
+      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
+        if (cresult->distance_lower_bound > 0 &&
+            sqrDistLowerBound <
+                cresult->distance_lower_bound * cresult->distance_lower_bound)
+          cresult->distance_lower_bound =
+              sqrt(sqrDistLowerBound) - crequest->security_margin;
+        return false;
+      }
+      if (crequest->enable_contact) {  // Overlap
+        if (cresult->numContacts() < crequest->num_max_contacts)
+          cresult->addContact(
+              Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
+                      static_cast<int>(root2 - tree2->getRoot())));
+        return crequest->isSatisfied(*cresult);
+      }
+    }
+
+    // Both node are leaves
+    if (bothAreLeaves) {
+      assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2));
+
+      Box box1, box2;
+      Transform3s box1_tf, box2_tf;
+      constructBox(bv1, tf1, box1, box1_tf);
+      constructBox(bv2, tf2, box2, box2_tf);
+
+      if (this->solver->gjk_initial_guess ==
+          GJKInitialGuess::BoundingVolumeGuess) {
+        box1.computeLocalAABB();
+        box2.computeLocalAABB();
+      }
+
+      // When reaching this point, `this->solver` has already been set up
+      // by the CollisionRequest `this->request`.
+      // The only thing we need to (and can) pass to `ShapeShapeDistance` is
+      // whether or not penetration information is should be computed in case of
+      // collision.
+      const bool compute_penetration = (this->crequest->enable_contact ||
+                                        (this->crequest->security_margin < 0));
+      Vec3s c1, c2, normal;
+      CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
+          &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1,
+          c2, normal);
+
+      const CoalScalar distToCollision =
+          distance - this->crequest->security_margin;
+
+      internal::updateDistanceLowerBoundFromLeaf(
+          *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
+
+      if (this->cresult->numContacts() < this->crequest->num_max_contacts) {
+        if (distToCollision <= this->crequest->collision_distance_threshold)
+          this->cresult->addContact(
+              Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
+                      static_cast<int>(root2 - tree2->getRoot()), c1, c2,
+                      normal, distance));
+      }
+
+      return crequest->isSatisfied(*cresult);
+    }
+
+    // Determine which tree to traverse first.
+    if (!tree2->nodeHasChildren(root2) ||
+        (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
+                                     tf1, tf2))
+            return true;
+        }
+      }
+    } else {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree2->nodeChildExists(root2, i)) {
+          const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
+          AABB child_bv;
+          computeChildBV(bv2, i, child_bv);
+
+          if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
+                                     tf1, tf2))
+            return true;
+        }
+      }
+    }
+
+    return false;
+  }
+};
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @brief Traversal node for octree collision
+class COAL_DLLAPI OcTreeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  OcTreeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned, unsigned, CoalScalar&) const { return false; }
+
+  void leafCollides(unsigned, unsigned, CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const OcTree* model1;
+  const OcTree* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for shape-octree collision
+template <typename S>
+class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const S* model1;
+  const OcTree* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-shape collision
+
+template <typename S>
+class COAL_DLLAPI OcTreeShapeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, coal::CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const OcTree* model1;
+  const S* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for mesh-octree collision
+template <typename BV>
+class COAL_DLLAPI MeshOcTreeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const BVHModel<BV>* model1;
+  const OcTree* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-mesh collision
+template <typename BV>
+class COAL_DLLAPI OcTreeMeshCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const OcTree* model1;
+  const BVHModel<BV>* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-height-field collision
+template <typename BV>
+class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request,
+                                         *result, sqrDistLowerBound);
+  }
+
+  const OcTree* model1;
+  const HeightField<BV>* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-height-field collision
+template <typename BV>
+class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request,
+                                         *result, sqrDistLowerBound);
+  }
+
+  const HeightField<BV>* model1;
+  const OcTree* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @}
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for octree distance
+class COAL_DLLAPI OcTreeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  OcTreeDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
+
+  bool BVDistanceLowerBound(unsigned, unsigned, CoalScalar&) const {
+    return false;
+  }
+
+  void leafComputeDistance(unsigned, unsigned int) const {
+    otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
+  }
+
+  const OcTree* model1;
+  const OcTree* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for shape-octree distance
+template <typename Shape>
+class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  ShapeOcTreeDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;
+  }
+
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
+  }
+
+  const Shape* model1;
+  const OcTree* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-shape distance
+template <typename Shape>
+class COAL_DLLAPI OcTreeShapeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  OcTreeShapeDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;
+  }
+
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
+  }
+
+  const OcTree* model1;
+  const Shape* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for mesh-octree distance
+template <typename BV>
+class COAL_DLLAPI MeshOcTreeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  MeshOcTreeDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;
+  }
+
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
+  }
+
+  const BVHModel<BV>* model1;
+  const OcTree* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-mesh distance
+template <typename BV>
+class COAL_DLLAPI OcTreeMeshDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  OcTreeMeshDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;
+  }
+
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
+  }
+
+  const OcTree* model1;
+  const BVHModel<BV>* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h
new file mode 100644
index 0000000000000000000000000000000000000000..120633edc049c8f3a53069a1bdf6b078fc95ddc0
--- /dev/null
+++ b/include/coal/internal/traversal_node_setup.h
@@ -0,0 +1,815 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_TRAVERSAL_NODE_SETUP_H
+#define COAL_TRAVERSAL_NODE_SETUP_H
+
+/// @cond INTERNAL
+
+#include "coal/internal/tools.h"
+#include "coal/internal/traversal_node_shapes.h"
+
+#include "coal/internal/traversal_node_bvhs.h"
+#include "coal/internal/traversal_node_bvh_shape.h"
+
+// #include <hpp/fcl/internal/traversal_node_hfields.h>
+#include "coal/internal/traversal_node_hfield_shape.h"
+
+#ifdef COAL_HAS_OCTOMAP
+#include "coal/internal/traversal_node_octree.h"
+#endif
+
+#include "coal/BVH/BVH_utility.h"
+
+namespace coal {
+
+#ifdef COAL_HAS_OCTOMAP
+/// @brief Initialize traversal node for collision between two octrees, given
+/// current object transform
+inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1,
+                       const Transform3s& tf1, const OcTree& model2,
+                       const Transform3s& tf2, const OcTreeSolver* otsolver,
+                       CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between two octrees, given
+/// current object transform
+inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1,
+                       const Transform3s& tf1, const OcTree& model2,
+                       const Transform3s& tf2, const OcTreeSolver* otsolver,
+                       const DistanceRequest& request, DistanceResult& result)
+
+{
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one shape and one
+/// octree, given current object transform
+template <typename S>
+bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node, const S& model1,
+                const Transform3s& tf1, const OcTree& model2,
+                const Transform3s& tf2, const OcTreeSolver* otsolver,
+                CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one octree and one
+/// shape, given current object transform
+template <typename S>
+bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
+                const OcTree& model1, const Transform3s& tf1, const S& model2,
+                const Transform3s& tf2, const OcTreeSolver* otsolver,
+                CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between one shape and one
+/// octree, given current object transform
+template <typename S>
+bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node, const S& model1,
+                const Transform3s& tf1, const OcTree& model2,
+                const Transform3s& tf2, const OcTreeSolver* otsolver,
+                const DistanceRequest& request, DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between one octree and one
+/// shape, given current object transform
+template <typename S>
+bool initialize(OcTreeShapeDistanceTraversalNode<S>& node, const OcTree& model1,
+                const Transform3s& tf1, const S& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// octree, given current object transform
+template <typename BV>
+bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const OcTree& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one octree and one
+/// mesh, given current object transform
+template <typename BV>
+bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
+                const OcTree& model1, const Transform3s& tf1,
+                const BVHModel<BV>& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one octree and one
+/// height field, given current object transform
+template <typename BV>
+bool initialize(OcTreeHeightFieldCollisionTraversalNode<BV>& node,
+                const OcTree& model1, const Transform3s& tf1,
+                const HeightField<BV>& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one height field and
+/// one octree, given current object transform
+template <typename BV>
+bool initialize(HeightFieldOcTreeCollisionTraversalNode<BV>& node,
+                const HeightField<BV>& model1, const Transform3s& tf1,
+                const OcTree& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between one mesh and one
+/// octree, given current object transform
+template <typename BV>
+bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const OcTree& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one octree and one
+/// mesh, given current object transform
+template <typename BV>
+bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node, const OcTree& model1,
+                const Transform3s& tf1, const BVHModel<BV>& model2,
+                const Transform3s& tf2, const OcTreeSolver* otsolver,
+                const DistanceRequest& request, DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+#endif
+
+/// @brief Initialize traversal node for collision between two geometric shapes,
+/// given current object transform
+template <typename S1, typename S2>
+bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1,
+                const Transform3s& tf1, const S2& shape2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                CollisionResult& result) {
+  node.model1 = &shape1;
+  node.tf1 = tf1;
+  node.model2 = &shape2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  node.result = &result;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// shape, given current object transform
+template <typename BV, typename S>
+bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
+                BVHModel<BV>& model1, Transform3s& tf1, const S& model2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                CollisionResult& result, bool use_refit = false,
+                bool refit_bottomup = false) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  if (!tf1.isIdentity() &&
+      model1.vertices.get())  // TODO(jcarpent): vectorized version
+  {
+    std::vector<Vec3s> vertices_transformed(model1.num_vertices);
+    const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
+      const Vec3s& p = model1_vertices_[i];
+      Vec3s new_v = tf1.transform(p);
+      vertices_transformed[i] = new_v;
+    }
+
+    model1.beginReplaceModel();
+    model1.replaceSubModel(vertices_transformed);
+    model1.endReplaceModel(use_refit, refit_bottomup);
+
+    tf1.setIdentity();
+  }
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.tri_indices =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+
+  node.result = &result;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// shape
+template <typename BV, typename S>
+bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, CollisionResult& result) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.tri_indices =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+
+  node.result = &result;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// shape, given current object transform
+template <typename BV, typename S>
+bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
+                HeightField<BV>& model1, Transform3s& tf1, const S& model2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                CollisionResult& result, bool use_refit = false,
+                bool refit_bottomup = false);
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// shape
+template <typename BV, typename S>
+bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
+                const HeightField<BV>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, CollisionResult& result) {
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  node.result = &result;
+
+  return true;
+}
+
+/// @cond IGNORE
+namespace details {
+template <typename S, typename BV, template <typename> class OrientedNode>
+static inline bool setupShapeMeshCollisionOrientedNode(
+    OrientedNode<S>& node, const S& model1, const Transform3s& tf1,
+    const BVHModel<BV>& model2, const Transform3s& tf2,
+    const GJKSolver* nsolver, CollisionResult& result) {
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model1, tf1, node.model1_bv);
+
+  node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
+  node.tri_indices =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  node.result = &result;
+
+  return true;
+}
+}  // namespace details
+/// @endcond
+
+/// @brief Initialize traversal node for collision between two meshes, given the
+/// current transforms
+template <typename BV>
+bool initialize(
+    MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
+    BVHModel<BV>& model1, Transform3s& tf1, BVHModel<BV>& model2,
+    Transform3s& tf2, CollisionResult& result, bool use_refit = false,
+    bool refit_bottomup = false) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  if (!tf1.isIdentity() && model1.vertices.get()) {
+    std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
+    const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
+      const Vec3s& p = model1_vertices_[i];
+      Vec3s new_v = tf1.transform(p);
+      vertices_transformed1[i] = new_v;
+    }
+
+    model1.beginReplaceModel();
+    model1.replaceSubModel(vertices_transformed1);
+    model1.endReplaceModel(use_refit, refit_bottomup);
+
+    tf1.setIdentity();
+  }
+
+  if (!tf2.isIdentity() && model2.vertices.get()) {
+    std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
+    const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
+    for (unsigned int i = 0; i < model2.num_vertices; ++i) {
+      const Vec3s& p = model2_vertices_[i];
+      Vec3s new_v = tf2.transform(p);
+      vertices_transformed2[i] = new_v;
+    }
+
+    model2.beginReplaceModel();
+    model2.replaceSubModel(vertices_transformed2);
+    model2.endReplaceModel(use_refit, refit_bottomup);
+
+    tf2.setIdentity();
+  }
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
+  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
+
+  node.tri_indices1 =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+  node.tri_indices2 =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  node.result = &result;
+
+  return true;
+}
+
+template <typename BV>
+bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const BVHModel<BV>& model2, const Transform3s& tf2,
+                CollisionResult& result) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.vertices1 = model1.vertices ? model1.vertices->data() : NULL;
+  node.vertices2 = model2.vertices ? model2.vertices->data() : NULL;
+
+  node.tri_indices1 =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+  node.tri_indices2 =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
+  node.result = &result;
+
+  node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
+  node.RT.T.noalias() = tf1.getRotation().transpose() *
+                        (tf2.getTranslation() - tf1.getTranslation());
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between two geometric shapes
+template <typename S1, typename S2>
+bool initialize(ShapeDistanceTraversalNode<S1, S2>& node, const S1& shape1,
+                const Transform3s& tf1, const S2& shape2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                const DistanceRequest& request, DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &shape1;
+  node.tf1 = tf1;
+  node.model2 = &shape2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance computation between two
+/// meshes, given the current transforms
+template <typename BV>
+bool initialize(
+    MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
+    BVHModel<BV>& model1, Transform3s& tf1, BVHModel<BV>& model2,
+    Transform3s& tf2, const DistanceRequest& request, DistanceResult& result,
+    bool use_refit = false, bool refit_bottomup = false) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  if (!tf1.isIdentity() && model1.vertices.get()) {
+    std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
+    const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
+      const Vec3s& p = model1_vertices_[i];
+      Vec3s new_v = tf1.transform(p);
+      vertices_transformed1[i] = new_v;
+    }
+
+    model1.beginReplaceModel();
+    model1.replaceSubModel(vertices_transformed1);
+    model1.endReplaceModel(use_refit, refit_bottomup);
+
+    tf1.setIdentity();
+  }
+
+  if (!tf2.isIdentity() && model2.vertices.get()) {
+    std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
+    const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
+    for (unsigned int i = 0; i < model2.num_vertices; ++i) {
+      const Vec3s& p = model2_vertices_[i];
+      Vec3s new_v = tf2.transform(p);
+      vertices_transformed2[i] = new_v;
+    }
+
+    model2.beginReplaceModel();
+    model2.replaceSubModel(vertices_transformed2);
+    model2.endReplaceModel(use_refit, refit_bottomup);
+
+    tf2.setIdentity();
+  }
+
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
+  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
+
+  node.tri_indices1 =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+  node.tri_indices2 =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance computation between two meshes
+template <typename BV>
+bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const BVHModel<BV>& model2, const Transform3s& tf2,
+                const DistanceRequest& request, DistanceResult& result) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
+  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
+
+  node.tri_indices1 =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+  node.tri_indices2 =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
+
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
+                    tf2.getTranslation(), node.RT.R, node.RT.T);
+
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance computation between one mesh
+/// and one shape, given the current transforms
+template <typename BV, typename S>
+bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
+                BVHModel<BV>& model1, Transform3s& tf1, const S& model2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                const DistanceRequest& request, DistanceResult& result,
+                bool use_refit = false, bool refit_bottomup = false) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  if (!tf1.isIdentity() && model1.vertices.get()) {
+    const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+    std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
+    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
+      const Vec3s& p = model1_vertices_[i];
+      Vec3s new_v = tf1.transform(p);
+      vertices_transformed1[i] = new_v;
+    }
+
+    model1.beginReplaceModel();
+    model1.replaceSubModel(vertices_transformed1);
+    model1.endReplaceModel(use_refit, refit_bottomup);
+
+    tf1.setIdentity();
+  }
+
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.tri_indices =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  return true;
+}
+
+/// @cond IGNORE
+namespace details {
+
+template <typename BV, typename S, template <typename> class OrientedNode>
+static inline bool setupMeshShapeDistanceOrientedNode(
+    OrientedNode<S>& node, const BVHModel<BV>& model1, const Transform3s& tf1,
+    const S& model2, const Transform3s& tf2, const GJKSolver* nsolver,
+    const DistanceRequest& request, DistanceResult& result) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.tri_indices =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+
+  return true;
+}
+}  // namespace details
+/// @endcond
+
+/// @brief Initialize traversal node for distance computation between one mesh
+/// and one shape, specialized for RSS type
+template <typename S>
+bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& node,
+                const BVHModel<RSS>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  return details::setupMeshShapeDistanceOrientedNode(
+      node, model1, tf1, model2, tf2, nsolver, request, result);
+}
+
+/// @brief Initialize traversal node for distance computation between one mesh
+/// and one shape, specialized for kIOS type
+template <typename S>
+bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& node,
+                const BVHModel<kIOS>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  return details::setupMeshShapeDistanceOrientedNode(
+      node, model1, tf1, model2, tf2, nsolver, request, result);
+}
+
+/// @brief Initialize traversal node for distance computation between one mesh
+/// and one shape, specialized for OBBRSS type
+template <typename S>
+bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& node,
+                const BVHModel<OBBRSS>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  return details::setupMeshShapeDistanceOrientedNode(
+      node, model1, tf1, model2, tf2, nsolver, request, result);
+}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/traversal_node_shapes.h b/include/coal/internal/traversal_node_shapes.h
new file mode 100644
index 0000000000000000000000000000000000000000..a10d8b71523f5eec8d22c4be6a6b8b2e9fcdc8e5
--- /dev/null
+++ b/include/coal/internal/traversal_node_shapes.h
@@ -0,0 +1,124 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_TRAVERSAL_NODE_SHAPES_H
+#define COAL_TRAVERSAL_NODE_SHAPES_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/BV/BV.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/shape_shape_func.h"
+
+namespace coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @brief Traversal node for collision between two shapes
+template <typename S1, typename S2>
+class COAL_DLLAPI ShapeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  ShapeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    nsolver = NULL;
+  }
+
+  /// @brief BV culling test in one BVTT node
+  bool BVDisjoints(int, int, CoalScalar&) const {
+    COAL_THROW_PRETTY("Not implemented", std::runtime_error);
+  }
+
+  /// @brief Intersection testing between leaves (two shapes)
+  void leafCollides(int, int, CoalScalar&) const {
+    ShapeShapeCollide<S1, S2>(this->model1, this->tf1, this->model2, this->tf2,
+                              this->nsolver, this->request, *(this->result));
+  }
+
+  const S1* model1;
+  const S2* model2;
+
+  const GJKSolver* nsolver;
+};
+
+/// @}
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for distance between two shapes
+template <typename S1, typename S2>
+class COAL_DLLAPI ShapeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    nsolver = NULL;
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;  // should not be used
+  }
+
+  /// @brief Distance testing between leaves (two shapes)
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    ShapeShapeDistance<S1, S2>(this->model1, this->tf1, this->model2, this->tf2,
+                               this->nsolver, this->request, *(this->result));
+  }
+
+  const S1* model1;
+  const S2* model2;
+
+  const GJKSolver* nsolver;
+};
+
+/// @}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/internal/traversal_recurse.h b/include/coal/internal/traversal_recurse.h
new file mode 100644
index 0000000000000000000000000000000000000000..c0626b5142148f24f6ca46457f03b3cc049c9235
--- /dev/null
+++ b/include/coal/internal/traversal_recurse.h
@@ -0,0 +1,81 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_TRAVERSAL_RECURSE_H
+#define COAL_TRAVERSAL_RECURSE_H
+
+/// @cond INTERNAL
+
+#include "coal/BVH/BVH_front.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal_node_bvhs.h"
+#include <queue>
+
+namespace coal {
+
+/// Recurse function for collision
+/// @param node collision node,
+/// @param b1, b2 ids of bounding volume nodes for object 1 and object 2
+/// @retval sqrDistLowerBound squared lower bound on distance between objects.
+void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1,
+                      unsigned int b2, BVHFrontList* front_list,
+                      CoalScalar& sqrDistLowerBound);
+
+void collisionNonRecurse(CollisionTraversalNodeBase* node,
+                         BVHFrontList* front_list,
+                         CoalScalar& sqrDistLowerBound);
+
+/// @brief Recurse function for distance
+void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
+                     unsigned int b2, BVHFrontList* front_list);
+
+/// @brief Recurse function for distance, using queue acceleration
+void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
+                          unsigned int b2, BVHFrontList* front_list,
+                          unsigned int qsize);
+
+/// @brief Recurse function for front list propagation
+void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
+                                           const CollisionRequest& request,
+                                           CollisionResult& result,
+                                           BVHFrontList* front_list);
+
+}  // namespace coal
+
+/// @endcond
+
+#endif
diff --git a/include/coal/logging.h b/include/coal/logging.h
new file mode 100644
index 0000000000000000000000000000000000000000..235b475c53a0f9e135811510927f892d9a97b3c3
--- /dev/null
+++ b/include/coal/logging.h
@@ -0,0 +1,54 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, INRIA
+ *  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.
+ */
+
+/// This file defines basic logging macros for Coal, based on Boost.Log.
+/// To enable logging, define the preprocessor macro `COAL_ENABLE_LOGGING`.
+
+#ifndef COAL_LOGGING_H
+#define COAL_LOGGING_H
+
+#ifdef COAL_ENABLE_LOGGING
+#include <boost/log/trivial.hpp>
+#define COAL_LOG_INFO(message) BOOST_LOG_TRIVIAL(info) << message
+#define COAL_LOG_DEBUG(message) BOOST_LOG_TRIVIAL(debug) << message
+#define COAL_LOG_WARNING(message) BOOST_LOG_TRIVIAL(warning) << message
+#define COAL_LOG_ERROR(message) BOOST_LOG_TRIVIAL(error) << message
+#else
+#define COAL_LOG_INFO(message)
+#define COAL_LOG_DEBUG(message)
+#define COAL_LOG_WARNING(message)
+#define COAL_LOG_ERROR(message)
+#endif
+
+#endif  // COAL_LOGGING_H
diff --git a/include/coal/math/matrix_3f.h b/include/coal/math/matrix_3f.h
new file mode 100644
index 0000000000000000000000000000000000000000..70f555a02e8e35a6ab471cd6ae0d657303e653d4
--- /dev/null
+++ b/include/coal/math/matrix_3f.h
@@ -0,0 +1,46 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_MATRIX_3F_H
+#define COAL_MATRIX_3F_H
+
+#warning "This file is deprecated. Include <coal/data_types.h> instead."
+
+// List of equivalent includes.
+#include "coal/data_types.h"
+
+#endif
diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h
new file mode 100644
index 0000000000000000000000000000000000000000..7d712fadc18f0383d959807c8d127272d3f21ca6
--- /dev/null
+++ b/include/coal/math/transform.h
@@ -0,0 +1,272 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_TRANSFORM_H
+#define COAL_TRANSFORM_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+
+namespace coal {
+
+COAL_DEPRECATED typedef Eigen::Quaternion<CoalScalar> Quaternion3f;
+typedef Eigen::Quaternion<CoalScalar> Quatf;
+
+static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) {
+  o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
+  return o;
+}
+
+/// @brief Simple transform class used locally by InterpMotion
+class COAL_DLLAPI Transform3s {
+  /// @brief Matrix cache
+  Matrix3s R;
+
+  /// @brief Translation vector
+  Vec3s T;
+
+ public:
+  /// @brief Default transform is no movement
+  Transform3s() {
+    setIdentity();  // set matrix_set true
+  }
+
+  static Transform3s Identity() { return Transform3s(); }
+
+  /// @brief Construct transform from rotation and translation
+  template <typename Matrixx3Like, typename Vector3Like>
+  Transform3s(const Eigen::MatrixBase<Matrixx3Like>& R_,
+              const Eigen::MatrixBase<Vector3Like>& T_)
+      : R(R_), T(T_) {}
+
+  /// @brief Construct transform from rotation and translation
+  template <typename Vector3Like>
+  Transform3s(const Quatf& q_, const Eigen::MatrixBase<Vector3Like>& T_)
+      : R(q_.toRotationMatrix()), T(T_) {}
+
+  /// @brief Construct transform from rotation
+  Transform3s(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {}
+
+  /// @brief Construct transform from rotation
+  Transform3s(const Quatf& q_) : R(q_), T(Vec3s::Zero()) {}
+
+  /// @brief Construct transform from translation
+  Transform3s(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {}
+
+  /// @brief Construct transform from other transform
+  Transform3s(const Transform3s& tf) : R(tf.R), T(tf.T) {}
+
+  /// @brief operator =
+  Transform3s& operator=(const Transform3s& tf) {
+    R = tf.R;
+    T = tf.T;
+    return *this;
+  }
+
+  /// @brief get translation
+  inline const Vec3s& getTranslation() const { return T; }
+
+  /// @brief get translation
+  inline const Vec3s& translation() const { return T; }
+
+  /// @brief get translation
+  inline Vec3s& translation() { return T; }
+
+  /// @brief get rotation
+  inline const Matrix3s& getRotation() const { return R; }
+
+  /// @brief get rotation
+  inline const Matrix3s& rotation() const { return R; }
+
+  /// @brief get rotation
+  inline Matrix3s& rotation() { return R; }
+
+  /// @brief get quaternion
+  inline Quatf getQuatRotation() const { return Quatf(R); }
+
+  /// @brief set transform from rotation and translation
+  template <typename Matrix3Like, typename Vector3Like>
+  inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
+                           const Eigen::MatrixBase<Vector3Like>& T_) {
+    R.noalias() = R_;
+    T.noalias() = T_;
+  }
+
+  /// @brief set transform from rotation and translation
+  inline void setTransform(const Quatf& q_, const Vec3s& T_) {
+    R = q_.toRotationMatrix();
+    T = T_;
+  }
+
+  /// @brief set transform from rotation
+  template <typename Derived>
+  inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
+    R.noalias() = R_;
+  }
+
+  /// @brief set transform from translation
+  template <typename Derived>
+  inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
+    T.noalias() = T_;
+  }
+
+  /// @brief set transform from rotation
+  inline void setQuatRotation(const Quatf& q_) { R = q_.toRotationMatrix(); }
+
+  /// @brief transform a given vector by the transform
+  template <typename Derived>
+  inline Vec3s transform(const Eigen::MatrixBase<Derived>& v) const {
+    return R * v + T;
+  }
+
+  /// @brief transform a given vector by the inverse of the transform
+  template <typename Derived>
+  inline Vec3s inverseTransform(const Eigen::MatrixBase<Derived>& v) const {
+    return R.transpose() * (v - T);
+  }
+
+  /// @brief inverse transform
+  inline Transform3s& inverseInPlace() {
+    R.transposeInPlace();
+    T = -R * T;
+    return *this;
+  }
+
+  /// @brief inverse transform
+  inline Transform3s inverse() {
+    return Transform3s(R.transpose(), -R.transpose() * T);
+  }
+
+  /// @brief inverse the transform and multiply with another
+  inline Transform3s inverseTimes(const Transform3s& other) const {
+    return Transform3s(R.transpose() * other.R, R.transpose() * (other.T - T));
+  }
+
+  /// @brief multiply with another transform
+  inline const Transform3s& operator*=(const Transform3s& other) {
+    T += R * other.T;
+    R *= other.R;
+    return *this;
+  }
+
+  /// @brief multiply with another transform
+  inline Transform3s operator*(const Transform3s& other) const {
+    return Transform3s(R * other.R, R * other.T + T);
+  }
+
+  /// @brief check whether the transform is identity
+  inline bool isIdentity(
+      const CoalScalar& prec =
+          Eigen::NumTraits<CoalScalar>::dummy_precision()) const {
+    return R.isIdentity(prec) && T.isZero(prec);
+  }
+
+  /// @brief set the transform to be identity transform
+  inline void setIdentity() {
+    R.setIdentity();
+    T.setZero();
+  }
+
+  /// @brief return a random transform
+  static Transform3s Random() {
+    Transform3s tf = Transform3s();
+    tf.setRandom();
+    return tf;
+  }
+
+  /// @brief set the transform to a random transform
+  inline void setRandom();
+
+  bool operator==(const Transform3s& other) const {
+    return (R == other.getRotation()) && (T == other.getTranslation());
+  }
+
+  bool operator!=(const Transform3s& other) const { return !(*this == other); }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template <typename Derived>
+inline Quatf fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
+                           CoalScalar angle) {
+  return Quatf(Eigen::AngleAxis<CoalScalar>(angle, axis));
+}
+
+/// @brief Uniformly random quaternion sphere.
+/// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio).
+inline Quatf uniformRandomQuaternion() {
+  // Rotational part
+  const CoalScalar u1 = (CoalScalar)rand() / RAND_MAX;
+  const CoalScalar u2 = (CoalScalar)rand() / RAND_MAX;
+  const CoalScalar u3 = (CoalScalar)rand() / RAND_MAX;
+
+  const CoalScalar mult1 = std::sqrt(CoalScalar(1.0) - u1);
+  const CoalScalar mult2 = std::sqrt(u1);
+
+  static const CoalScalar PI_value = static_cast<CoalScalar>(EIGEN_PI);
+  CoalScalar s2 = std::sin(2 * PI_value * u2);
+  CoalScalar c2 = std::cos(2 * PI_value * u2);
+  CoalScalar s3 = std::sin(2 * PI_value * u3);
+  CoalScalar c3 = std::cos(2 * PI_value * u3);
+
+  Quatf q;
+  q.w() = mult1 * s2;
+  q.x() = mult1 * c2;
+  q.y() = mult2 * s3;
+  q.z() = mult2 * c3;
+  return q;
+}
+
+inline void Transform3s::setRandom() {
+  const Quatf q = uniformRandomQuaternion();
+  this->rotation() = q.matrix();
+  this->translation().setRandom();
+}
+
+/// @brief Construct othonormal basis from vector.
+/// The z-axis is the normalized input vector.
+inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) {
+  Matrix3s basis = Matrix3s::Zero();
+  basis.col(2) = vec.normalized();
+  basis.col(1) = -vec.unitOrthogonal();
+  basis.col(0) = basis.col(1).cross(vec);
+  return basis;
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/math/types.h b/include/coal/math/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..4b9b5a91f28ce842ac802a07defed139652189a4
--- /dev/null
+++ b/include/coal/math/types.h
@@ -0,0 +1,46 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 Joseph Mirabel */
+
+#ifndef COAL_MATH_TYPES_H
+#define COAL_MATH_TYPES_H
+
+#warning "This file is deprecated. Include <coal/data_types.h> instead."
+
+// List of equivalent includes.
+#include "coal/data_types.h"
+
+#endif
diff --git a/include/coal/math/vec_3f.h b/include/coal/math/vec_3f.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b79b21915c39fe011261c7f66c0cf3c89596a8c
--- /dev/null
+++ b/include/coal/math/vec_3f.h
@@ -0,0 +1,46 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_VEC_3F_H
+#define COAL_VEC_3F_H
+
+#warning "This file is deprecated. Include <coal/data_types.h> instead."
+
+// List of equivalent includes.
+#include "coal/data_types.h"
+
+#endif
diff --git a/include/coal/mesh_loader/assimp.h b/include/coal/mesh_loader/assimp.h
new file mode 100644
index 0000000000000000000000000000000000000000..0133f4569b42b357ac4eeca6f5daa25c4fc5a6d1
--- /dev/null
+++ b/include/coal/mesh_loader/assimp.h
@@ -0,0 +1,127 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2016-2019, CNRS - LAAS
+ *  Copyright (c) 2019, INRIA
+ *  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.
+ */
+
+#ifndef COAL_MESH_LOADER_ASSIMP_H
+#define COAL_MESH_LOADER_ASSIMP_H
+
+#include "coal/fwd.hh"
+#include "coal/config.hh"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BVH/BVH_model.h"
+
+struct aiScene;
+namespace Assimp {
+class Importer;
+}
+
+namespace coal {
+
+namespace internal {
+
+struct COAL_DLLAPI TriangleAndVertices {
+  std::vector<coal::Vec3s> vertices_;
+  std::vector<coal::Triangle> triangles_;
+};
+
+struct COAL_DLLAPI Loader {
+  Loader();
+  ~Loader();
+
+  void load(const std::string& resource_path);
+
+  Assimp::Importer* importer;
+  aiScene const* scene;
+};
+
+/**
+ * @brief      Recursive procedure for building a mesh
+ *
+ * @param[in]  scale           Scale to apply when reading the ressource
+ * @param[in]  scene           Pointer to the assimp scene
+ * @param[in]  vertices_offset Current number of vertices in the model
+ * @param      tv              Triangles and Vertices of the mesh submodels
+ */
+COAL_DLLAPI void buildMesh(const coal::Vec3s& scale, const aiScene* scene,
+                           unsigned vertices_offset, TriangleAndVertices& tv);
+
+/**
+ * @brief      Convert an assimp scene to a mesh
+ *
+ * @param[in]  scale  Scale to apply when reading the ressource
+ * @param[in]  scene  Pointer to the assimp scene
+ * @param[out] mesh  The mesh that must be built
+ */
+template <class BoundingVolume>
+inline void meshFromAssimpScene(
+    const coal::Vec3s& scale, const aiScene* scene,
+    const shared_ptr<BVHModel<BoundingVolume> >& mesh) {
+  TriangleAndVertices tv;
+
+  int res = mesh->beginModel();
+
+  if (res != coal::BVH_OK) {
+    COAL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error);
+  }
+
+  buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv);
+  mesh->addSubModel(tv.vertices_, tv.triangles_);
+
+  mesh->endModel();
+}
+
+}  // namespace internal
+
+/**
+ * @brief      Read a mesh file and convert it to a polyhedral mesh
+ *
+ * @param[in]  resource_path  Path to the ressource mesh file to be read
+ * @param[in]  scale          Scale to apply when reading the ressource
+ * @param[out] polyhedron     The resulted polyhedron
+ */
+template <class BoundingVolume>
+inline void loadPolyhedronFromResource(
+    const std::string& resource_path, const coal::Vec3s& scale,
+    const shared_ptr<BVHModel<BoundingVolume> >& polyhedron) {
+  internal::Loader scene;
+  scene.load(resource_path);
+
+  internal::meshFromAssimpScene(scale, scene.scene, polyhedron);
+}
+
+}  // namespace coal
+
+#endif  // COAL_MESH_LOADER_ASSIMP_H
diff --git a/include/coal/mesh_loader/loader.h b/include/coal/mesh_loader/loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..18b2255b837b756dae0f33b8cbb15bc47e5b3aa5
--- /dev/null
+++ b/include/coal/mesh_loader/loader.h
@@ -0,0 +1,102 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2016, CNRS - LAAS
+ *  Copyright (c) 2020, INRIA
+ *  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.
+ */
+
+#ifndef COAL_MESH_LOADER_LOADER_H
+#define COAL_MESH_LOADER_LOADER_H
+
+#include "coal/fwd.hh"
+#include "coal/config.hh"
+#include "coal/data_types.h"
+#include "coal/collision_object.h"
+
+#include <map>
+#include <ctime>
+
+namespace coal {
+/// Base class for building polyhedron from files.
+/// This class builds a new object for each file.
+class COAL_DLLAPI MeshLoader {
+ public:
+  virtual ~MeshLoader() {}
+
+  virtual BVHModelPtr_t load(const std::string& filename,
+                             const Vec3s& scale = Vec3s::Ones());
+
+  /// Create an OcTree from a file in binary octomap format.
+  /// \todo add OctreePtr_t
+  virtual CollisionGeometryPtr_t loadOctree(const std::string& filename);
+
+  MeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : bvType_(bvType) {}
+
+ private:
+  const NODE_TYPE bvType_;
+};
+
+/// Class for building polyhedron from files with cache mechanism.
+/// This class builds a new object for each different file.
+/// If method CachedMeshLoader::load is called twice with the same arguments,
+/// the second call returns the result of the first call.
+class COAL_DLLAPI CachedMeshLoader : public MeshLoader {
+ public:
+  virtual ~CachedMeshLoader() {}
+
+  CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {}
+
+  virtual BVHModelPtr_t load(const std::string& filename, const Vec3s& scale);
+
+  struct COAL_DLLAPI Key {
+    std::string filename;
+    Vec3s scale;
+
+    Key(const std::string& f, const Vec3s& s) : filename(f), scale(s) {}
+
+    bool operator<(const CachedMeshLoader::Key& b) const;
+  };
+  struct COAL_DLLAPI Value {
+    BVHModelPtr_t model;
+    std::time_t mtime;
+  };
+  typedef std::map<Key, Value> Cache_t;
+
+  const Cache_t& cache() const { return cache_; }
+
+ private:
+  Cache_t cache_;
+};
+}  // namespace coal
+
+#endif  // COAL_MESH_LOADER_LOADER_H
diff --git a/include/coal/narrowphase/gjk.h b/include/coal/narrowphase/gjk.h
new file mode 100644
index 0000000000000000000000000000000000000000..ea8ed8f032964f3c4a57e586233850b1eeea1592
--- /dev/null
+++ b/include/coal/narrowphase/gjk.h
@@ -0,0 +1,454 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2021-2024, INRIA
+ *  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 COAL_GJK_H
+#define COAL_GJK_H
+
+#include <vector>
+
+#include "coal/narrowphase/minkowski_difference.h"
+
+namespace coal {
+
+namespace details {
+
+/// @brief class for GJK algorithm
+///
+/// @note The computations are performed in the frame of the first shape.
+struct COAL_DLLAPI GJK {
+  struct COAL_DLLAPI SimplexV {
+    /// @brief support vector for shape 0 and 1.
+    Vec3s w0, w1;
+    /// @brief support vector (i.e., the furthest point on the shape along the
+    /// support direction)
+    Vec3s w;
+  };
+
+  typedef unsigned char vertex_id_t;
+
+  /// @brief A simplex is a set of up to 4 vertices.
+  /// Its rank is the number of vertices it contains.
+  /// @note This data structure does **not** own the vertices it refers to.
+  /// To be efficient, the constructor of `GJK` creates storage for 4 vertices.
+  /// Since GJK does not need any more storage, it reuses these vertices
+  /// throughout the algorithm by using multiple instance of this `Simplex`
+  /// class.
+  struct COAL_DLLAPI Simplex {
+    /// @brief simplex vertex
+    SimplexV* vertex[4];
+    /// @brief size of simplex (number of vertices)
+    vertex_id_t rank;
+
+    Simplex() {}
+
+    void reset() {
+      rank = 0;
+      for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr;
+    }
+  };
+
+  /// @brief Status of the GJK algorithm:
+  /// DidNotRun: GJK has not been run.
+  /// Failed: GJK did not converge (it exceeded the maximum number of
+  /// iterations).
+  /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited
+  ///     before converting. The shapes are not in collision.
+  /// NoCollision: GJK converged and the shapes are not in collision.
+  /// Collision: GJK converged and the shapes are in collision.
+  /// Failed: GJK did not converge.
+  enum Status {
+    DidNotRun,
+    Failed,
+    NoCollisionEarlyStopped,
+    NoCollision,
+    CollisionWithPenetrationInformation,
+    Collision
+  };
+
+ public:
+  CoalScalar distance_upper_bound;
+  Status status;
+  GJKVariant gjk_variant;
+  GJKConvergenceCriterion convergence_criterion;
+  GJKConvergenceCriterionType convergence_criterion_type;
+
+  MinkowskiDiff const* shape;
+  Vec3s ray;
+  support_func_guess_t support_hint;
+  /// @brief The distance between the two shapes, computed by GJK.
+  /// If the distance is below GJK's threshold, the shapes are in collision in
+  /// the eyes of GJK. If `distance_upper_bound` is set to a value lower than
+  /// infinity, GJK will early stop as soon as it finds `distance` to be greater
+  /// than `distance_upper_bound`.
+  CoalScalar distance;
+  Simplex* simplex;  // Pointer to the result of the last run of GJK.
+
+ private:
+  // max_iteration and tolerance are made private
+  // because they are meant to be set by the `reset` function.
+  size_t max_iterations;
+  CoalScalar tolerance;
+
+  SimplexV store_v[4];
+  SimplexV* free_v[4];
+  vertex_id_t nfree;
+  vertex_id_t current;
+  Simplex simplices[2];
+  size_t iterations;
+  size_t iterations_momentum_stop;
+
+ public:
+  /// \param max_iterations_ number of iteration before GJK returns failure.
+  /// \param tolerance_ precision of the algorithm.
+  ///
+  /// The tolerance argument is useful for continuous shapes and for polyhedron
+  /// with some vertices closer than this threshold.
+  ///
+  /// Suggested values are 100 iterations and a tolerance of 1e-6.
+  GJK(size_t max_iterations_, CoalScalar tolerance_)
+      : max_iterations(max_iterations_), tolerance(tolerance_) {
+    COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.",
+                std::invalid_argument);
+    initialize();
+  }
+
+  /// @brief resets the GJK algorithm, preparing it for a new run.
+  /// Other than the maximum number of iterations and the tolerance,
+  /// this function does **not** modify the parameters of the GJK algorithm.
+  void reset(size_t max_iterations_, CoalScalar tolerance_);
+
+  /// @brief GJK algorithm, given the initial value guess
+  Status evaluate(
+      const MinkowskiDiff& shape, const Vec3s& guess,
+      const support_func_guess_t& supportHint = support_func_guess_t::Zero());
+
+  /// @brief apply the support function along a direction, the result is return
+  /// in sv
+  inline void getSupport(const Vec3s& d, SimplexV& sv,
+                         support_func_guess_t& hint) const {
+    shape->support(d, sv.w0, sv.w1, hint);
+    sv.w = sv.w0 - sv.w1;
+  }
+
+  /// @brief whether the simplex enclose the origin
+  bool encloseOrigin();
+
+  /// @brief get the underlying simplex using in GJK, can be used for cache in
+  /// next iteration
+  inline Simplex* getSimplex() const { return simplex; }
+
+  /// Tells whether the closest points are available.
+  bool hasClosestPoints() const { return distance < distance_upper_bound; }
+
+  /// Get the witness points on each object, and the corresponding normal.
+  /// @param[in] shape is the Minkowski difference of the two shapes.
+  /// @param[out] w0 is the witness point on shape0.
+  /// @param[out] w1 is the witness point on shape1.
+  /// @param[out] normal is the normal of the separating plane found by
+  /// GJK. It points from shape0 to shape1.
+  void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+                                 Vec3s& w1, Vec3s& normal) const;
+
+  /// @brief get the guess from current simplex
+  Vec3s getGuessFromSimplex() const;
+
+  /// @brief Distance threshold for early break.
+  /// GJK stops when it proved the distance is more than this threshold.
+  /// @note The closest points will be erroneous in this case.
+  ///       If you want the closest points, set this to infinity (the default).
+  void setDistanceEarlyBreak(const CoalScalar& dup) {
+    distance_upper_bound = dup;
+  }
+
+  /// @brief Convergence check used to stop GJK when shapes are not in
+  /// collision.
+  bool checkConvergence(const Vec3s& w, const CoalScalar& rl, CoalScalar& alpha,
+                        const CoalScalar& omega) const;
+
+  /// @brief Get the max number of iterations of GJK.
+  size_t getNumMaxIterations() const { return max_iterations; }
+
+  /// @brief Get the tolerance of GJK.
+  CoalScalar getTolerance() const { return tolerance; }
+
+  /// @brief Get the number of iterations of the last run of GJK.
+  size_t getNumIterations() const { return iterations; }
+
+  /// @brief Get GJK number of iterations before momentum stops.
+  /// Only usefull if the Nesterov or Polyak acceleration activated.
+  size_t getNumIterationsMomentumStopped() const {
+    return iterations_momentum_stop;
+  }
+
+ private:
+  /// @brief Initializes the GJK algorithm.
+  /// This function should only be called by the constructor.
+  /// Otherwise use \ref reset.
+  void initialize();
+
+  /// @brief discard one vertex from the simplex
+  inline void removeVertex(Simplex& simplex);
+
+  /// @brief append one vertex to the simplex
+  inline void appendVertex(Simplex& simplex, const Vec3s& v,
+                           support_func_guess_t& hint);
+
+  /// @brief Project origin (0) onto line a-b
+  /// For a detailed explanation of how to efficiently project onto a simplex,
+  /// check out Ericson's book, page 403:
+  /// https://realtimecollisiondetection.net/ To sum up, a simplex has a voronoi
+  /// region for each feature it has (vertex, edge, face). We find the voronoi
+  /// region in which the origin lies and stop as soon as we find it; we then
+  /// project onto it and return the result. We start by voronoi regions
+  /// generated by vertices then move on to edges then faces. Checking voronoi
+  /// regions is done using simple dot products. Moreover, edges voronoi checks
+  /// reuse computations of vertices voronoi checks. The same goes for faces
+  /// which reuse checks from edges.
+  /// Finally, in addition to the voronoi procedure, checks relying on the order
+  /// of construction
+  /// of the simplex are added. To know more about these, visit
+  /// https://caseymuratori.com/blog_0003.
+  bool projectLineOrigin(const Simplex& current, Simplex& next);
+
+  /// @brief Project origin (0) onto triangle a-b-c
+  /// See \ref projectLineOrigin for an explanation on simplex projections.
+  bool projectTriangleOrigin(const Simplex& current, Simplex& next);
+
+  /// @brief Project origin (0) onto tetrahedron a-b-c-d
+  /// See \ref projectLineOrigin for an explanation on simplex projections.
+  bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
+};
+
+/// @brief class for EPA algorithm
+struct COAL_DLLAPI EPA {
+  typedef GJK::SimplexV SimplexVertex;
+  struct COAL_DLLAPI SimplexFace {
+    Vec3s n;
+    CoalScalar d;
+    bool ignore;          // If the origin does not project inside the face, we
+                          // ignore this face.
+    size_t vertex_id[3];  // Index of vertex in sv_store.
+    SimplexFace* adjacent_faces[3];  // A face has three adjacent faces.
+    SimplexFace* prev_face;          // The previous face in the list.
+    SimplexFace* next_face;          // The next face in the list.
+    size_t adjacent_edge[3];         // Each face has 3 edges: `0`, `1` and `2`.
+                              // The i-th adjacent face is bound (to this face)
+                              // along its `adjacent_edge[i]`-th edge
+                              // (with 0 <= i <= 2).
+    size_t pass;
+
+    SimplexFace() : n(Vec3s::Zero()), ignore(false) {};
+  };
+
+  /// @brief The simplex list of EPA is a linked list of faces.
+  /// Note: EPA's linked list does **not** own any memory.
+  /// The memory it refers to is contiguous and owned by a std::vector.
+  struct COAL_DLLAPI SimplexFaceList {
+    SimplexFace* root;
+    size_t count;
+    SimplexFaceList() : root(nullptr), count(0) {}
+
+    void reset() {
+      root = nullptr;
+      count = 0;
+    }
+
+    void append(SimplexFace* face) {
+      face->prev_face = nullptr;
+      face->next_face = root;
+      if (root != nullptr) root->prev_face = face;
+      root = face;
+      ++count;
+    }
+
+    void remove(SimplexFace* face) {
+      if (face->next_face != nullptr)
+        face->next_face->prev_face = face->prev_face;
+      if (face->prev_face != nullptr)
+        face->prev_face->next_face = face->next_face;
+      if (face == root) root = face->next_face;
+      --count;
+    }
+  };
+
+  /// @brief We bind the face `fa` along its edge `ea` to the face `fb` along
+  /// its edge `fb`.
+  static inline void bind(SimplexFace* fa, size_t ea, SimplexFace* fb,
+                          size_t eb) {
+    assert(ea == 0 || ea == 1 || ea == 2);
+    assert(eb == 0 || eb == 1 || eb == 2);
+    fa->adjacent_edge[ea] = eb;
+    fa->adjacent_faces[ea] = fb;
+    fb->adjacent_edge[eb] = ea;
+    fb->adjacent_faces[eb] = fa;
+  }
+
+  struct COAL_DLLAPI SimplexHorizon {
+    SimplexFace* current_face;  // current face in the horizon
+    SimplexFace* first_face;    // first face in the horizon
+    size_t num_faces;           // number of faces in the horizon
+    SimplexHorizon()
+        : current_face(nullptr), first_face(nullptr), num_faces(0) {}
+  };
+
+  enum Status {
+    DidNotRun = -1,
+    Failed = 0,
+    Valid = 1,
+    AccuracyReached = 1 << 1 | Valid,
+    Degenerated = 1 << 1 | Failed,
+    NonConvex = 2 << 1 | Failed,
+    InvalidHull = 3 << 1 | Failed,
+    OutOfFaces = 4 << 1 | Failed,
+    OutOfVertices = 5 << 1 | Failed,
+    FallBack = 6 << 1 | Failed
+  };
+
+ public:
+  Status status;
+  GJK::Simplex result;
+  Vec3s normal;
+  support_func_guess_t support_hint;
+  CoalScalar depth;
+  SimplexFace* closest_face;
+
+ private:
+  // max_iteration and tolerance are made private
+  // because they are meant to be set by the `reset` function.
+  size_t max_iterations;
+  CoalScalar tolerance;
+
+  std::vector<SimplexVertex> sv_store;
+  std::vector<SimplexFace> fc_store;
+  SimplexFaceList hull, stock;
+  size_t num_vertices;  // number of vertices in polytpoe constructed by EPA
+  size_t iterations;
+
+ public:
+  EPA(size_t max_iterations_, CoalScalar tolerance_)
+      : max_iterations(max_iterations_), tolerance(tolerance_) {
+    initialize();
+  }
+
+  /// @brief Copy constructor of EPA.
+  /// Mostly needed for the copy constructor of `GJKSolver`.
+  EPA(const EPA& other)
+      : max_iterations(other.max_iterations),
+        tolerance(other.tolerance),
+        sv_store(other.sv_store),
+        fc_store(other.fc_store) {
+    initialize();
+  }
+
+  /// @brief Get the max number of iterations of EPA.
+  size_t getNumMaxIterations() const { return max_iterations; }
+
+  /// @brief Get the max number of vertices of EPA.
+  size_t getNumMaxVertices() const { return sv_store.size(); }
+
+  /// @brief Get the max number of faces of EPA.
+  size_t getNumMaxFaces() const { return fc_store.size(); }
+
+  /// @brief Get the tolerance of EPA.
+  CoalScalar getTolerance() const { return tolerance; }
+
+  /// @brief Get the number of iterations of the last run of EPA.
+  size_t getNumIterations() const { return iterations; }
+
+  /// @brief Get the number of vertices in the polytope of the last run of EPA.
+  size_t getNumVertices() const { return num_vertices; }
+
+  /// @brief Get the number of faces in the polytope of the last run of EPA.
+  size_t getNumFaces() const { return hull.count; }
+
+  /// @brief resets the EPA algorithm, preparing it for a new run.
+  /// It potentially reallocates memory for the vertices and faces
+  /// if the passed parameters are bigger than the previous ones.
+  /// This function does **not** modify the parameters of the EPA algorithm,
+  /// i.e. the maximum number of iterations and the tolerance.
+  /// @note calling this function destroys the previous state of EPA.
+  /// In the future, we may want to copy it instead, i.e. when EPA will
+  /// be (properly) warm-startable.
+  void reset(size_t max_iterations, CoalScalar tolerance);
+
+  /// \return a Status which can be demangled using (status & Valid) or
+  ///         (status & Failed). The other values provide a more detailled
+  ///         status
+  Status evaluate(GJK& gjk, const Vec3s& guess);
+
+  /// Get the witness points on each object, and the corresponding normal.
+  /// @param[in] shape is the Minkowski difference of the two shapes.
+  /// @param[out] w0 is the witness point on shape0.
+  /// @param[out] w1 is the witness point on shape1.
+  /// @param[in] normal is the normal found by EPA. It points from shape0 to
+  /// shape1. The normal is used to correct the witness points on the shapes if
+  /// the shapes have a non-zero swept-sphere radius.
+  void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+                                 Vec3s& w1, Vec3s& normal) const;
+
+ private:
+  /// @brief Allocates memory for the EPA algorithm.
+  /// This function should only be called by the constructor.
+  /// Otherwise use \ref reset.
+  void initialize();
+
+  bool getEdgeDist(SimplexFace* face, const SimplexVertex& a,
+                   const SimplexVertex& b, CoalScalar& dist);
+
+  /// @brief Add a new face to the polytope.
+  /// This function sets the `ignore` flag to `true` if the origin does not
+  /// project inside the face.
+  SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex,
+                       bool force = false);
+
+  /// @brief Find the best polytope face to split
+  SimplexFace* findClosestFace();
+
+  /// @brief the goal is to add a face connecting vertex w and face edge f[e]
+  bool expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
+              SimplexHorizon& horizon);
+
+  // @brief Use this function to debug expand if needed.
+  // void PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w);
+};
+
+}  // namespace details
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h
new file mode 100644
index 0000000000000000000000000000000000000000..1d9e36bfa36f0ed1a64536cad5ac5922f4106917
--- /dev/null
+++ b/include/coal/narrowphase/minkowski_difference.h
@@ -0,0 +1,186 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2021-2024, INRIA
+ *  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.
+ */
+
+/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
+
+#ifndef COAL_MINKOWSKI_DIFFERENCE_H
+#define COAL_MINKOWSKI_DIFFERENCE_H
+
+#include "coal/shape/geometric_shapes.h"
+#include "coal/math/transform.h"
+#include "coal/narrowphase/support_functions.h"
+
+namespace coal {
+
+namespace details {
+
+/// @brief Minkowski difference class of two shapes
+///
+/// @note The Minkowski difference is expressed in the frame of the first shape.
+struct COAL_DLLAPI MinkowskiDiff {
+  typedef Eigen::Array<CoalScalar, 1, 2> Array2d;
+
+  /// @brief points to two shapes
+  const ShapeBase* shapes[2];
+
+  /// @brief Store temporary data for the computation of the support point for
+  /// each shape.
+  ShapeSupportData data[2];
+
+  /// @brief rotation from shape1 to shape0
+  /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
+  Matrix3s oR1;
+
+  /// @brief translation from shape1 to shape0
+  /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
+  Vec3s ot1;
+
+  /// @brief The radii of the sphere swepted around each shape of the Minkowski
+  /// difference. The 2 values correspond to the swept-sphere radius of shape 0
+  /// and shape 1.
+  Array2d swept_sphere_radius;
+
+  /// @brief Wether or not to use the normalize heuristic in the GJK Nesterov
+  /// acceleration. This setting is only applied if the Nesterov acceleration in
+  /// the GJK class is active.
+  bool normalize_support_direction;
+
+  typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
+                                     const Vec3s& dir, Vec3s& support0,
+                                     Vec3s& support1,
+                                     support_func_guess_t& hint,
+                                     ShapeSupportData data[2]);
+  GetSupportFunction getSupportFunc;
+
+  MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {}
+
+  /// @brief Set the two shapes, assuming the relative transformation between
+  /// them is identity.
+  /// Consequently, all support points computed with the MinkowskiDiff
+  /// will be expressed in the world frame.
+  /// @param shape0 the first shape.
+  /// @param shape1 the second shape.
+  /// @tparam SupportOptions is a value of the SupportOptions enum. If set to
+  /// `WithSweptSphere`, the support computation will take into account the
+  /// swept sphere radius of the shapes. If set to `NoSweptSphere`, where
+  /// this information is simply stored in the Minkowski's difference
+  /// `swept_sphere_radius` array. This array is then used to correct the
+  /// solution found when GJK or EPA have converged.
+  ///
+  /// @note In practice, there is no need to take into account the swept-sphere
+  /// radius in the iterations of GJK/EPA. It is in fact detrimental to the
+  /// convergence of both algos. This is because it makes corners and edges of
+  /// shapes look strictly convex to the algorithms, which leads to poor
+  /// convergence. This swept sphere template parameter is only here for
+  /// debugging purposes and for specific uses cases where the swept sphere
+  /// radius is needed in the support function. The rule is simple. When
+  /// interacting with GJK/EPA, the `SupportOptions` template
+  /// parameter should **always** be set to `NoSweptSphere` (except for
+  /// debugging or testing purposes). When working directly with the shapes
+  /// involved in the collision, and not relying on GJK/EPA, the
+  /// `SupportOptions` template parameter should be set to `WithSweptSphere`.
+  /// This is for example the case for specialized collision/distance functions.
+  template <int _SupportOptions = SupportOptions::NoSweptSphere>
+  void set(const ShapeBase* shape0, const ShapeBase* shape1);
+
+  /// @brief Set the two shapes, with a relative transformation from shape0 to
+  /// shape1. Consequently, all support points computed with the MinkowskiDiff
+  /// will be expressed in the frame of shape0.
+  /// @param shape0 the first shape.
+  /// @param shape1 the second shape.
+  /// @param tf0 the transformation of the first shape.
+  /// @param tf1 the transformation of the second shape.
+  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
+  /// ShapeBase*)` for more details.
+  template <int _SupportOptions = SupportOptions::NoSweptSphere>
+  void set(const ShapeBase* shape0, const ShapeBase* shape1,
+           const Transform3s& tf0, const Transform3s& tf1);
+
+  /// @brief support function for shape0.
+  /// The output vector is expressed in the local frame of shape0.
+  /// @return a point which belongs to the set {argmax_{v in shape0}
+  /// v.dot(dir)}, i.e a support of shape0 in direction dir.
+  /// @param dir support direction.
+  /// @param hint used to initialize the search when shape is a ConvexBase
+  /// object.
+  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
+  /// ShapeBase*)` for more details.
+  template <int _SupportOptions = SupportOptions::NoSweptSphere>
+  inline Vec3s support0(const Vec3s& dir, int& hint) const {
+    return getSupport<_SupportOptions>(shapes[0], dir, hint);
+  }
+
+  /// @brief support function for shape1.
+  /// The output vector is expressed in the local frame of shape0.
+  /// This is mandatory because in the end we are interested in support points
+  /// of the Minkowski difference; hence the supports of shape0 and shape1 must
+  /// live in the same frame.
+  /// @return a point which belongs to the set {tf * argmax_{v in shape1}
+  /// v.dot(R^T * dir)}, i.e. the support of shape1 in direction dir (tf is the
+  /// tranform from shape1 to shape0).
+  /// @param dir support direction.
+  /// @param hint used to initialize the search when shape is a ConvexBase.
+  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
+  /// ShapeBase*)` for more details.
+  template <int _SupportOptions = SupportOptions::NoSweptSphere>
+  inline Vec3s support1(const Vec3s& dir, int& hint) const {
+    // clang-format off
+    return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1;
+    // clang-format on
+  }
+
+  /// @brief Support function for the pair of shapes. This method assumes `set`
+  /// has already been called.
+  /// @param[in] dir the support direction.
+  /// @param[out] supp0 support of shape0 in direction dir, expressed in the
+  /// frame of shape0.
+  /// @param[out] supp1 support of shape1 in direction -dir, expressed in the
+  /// frame of shape0.
+  /// @param[in/out] hint used to initialize the search when shape is a
+  /// ConvexBase object.
+  inline void support(const Vec3s& dir, Vec3s& supp0, Vec3s& supp1,
+                      support_func_guess_t& hint) const {
+    assert(getSupportFunc != NULL);
+    getSupportFunc(*this, dir, supp0, supp1, hint,
+                   const_cast<ShapeSupportData*>(data));
+  }
+};
+
+}  // namespace details
+
+}  // namespace coal
+
+#endif  // COAL_MINKOWSKI_DIFFERENCE_H
diff --git a/include/coal/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h
new file mode 100644
index 0000000000000000000000000000000000000000..65cd31b13c7480c343128efd67090b37a26cd632
--- /dev/null
+++ b/include/coal/narrowphase/narrowphase.h
@@ -0,0 +1,727 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
+ *  All rights reserved.
+ *  Copyright (c) 2021-2024, INRIA
+ *  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, Florent Lamiraux */
+
+#ifndef COAL_NARROWPHASE_H
+#define COAL_NARROWPHASE_H
+
+#include <limits>
+
+#include "coal/narrowphase/gjk.h"
+#include "coal/collision_data.h"
+#include "coal/narrowphase/narrowphase_defaults.h"
+#include "coal/logging.h"
+
+namespace coal {
+
+/// @brief collision and distance solver based on the GJK and EPA algorithms.
+/// Originally, GJK and EPA were implemented in fcl which itself took
+/// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA
+/// have been largely modified to be faster and more robust to numerical
+/// accuracy and edge cases.
+struct COAL_DLLAPI GJKSolver {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief GJK algorithm
+  mutable details::GJK gjk;
+
+  /// @brief maximum number of iterations of GJK
+  size_t gjk_max_iterations;
+
+  /// @brief tolerance of GJK
+  CoalScalar gjk_tolerance;
+
+  /// @brief which warm start to use for GJK
+  GJKInitialGuess gjk_initial_guess;
+
+  /// @brief Whether smart guess can be provided
+  /// @Deprecated Use gjk_initial_guess instead
+  COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
+  bool enable_cached_guess;
+
+  /// @brief smart guess
+  mutable Vec3s cached_guess;
+
+  /// @brief smart guess for the support function
+  mutable support_func_guess_t support_func_cached_guess;
+
+  /// @brief If GJK can guarantee that the distance between the shapes is
+  /// greater than this value, it will early stop.
+  CoalScalar distance_upper_bound;
+
+  /// @brief Variant of the GJK algorithm (Default, Nesterov or Polyak).
+  GJKVariant gjk_variant;
+
+  /// @brief Convergence criterion for GJK
+  GJKConvergenceCriterion gjk_convergence_criterion;
+
+  /// @brief Absolute or relative convergence criterion for GJK
+  GJKConvergenceCriterionType gjk_convergence_criterion_type;
+
+  /// @brief EPA algorithm
+  mutable details::EPA epa;
+
+  /// @brief maximum number of iterations of EPA
+  size_t epa_max_iterations;
+
+  /// @brief tolerance of EPA
+  CoalScalar epa_tolerance;
+
+  /// @brief Minkowski difference used by GJK and EPA algorithms
+  mutable details::MinkowskiDiff minkowski_difference;
+
+ private:
+  // Used internally for assertion checks.
+  static constexpr CoalScalar m_dummy_precision = 1e-6;
+
+ public:
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  /// @brief Default constructor for GJK algorithm
+  /// By default, we don't want EPA to allocate memory because
+  /// certain functions of the `GJKSolver` class have specializations
+  /// which don't use EPA (and/or GJK).
+  /// So we give EPA's constructor a max number of iterations of zero.
+  /// Only the functions that need EPA will reset the algorithm and allocate
+  /// memory if needed.
+  GJKSolver()
+      : gjk(GJK_DEFAULT_MAX_ITERATIONS, GJK_DEFAULT_TOLERANCE),
+        gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
+        gjk_tolerance(GJK_DEFAULT_TOLERANCE),
+        gjk_initial_guess(GJKInitialGuess::DefaultGuess),
+        enable_cached_guess(false),  // Use gjk_initial_guess instead
+        cached_guess(Vec3s(1, 0, 0)),
+        support_func_cached_guess(support_func_guess_t::Zero()),
+        distance_upper_bound((std::numeric_limits<CoalScalar>::max)()),
+        gjk_variant(GJKVariant::DefaultGJK),
+        gjk_convergence_criterion(GJKConvergenceCriterion::Default),
+        gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute),
+        epa(0, EPA_DEFAULT_TOLERANCE),
+        epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
+        epa_tolerance(EPA_DEFAULT_TOLERANCE) {}
+
+  /// @brief Constructor from a DistanceRequest
+  ///
+  /// \param[in] request DistanceRequest input
+  ///
+  /// See the default constructor; by default, we don't want
+  /// EPA to allocate memory so we call EPA's constructor with 0 max
+  /// number of iterations.
+  /// However, the `set` method stores the actual values of the request.
+  /// EPA will thus allocate memory only if needed.
+  explicit GJKSolver(const DistanceRequest& request)
+      : gjk(request.gjk_max_iterations, request.gjk_tolerance),
+        epa(0, request.epa_tolerance) {
+    this->cached_guess = Vec3s(1, 0, 0);
+    this->support_func_cached_guess = support_func_guess_t::Zero();
+
+    set(request);
+  }
+
+  /// @brief setter from a DistanceRequest
+  ///
+  /// \param[in] request DistanceRequest input
+  ///
+  void set(const DistanceRequest& request) {
+    // ---------------------
+    // GJK settings
+    this->gjk_initial_guess = request.gjk_initial_guess;
+    this->enable_cached_guess = request.enable_cached_gjk_guess;
+    if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+        this->enable_cached_guess) {
+      this->cached_guess = request.cached_gjk_guess;
+      this->support_func_cached_guess = request.cached_support_func_guess;
+    }
+    this->gjk_max_iterations = request.gjk_max_iterations;
+    this->gjk_tolerance = request.gjk_tolerance;
+    // For distance computation, we don't want GJK to early stop
+    this->distance_upper_bound = (std::numeric_limits<CoalScalar>::max)();
+    this->gjk_variant = request.gjk_variant;
+    this->gjk_convergence_criterion = request.gjk_convergence_criterion;
+    this->gjk_convergence_criterion_type =
+        request.gjk_convergence_criterion_type;
+
+    // ---------------------
+    // EPA settings
+    this->epa_max_iterations = request.epa_max_iterations;
+    this->epa_tolerance = request.epa_tolerance;
+
+    // ---------------------
+    // Reset GJK and EPA status
+    this->epa.status = details::EPA::Status::DidNotRun;
+    this->gjk.status = details::GJK::Status::DidNotRun;
+  }
+
+  /// @brief Constructor from a CollisionRequest
+  ///
+  /// \param[in] request CollisionRequest input
+  ///
+  /// See the default constructor; by default, we don't want
+  /// EPA to allocate memory so we call EPA's constructor with 0 max
+  /// number of iterations.
+  /// However, the `set` method stores the actual values of the request.
+  /// EPA will thus allocate memory only if needed.
+  explicit GJKSolver(const CollisionRequest& request)
+      : gjk(request.gjk_max_iterations, request.gjk_tolerance),
+        epa(0, request.epa_tolerance) {
+    this->cached_guess = Vec3s(1, 0, 0);
+    this->support_func_cached_guess = support_func_guess_t::Zero();
+
+    set(request);
+  }
+
+  /// @brief setter from a CollisionRequest
+  ///
+  /// \param[in] request CollisionRequest input
+  ///
+  void set(const CollisionRequest& request) {
+    // ---------------------
+    // GJK settings
+    this->gjk_initial_guess = request.gjk_initial_guess;
+    this->enable_cached_guess = request.enable_cached_gjk_guess;
+    if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+        this->enable_cached_guess) {
+      this->cached_guess = request.cached_gjk_guess;
+      this->support_func_cached_guess = request.cached_support_func_guess;
+    }
+    this->gjk_tolerance = request.gjk_tolerance;
+    this->gjk_max_iterations = request.gjk_max_iterations;
+    // The distance upper bound should be at least greater to the requested
+    // security margin. Otherwise, we will likely miss some collisions.
+    this->distance_upper_bound = (std::max)(
+        0., (std::max)(request.distance_upper_bound, request.security_margin));
+    this->gjk_variant = request.gjk_variant;
+    this->gjk_convergence_criterion = request.gjk_convergence_criterion;
+    this->gjk_convergence_criterion_type =
+        request.gjk_convergence_criterion_type;
+
+    // ---------------------
+    // EPA settings
+    this->epa_max_iterations = request.epa_max_iterations;
+    this->epa_tolerance = request.epa_tolerance;
+
+    // ---------------------
+    // Reset GJK and EPA status
+    this->gjk.status = details::GJK::Status::DidNotRun;
+    this->epa.status = details::EPA::Status::DidNotRun;
+  }
+
+  /// @brief Copy constructor
+  GJKSolver(const GJKSolver& other) = default;
+
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  bool operator==(const GJKSolver& other) const {
+    return this->enable_cached_guess ==
+               other.enable_cached_guess &&  // use gjk_initial_guess instead
+           this->cached_guess == other.cached_guess &&
+           this->support_func_cached_guess == other.support_func_cached_guess &&
+           this->gjk_max_iterations == other.gjk_max_iterations &&
+           this->gjk_tolerance == other.gjk_tolerance &&
+           this->distance_upper_bound == other.distance_upper_bound &&
+           this->gjk_variant == other.gjk_variant &&
+           this->gjk_convergence_criterion == other.gjk_convergence_criterion &&
+           this->gjk_convergence_criterion_type ==
+               other.gjk_convergence_criterion_type &&
+           this->gjk_initial_guess == other.gjk_initial_guess &&
+           this->epa_max_iterations == other.epa_max_iterations &&
+           this->epa_tolerance == other.epa_tolerance;
+  }
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  bool operator!=(const GJKSolver& other) const { return !(*this == other); }
+
+  /// @brief Helper to return the precision of the solver on the distance
+  /// estimate, depending on whether or not `compute_penetration` is true.
+  CoalScalar getDistancePrecision(const bool compute_penetration) const {
+    return compute_penetration
+               ? (std::max)(this->gjk_tolerance, this->epa_tolerance)
+               : this->gjk_tolerance;
+  }
+
+  /// @brief Uses GJK and EPA to compute the distance between two shapes.
+  /// @param `s1` the first shape.
+  /// @param `tf1` the transformation of the first shape.
+  /// @param `s2` the second shape.
+  /// @param `tf2` the transformation of the second shape.
+  /// @param `compute_penetration` if true and GJK finds the shape in collision,
+  /// the EPA algorithm is also ran to compute penetration information.
+  /// @param[out] `p1` the witness point on the first shape.
+  /// @param[out] `p2` the witness point on the second shape.
+  /// @param[out] `normal` the normal of the collision, pointing from the first
+  /// to the second shape.
+  /// @return the estimate of the distance between the two shapes.
+  ///
+  /// @note: if `this->distance_upper_bound` is set to a positive value, GJK
+  /// will early stop if it finds the distance to be above this value. The
+  /// distance returned by `this->shapeDistance` will be a lower bound on the
+  /// distance between the two shapes.
+  ///
+  /// @note: the variables `this->gjk.status` and `this->epa.status` can be used
+  /// to examine the status of GJK and EPA.
+  ///
+  /// @note: GJK and EPA give an estimate of the distance between the two
+  /// shapes. This estimate is precise up to the tolerance of the algorithms:
+  ///   - If `compute_penetration` is false, the distance is precise up to
+  ///     `gjk_tolerance`.
+  ///   - If `compute_penetration` is true, the distance is precise up to
+  ///     `std::max(gjk_tolerance, epa_tolerance)`
+  /// It's up to the user to decide whether the shapes are in collision or not,
+  /// based on that estimate.
+  template <typename S1, typename S2>
+  CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1, const S2& s2,
+                           const Transform3s& tf2,
+                           const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+                           Vec3s& normal) const {
+    constexpr bool relative_transformation_already_computed = false;
+    CoalScalar distance;
+    this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2,
+                       normal, relative_transformation_already_computed);
+    return distance;
+  }
+
+  /// @brief Partial specialization of `shapeDistance` for the case where the
+  /// second shape is a triangle. It is more efficient to pre-compute the
+  /// relative transformation between the two shapes before calling GJK/EPA.
+  template <typename S1>
+  CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1,
+                           const TriangleP& s2, const Transform3s& tf2,
+                           const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+                           Vec3s& normal) const {
+    const Transform3s tf_1M2(tf1.inverseTimes(tf2));
+    TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b),
+                  tf_1M2.transform(s2.c));
+
+    constexpr bool relative_transformation_already_computed = true;
+    CoalScalar distance;
+    this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1,
+                       p2, normal, relative_transformation_already_computed);
+    return distance;
+  }
+
+  /// @brief See other partial template specialization of shapeDistance above.
+  template <typename S2>
+  CoalScalar shapeDistance(const TriangleP& s1, const Transform3s& tf1,
+                           const S2& s2, const Transform3s& tf2,
+                           const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+                           Vec3s& normal) const {
+    CoalScalar distance = this->shapeDistance<S2>(
+        s2, tf2, s1, tf1, compute_penetration, p2, p1, normal);
+    normal = -normal;
+    return distance;
+  }
+
+ protected:
+  /// @brief initialize GJK.
+  /// This method assumes `minkowski_difference` has been set.
+  template <typename S1, typename S2>
+  void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3s& guess,
+                          support_func_guess_t& support_hint,
+                          const Vec3s& default_guess = Vec3s(1, 0, 0)) const {
+    // There is no reason not to warm-start the support function, so we always
+    // do it.
+    support_hint = this->support_func_cached_guess;
+    // The following switch takes care of the GJK warm-start.
+    switch (gjk_initial_guess) {
+      case GJKInitialGuess::DefaultGuess:
+        guess = default_guess;
+        break;
+      case GJKInitialGuess::CachedGuess:
+        guess = this->cached_guess;
+        break;
+      case GJKInitialGuess::BoundingVolumeGuess:
+        if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
+          COAL_THROW_PRETTY(
+              "computeLocalAABB must have been called on the shapes before "
+              "using "
+              "GJKInitialGuess::BoundingVolumeGuess.",
+              std::logic_error);
+        }
+        guess.noalias() =
+            s1.aabb_local.center() -
+            (this->minkowski_difference.oR1 * s2.aabb_local.center() +
+             this->minkowski_difference.ot1);
+        break;
+      default:
+        COAL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error);
+    }
+    // TODO: use gjk_initial_guess instead
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    if (this->enable_cached_guess) {
+      guess = this->cached_guess;
+    }
+    COAL_COMPILER_DIAGNOSTIC_POP
+  }
+
+  /// @brief Runs the GJK algorithm.
+  /// @param `s1` the first shape.
+  /// @param `tf1` the transformation of the first shape.
+  /// @param `s2` the second shape.
+  /// @param `tf2` the transformation of the second shape.
+  /// @param `compute_penetration` if true and if the shapes are in found in
+  /// collision, the EPA algorithm is also ran to compute penetration
+  /// information.
+  /// @param[out] `distance` the distance between the two shapes.
+  /// @param[out] `p1` the witness point on the first shape.
+  /// @param[out] `p2` the witness point on the second shape.
+  /// @param[out] `normal` the normal of the collision, pointing from the first
+  /// to the second shape.
+  /// @param `relative_transformation_already_computed` whether the relative
+  /// transformation between the two shapes has already been computed.
+  /// @tparam SupportOptions, see `MinkowskiDiff::set`. Whether the support
+  /// computations should take into account the shapes' swept-sphere radii
+  /// during the iterations of GJK and EPA. Please leave this default value to
+  /// `false` unless you know what you are doing. This template parameter is
+  /// only used for debugging/testing purposes. In short, there is no need to
+  /// take into account the swept sphere radius when computing supports in the
+  /// iterations of GJK and EPA. GJK and EPA will correct the solution once they
+  /// have converged.
+  /// @return the estimate of the distance between the two shapes.
+  ///
+  /// @note: The variables `this->gjk.status` and `this->epa.status` can be used
+  /// to examine the status of GJK and EPA.
+  template <typename S1, typename S2,
+            int _SupportOptions = details::SupportOptions::NoSweptSphere>
+  void runGJKAndEPA(
+      const S1& s1, const Transform3s& tf1, const S2& s2,
+      const Transform3s& tf2, const bool compute_penetration,
+      CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal,
+      const bool relative_transformation_already_computed = false) const {
+    // Reset internal state of GJK algorithm
+    if (relative_transformation_already_computed)
+      this->minkowski_difference.set<_SupportOptions>(&s1, &s2);
+    else
+      this->minkowski_difference.set<_SupportOptions>(&s1, &s2, tf1, tf2);
+    this->gjk.reset(this->gjk_max_iterations, this->gjk_tolerance);
+    this->gjk.setDistanceEarlyBreak(this->distance_upper_bound);
+    this->gjk.gjk_variant = this->gjk_variant;
+    this->gjk.convergence_criterion = this->gjk_convergence_criterion;
+    this->gjk.convergence_criterion_type = this->gjk_convergence_criterion_type;
+    this->epa.status = details::EPA::Status::DidNotRun;
+
+    // Get initial guess for GJK: default, cached or bounding volume guess
+    Vec3s guess;
+    support_func_guess_t support_hint;
+    getGJKInitialGuess(*(this->minkowski_difference.shapes[0]),
+                       *(this->minkowski_difference.shapes[1]), guess,
+                       support_hint);
+
+    this->gjk.evaluate(this->minkowski_difference, guess, support_hint);
+
+    switch (this->gjk.status) {
+      case details::GJK::DidNotRun:
+        COAL_ASSERT(false, "GJK did not run. It should have!",
+                    std::logic_error);
+        this->cached_guess = Vec3s(1, 0, 0);
+        this->support_func_cached_guess.setZero();
+        distance = -(std::numeric_limits<CoalScalar>::max)();
+        p1 = p2 = normal =
+            Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+        break;
+      case details::GJK::Failed:
+        //
+        // GJK ran out of iterations.
+        COAL_LOG_WARNING("GJK ran out of iterations.");
+        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+        break;
+      case details::GJK::NoCollisionEarlyStopped:
+        //
+        // Case where GJK early stopped because the distance was found to be
+        // above the `distance_upper_bound`.
+        // The two witness points have no meaning.
+        GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
+                                                  normal);
+        COAL_ASSERT(distance >= this->gjk.distance_upper_bound -
+                                    this->m_dummy_precision,
+                    "The distance should be bigger than GJK's "
+                    "`distance_upper_bound`.",
+                    std::logic_error);
+        break;
+      case details::GJK::NoCollision:
+        //
+        // Case where GJK converged and proved that the shapes are not in
+        // collision, i.e their distance is above GJK's tolerance (default
+        // 1e-6).
+        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+        COAL_ASSERT(std::abs((p1 - p2).norm() - distance) <=
+                        this->gjk.getTolerance() + this->m_dummy_precision,
+                    "The distance found by GJK should coincide with the "
+                    "distance between the closest points.",
+                    std::logic_error);
+        break;
+      //
+      // Next are the cases where GJK found the shapes to be in collision, i.e.
+      // their distance is below GJK's tolerance (default 1e-6).
+      case details::GJK::CollisionWithPenetrationInformation:
+        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+        COAL_ASSERT(
+            distance <= this->gjk.getTolerance() + this->m_dummy_precision,
+            "The distance found by GJK should be negative or at "
+            "least below GJK's tolerance.",
+            std::logic_error);
+        break;
+      case details::GJK::Collision:
+        if (!compute_penetration) {
+          // Skip EPA and set the witness points and the normal to nans.
+          GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
+                                                    normal);
+        } else {
+          //
+          // GJK was not enough to recover the penetration information.
+          // We need to run the EPA algorithm to find the witness points,
+          // penetration depth and the normal.
+
+          // Reset EPA algorithm. Potentially allocate memory if
+          // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's
+          // current storage.
+          this->epa.reset(this->epa_max_iterations, this->epa_tolerance);
+
+          // TODO: understand why EPA's performance is so bad on cylinders and
+          // cones.
+          this->epa.evaluate(this->gjk, -guess);
+
+          switch (epa.status) {
+            //
+            // In the following switch cases, until the "Valid" case,
+            // EPA either ran out of iterations, of faces or of vertices.
+            // The depth, witness points and the normal are still valid,
+            // simply not at the precision of EPA's tolerance.
+            // The flag `COAL_ENABLE_LOGGING` enables feebdack on these
+            // cases.
+            //
+            // TODO: Remove OutOfFaces and OutOfVertices statuses and simply
+            // compute the upper bound on max faces and max vertices as a
+            // function of the number of iterations.
+            case details::EPA::OutOfFaces:
+              COAL_LOG_WARNING("EPA ran out of faces.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::OutOfVertices:
+              COAL_LOG_WARNING("EPA ran out of vertices.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::Failed:
+              COAL_LOG_WARNING("EPA ran out of iterations.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::Valid:
+            case details::EPA::AccuracyReached:
+              COAL_ASSERT(
+                  -epa.depth <= epa.getTolerance() + this->m_dummy_precision,
+                  "EPA's penetration distance should be negative (or "
+                  "at least below EPA's tolerance).",
+                  std::logic_error);
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::Degenerated:
+              COAL_LOG_WARNING(
+                  "EPA warning: created a polytope with a degenerated face.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::NonConvex:
+              COAL_LOG_WARNING(
+                  "EPA warning: EPA got called onto non-convex shapes.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::InvalidHull:
+              COAL_LOG_WARNING("EPA warning: created an invalid polytope.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::DidNotRun:
+              COAL_ASSERT(false, "EPA did not run. It should have!",
+                          std::logic_error);
+              COAL_LOG_ERROR("EPA error: did not run. It should have.");
+              EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
+                                                     normal);
+              break;
+            case details::EPA::FallBack:
+              COAL_ASSERT(
+                  false,
+                  "EPA went into fallback mode. It should never do that.",
+                  std::logic_error);
+              COAL_LOG_ERROR("EPA error: FallBack.");
+              EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
+                                                     normal);
+              break;
+          }
+        }
+        break;  // End of case details::GJK::Collision
+    }
+  }
+
+  void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                                 CoalScalar& distance,
+                                                 Vec3s& p1, Vec3s& p2,
+                                                 Vec3s& normal) const {
+    COAL_UNUSED_VARIABLE(tf1);
+    // Cache gjk result for potential future call to this GJKSolver.
+    this->cached_guess = this->gjk.ray;
+    this->support_func_cached_guess = this->gjk.support_hint;
+
+    distance = this->gjk.distance;
+    p1 = p2 = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+    // If we absolutely want to return some witness points, we could use
+    // the following code (or simply merge the early stopped case with the
+    // valid case below):
+    // gjk.getWitnessPointsAndNormal(minkowski_difference, p1, p2, normal);
+    // p1 = tf1.transform(p1);
+    // p2 = tf1.transform(p2);
+    // normal = tf1.getRotation() * normal;
+  }
+
+  void GJKExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                        CoalScalar& distance, Vec3s& p1,
+                                        Vec3s& p2, Vec3s& normal) const {
+    // Apart from early stopping, there are two cases where GJK says there is no
+    // collision:
+    // 1. GJK proved the distance is above its tolerance (default 1e-6).
+    // 2. GJK ran out of iterations.
+    // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus
+    // it can safely be normalized.
+    COAL_ASSERT(this->gjk.ray.norm() >
+                    this->gjk.getTolerance() - this->m_dummy_precision,
+                "The norm of GJK's ray should be bigger than GJK's tolerance.",
+                std::logic_error);
+    // Cache gjk result for potential future call to this GJKSolver.
+    this->cached_guess = this->gjk.ray;
+    this->support_func_cached_guess = this->gjk.support_hint;
+
+    distance = this->gjk.distance;
+    // TODO: On degenerated case, the closest points may be non-unique.
+    // (i.e. an object face normal is colinear to `gjk.ray`)
+    gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal);
+    Vec3s p = tf1.transform(0.5 * (p1 + p2));
+    normal = tf1.getRotation() * normal;
+    p1.noalias() = p - 0.5 * distance * normal;
+    p2.noalias() = p + 0.5 * distance * normal;
+  }
+
+  void GJKCollisionExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                                 CoalScalar& distance,
+                                                 Vec3s& p1, Vec3s& p2,
+                                                 Vec3s& normal) const {
+    COAL_UNUSED_VARIABLE(tf1);
+    COAL_ASSERT(this->gjk.distance <=
+                    this->gjk.getTolerance() + this->m_dummy_precision,
+                "The distance should be lower than GJK's tolerance.",
+                std::logic_error);
+    // Because GJK has returned the `Collision` status and EPA has not run,
+    // we purposefully do not cache the result of GJK, because ray is zero.
+    // However, we can cache the support function hint.
+    // this->cached_guess = this->gjk.ray;
+    this->support_func_cached_guess = this->gjk.support_hint;
+
+    distance = this->gjk.distance;
+    p1 = p2 = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  void EPAExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                        CoalScalar& distance, Vec3s& p1,
+                                        Vec3s& p2, Vec3s& normal) const {
+    // Cache EPA result for potential future call to this GJKSolver.
+    // This caching allows to warm-start the next GJK call.
+    this->cached_guess = -(this->epa.depth * this->epa.normal);
+    this->support_func_cached_guess = this->epa.support_hint;
+    distance = (std::min)(0., -this->epa.depth);
+    this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2,
+                                        normal);
+    // The following is very important to understand why EPA can sometimes
+    // return a normal that is not colinear to the vector $p_1 - p_2$ when
+    // working with tolerances like $\epsilon = 10^{-3}$.
+    // It can be resumed with a simple idea:
+    //     EPA is an algorithm meant to find the penetration depth and the
+    //     normal. It is not meant to find the closest points.
+    // Again, the issue here is **not** the normal, it's $p_1$ and $p_2$.
+    //
+    // More details:
+    // We'll denote $S_1$ and $S_2$ the two shapes, $n$ the normal and $p_1$ and
+    // $p_2$ the witness points. In theory, when EPA converges to $\epsilon =
+    // 0$, the normal and witness points verify the following property (P):
+    //   - $p_1 \in \partial \sigma_{S_1}(n)$,
+    //   - $p_2 \in \partial \sigma_{S_2}(-n),
+    // where $\sigma_{S_1}$ and $\sigma_{S_2}$ are the support functions of
+    // $S_1$ and $S_2$. The $\partial \sigma(n)$ simply denotes the support set
+    // of the support function in the direction $n$. (Note: I am leaving out the
+    // details of frame choice for the support function, to avoid making the
+    // mathematical notation too heavy.)
+    // --> In practice, EPA converges to $\epsilon > 0$.
+    // On polytopes and the likes, this does not change much and the property
+    // given above is still valid.
+    // --> However, this is very different on curved surfaces, such as
+    // ellipsoids, cylinders, cones, capsules etc. For these shapes, converging
+    // at $\epsilon = 10^{-6}$ or to $\epsilon = 10^{-3}$ does not change the
+    // normal much, but the property (P) given above is no longer valid, which
+    // means that the points $p_1$ and $p_2$ do not necessarily belong to the
+    // support sets in the direction of $n$ and thus $n$ and $p_1 - p_2$ are not
+    // colinear.
+    //
+    // Do not panic! This is fine.
+    // Although the property above is not verified, it's almost verified,
+    // meaning that $p_1$ and $p_2$ belong to support sets in directions that
+    // are very close to $n$.
+    //
+    // Solution to compute better $p_1$ and $p_2$:
+    // We compute the middle points of the current $p_1$ and $p_2$ and we use
+    // the normal and the distance given by EPA to compute the new $p_1$ and
+    // $p_2$.
+    Vec3s p = tf1.transform(0.5 * (p1 + p2));
+    normal = tf1.getRotation() * normal;
+    p1.noalias() = p - 0.5 * distance * normal;
+    p2.noalias() = p + 0.5 * distance * normal;
+  }
+
+  void EPAFailedExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                              CoalScalar& distance, Vec3s& p1,
+                                              Vec3s& p2, Vec3s& normal) const {
+    this->cached_guess = Vec3s(1, 0, 0);
+    this->support_func_cached_guess.setZero();
+
+    COAL_UNUSED_VARIABLE(tf1);
+    distance = -(std::numeric_limits<CoalScalar>::max)();
+    p1 = p2 = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/narrowphase/narrowphase_defaults.h b/include/coal/narrowphase/narrowphase_defaults.h
new file mode 100644
index 0000000000000000000000000000000000000000..1fd2775a85ee87cb7f86c3b0ef3e38ec84510241
--- /dev/null
+++ b/include/coal/narrowphase/narrowphase_defaults.h
@@ -0,0 +1,65 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, INRIA
+ *  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.
+ */
+
+/// This file defines different macros used to characterize the default behavior
+/// of the narrowphase algorithms GJK and EPA.
+
+#ifndef COAL_NARROWPHASE_DEFAULTS_H
+#define COAL_NARROWPHASE_DEFAULTS_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+/// GJK
+constexpr size_t GJK_DEFAULT_MAX_ITERATIONS = 128;
+constexpr CoalScalar GJK_DEFAULT_TOLERANCE = 1e-6;
+/// Note: if the considered shapes are on the order of the meter, and the
+/// convergence criterion of GJK is the default VDB criterion,
+/// setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to
+/// the micro-meter.
+/// The same is true for EPA.
+constexpr CoalScalar GJK_MINIMUM_TOLERANCE = 1e-6;
+
+/// EPA
+/// EPA build a polytope which maximum size is:
+///   - `#iterations + 4` vertices
+///   - `2 x #iterations + 4` faces
+constexpr size_t EPA_DEFAULT_MAX_ITERATIONS = 64;
+constexpr CoalScalar EPA_DEFAULT_TOLERANCE = 1e-6;
+constexpr CoalScalar EPA_MINIMUM_TOLERANCE = 1e-6;
+
+}  // namespace coal
+
+#endif  // COAL_NARROWPHASE_DEFAULTS_H
diff --git a/include/coal/narrowphase/support_functions.h b/include/coal/narrowphase/support_functions.h
new file mode 100644
index 0000000000000000000000000000000000000000..df79a1353fb20f1743b95905f64bc5ba43bf1f7b
--- /dev/null
+++ b/include/coal/narrowphase/support_functions.h
@@ -0,0 +1,308 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2021-2024, INRIA
+ *  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.
+ */
+
+/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
+
+#ifndef COAL_SUPPORT_FUNCTIONS_H
+#define COAL_SUPPORT_FUNCTIONS_H
+
+#include "coal/shape/geometric_shapes.h"
+#include "coal/math/transform.h"
+#include "coal/collision_data.h"
+
+namespace coal {
+
+namespace details {
+
+/// @brief Options for the computation of support points.
+/// `NoSweptSphere` option is used when the support function is called
+/// by GJK or EPA. In this case, the swept sphere radius is not taken into
+/// account in the support function. It is used by GJK and EPA after they have
+/// converged to correct the solution.
+/// `WithSweptSphere` option is used when the support function is called
+/// directly by the user. In this case, the swept sphere radius is taken into
+/// account in the support function.
+enum SupportOptions {
+  NoSweptSphere = 0,
+  WithSweptSphere = 0x1,
+};
+
+// ============================================================================
+// ============================ SUPPORT FUNCTIONS =============================
+// ============================================================================
+/// @brief the support function for shape.
+/// The output support point is expressed in the local frame of the shape.
+/// @return a point which belongs to the set {argmax_{v in shape} v.dot(dir)}.
+/// @param shape the shape.
+/// @param dir support direction.
+/// @param hint used to initialize the search when shape is a ConvexBase object.
+/// @tparam SupportOptions is a value of the SupportOptions enum. If set to
+/// `WithSweptSphere`, the support functions take into account the shapes' swept
+/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const
+/// ShapeBase*)` for more details.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint);
+
+/// @brief Stores temporary data for the computation of support points.
+struct COAL_DLLAPI ShapeSupportData {
+  // @brief Tracks which points have been visited in a ConvexBase.
+  std::vector<int8_t> visited;
+
+  // @brief Tracks the last support direction used on this shape; used to
+  // warm-start the ConvexBase support function.
+  Vec3s last_dir = Vec3s::Zero();
+
+  // @brief Temporary set used to compute the convex-hull of a support set.
+  // Only used for ConvexBase and Box.
+  SupportSet::Polygon polygon;
+};
+
+/// @brief Triangle support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const TriangleP* triangle, const Vec3s& dir,
+                     Vec3s& support, int& /*unused*/,
+                     ShapeSupportData& /*unused*/);
+
+/// @brief Box support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief Sphere support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief Ellipsoid support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir,
+                     Vec3s& support, int& /*unused*/,
+                     ShapeSupportData& /*unused*/);
+
+/// @brief Capsule support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief Cone support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief Cylinder support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief ConvexBase support function.
+/// @note See @ref LargeConvex and SmallConvex to see how to optimize
+/// ConvexBase's support computation.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support,
+                     int& hint, ShapeSupportData& /*unused*/);
+
+/// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of
+/// `getShapeSupport`. This is **much** faster than the linear version of
+/// `getShapeSupport` when a `ConvexBase` has more than a few dozen of vertices.
+/// @note WARNING: when using a LargeConvex, the neighbors in `ConvexBase` must
+/// have been constructed! Otherwise the support function will segfault.
+struct LargeConvex : ShapeBase {};
+/// @brief See @ref LargeConvex.
+struct SmallConvex : ShapeBase {};
+
+/// @brief Support function for large ConvexBase (>32 vertices).
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const SmallConvex* convex, const Vec3s& dir,
+                     Vec3s& support, int& hint, ShapeSupportData& data);
+
+/// @brief Support function for small ConvexBase (<32 vertices).
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const LargeConvex* convex, const Vec3s& dir,
+                     Vec3s& support, int& hint, ShapeSupportData& support_data);
+
+// ============================================================================
+// ========================== SUPPORT SET FUNCTIONS ===========================
+// ============================================================================
+/// @brief Computes the support set for shape.
+/// This function assumes the frame of the support set has already been
+/// computed and that this frame is expressed w.r.t the local frame of the
+/// shape (i.e. the local frame of the shape is the WORLD frame of the support
+/// set). The support direction used to compute the support set is the positive
+/// z-axis if the support set has the DEFAULT direction; negative z-axis if it
+/// has the INVERTED direction. (In short, a shape's support set is has the
+/// DEFAULT direction if the shape is the first shape in a collision pair. It
+/// has the INVERTED direction if the shape is the second one in the collision
+/// pair).
+/// @return an approximation of the set {argmax_{v in shape} v.dot(dir)}, where
+/// dir is the support set's support direction.
+/// The support set is a plane passing by the origin of the support set frame
+/// and supported by the direction dir. As a consequence, any point added to the
+/// set is automatically projected onto this plane.
+/// @param[in] shape the shape.
+/// @param[in/out] support_set of shape.
+/// @param[in/out] hint used to initialize the search when shape is a ConvexBase
+/// object.
+/// @param[in] num_sampled_supports is only used for shapes with smooth
+/// non-strictly convex bases like cones and cylinders (their bases are
+/// circles). In such a case, if the support direction points to their base, we
+/// have to choose which points we want to add to the set. This is not needed
+/// for boxes or ConvexBase for example. Indeed, because their support sets are
+/// always polygons, we can characterize the entire support set with the
+/// vertices of the polygon.
+/// @param[in] tol given a point v on the shape, if
+/// `max_{p in shape}(p.dot(dir)) - v.dot(dir) <= tol`, where dir is the set's
+/// support direction, then v is added to the support set.
+/// Otherwise said, if a point p of the shape is at a distance `tol` from the
+/// support plane, it is added to the set. Thus, `tol` can be seen as the
+/// "thickness" of the support plane.
+/// @tparam SupportOptions is a value of the SupportOptions enum. If set to
+/// `WithSweptSphere`, the support functions take into account the shapes' swept
+/// sphere radii.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint,
+                   size_t num_sampled_supports = 6, CoalScalar tol = 1e-3);
+
+/// @brief Same as @ref getSupportSet(const ShapeBase*, const CoalScalar,
+/// SupportSet&, const int) but also constructs the support set frame from
+/// `dir`.
+/// @note The support direction `dir` is expressed in the local frame of the
+/// shape.
+/// @note This function automatically deals with the `direction` of the
+/// SupportSet.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getSupportSet(const ShapeBase* shape, const Vec3s& dir,
+                   SupportSet& support_set, int& hint,
+                   size_t num_sampled_supports = 6, CoalScalar tol = 1e-3) {
+  support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir);
+  const Vec3s& support_dir = support_set.getNormal();
+  const Vec3s support = getSupport<_SupportOptions>(shape, support_dir, hint);
+  getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports,
+                                 tol);
+}
+
+/// @brief Triangle support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Box support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Box* box, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& support_data,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Sphere support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar /*unused*/ tol = 1e-3);
+
+/// @brief Ellipsoid support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar /*unused*/ tol = 1e-3);
+
+/// @brief Capsule support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Cone support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t num_sampled_supports = 6, CoalScalar tol = 1e-3);
+
+/// @brief Cylinder support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t num_sampled_supports = 6, CoalScalar tol = 1e-3);
+
+/// @brief ConvexBase support set function.
+/// Assumes the support set frame has already been computed.
+/// @note See @ref LargeConvex and SmallConvex to see how to optimize
+/// ConvexBase's support computation.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set,
+                        int& hint, ShapeSupportData& support_data,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Support set function for large ConvexBase (>32 vertices).
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Support set function for small ConvexBase (<32 vertices).
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set,
+                        int& hint, ShapeSupportData& support_data,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Computes the convex-hull of support_set. For now, this function is
+/// only needed for Box and ConvexBase.
+/// @param[in] cloud data which contains the 2d points of the support set which
+/// convex-hull we want to compute.
+/// @param[out] 2d points of the the support set's convex-hull.
+COAL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
+                                             SupportSet::Polygon& cvx_hull);
+
+}  // namespace details
+
+}  // namespace coal
+
+#endif  // COAL_SUPPORT_FUNCTIONS_H
diff --git a/include/coal/octree.h b/include/coal/octree.h
new file mode 100644
index 0000000000000000000000000000000000000000..52c507953541447fd9fd989d39b03f151d217a9c
--- /dev/null
+++ b/include/coal/octree.h
@@ -0,0 +1,340 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2022-2024, Inria
+ *  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 COAL_OCTREE_H
+#define COAL_OCTREE_H
+
+#include <algorithm>
+
+#include <octomap/octomap.h>
+#include "coal/fwd.hh"
+#include "coal/BV/AABB.h"
+#include "coal/collision_object.h"
+
+namespace coal {
+
+/// @brief Octree is one type of collision geometry which can encode uncertainty
+/// information in the sensor data.
+class COAL_DLLAPI OcTree : public CollisionGeometry {
+ protected:
+  shared_ptr<const octomap::OcTree> tree;
+
+  CoalScalar default_occupancy;
+
+  CoalScalar occupancy_threshold;
+  CoalScalar free_threshold;
+
+ public:
+  typedef octomap::OcTreeNode OcTreeNode;
+
+  /// @brief construct octree with a given resolution
+  explicit OcTree(CoalScalar resolution)
+      : tree(shared_ptr<const octomap::OcTree>(
+            new octomap::OcTree(resolution))) {
+    default_occupancy = tree->getOccupancyThres();
+
+    // default occupancy/free threshold is consistent with default setting from
+    // octomap
+    occupancy_threshold = tree->getOccupancyThres();
+    free_threshold = 0;
+  }
+
+  /// @brief construct octree from octomap
+  explicit OcTree(const shared_ptr<const octomap::OcTree>& tree_)
+      : tree(tree_) {
+    default_occupancy = tree->getOccupancyThres();
+
+    // default occupancy/free threshold is consistent with default setting from
+    // octomap
+    occupancy_threshold = tree->getOccupancyThres();
+    free_threshold = 0;
+  }
+
+  ///  \brief Copy constructor
+  OcTree(const OcTree& other)
+      : CollisionGeometry(other),
+        tree(other.tree),
+        default_occupancy(other.default_occupancy),
+        occupancy_threshold(other.occupancy_threshold),
+        free_threshold(other.free_threshold) {}
+
+  /// \brief Clone *this into a new Octree
+  OcTree* clone() const { return new OcTree(*this); }
+
+  /// \brief Returns the tree associated to the underlying octomap OcTree.
+  shared_ptr<const octomap::OcTree> getTree() const { return tree; }
+
+  void exportAsObjFile(const std::string& filename) const;
+
+  /// @brief compute the AABB for the octree in its local coordinate system
+  void computeLocalAABB() {
+    typedef Eigen::Matrix<float, 3, 1> Vec3sloat;
+    Vec3sloat max_extent, min_extent;
+
+    octomap::OcTree::iterator it =
+        tree->begin((unsigned char)tree->getTreeDepth());
+    octomap::OcTree::iterator end = tree->end();
+
+    if (it == end) return;
+
+    {
+      const octomap::point3d& coord =
+          it.getCoordinate();  // getCoordinate returns a copy
+      max_extent = min_extent = Eigen::Map<const Vec3sloat>(&coord.x());
+      for (++it; it != end; ++it) {
+        const octomap::point3d& coord = it.getCoordinate();
+        const Vec3sloat pos = Eigen::Map<const Vec3sloat>(&coord.x());
+        max_extent = max_extent.array().max(pos.array());
+        min_extent = min_extent.array().min(pos.array());
+      }
+    }
+
+    // Account for the size of the boxes.
+    const CoalScalar resolution = tree->getResolution();
+    max_extent.array() += float(resolution / 2.);
+    min_extent.array() -= float(resolution / 2.);
+
+    aabb_local =
+        AABB(min_extent.cast<CoalScalar>(), max_extent.cast<CoalScalar>());
+    aabb_center = aabb_local.center();
+    aabb_radius = (aabb_local.min_ - aabb_center).norm();
+  }
+
+  /// @brief get the bounding volume for the root
+  AABB getRootBV() const {
+    CoalScalar delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
+
+    // std::cout << "octree size " << delta << std::endl;
+    return AABB(Vec3s(-delta, -delta, -delta), Vec3s(delta, delta, delta));
+  }
+
+  /// @brief Returns the depth of the octree
+  unsigned int getTreeDepth() const { return tree->getTreeDepth(); }
+
+  /// @brief Returns the size of the octree
+  unsigned long size() const { return tree->size(); }
+
+  /// @brief Returns the resolution of the octree
+  CoalScalar getResolution() const { return tree->getResolution(); }
+
+  /// @brief get the root node of the octree
+  OcTreeNode* getRoot() const { return tree->getRoot(); }
+
+  /// @brief whether one node is completely occupied
+  bool isNodeOccupied(const OcTreeNode* node) const {
+    // return tree->isNodeOccupied(node);
+    return node->getOccupancy() >= occupancy_threshold;
+  }
+
+  /// @brief whether one node is completely free
+  bool isNodeFree(const OcTreeNode* node) const {
+    // return false; // default no definitely free node
+    return node->getOccupancy() <= free_threshold;
+  }
+
+  /// @brief whether one node is uncertain
+  bool isNodeUncertain(const OcTreeNode* node) const {
+    return (!isNodeOccupied(node)) && (!isNodeFree(node));
+  }
+
+  /// @brief transform the octree into a bunch of boxes; uncertainty information
+  /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the
+  /// boxes whose occupied probability is higher enough).
+  std::vector<Vec6s> toBoxes() const {
+    std::vector<Vec6s> boxes;
+    boxes.reserve(tree->size() / 2);
+    for (octomap::OcTree::iterator
+             it = tree->begin((unsigned char)tree->getTreeDepth()),
+             end = tree->end();
+         it != end; ++it) {
+      // if(tree->isNodeOccupied(*it))
+      if (isNodeOccupied(&*it)) {
+        CoalScalar x = it.getX();
+        CoalScalar y = it.getY();
+        CoalScalar z = it.getZ();
+        CoalScalar size = it.getSize();
+        CoalScalar c = (*it).getOccupancy();
+        CoalScalar t = tree->getOccupancyThres();
+
+        Vec6s box;
+        box << x, y, z, size, c, t;
+        boxes.push_back(box);
+      }
+    }
+    return boxes;
+  }
+
+  /// \brief Returns a byte description of *this
+  std::vector<uint8_t> tobytes() const {
+    typedef Eigen::Matrix<float, 3, 1> Vec3sloat;
+    const size_t total_size = (tree->size() * sizeof(CoalScalar) * 3) / 2;
+    std::vector<uint8_t> bytes;
+    bytes.reserve(total_size);
+
+    for (octomap::OcTree::iterator
+             it = tree->begin((unsigned char)tree->getTreeDepth()),
+             end = tree->end();
+         it != end; ++it) {
+      const Vec3s box_pos =
+          Eigen::Map<Vec3sloat>(&it.getCoordinate().x()).cast<CoalScalar>();
+      if (isNodeOccupied(&*it))
+        std::copy(box_pos.data(), box_pos.data() + sizeof(CoalScalar) * 3,
+                  std::back_inserter(bytes));
+    }
+
+    return bytes;
+  }
+
+  /// @brief the threshold used to decide whether one node is occupied, this is
+  /// NOT the octree occupied_thresold
+  CoalScalar getOccupancyThres() const { return occupancy_threshold; }
+
+  /// @brief the threshold used to decide whether one node is free, this is NOT
+  /// the octree free_threshold
+  CoalScalar getFreeThres() const { return free_threshold; }
+
+  CoalScalar getDefaultOccupancy() const { return default_occupancy; }
+
+  void setCellDefaultOccupancy(CoalScalar d) { default_occupancy = d; }
+
+  void setOccupancyThres(CoalScalar d) { occupancy_threshold = d; }
+
+  void setFreeThres(CoalScalar d) { free_threshold = d; }
+
+  /// @return ptr to child number childIdx of node
+  OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) {
+#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
+    return tree->getNodeChild(node, childIdx);
+#else
+    return node->getChild(childIdx);
+#endif
+  }
+
+  /// @return const ptr to child number childIdx of node
+  const OcTreeNode* getNodeChild(const OcTreeNode* node,
+                                 unsigned int childIdx) const {
+#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
+    return tree->getNodeChild(node, childIdx);
+#else
+    return node->getChild(childIdx);
+#endif
+  }
+
+  /// @brief return true if the child at childIdx exists
+  bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const {
+#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
+    return tree->nodeChildExists(node, childIdx);
+#else
+    return node->childExists(childIdx);
+#endif
+  }
+
+  /// @brief return true if node has at least one child
+  bool nodeHasChildren(const OcTreeNode* node) const {
+#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
+    return tree->nodeHasChildren(node);
+#else
+    return node->hasChildren();
+#endif
+  }
+
+  /// @brief return object type, it is an octree
+  OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
+
+  /// @brief return node type, it is an octree
+  NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const OcTree* other_ptr = dynamic_cast<const OcTree*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const OcTree& other = *other_ptr;
+
+    return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) &&
+           default_occupancy == other.default_occupancy &&
+           occupancy_threshold == other.occupancy_threshold &&
+           free_threshold == other.free_threshold;
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief compute the bounding volume of an octree node's i-th child
+static inline void computeChildBV(const AABB& root_bv, unsigned int i,
+                                  AABB& child_bv) {
+  if (i & 1) {
+    child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
+    child_bv.max_[0] = root_bv.max_[0];
+  } else {
+    child_bv.min_[0] = root_bv.min_[0];
+    child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
+  }
+
+  if (i & 2) {
+    child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
+    child_bv.max_[1] = root_bv.max_[1];
+  } else {
+    child_bv.min_[1] = root_bv.min_[1];
+    child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
+  }
+
+  if (i & 4) {
+    child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
+    child_bv.max_[2] = root_bv.max_[2];
+  } else {
+    child_bv.min_[2] = root_bv.min_[2];
+    child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
+  }
+}
+
+///
+/// \brief Build an OcTree from a point cloud and a given resolution
+///
+/// \param[in] point_cloud The input points to insert in the OcTree
+/// \param[in] resolution of the octree.
+///
+/// \returns An OcTree that can be used for collision checking and more.
+///
+COAL_DLLAPI OcTreePtr_t
+makeOctree(const Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3>& point_cloud,
+           const CoalScalar resolution);
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/serialization/AABB.h b/include/coal/serialization/AABB.h
new file mode 100644
index 0000000000000000000000000000000000000000..d68b2c54f1d92479018f2957ebbeb993e97f8867
--- /dev/null
+++ b/include/coal/serialization/AABB.h
@@ -0,0 +1,23 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_AABB_H
+#define COAL_SERIALIZATION_AABB_H
+
+#include "coal/BV/AABB.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::AABB& aabb, const unsigned int /*version*/) {
+  ar& make_nvp("min_", aabb.min_);
+  ar& make_nvp("max_", aabb.max_);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_AABB_H
diff --git a/include/coal/serialization/BVH_model.h b/include/coal/serialization/BVH_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..0b59a21974b866c9366f47141ef711295a3b8ba9
--- /dev/null
+++ b/include/coal/serialization/BVH_model.h
@@ -0,0 +1,250 @@
+//
+// Copyright (c) 2021-2022 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_BVH_MODEL_H
+#define COAL_SERIALIZATION_BVH_MODEL_H
+
+#include "coal/BVH/BVH_model.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/BV_node.h"
+#include "coal/serialization/BV_splitter.h"
+#include "coal/serialization/collision_object.h"
+#include "coal/serialization/memory.h"
+#include "coal/serialization/triangle.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct BVHModelBaseAccessor : coal::BVHModelBase {
+  typedef coal::BVHModelBase Base;
+  using Base::num_tris_allocated;
+  using Base::num_vertices_allocated;
+};
+}  // namespace internal
+
+template <class Archive>
+void save(Archive &ar, const coal::BVHModelBase &bvh_model,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED ||
+        bvh_model.build_state == BVH_BUILD_STATE_UPDATED) &&
+      (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) {
+    COAL_THROW_PRETTY(
+        "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or "
+        "BVH_BUILD_STATE_UPDATED state.\n"
+        "The BVHModel could not be serialized.",
+        std::invalid_argument);
+  }
+
+  ar &make_nvp(
+      "base",
+      boost::serialization::base_object<coal::CollisionGeometry>(bvh_model));
+
+  ar &make_nvp("num_vertices", bvh_model.num_vertices);
+  ar &make_nvp("vertices", bvh_model.vertices);
+
+  ar &make_nvp("num_tris", bvh_model.num_tris);
+  ar &make_nvp("tri_indices", bvh_model.tri_indices);
+  ar &make_nvp("build_state", bvh_model.build_state);
+
+  ar &make_nvp("prev_vertices", bvh_model.prev_vertices);
+
+  //      if(bvh_model.convex)
+  //      {
+  //        const bool has_convex = true;
+  //        ar << make_nvp("has_convex",has_convex);
+  //      }
+  //      else
+  //      {
+  //        const bool has_convex = false;
+  //        ar << make_nvp("has_convex",has_convex);
+  //      }
+}
+
+template <class Archive>
+void load(Archive &ar, coal::BVHModelBase &bvh_model,
+          const unsigned int /*version*/) {
+  using namespace coal;
+
+  ar >> make_nvp("base",
+                 boost::serialization::base_object<coal::CollisionGeometry>(
+                     bvh_model));
+
+  ar >> make_nvp("num_vertices", bvh_model.num_vertices);
+  ar >> make_nvp("vertices", bvh_model.vertices);
+
+  ar >> make_nvp("num_tris", bvh_model.num_tris);
+  ar >> make_nvp("tri_indices", bvh_model.tri_indices);
+  ar >> make_nvp("build_state", bvh_model.build_state);
+
+  ar >> make_nvp("prev_vertices", bvh_model.prev_vertices);
+
+  //      bool has_convex = true;
+  //      ar >> make_nvp("has_convex",has_convex);
+}
+
+COAL_SERIALIZATION_SPLIT(coal::BVHModelBase)
+
+namespace internal {
+template <typename BV>
+struct BVHModelAccessor : coal::BVHModel<BV> {
+  typedef coal::BVHModel<BV> Base;
+  using Base::bvs;
+  using Base::num_bvs;
+  using Base::num_bvs_allocated;
+  using Base::primitive_indices;
+};
+}  // namespace internal
+
+template <class Archive, typename BV>
+void serialize(Archive &ar, coal::BVHModel<BV> &bvh_model,
+               const unsigned int version) {
+  split_free(ar, bvh_model, version);
+}
+
+template <class Archive, typename BV>
+void save(Archive &ar, const coal::BVHModel<BV> &bvh_model_,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::BVHModelAccessor<BV> Accessor;
+  typedef BVNode<BV> Node;
+
+  const Accessor &bvh_model = reinterpret_cast<const Accessor &>(bvh_model_);
+  ar &make_nvp("base",
+               boost::serialization::base_object<BVHModelBase>(bvh_model));
+
+  //      if(bvh_model.primitive_indices)
+  //      {
+  //        const bool with_primitive_indices = true;
+  //        ar & make_nvp("with_primitive_indices",with_primitive_indices);
+  //
+  //        int num_primitives = 0;
+  //        switch(bvh_model.getModelType())
+  //        {
+  //          case BVH_MODEL_TRIANGLES:
+  //            num_primitives = bvh_model.num_tris;
+  //            break;
+  //          case BVH_MODEL_POINTCLOUD:
+  //            num_primitives = bvh_model.num_vertices;
+  //            break;
+  //          default:
+  //            ;
+  //        }
+  //
+  //        ar & make_nvp("num_primitives",num_primitives);
+  //        if(num_primitives > 0)
+  //        {
+  //          typedef Eigen::Matrix<unsigned int,1,Eigen::Dynamic>
+  //          AsPrimitiveIndexVector; const Eigen::Map<const
+  //          AsPrimitiveIndexVector>
+  //          primitive_indices_map(reinterpret_cast<const unsigned int
+  //          *>(bvh_model.primitive_indices),1,num_primitives); ar &
+  //          make_nvp("primitive_indices",primitive_indices_map);
+  ////          ar &
+  /// make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
+  //        }
+  //      }
+  //      else
+  //      {
+  //        const bool with_primitive_indices = false;
+  //        ar & make_nvp("with_primitive_indices",with_primitive_indices);
+  //      }
+  //
+
+  if (bvh_model.bvs.get()) {
+    const bool with_bvs = true;
+    ar &make_nvp("with_bvs", with_bvs);
+    ar &make_nvp("num_bvs", bvh_model.num_bvs);
+    ar &make_nvp(
+        "bvs",
+        make_array(
+            reinterpret_cast<const char *>(bvh_model.bvs->data()),
+            sizeof(Node) *
+                (std::size_t)bvh_model.num_bvs));  // Assuming BVs are POD.
+  } else {
+    const bool with_bvs = false;
+    ar &make_nvp("with_bvs", with_bvs);
+  }
+}
+
+template <class Archive, typename BV>
+void load(Archive &ar, coal::BVHModel<BV> &bvh_model_,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::BVHModelAccessor<BV> Accessor;
+  typedef BVNode<BV> Node;
+
+  Accessor &bvh_model = reinterpret_cast<Accessor &>(bvh_model_);
+
+  ar >> make_nvp("base",
+                 boost::serialization::base_object<BVHModelBase>(bvh_model));
+
+  //      bool with_primitive_indices;
+  //      ar >> make_nvp("with_primitive_indices",with_primitive_indices);
+  //      if(with_primitive_indices)
+  //      {
+  //        int num_primitives;
+  //        ar >> make_nvp("num_primitives",num_primitives);
+  //
+  //        delete[] bvh_model.primitive_indices;
+  //        if(num_primitives > 0)
+  //        {
+  //          bvh_model.primitive_indices = new unsigned int[num_primitives];
+  //          ar &
+  //          make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
+  //        }
+  //        else
+  //          bvh_model.primitive_indices = NULL;
+  //      }
+
+  bool with_bvs;
+  ar >> make_nvp("with_bvs", with_bvs);
+  if (with_bvs) {
+    unsigned int num_bvs;
+    ar >> make_nvp("num_bvs", num_bvs);
+
+    if (num_bvs != bvh_model.num_bvs) {
+      bvh_model.bvs.reset();
+      bvh_model.num_bvs = num_bvs;
+      if (num_bvs > 0)
+        bvh_model.bvs.reset(new
+                            typename BVHModel<BV>::bv_node_vector_t(num_bvs));
+    }
+    if (num_bvs > 0) {
+      ar >> make_nvp("bvs",
+                     make_array(reinterpret_cast<char *>(bvh_model.bvs->data()),
+                                sizeof(Node) * (std::size_t)num_bvs));
+    } else
+      bvh_model.bvs.reset();
+  }
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+namespace coal {
+
+namespace internal {
+template <typename BV>
+struct memory_footprint_evaluator<::coal::BVHModel<BV>> {
+  static size_t run(const ::coal::BVHModel<BV> &bvh_model) {
+    return static_cast<size_t>(bvh_model.memUsage(false));
+  }
+};
+}  // namespace internal
+
+}  // namespace coal
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::AABB>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBB>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::RSS>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBBRSS>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::kIOS>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<16>>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<18>>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<24>>)
+
+#endif  // ifndef COAL_SERIALIZATION_BVH_MODEL_H
diff --git a/include/coal/serialization/BV_node.h b/include/coal/serialization/BV_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e207ee662aa02f153c75415ba097d4290f620ca
--- /dev/null
+++ b/include/coal/serialization/BV_node.h
@@ -0,0 +1,35 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_BV_NODE_H
+#define COAL_SERIALIZATION_BV_NODE_H
+
+#include "coal/BV/BV_node.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/OBBRSS.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::BVNodeBase& node,
+               const unsigned int /*version*/) {
+  ar& make_nvp("first_child", node.first_child);
+  ar& make_nvp("first_primitive", node.first_primitive);
+  ar& make_nvp("num_primitives", node.num_primitives);
+}
+
+template <class Archive, typename BV>
+void serialize(Archive& ar, coal::BVNode<BV>& node,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::BVNodeBase>(node));
+  ar& make_nvp("bv", node.bv);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_BV_NODE_H
diff --git a/include/coal/serialization/BV_splitter.h b/include/coal/serialization/BV_splitter.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f6d7e98f731b1e05a60e46fd0ffde1fc14f2cc7
--- /dev/null
+++ b/include/coal/serialization/BV_splitter.h
@@ -0,0 +1,62 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_BV_SPLITTER_H
+#define COAL_SERIALIZATION_BV_SPLITTER_H
+
+#include "coal/internal/BV_splitter.h"
+
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+template <typename BV>
+struct BVSplitterAccessor : coal::BVSplitter<BV> {
+  typedef coal::BVSplitter<BV> Base;
+  using Base::split_axis;
+  using Base::split_method;
+  using Base::split_value;
+  using Base::split_vector;
+  using Base::tri_indices;
+  using Base::type;
+  using Base::vertices;
+};
+}  // namespace internal
+
+template <class Archive, typename BV>
+void save(Archive &ar, const coal::BVSplitter<BV> &splitter_,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::BVSplitterAccessor<BV> Accessor;
+  const Accessor &splitter = reinterpret_cast<const Accessor &>(splitter_);
+
+  ar &make_nvp("split_axis", splitter.split_axis);
+  ar &make_nvp("split_vector", splitter.split_vector);
+  ar &make_nvp("split_value", splitter.split_value);
+  ar &make_nvp("type", splitter.type);
+  ar &make_nvp("split_method", splitter.split_method);
+}
+
+template <class Archive, typename BV>
+void load(Archive &ar, coal::BVSplitter<BV> &splitter_,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::BVSplitterAccessor<BV> Accessor;
+  Accessor &splitter = reinterpret_cast<Accessor &>(splitter_);
+
+  ar >> make_nvp("split_axis", splitter.split_axis);
+  ar >> make_nvp("split_vector", splitter.split_vector);
+  ar >> make_nvp("split_value", splitter.split_value);
+  ar >> make_nvp("type", splitter.type);
+  ar >> make_nvp("split_method", splitter.split_method);
+
+  splitter.vertices = NULL;
+  splitter.tri_indices = NULL;
+}
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_BV_SPLITTER_H
diff --git a/include/coal/serialization/OBB.h b/include/coal/serialization/OBB.h
new file mode 100644
index 0000000000000000000000000000000000000000..298ec15a6251a2e013946a7cfa521557014a3ceb
--- /dev/null
+++ b/include/coal/serialization/OBB.h
@@ -0,0 +1,25 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_OBB_H
+#define COAL_SERIALIZATION_OBB_H
+
+#include "coal/BV/OBB.h"
+
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::OBB& bv, const unsigned int /*version*/) {
+  ar& make_nvp("axes", bv.axes);
+  ar& make_nvp("To", bv.To);
+  ar& make_nvp("extent", bv.extent);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_OBB_H
diff --git a/include/coal/serialization/OBBRSS.h b/include/coal/serialization/OBBRSS.h
new file mode 100644
index 0000000000000000000000000000000000000000..990587c7fb6834b53a59b55ed8e69b620298967f
--- /dev/null
+++ b/include/coal/serialization/OBBRSS.h
@@ -0,0 +1,26 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_OBBRSS_H
+#define COAL_SERIALIZATION_OBBRSS_H
+
+#include "coal/BV/OBBRSS.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/OBB.h"
+#include "coal/serialization/RSS.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::OBBRSS& bv, const unsigned int /*version*/) {
+  ar& make_nvp("obb", bv.obb);
+  ar& make_nvp("rss", bv.rss);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_OBBRSS_H
diff --git a/include/coal/serialization/RSS.h b/include/coal/serialization/RSS.h
new file mode 100644
index 0000000000000000000000000000000000000000..0e317c1f9f6a19ced170496852aab97ccad220a6
--- /dev/null
+++ b/include/coal/serialization/RSS.h
@@ -0,0 +1,26 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_RSS_H
+#define COAL_SERIALIZATION_RSS_H
+
+#include "coal/BV/RSS.h"
+
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::RSS& bv, const unsigned int /*version*/) {
+  ar& make_nvp("axes", bv.axes);
+  ar& make_nvp("Tr", bv.Tr);
+  ar& make_nvp("length", make_array(bv.length, 2));
+  ar& make_nvp("radius", bv.radius);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_RSS_H
diff --git a/include/coal/serialization/archive.h b/include/coal/serialization/archive.h
new file mode 100644
index 0000000000000000000000000000000000000000..9d7a410c765c59e07db06461f1a0af5e0969aa0b
--- /dev/null
+++ b/include/coal/serialization/archive.h
@@ -0,0 +1,292 @@
+//
+// Copyright (c) 2017-2024 CNRS INRIA
+// This file was borrowed from the Pinocchio library:
+// https://github.com/stack-of-tasks/pinocchio
+//
+
+#ifndef COAL_SERIALIZATION_ARCHIVE_H
+#define COAL_SERIALIZATION_ARCHIVE_H
+
+#include "coal/fwd.hh"
+
+#include <boost/serialization/nvp.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/archive/xml_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+
+#include <fstream>
+#include <string>
+#include <sstream>
+#include <stdexcept>
+
+#if BOOST_VERSION / 100 % 1000 == 78 && __APPLE__
+// See https://github.com/qcscine/utilities/issues/5#issuecomment-1246897049 for
+// further details
+
+#ifndef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#define DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#define BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#endif
+
+#include <boost/asio/streambuf.hpp>
+
+#ifdef DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#undef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#endif
+
+#else
+#include <boost/asio/streambuf.hpp>
+#endif
+
+#include <boost/iostreams/device/array.hpp>
+#include <boost/iostreams/stream.hpp>
+#include <boost/iostreams/stream_buffer.hpp>
+
+// Handle NAN inside TXT or XML archives
+#include <boost/math/special_functions/nonfinite_num_facets.hpp>
+
+namespace coal {
+namespace serialization {
+
+///
+/// \brief Loads an object from a TXT file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in]  filename Name of the file containing the serialized data.
+///
+template <typename T>
+inline void loadFromText(T& object, const std::string& filename) {
+  std::ifstream ifs(filename.c_str());
+  if (ifs) {
+    std::locale const new_loc(ifs.getloc(),
+                              new boost::math::nonfinite_num_get<char>);
+    ifs.imbue(new_loc);
+    boost::archive::text_iarchive ia(ifs, boost::archive::no_codecvt);
+    ia >> object;
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Saves an object inside a TXT file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in]  object Object in which the loaded data are copied.
+/// \param[in]  filename Name of the file containing the serialized data.
+///
+template <typename T>
+inline void saveToText(const T& object, const std::string& filename) {
+  std::ofstream ofs(filename.c_str());
+  if (ofs) {
+    boost::archive::text_oarchive oa(ofs);
+    oa & object;
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Loads an object from a std::stringstream.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in]  is  string stream constaining the serialized content of the
+/// object.
+///
+template <typename T>
+inline void loadFromStringStream(T& object, std::istringstream& is) {
+  std::locale const new_loc(is.getloc(),
+                            new boost::math::nonfinite_num_get<char>);
+  is.imbue(new_loc);
+  boost::archive::text_iarchive ia(is, boost::archive::no_codecvt);
+  ia >> object;
+}
+
+///
+/// \brief Saves an object inside a std::stringstream.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in]   object Object in which the loaded data are copied.
+/// \param[out]  ss String stream constaining the serialized content of the
+/// object.
+///
+template <typename T>
+inline void saveToStringStream(const T& object, std::stringstream& ss) {
+  boost::archive::text_oarchive oa(ss);
+  oa & object;
+}
+
+///
+/// \brief Loads an object from a std::string
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in]  str  string constaining the serialized content of the object.
+///
+template <typename T>
+inline void loadFromString(T& object, const std::string& str) {
+  std::istringstream is(str);
+  loadFromStringStream(object, is);
+}
+
+///
+/// \brief Saves an object inside a std::string
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in] object Object in which the loaded data are copied.
+///
+/// \returns a string  constaining the serialized content of the object.
+///
+template <typename T>
+inline std::string saveToString(const T& object) {
+  std::stringstream ss;
+  saveToStringStream(object, ss);
+  return ss.str();
+}
+
+///
+/// \brief Loads an object from a XML file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in] filename Name of the file containing the serialized data.
+/// \param[in] tag_name XML Tag for the given object.
+///
+template <typename T>
+inline void loadFromXML(T& object, const std::string& filename,
+                        const std::string& tag_name) {
+  if (filename.empty()) {
+    COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument);
+  }
+
+  std::ifstream ifs(filename.c_str());
+  if (ifs) {
+    std::locale const new_loc(ifs.getloc(),
+                              new boost::math::nonfinite_num_get<char>);
+    ifs.imbue(new_loc);
+    boost::archive::xml_iarchive ia(ifs, boost::archive::no_codecvt);
+    ia >> boost::serialization::make_nvp(tag_name.c_str(), object);
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Saves an object inside a XML file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in] object Object in which the loaded data are copied.
+/// \param[in] filename Name of the file containing the serialized data.
+/// \param[in] tag_name XML Tag for the given object.
+///
+template <typename T>
+inline void saveToXML(const T& object, const std::string& filename,
+                      const std::string& tag_name) {
+  if (filename.empty()) {
+    COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument);
+  }
+
+  std::ofstream ofs(filename.c_str());
+  if (ofs) {
+    boost::archive::xml_oarchive oa(ofs);
+    oa& boost::serialization::make_nvp(tag_name.c_str(), object);
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Loads an object from a binary file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in] filename Name of the file containing the serialized data.
+///
+template <typename T>
+inline void loadFromBinary(T& object, const std::string& filename) {
+  std::ifstream ifs(filename.c_str(), std::ios::binary);
+  if (ifs) {
+    boost::archive::binary_iarchive ia(ifs);
+    ia >> object;
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Saves an object inside a binary file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in] object Object in which the loaded data are copied.
+/// \param[in] filename Name of the file containing the serialized data.
+///
+template <typename T>
+void saveToBinary(const T& object, const std::string& filename) {
+  std::ofstream ofs(filename.c_str(), std::ios::binary);
+  if (ofs) {
+    boost::archive::binary_oarchive oa(ofs);
+    oa & object;
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Loads an object from a binary buffer.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in] buffer Input buffer containing the serialized data.
+///
+template <typename T>
+inline void loadFromBuffer(T& object, boost::asio::streambuf& buffer) {
+  boost::archive::binary_iarchive ia(buffer);
+  ia >> object;
+}
+
+///
+/// \brief Saves an object to a binary buffer.
+///
+/// \tparam T Type of the object to serialize.
+///
+/// \param[in]  object Object in which the loaded data are copied.
+/// \param[out] buffer Output buffer containing the serialized data.
+///
+template <typename T>
+void saveToBuffer(const T& object, boost::asio::streambuf& buffer) {
+  boost::archive::binary_oarchive oa(buffer);
+  oa & object;
+}
+
+}  // namespace serialization
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_ARCHIVE_H
diff --git a/include/coal/serialization/collision_data.h b/include/coal/serialization/collision_data.h
new file mode 100644
index 0000000000000000000000000000000000000000..0c1466dc2ee253a8d79cc80d140bfd863e0ce0c1
--- /dev/null
+++ b/include/coal/serialization/collision_data.h
@@ -0,0 +1,179 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_COLLISION_DATA_H
+#define COAL_SERIALIZATION_COLLISION_DATA_H
+
+#include "coal/collision_data.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void save(Archive& ar, const coal::Contact& contact,
+          const unsigned int /*version*/) {
+  ar& make_nvp("b1", contact.b1);
+  ar& make_nvp("b2", contact.b2);
+  ar& make_nvp("normal", contact.normal);
+  ar& make_nvp("nearest_points", contact.nearest_points);
+  ar& make_nvp("pos", contact.pos);
+  ar& make_nvp("penetration_depth", contact.penetration_depth);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::Contact& contact, const unsigned int /*version*/) {
+  ar >> make_nvp("b1", contact.b1);
+  ar >> make_nvp("b2", contact.b2);
+  ar >> make_nvp("normal", contact.normal);
+  std::array<coal::Vec3s, 2> nearest_points;
+  ar >> make_nvp("nearest_points", nearest_points);
+  contact.nearest_points[0] = nearest_points[0];
+  contact.nearest_points[1] = nearest_points[1];
+  ar >> make_nvp("pos", contact.pos);
+  ar >> make_nvp("penetration_depth", contact.penetration_depth);
+  contact.o1 = NULL;
+  contact.o2 = NULL;
+}
+
+COAL_SERIALIZATION_SPLIT(coal::Contact)
+
+template <class Archive>
+void serialize(Archive& ar, coal::QueryRequest& query_request,
+               const unsigned int /*version*/) {
+  ar& make_nvp("gjk_initial_guess", query_request.gjk_initial_guess);
+  // TODO: use gjk_initial_guess instead
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  ar& make_nvp("enable_cached_gjk_guess",
+               query_request.enable_cached_gjk_guess);
+  COAL_COMPILER_DIAGNOSTIC_POP
+  ar& make_nvp("cached_gjk_guess", query_request.cached_gjk_guess);
+  ar& make_nvp("cached_support_func_guess",
+               query_request.cached_support_func_guess);
+  ar& make_nvp("gjk_max_iterations", query_request.gjk_max_iterations);
+  ar& make_nvp("gjk_tolerance", query_request.gjk_tolerance);
+  ar& make_nvp("gjk_variant", query_request.gjk_variant);
+  ar& make_nvp("gjk_convergence_criterion",
+               query_request.gjk_convergence_criterion);
+  ar& make_nvp("gjk_convergence_criterion_type",
+               query_request.gjk_convergence_criterion_type);
+  ar& make_nvp("epa_max_iterations", query_request.epa_max_iterations);
+  ar& make_nvp("epa_tolerance", query_request.epa_tolerance);
+  ar& make_nvp("collision_distance_threshold",
+               query_request.collision_distance_threshold);
+  ar& make_nvp("enable_timings", query_request.enable_timings);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::QueryResult& query_result,
+               const unsigned int /*version*/) {
+  ar& make_nvp("cached_gjk_guess", query_result.cached_gjk_guess);
+  ar& make_nvp("cached_support_func_guess",
+               query_result.cached_support_func_guess);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::CollisionRequest& collision_request,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::QueryRequest>(
+                           collision_request));
+  ar& make_nvp("num_max_contacts", collision_request.num_max_contacts);
+  ar& make_nvp("enable_contact", collision_request.enable_contact);
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  ar& make_nvp("enable_distance_lower_bound",
+               collision_request.enable_distance_lower_bound);
+  COAL_COMPILER_DIAGNOSTIC_POP
+  ar& make_nvp("security_margin", collision_request.security_margin);
+  ar& make_nvp("break_distance", collision_request.break_distance);
+  ar& make_nvp("distance_upper_bound", collision_request.distance_upper_bound);
+}
+
+template <class Archive>
+void save(Archive& ar, const coal::CollisionResult& collision_result,
+          const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
+                           collision_result));
+  ar& make_nvp("contacts", collision_result.getContacts());
+  ar& make_nvp("distance_lower_bound", collision_result.distance_lower_bound);
+  ar& make_nvp("nearest_points", collision_result.nearest_points);
+  ar& make_nvp("normal", collision_result.normal);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::CollisionResult& collision_result,
+          const unsigned int /*version*/) {
+  ar >> make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
+                             collision_result));
+  std::vector<coal::Contact> contacts;
+  ar >> make_nvp("contacts", contacts);
+  collision_result.clear();
+  for (size_t k = 0; k < contacts.size(); ++k)
+    collision_result.addContact(contacts[k]);
+  ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound);
+  std::array<coal::Vec3s, 2> nearest_points;
+  ar >> make_nvp("nearest_points", nearest_points);
+  collision_result.nearest_points[0] = nearest_points[0];
+  collision_result.nearest_points[1] = nearest_points[1];
+  ar >> make_nvp("normal", collision_result.normal);
+}
+
+COAL_SERIALIZATION_SPLIT(coal::CollisionResult)
+
+template <class Archive>
+void serialize(Archive& ar, coal::DistanceRequest& distance_request,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::QueryRequest>(
+                           distance_request));
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points);
+  COAL_COMPILER_DIAGNOSTIC_POP
+  ar& make_nvp("enable_signed_distance",
+               distance_request.enable_signed_distance);
+  ar& make_nvp("rel_err", distance_request.rel_err);
+  ar& make_nvp("abs_err", distance_request.abs_err);
+}
+
+template <class Archive>
+void save(Archive& ar, const coal::DistanceResult& distance_result,
+          const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
+                           distance_result));
+  ar& make_nvp("min_distance", distance_result.min_distance);
+  ar& make_nvp("nearest_points", distance_result.nearest_points);
+  ar& make_nvp("normal", distance_result.normal);
+  ar& make_nvp("b1", distance_result.b1);
+  ar& make_nvp("b2", distance_result.b2);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::DistanceResult& distance_result,
+          const unsigned int /*version*/) {
+  ar >> make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
+                             distance_result));
+  ar >> make_nvp("min_distance", distance_result.min_distance);
+  std::array<coal::Vec3s, 2> nearest_points;
+  ar >> make_nvp("nearest_points", nearest_points);
+  distance_result.nearest_points[0] = nearest_points[0];
+  distance_result.nearest_points[1] = nearest_points[1];
+  ar >> make_nvp("normal", distance_result.normal);
+  ar >> make_nvp("b1", distance_result.b1);
+  ar >> make_nvp("b2", distance_result.b2);
+  distance_result.o1 = NULL;
+  distance_result.o2 = NULL;
+}
+
+COAL_SERIALIZATION_SPLIT(coal::DistanceResult)
+
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionRequest)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionResult)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceRequest)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceResult)
+
+#endif  // ifndef COAL_SERIALIZATION_COLLISION_DATA_H
diff --git a/include/coal/serialization/collision_object.h b/include/coal/serialization/collision_object.h
new file mode 100644
index 0000000000000000000000000000000000000000..c37ee465c4564aa29f765e1a33330668a2265d86
--- /dev/null
+++ b/include/coal/serialization/collision_object.h
@@ -0,0 +1,93 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H
+#define COAL_SERIALIZATION_COLLISION_OBJECT_H
+
+#include "coal/collision_object.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/AABB.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void save(Archive& ar, const coal::CollisionGeometry& collision_geometry,
+          const unsigned int /*version*/) {
+  ar& make_nvp("aabb_center", collision_geometry.aabb_center);
+  ar& make_nvp("aabb_radius", collision_geometry.aabb_radius);
+  ar& make_nvp("aabb_local", collision_geometry.aabb_local);
+  ar& make_nvp("cost_density", collision_geometry.cost_density);
+  ar& make_nvp("threshold_occupied", collision_geometry.threshold_occupied);
+  ar& make_nvp("threshold_free", collision_geometry.threshold_free);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::CollisionGeometry& collision_geometry,
+          const unsigned int /*version*/) {
+  ar >> make_nvp("aabb_center", collision_geometry.aabb_center);
+  ar >> make_nvp("aabb_radius", collision_geometry.aabb_radius);
+  ar >> make_nvp("aabb_local", collision_geometry.aabb_local);
+  ar >> make_nvp("cost_density", collision_geometry.cost_density);
+  ar >> make_nvp("threshold_occupied", collision_geometry.threshold_occupied);
+  ar >> make_nvp("threshold_free", collision_geometry.threshold_free);
+  collision_geometry.user_data = NULL;  // no way to recover this
+}
+
+COAL_SERIALIZATION_SPLIT(coal::CollisionGeometry)
+
+}  // namespace serialization
+}  // namespace boost
+
+namespace coal {
+
+// fwd declaration
+template <typename BV>
+class HeightField;
+
+template <typename PolygonT>
+class Convex;
+
+struct OBB;
+struct OBBRSS;
+class AABB;
+
+class OcTree;
+class Box;
+class Sphere;
+class Ellipsoid;
+class Capsule;
+class Cone;
+class TriangleP;
+class Cylinder;
+class Halfspace;
+class Plane;
+
+namespace serialization {
+template <>
+struct register_type<CollisionGeometry> {
+  template <class Archive>
+  static void on(Archive& ar) {
+    ar.template register_type<Box>();
+    ar.template register_type<Sphere>();
+    ar.template register_type<Ellipsoid>();
+    ar.template register_type<TriangleP>();
+    ar.template register_type<Capsule>();
+    ar.template register_type<Cone>();
+    ar.template register_type<Cylinder>();
+    ar.template register_type<Halfspace>();
+    ar.template register_type<Plane>();
+    ar.template register_type<OcTree>();
+    ar.template register_type<HeightField<OBB>>();
+    ar.template register_type<HeightField<OBBRSS>>();
+    ar.template register_type<HeightField<AABB>>();
+    ar.template register_type<Convex<Triangle>>();
+    ;
+  }
+};
+}  // namespace serialization
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H
diff --git a/include/coal/serialization/contact_patch.h b/include/coal/serialization/contact_patch.h
new file mode 100644
index 0000000000000000000000000000000000000000..aae049daa460a56f4f53187519b8f558b87aefab
--- /dev/null
+++ b/include/coal/serialization/contact_patch.h
@@ -0,0 +1,87 @@
+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_CONTACT_PATCH_H
+#define COAL_SERIALIZATION_CONTACT_PATCH_H
+
+#include "coal/collision_data.h"
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/transform.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::ContactPatch& contact_patch,
+               const unsigned int /*version*/) {
+  using namespace coal;
+  typedef Eigen::Matrix<CoalScalar, 2, Eigen::Dynamic> PolygonPoints;
+
+  size_t patch_size = contact_patch.size();
+  ar& make_nvp("patch_size", patch_size);
+  if (patch_size > 0) {
+    if (Archive::is_loading::value) {
+      contact_patch.points().resize(patch_size);
+    }
+    Eigen::Map<PolygonPoints> points_map(
+        reinterpret_cast<CoalScalar*>(contact_patch.points().data()), 2,
+        static_cast<Eigen::Index>(patch_size));
+    ar& make_nvp("points", points_map);
+  }
+
+  ar& make_nvp("penetration_depth", contact_patch.penetration_depth);
+  ar& make_nvp("direction", contact_patch.direction);
+  ar& make_nvp("tf", contact_patch.tf);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::ContactPatchRequest& request,
+               const unsigned int /*version*/) {
+  using namespace coal;
+
+  ar& make_nvp("max_num_patch", request.max_num_patch);
+
+  size_t num_samples_curved_shapes = request.getNumSamplesCurvedShapes();
+  CoalScalar patch_tolerance = request.getPatchTolerance();
+  ar& make_nvp("num_samples_curved_shapes", num_samples_curved_shapes);
+  ar& make_nvp("patch_tolerance", num_samples_curved_shapes);
+
+  if (Archive::is_loading::value) {
+    request.setNumSamplesCurvedShapes(num_samples_curved_shapes);
+    request.setPatchTolerance(patch_tolerance);
+  }
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::ContactPatchResult& result,
+               const unsigned int /*version*/) {
+  using namespace coal;
+
+  size_t num_patches = result.numContactPatches();
+  ar& make_nvp("num_patches", num_patches);
+
+  std::vector<ContactPatch> patches;
+  patches.resize(num_patches);
+  if (Archive::is_loading::value) {
+    ar& make_nvp("patches", patches);
+
+    const ContactPatchRequest request(num_patches);
+    result.set(request);
+    for (size_t i = 0; i < num_patches; ++i) {
+      ContactPatch& patch = result.getUnusedContactPatch();
+      patch = patches[i];
+    }
+  } else {
+    patches.clear();
+    for (size_t i = 0; i < num_patches; ++i) {
+      patches.emplace_back(result.getContactPatch(i));
+    }
+    ar& make_nvp("patches", patches);
+  }
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // COAL_SERIALIZATION_CONTACT_PATCH_H
diff --git a/include/coal/serialization/convex.h b/include/coal/serialization/convex.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c89a918d045accfb639cdab8768cbbe6d540a4e
--- /dev/null
+++ b/include/coal/serialization/convex.h
@@ -0,0 +1,167 @@
+//
+// Copyright (c) 2022-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_CONVEX_H
+#define COAL_SERIALIZATION_CONVEX_H
+
+#include "coal/shape/convex.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/geometric_shapes.h"
+#include "coal/serialization/memory.h"
+#include "coal/serialization/triangle.h"
+#include "coal/serialization/quadrilateral.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct ConvexBaseAccessor : coal::ConvexBase {
+  typedef coal::ConvexBase Base;
+};
+
+}  // namespace internal
+
+template <class Archive>
+void serialize(Archive& ar, coal::ConvexBase& convex_base,
+               const unsigned int /*version*/) {
+  using namespace coal;
+
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(convex_base));
+
+  const unsigned int num_points_previous = convex_base.num_points;
+  ar& make_nvp("num_points", convex_base.num_points);
+
+  const unsigned int num_normals_and_offsets_previous =
+      convex_base.num_normals_and_offsets;
+  ar& make_nvp("num_normals_and_offsets", convex_base.num_normals_and_offsets);
+
+  const int num_warm_start_supports_previous =
+      static_cast<int>(convex_base.support_warm_starts.points.size());
+  assert(num_warm_start_supports_previous ==
+         static_cast<int>(convex_base.support_warm_starts.indices.size()));
+  int num_warm_start_supports = num_warm_start_supports_previous;
+  ar& make_nvp("num_warm_start_supports", num_warm_start_supports);
+
+  if (Archive::is_loading::value) {
+    if (num_points_previous != convex_base.num_points) {
+      convex_base.points.reset();
+      if (convex_base.num_points > 0)
+        convex_base.points.reset(
+            new std::vector<Vec3s>(convex_base.num_points));
+    }
+
+    if (num_normals_and_offsets_previous !=
+        convex_base.num_normals_and_offsets) {
+      convex_base.normals.reset();
+      convex_base.offsets.reset();
+      if (convex_base.num_normals_and_offsets > 0) {
+        convex_base.normals.reset(
+            new std::vector<Vec3s>(convex_base.num_normals_and_offsets));
+        convex_base.offsets.reset(
+            new std::vector<CoalScalar>(convex_base.num_normals_and_offsets));
+      }
+    }
+
+    if (num_warm_start_supports_previous != num_warm_start_supports) {
+      convex_base.support_warm_starts.points.resize(
+          static_cast<size_t>(num_warm_start_supports));
+      convex_base.support_warm_starts.indices.resize(
+          static_cast<size_t>(num_warm_start_supports));
+    }
+  }
+
+  typedef Eigen::Matrix<CoalScalar, 3, Eigen::Dynamic> MatrixPoints;
+  if (convex_base.num_points > 0) {
+    Eigen::Map<MatrixPoints> points_map(
+        reinterpret_cast<CoalScalar*>(convex_base.points->data()), 3,
+        convex_base.num_points);
+    ar& make_nvp("points", points_map);
+  }
+
+  typedef Eigen::Matrix<CoalScalar, 1, Eigen::Dynamic> VecOfReals;
+  if (convex_base.num_normals_and_offsets > 0) {
+    Eigen::Map<MatrixPoints> normals_map(
+        reinterpret_cast<CoalScalar*>(convex_base.normals->data()), 3,
+        convex_base.num_normals_and_offsets);
+    ar& make_nvp("normals", normals_map);
+
+    Eigen::Map<VecOfReals> offsets_map(
+        reinterpret_cast<CoalScalar*>(convex_base.offsets->data()), 1,
+        convex_base.num_normals_and_offsets);
+    ar& make_nvp("offsets", offsets_map);
+  }
+
+  typedef Eigen::Matrix<int, 1, Eigen::Dynamic> VecOfInts;
+  if (num_warm_start_supports > 0) {
+    Eigen::Map<MatrixPoints> warm_start_support_points_map(
+        reinterpret_cast<CoalScalar*>(
+            convex_base.support_warm_starts.points.data()),
+        3, num_warm_start_supports);
+    ar& make_nvp("warm_start_support_points", warm_start_support_points_map);
+
+    Eigen::Map<VecOfInts> warm_start_support_indices_map(
+        reinterpret_cast<int*>(convex_base.support_warm_starts.indices.data()),
+        1, num_warm_start_supports);
+    ar& make_nvp("warm_start_support_indices", warm_start_support_indices_map);
+  }
+
+  ar& make_nvp("center", convex_base.center);
+  // We don't save neighbors as they will be computed directly by calling
+  // fillNeighbors.
+}
+
+namespace internal {
+template <typename PolygonT>
+struct ConvexAccessor : coal::Convex<PolygonT> {
+  typedef coal::Convex<PolygonT> Base;
+  using Base::fillNeighbors;
+};
+
+}  // namespace internal
+
+template <class Archive, typename PolygonT>
+void serialize(Archive& ar, coal::Convex<PolygonT>& convex_,
+               const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::ConvexAccessor<PolygonT> Accessor;
+
+  Accessor& convex = reinterpret_cast<Accessor&>(convex_);
+  ar& make_nvp("base", boost::serialization::base_object<ConvexBase>(convex_));
+
+  const unsigned int num_polygons_previous = convex.num_polygons;
+  ar& make_nvp("num_polygons", convex.num_polygons);
+
+  if (Archive::is_loading::value) {
+    if (num_polygons_previous != convex.num_polygons) {
+      convex.polygons.reset(new std::vector<PolygonT>(convex.num_polygons));
+    }
+  }
+
+  ar& make_array<PolygonT>(convex.polygons->data(), convex.num_polygons);
+
+  if (Archive::is_loading::value) convex.fillNeighbors();
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(coal::Convex<coal::Triangle>)
+COAL_SERIALIZATION_DECLARE_EXPORT(coal::Convex<coal::Quadrilateral>)
+
+namespace coal {
+
+// namespace internal {
+// template <typename BV>
+// struct memory_footprint_evaluator< ::coal::BVHModel<BV> > {
+//   static size_t run(const ::coal::BVHModel<BV> &bvh_model) {
+//     return static_cast<size_t>(bvh_model.memUsage(false));
+//   }
+// };
+// }  // namespace internal
+
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_CONVEX_H
diff --git a/include/coal/serialization/eigen.h b/include/coal/serialization/eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..24430cf029abe87ac162b7584d6a0cc192c60e23
--- /dev/null
+++ b/include/coal/serialization/eigen.h
@@ -0,0 +1,122 @@
+//
+// Copyright (c) 2017-2021 CNRS INRIA
+//
+
+/*
+ Code adapted from Pinocchio and
+ https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp
+ Copyright (c) 2015 Michael Tao
+*/
+
+#ifndef COAL_SERIALIZATION_EIGEN_H
+#define COAL_SERIALIZATION_EIGEN_H
+
+#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL
+#ifdef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION
+#define COAL_SKIP_EIGEN_BOOST_SERIALIZATION
+#endif
+#endif
+
+#ifndef COAL_SKIP_EIGEN_BOOST_SERIALIZATION
+
+#include <Eigen/Dense>
+
+#include <boost/serialization/split_free.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/serialization/array.hpp>
+
+// Workaround a bug in GCC >= 7 and C++17
+// ref. https://gitlab.com/libeigen/eigen/-/issues/1676
+#ifdef __GNUC__
+#if __GNUC__ >= 7 && __cplusplus >= 201703L
+namespace boost {
+namespace serialization {
+struct U;
+}
+}  // namespace boost
+namespace Eigen {
+namespace internal {
+template <>
+struct traits<boost::serialization::U> {
+  enum { Flags = 0 };
+};
+}  // namespace internal
+}  // namespace Eigen
+#endif
+#endif
+
+namespace boost {
+namespace serialization {
+
+template <class Archive, typename S, int Rows, int Cols, int Options,
+          int MaxRows, int MaxCols>
+void save(Archive& ar,
+          const Eigen::Matrix<S, Rows, Cols, Options, MaxRows, MaxCols>& m,
+          const unsigned int /*version*/) {
+  Eigen::DenseIndex rows(m.rows()), cols(m.cols());
+  if (Rows == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows);
+  if (Cols == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(cols);
+  ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
+}
+
+template <class Archive, typename S, int Rows, int Cols, int Options,
+          int MaxRows, int MaxCols>
+void load(Archive& ar,
+          Eigen::Matrix<S, Rows, Cols, Options, MaxRows, MaxCols>& m,
+          const unsigned int /*version*/) {
+  Eigen::DenseIndex rows = Rows, cols = Cols;
+  if (Rows == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows);
+  if (Cols == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(cols);
+  m.resize(rows, cols);
+  ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
+}
+
+template <class Archive, typename S, int Rows, int Cols, int Options,
+          int MaxRows, int MaxCols>
+void serialize(Archive& ar,
+               Eigen::Matrix<S, Rows, Cols, Options, MaxRows, MaxCols>& m,
+               const unsigned int version) {
+  split_free(ar, m, version);
+}
+
+template <class Archive, typename PlainObjectBase, int MapOptions,
+          typename StrideType>
+void save(Archive& ar,
+          const Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
+          const unsigned int /*version*/) {
+  Eigen::DenseIndex rows(m.rows()), cols(m.cols());
+  if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic)
+    ar& BOOST_SERIALIZATION_NVP(rows);
+  if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic)
+    ar& BOOST_SERIALIZATION_NVP(cols);
+  ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
+}
+
+template <class Archive, typename PlainObjectBase, int MapOptions,
+          typename StrideType>
+void load(Archive& ar, Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
+          const unsigned int /*version*/) {
+  Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime,
+                    cols = PlainObjectBase::ColsAtCompileTime;
+  if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic)
+    ar >> BOOST_SERIALIZATION_NVP(rows);
+  if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic)
+    ar >> BOOST_SERIALIZATION_NVP(cols);
+  m.resize(rows, cols);
+  ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
+}
+
+template <class Archive, typename PlainObjectBase, int MapOptions,
+          typename StrideType>
+void serialize(Archive& ar,
+               Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
+               const unsigned int version) {
+  split_free(ar, m, version);
+}
+
+}  // namespace serialization
+}  // namespace boost
+//
+#endif  // ifned COAL_SKIP_EIGEN_BOOST_SERIALIZATION
+
+#endif  // ifndef COAL_SERIALIZATION_EIGEN_H
diff --git a/include/coal/serialization/fwd.h b/include/coal/serialization/fwd.h
new file mode 100644
index 0000000000000000000000000000000000000000..18164b334643a32d4214ae3a3cd9a631a02f296c
--- /dev/null
+++ b/include/coal/serialization/fwd.h
@@ -0,0 +1,113 @@
+//
+// Copyright (c) 2021-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_FWD_H
+#define COAL_SERIALIZATION_FWD_H
+
+#include <type_traits>
+
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/archive/xml_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+
+#include <boost/serialization/split_free.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/export.hpp>
+
+#include "coal/fwd.hh"
+#include "coal/serialization/eigen.h"
+
+#define COAL_SERIALIZATION_SPLIT(Type)                                   \
+  template <class Archive>                                               \
+  void serialize(Archive& ar, Type& value, const unsigned int version) { \
+    split_free(ar, value, version);                                      \
+  }
+
+#define COAL_SERIALIZATION_DECLARE_EXPORT(T) \
+  BOOST_CLASS_EXPORT_KEY(T)                  \
+  namespace boost {                          \
+  namespace archive {                        \
+  namespace detail {                         \
+  namespace extra_detail {                   \
+  template <>                                \
+  struct init_guid<T> {                      \
+    static guid_initializer<T> const& g;     \
+  };                                         \
+  }                                          \
+  }                                          \
+  }                                          \
+  }                                          \
+  /**/
+
+#define COAL_SERIALIZATION_DEFINE_EXPORT(T)             \
+  namespace boost {                                     \
+  namespace archive {                                   \
+  namespace detail {                                    \
+  namespace extra_detail {                              \
+  guid_initializer<T> const& init_guid<T>::g =          \
+      ::boost::serialization::singleton<                \
+          guid_initializer<T> >::get_mutable_instance() \
+          .export_guid();                               \
+  }                                                     \
+  }                                                     \
+  }                                                     \
+  }                                                     \
+  /**/
+
+namespace coal {
+namespace serialization {
+namespace detail {
+
+template <class Derived, class Base>
+struct init_cast_register {};
+
+template <class Derived, class Base>
+struct cast_register_initializer {
+  void init(std::true_type) const {
+    boost::serialization::void_cast_register<Derived, Base>();
+  }
+
+  void init(std::false_type) const {}
+
+  cast_register_initializer const& init() const {
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    _Pragma("GCC diagnostic ignored \"-Wconversion\"")
+        BOOST_STATIC_WARNING((std::is_base_of<Base, Derived>::value));
+    COAL_COMPILER_DIAGNOSTIC_POP
+    init(std::is_base_of<Base, Derived>());
+    return *this;
+  }
+};
+
+}  // namespace detail
+
+template <typename T>
+struct register_type {
+  template <class Archive>
+  static void on(Archive& /*ar*/) {}
+};
+}  // namespace serialization
+}  // namespace coal
+
+#define COAL_SERIALIZATION_CAST_REGISTER(Derived, Base)                      \
+  namespace coal {                                                           \
+  namespace serialization {                                                  \
+  namespace detail {                                                         \
+  template <>                                                                \
+  struct init_cast_register<Derived, Base> {                                 \
+    static cast_register_initializer<Derived, Base> const& g;                \
+  };                                                                         \
+  cast_register_initializer<Derived, Base> const& init_cast_register<        \
+      Derived, Base>::g =                                                    \
+      ::boost::serialization::singleton<                                     \
+          cast_register_initializer<Derived, Base> >::get_mutable_instance() \
+          .init();                                                           \
+  }                                                                          \
+  }                                                                          \
+  }
+
+#endif  // ifndef COAL_SERIALIZATION_FWD_H
diff --git a/include/coal/serialization/geometric_shapes.h b/include/coal/serialization/geometric_shapes.h
new file mode 100644
index 0000000000000000000000000000000000000000..2a14614c26e120377e2b5bec4ac6d8800511a42f
--- /dev/null
+++ b/include/coal/serialization/geometric_shapes.h
@@ -0,0 +1,120 @@
+//
+// Copyright (c) 2021-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H
+#define COAL_SERIALIZATION_GEOMETRIC_SHAPES_H
+
+#include "coal/shape/geometric_shapes.h"
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/collision_object.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::ShapeBase& shape_base,
+               const unsigned int /*version*/) {
+  ar& make_nvp(
+      "base",
+      boost::serialization::base_object<coal::CollisionGeometry>(shape_base));
+  ::coal::CoalScalar radius = shape_base.getSweptSphereRadius();
+  ar& make_nvp("swept_sphere_radius", radius);
+
+  if (Archive::is_loading::value) {
+    shape_base.setSweptSphereRadius(radius);
+  }
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::TriangleP& triangle,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(triangle));
+  ar& make_nvp("a", triangle.a);
+  ar& make_nvp("b", triangle.b);
+  ar& make_nvp("c", triangle.c);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Box& box, const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::ShapeBase>(box));
+  ar& make_nvp("halfSide", box.halfSide);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Sphere& sphere,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(sphere));
+  ar& make_nvp("radius", sphere.radius);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Ellipsoid& ellipsoid,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(ellipsoid));
+  ar& make_nvp("radii", ellipsoid.radii);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Capsule& capsule,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(capsule));
+  ar& make_nvp("radius", capsule.radius);
+  ar& make_nvp("halfLength", capsule.halfLength);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Cone& cone, const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(cone));
+  ar& make_nvp("radius", cone.radius);
+  ar& make_nvp("halfLength", cone.halfLength);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Cylinder& cylinder,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(cylinder));
+  ar& make_nvp("radius", cylinder.radius);
+  ar& make_nvp("halfLength", cylinder.halfLength);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Halfspace& half_space,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(half_space));
+  ar& make_nvp("n", half_space.n);
+  ar& make_nvp("d", half_space.d);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Plane& plane,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(plane));
+  ar& make_nvp("n", plane.n);
+  ar& make_nvp("d", plane.d);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::ShapeBase)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionGeometry)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::TriangleP)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Box)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Sphere)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Ellipsoid)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Capsule)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cone)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cylinder)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Halfspace)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Plane)
+
+#endif  // ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H
diff --git a/include/coal/serialization/hfield.h b/include/coal/serialization/hfield.h
new file mode 100644
index 0000000000000000000000000000000000000000..8ea3d71209c8dc1596218c3930f49718f20d8447
--- /dev/null
+++ b/include/coal/serialization/hfield.h
@@ -0,0 +1,80 @@
+//
+// Copyright (c) 2021-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_HFIELD_H
+#define COAL_SERIALIZATION_HFIELD_H
+
+#include "coal/hfield.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/OBBRSS.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive &ar, coal::HFNodeBase &node,
+               const unsigned int /*version*/) {
+  ar &make_nvp("first_child", node.first_child);
+  ar &make_nvp("x_id", node.x_id);
+  ar &make_nvp("x_size", node.x_size);
+  ar &make_nvp("y_id", node.y_id);
+  ar &make_nvp("y_size", node.y_size);
+  ar &make_nvp("max_height", node.max_height);
+  ar &make_nvp("contact_active_faces", node.contact_active_faces);
+}
+
+template <class Archive, typename BV>
+void serialize(Archive &ar, coal::HFNode<BV> &node,
+               const unsigned int /*version*/) {
+  ar &make_nvp("base",
+               boost::serialization::base_object<coal::HFNodeBase>(node));
+  ar &make_nvp("bv", node.bv);
+}
+
+namespace internal {
+template <typename BV>
+struct HeightFieldAccessor : coal::HeightField<BV> {
+  typedef coal::HeightField<BV> Base;
+  using Base::bvs;
+  using Base::heights;
+  using Base::max_height;
+  using Base::min_height;
+  using Base::num_bvs;
+  using Base::x_dim;
+  using Base::x_grid;
+  using Base::y_dim;
+  using Base::y_grid;
+};
+}  // namespace internal
+
+template <class Archive, typename BV>
+void serialize(Archive &ar, coal::HeightField<BV> &hf_model,
+               const unsigned int /*version*/) {
+  ar &make_nvp(
+      "base",
+      boost::serialization::base_object<coal::CollisionGeometry>(hf_model));
+
+  typedef internal::HeightFieldAccessor<BV> Accessor;
+  Accessor &access = reinterpret_cast<Accessor &>(hf_model);
+
+  ar &make_nvp("x_dim", access.x_dim);
+  ar &make_nvp("y_dim", access.y_dim);
+  ar &make_nvp("heights", access.heights);
+  ar &make_nvp("min_height", access.min_height);
+  ar &make_nvp("max_height", access.max_height);
+  ar &make_nvp("x_grid", access.x_grid);
+  ar &make_nvp("y_grid", access.y_grid);
+
+  ar &make_nvp("bvs", access.bvs);
+  ar &make_nvp("num_bvs", access.num_bvs);
+}
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::AABB>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBB>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBBRSS>)
+
+#endif  // ifndef COAL_SERIALIZATION_HFIELD_H
diff --git a/include/coal/serialization/kDOP.h b/include/coal/serialization/kDOP.h
new file mode 100644
index 0000000000000000000000000000000000000000..f4cf992c64fa06caa1a46a539842fa378452666b
--- /dev/null
+++ b/include/coal/serialization/kDOP.h
@@ -0,0 +1,34 @@
+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_kDOP_H
+#define COAL_SERIALIZATION_kDOP_H
+
+#include "coal/BV/kDOP.h"
+
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+template <short N>
+struct KDOPAccessor : coal::KDOP<N> {
+  typedef coal::KDOP<N> Base;
+  using Base::dist_;
+};
+}  // namespace internal
+
+template <class Archive, short N>
+void serialize(Archive& ar, coal::KDOP<N>& bv_,
+               const unsigned int /*version*/) {
+  typedef internal::KDOPAccessor<N> Accessor;
+  Accessor& access = reinterpret_cast<Accessor&>(bv_);
+  ar& make_nvp("distances", make_array(access.dist_.data(), N));
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // COAL_SERIALIZATION_kDOP_H
diff --git a/include/coal/serialization/kIOS.h b/include/coal/serialization/kIOS.h
new file mode 100644
index 0000000000000000000000000000000000000000..18d349db231f6670dd1d548b875093e2a0516160
--- /dev/null
+++ b/include/coal/serialization/kIOS.h
@@ -0,0 +1,57 @@
+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_kIOS_H
+#define COAL_SERIALIZATION_kIOS_H
+
+#include "coal/BV/kIOS.h"
+
+#include "coal/serialization/OBB.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::kIOS& bv, const unsigned int version) {
+  split_free(ar, bv, version);
+}
+
+template <class Archive>
+void save(Archive& ar, const coal::kIOS& bv, const unsigned int /*version*/) {
+  // Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres
+  ar& make_nvp("num_spheres", bv.num_spheres);
+
+  std::array<coal::Vec3s, coal::kIOS::max_num_spheres> centers{};
+  std::array<coal::CoalScalar, coal::kIOS::max_num_spheres> radii;
+  for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) {
+    centers[i] = bv.spheres[i].o;
+    radii[i] = bv.spheres[i].r;
+  }
+  ar& make_nvp("centers", make_array(centers.data(), centers.size()));
+  ar& make_nvp("radii", make_array(radii.data(), radii.size()));
+
+  ar& make_nvp("obb", bv.obb);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::kIOS& bv, const unsigned int /*version*/) {
+  ar >> make_nvp("num_spheres", bv.num_spheres);
+
+  std::array<coal::Vec3s, coal::kIOS::max_num_spheres> centers;
+  std::array<coal::CoalScalar, coal::kIOS::max_num_spheres> radii;
+  ar >> make_nvp("centers", make_array(centers.data(), centers.size()));
+  ar >> make_nvp("radii", make_array(radii.data(), radii.size()));
+  for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) {
+    bv.spheres[i].o = centers[i];
+    bv.spheres[i].r = radii[i];
+  }
+
+  ar >> make_nvp("obb", bv.obb);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // COAL_SERIALIZATION_kIOS_H
diff --git a/include/coal/serialization/memory.h b/include/coal/serialization/memory.h
new file mode 100644
index 0000000000000000000000000000000000000000..ee278bcede950e33b82d331488fb513bfcf98781
--- /dev/null
+++ b/include/coal/serialization/memory.h
@@ -0,0 +1,30 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_MEMORY_H
+#define COAL_SERIALIZATION_MEMORY_H
+
+namespace coal {
+
+namespace internal {
+template <typename T>
+struct memory_footprint_evaluator {
+  static size_t run(const T &) { return sizeof(T); }
+};
+}  // namespace internal
+
+/// \brief Returns the memory footpring of the input object.
+/// For POD objects, this function returns the result of sizeof(T)
+///
+/// \param[in] object whose memory footprint needs to be evaluated.
+///
+/// \return the memory footprint of the input object.
+template <typename T>
+size_t computeMemoryFootprint(const T &object) {
+  return internal::memory_footprint_evaluator<T>::run(object);
+}
+
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_MEMORY_H
diff --git a/include/coal/serialization/octree.h b/include/coal/serialization/octree.h
new file mode 100644
index 0000000000000000000000000000000000000000..769532379a88281cca6aefd3c5c2b42377c7debf
--- /dev/null
+++ b/include/coal/serialization/octree.h
@@ -0,0 +1,101 @@
+//
+// Copyright (c) 2023-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_OCTREE_H
+#define COAL_SERIALIZATION_OCTREE_H
+
+#include <sstream>
+#include <iostream>
+
+#include <boost/serialization/string.hpp>
+
+#include "coal/octree.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct OcTreeAccessor : coal::OcTree {
+  typedef coal::OcTree Base;
+  using Base::default_occupancy;
+  using Base::free_threshold;
+  using Base::occupancy_threshold;
+  using Base::tree;
+};
+}  // namespace internal
+
+template <class Archive>
+void save_construct_data(Archive &ar, const coal::OcTree *octree_ptr,
+                         const unsigned int /*version*/) {
+  const double resolution = octree_ptr->getResolution();
+  ar << make_nvp("resolution", resolution);
+}
+
+template <class Archive>
+void save(Archive &ar, const coal::OcTree &octree,
+          const unsigned int /*version*/) {
+  typedef internal::OcTreeAccessor Accessor;
+  const Accessor &access = reinterpret_cast<const Accessor &>(octree);
+
+  std::ostringstream stream;
+  access.tree->write(stream);
+  const std::string stream_str = stream.str();
+  auto size = stream_str.size();
+  // We can't directly serialize stream_str because it contains binary data.
+  // This create a bug on Windows with text_archive
+  ar << make_nvp("tree_data_size", size);
+  ar << make_nvp("tree_data",
+                 make_array(stream_str.c_str(), stream_str.size()));
+
+  ar << make_nvp("base", base_object<coal::CollisionGeometry>(octree));
+  ar << make_nvp("default_occupancy", access.default_occupancy);
+  ar << make_nvp("occupancy_threshold", access.occupancy_threshold);
+  ar << make_nvp("free_threshold", access.free_threshold);
+}
+
+template <class Archive>
+void load_construct_data(Archive &ar, coal::OcTree *octree_ptr,
+                         const unsigned int /*version*/) {
+  double resolution;
+  ar >> make_nvp("resolution", resolution);
+  new (octree_ptr) coal::OcTree(resolution);
+}
+
+template <class Archive>
+void load(Archive &ar, coal::OcTree &octree, const unsigned int /*version*/) {
+  typedef internal::OcTreeAccessor Accessor;
+  Accessor &access = reinterpret_cast<Accessor &>(octree);
+
+  std::size_t tree_data_size;
+  ar >> make_nvp("tree_data_size", tree_data_size);
+
+  std::string stream_str;
+  stream_str.resize(tree_data_size);
+  /// TODO use stream_str.data in C++17
+  assert(tree_data_size > 0 && "tree_data_size should be greater than 0");
+  ar >> make_nvp("tree_data", make_array(&stream_str[0], tree_data_size));
+  std::istringstream stream(stream_str);
+
+  octomap::AbstractOcTree *new_tree = octomap::AbstractOcTree::read(stream);
+  access.tree = std::shared_ptr<const octomap::OcTree>(
+      dynamic_cast<octomap::OcTree *>(new_tree));
+
+  ar >> make_nvp("base", base_object<coal::CollisionGeometry>(octree));
+  ar >> make_nvp("default_occupancy", access.default_occupancy);
+  ar >> make_nvp("occupancy_threshold", access.occupancy_threshold);
+  ar >> make_nvp("free_threshold", access.free_threshold);
+}
+
+template <class Archive>
+void serialize(Archive &ar, coal::OcTree &octree, const unsigned int version) {
+  split_free(ar, octree, version);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::OcTree)
+
+#endif  // ifndef COAL_SERIALIZATION_OCTREE_H
diff --git a/include/coal/serialization/quadrilateral.h b/include/coal/serialization/quadrilateral.h
new file mode 100644
index 0000000000000000000000000000000000000000..b805f5bde0d09221bffefc6957a939eb892ad4dd
--- /dev/null
+++ b/include/coal/serialization/quadrilateral.h
@@ -0,0 +1,26 @@
+//
+// Copyright (c) 2022 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_QUADRILATERAL_H
+#define COAL_SERIALIZATION_QUADRILATERAL_H
+
+#include "coal/data_types.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive &ar, coal::Quadrilateral &quadrilateral,
+               const unsigned int /*version*/) {
+  ar &make_nvp("p0", quadrilateral[0]);
+  ar &make_nvp("p1", quadrilateral[1]);
+  ar &make_nvp("p2", quadrilateral[2]);
+  ar &make_nvp("p3", quadrilateral[3]);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_QUADRILATERAL_H
diff --git a/include/coal/serialization/serializer.h b/include/coal/serialization/serializer.h
new file mode 100644
index 0000000000000000000000000000000000000000..daef9fc6458e72f24eb950d94591c56de2c81710
--- /dev/null
+++ b/include/coal/serialization/serializer.h
@@ -0,0 +1,92 @@
+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_SERIALIZER_H
+#define COAL_SERIALIZATION_SERIALIZER_H
+
+#include "coal/serialization/archive.h"
+
+namespace coal {
+namespace serialization {
+
+struct Serializer {
+  /// \brief Loads an object from a text file.
+  template <typename T>
+  static void loadFromText(T& object, const std::string& filename) {
+    ::coal::serialization::loadFromText(object, filename);
+  }
+
+  /// \brief Saves an object as a text file.
+  template <typename T>
+  static void saveToText(const T& object, const std::string& filename) {
+    ::coal::serialization::saveToText(object, filename);
+  }
+
+  /// \brief Loads an object from a stream string.
+  template <typename T>
+  static void loadFromStringStream(T& object, std::istringstream& is) {
+    ::coal::serialization::loadFromStringStream(object, is);
+  }
+
+  /// \brief Saves an object to a string stream.
+  template <typename T>
+  static void saveToStringStream(const T& object, std::stringstream& ss) {
+    ::coal::serialization::saveToStringStream(object, ss);
+  }
+
+  /// \brief Loads an object from a string.
+  template <typename T>
+  static void loadFromString(T& object, const std::string& str) {
+    ::coal::serialization::loadFromString(object, str);
+  }
+
+  /// \brief Saves a Derived object to a string.
+  template <typename T>
+  static std::string saveToString(const T& object) {
+    return ::coal::serialization::saveToString(object);
+  }
+
+  /// \brief Loads an object from an XML file.
+  template <typename T>
+  static void loadFromXML(T& object, const std::string& filename,
+                          const std::string& tag_name) {
+    ::coal::serialization::loadFromXML(object, filename, tag_name);
+  }
+
+  /// \brief Saves an object as an XML file.
+  template <typename T>
+  static void saveToXML(const T& object, const std::string& filename,
+                        const std::string& tag_name) {
+    ::coal::serialization::saveToXML(object, filename, tag_name);
+  }
+
+  /// \brief Loads a Derived object from an binary file.
+  template <typename T>
+  static void loadFromBinary(T& object, const std::string& filename) {
+    ::coal::serialization::loadFromBinary(object, filename);
+  }
+
+  /// \brief Saves a Derived object as an binary file.
+  template <typename T>
+  static void saveToBinary(const T& object, const std::string& filename) {
+    ::coal::serialization::saveToBinary(object, filename);
+  }
+
+  /// \brief Loads an object from a binary buffer.
+  template <typename T>
+  static void loadFromBuffer(T& object, boost::asio::streambuf& container) {
+    ::coal::serialization::loadFromBuffer(object, container);
+  }
+
+  /// \brief Saves an object as a binary buffer.
+  template <typename T>
+  static void saveToBuffer(const T& object, boost::asio::streambuf& container) {
+    ::coal::serialization::saveToBuffer(object, container);
+  }
+};
+
+}  // namespace serialization
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_SERIALIZER_H
diff --git a/include/coal/serialization/transform.h b/include/coal/serialization/transform.h
new file mode 100644
index 0000000000000000000000000000000000000000..c43542d7a8a0a677735c399bc1879c05d448b534
--- /dev/null
+++ b/include/coal/serialization/transform.h
@@ -0,0 +1,24 @@
+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_TRANSFORM_H
+#define COAL_SERIALIZATION_TRANSFORM_H
+
+#include "coal/math/transform.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::Transform3s& tf,
+               const unsigned int /*version*/) {
+  ar& make_nvp("R", tf.rotation());
+  ar& make_nvp("T", tf.translation());
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // COAL_SERIALIZATION_TRANSFORM_H
diff --git a/include/coal/serialization/triangle.h b/include/coal/serialization/triangle.h
new file mode 100644
index 0000000000000000000000000000000000000000..052706be52619f405423938582e12790d7876be1
--- /dev/null
+++ b/include/coal/serialization/triangle.h
@@ -0,0 +1,25 @@
+//
+// Copyright (c) 2021-2022 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_TRIANGLE_H
+#define COAL_SERIALIZATION_TRIANGLE_H
+
+#include "coal/data_types.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive &ar, coal::Triangle &triangle,
+               const unsigned int /*version*/) {
+  ar &make_nvp("p0", triangle[0]);
+  ar &make_nvp("p1", triangle[1]);
+  ar &make_nvp("p2", triangle[2]);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_TRIANGLE_H
diff --git a/include/coal/shape/convex.h b/include/coal/shape/convex.h
new file mode 100644
index 0000000000000000000000000000000000000000..a17108a6e61c2910c53f80b4696b3c129994aeb9
--- /dev/null
+++ b/include/coal/shape/convex.h
@@ -0,0 +1,112 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_SHAPE_CONVEX_H
+#define COAL_SHAPE_CONVEX_H
+
+#include "coal/shape/geometric_shapes.h"
+
+namespace coal {
+
+/// @brief Convex polytope
+/// @tparam PolygonT the polygon class. It must have method \c size() and
+///         \c operator[](int i)
+template <typename PolygonT>
+class Convex : public ConvexBase {
+ public:
+  /// @brief Construct an uninitialized convex object
+  Convex() : ConvexBase(), num_polygons(0) {}
+
+  /// @brief Constructing a convex, providing normal and offset of each polytype
+  /// surface, and the points and shape topology information \param ownStorage
+  /// whether this class owns the pointers of points and
+  ///                    polygons. If owned, they are deleted upon destruction.
+  /// \param points_ list of 3D points
+  /// \param num_points_ number of 3D points
+  /// \param polygons_ \copydoc Convex::polygons
+  /// \param num_polygons_ the number of polygons.
+  /// \note num_polygons_ is not the allocated size of polygons_.
+  Convex(std::shared_ptr<std::vector<Vec3s>> points_, unsigned int num_points_,
+         std::shared_ptr<std::vector<PolygonT>> polygons_,
+         unsigned int num_polygons_);
+
+  /// @brief Copy constructor
+  /// Only the list of neighbors is copied.
+  Convex(const Convex& other);
+
+  ~Convex();
+
+  /// based on http://number-none.com/blow/inertia/bb_inertia.doc
+  Matrix3s computeMomentofInertia() const;
+
+  Vec3s computeCOM() const;
+
+  CoalScalar computeVolume() const;
+
+  ///
+  /// @brief Set the current Convex from a list of points and polygons.
+  ///
+  /// \param ownStorage whether this class owns the pointers of points and
+  ///                    polygons. If owned, they are deleted upon destruction.
+  /// \param points list of 3D points
+  /// \param num_points number of 3D points
+  /// \param polygons \copydoc Convex::polygons
+  /// \param num_polygons the number of polygons.
+  /// \note num_polygons is not the allocated size of polygons.
+  ///
+  void set(std::shared_ptr<std::vector<Vec3s>> points, unsigned int num_points,
+           std::shared_ptr<std::vector<PolygonT>> polygons,
+           unsigned int num_polygons);
+
+  /// @brief Clone (deep copy)
+  virtual Convex<PolygonT>* clone() const;
+
+  /// @brief An array of PolygonT object.
+  /// PolygonT should contains a list of vertices for each polygon,
+  /// in counter clockwise order.
+  std::shared_ptr<std::vector<PolygonT>> polygons;
+  unsigned int num_polygons;
+
+ protected:
+  void fillNeighbors();
+};
+
+}  // namespace coal
+
+#include "coal/shape/details/convex.hxx"
+
+#endif
diff --git a/include/coal/shape/details/convex.hxx b/include/coal/shape/details/convex.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..0c0ccf58c3e36231770affa3180232b6c8f8de7a
--- /dev/null
+++ b/include/coal/shape/details/convex.hxx
@@ -0,0 +1,283 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, CNRS - LAAS
+ *  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 Joseph Mirabel */
+
+#ifndef COAL_SHAPE_CONVEX_HXX
+#define COAL_SHAPE_CONVEX_HXX
+
+#include <set>
+#include <vector>
+#include <iostream>
+
+namespace coal {
+
+template <typename PolygonT>
+Convex<PolygonT>::Convex(std::shared_ptr<std::vector<Vec3s>> points_,
+                         unsigned int num_points_,
+                         std::shared_ptr<std::vector<PolygonT>> polygons_,
+                         unsigned int num_polygons_)
+    : ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) {
+  this->initialize(points_, num_points_);
+  this->fillNeighbors();
+  this->buildSupportWarmStart();
+}
+
+template <typename PolygonT>
+Convex<PolygonT>::Convex(const Convex<PolygonT>& other)
+    : ConvexBase(other), num_polygons(other.num_polygons) {
+  if (other.polygons.get()) {
+    polygons.reset(new std::vector<PolygonT>(*(other.polygons)));
+  } else
+    polygons.reset();
+}
+
+template <typename PolygonT>
+Convex<PolygonT>::~Convex() {}
+
+template <typename PolygonT>
+void Convex<PolygonT>::set(std::shared_ptr<std::vector<Vec3s>> points_,
+                           unsigned int num_points_,
+                           std::shared_ptr<std::vector<PolygonT>> polygons_,
+                           unsigned int num_polygons_) {
+  ConvexBase::set(points_, num_points_);
+
+  this->num_polygons = num_polygons_;
+  this->polygons = polygons_;
+
+  this->fillNeighbors();
+  this->buildSupportWarmStart();
+}
+
+template <typename PolygonT>
+Convex<PolygonT>* Convex<PolygonT>::clone() const {
+  return new Convex(*this);
+}
+
+template <typename PolygonT>
+Matrix3s Convex<PolygonT>::computeMomentofInertia() const {
+  typedef typename PolygonT::size_type size_type;
+  typedef typename PolygonT::index_type index_type;
+
+  Matrix3s C = Matrix3s::Zero();
+
+  Matrix3s C_canonical;
+  C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0,
+      1 / 120.0, 1 / 120.0, 1 / 60.0;
+
+  if (!(points.get())) {
+    std::cerr
+        << "Error in `Convex::computeMomentofInertia`! Convex has no vertices."
+        << std::endl;
+    return C;
+  }
+  const std::vector<Vec3s>& points_ = *points;
+  if (!(polygons.get())) {
+    std::cerr
+        << "Error in `Convex::computeMomentofInertia`! Convex has no polygons."
+        << std::endl;
+    return C;
+  }
+  const std::vector<PolygonT>& polygons_ = *polygons;
+  for (unsigned int i = 0; i < num_polygons; ++i) {
+    const PolygonT& polygon = polygons_[i];
+
+    // compute the center of the polygon
+    Vec3s plane_center(0, 0, 0);
+    for (size_type j = 0; j < polygon.size(); ++j)
+      plane_center += points_[polygon[(index_type)j]];
+    plane_center /= polygon.size();
+
+    // compute the volume of tetrahedron making by neighboring two points, the
+    // plane center and the reference point (zero) of the convex shape
+    const Vec3s& v3 = plane_center;
+    for (size_type j = 0; j < polygon.size(); ++j) {
+      index_type e_first = polygon[static_cast<index_type>(j)];
+      index_type e_second =
+          polygon[static_cast<index_type>((j + 1) % polygon.size())];
+      const Vec3s& v1 = points_[e_first];
+      const Vec3s& v2 = points_[e_second];
+      Matrix3s A;
+      A << v1.transpose(), v2.transpose(),
+          v3.transpose();  // this is A' in the original document
+      C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
+    }
+  }
+
+  return C.trace() * Matrix3s::Identity() - C;
+}
+
+template <typename PolygonT>
+Vec3s Convex<PolygonT>::computeCOM() const {
+  typedef typename PolygonT::size_type size_type;
+  typedef typename PolygonT::index_type index_type;
+
+  Vec3s com(0, 0, 0);
+  CoalScalar vol = 0;
+  if (!(points.get())) {
+    std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices."
+              << std::endl;
+    return com;
+  }
+  const std::vector<Vec3s>& points_ = *points;
+  if (!(polygons.get())) {
+    std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons."
+              << std::endl;
+    return com;
+  }
+  const std::vector<PolygonT>& polygons_ = *polygons;
+  for (unsigned int i = 0; i < num_polygons; ++i) {
+    const PolygonT& polygon = polygons_[i];
+    // compute the center of the polygon
+    Vec3s plane_center(0, 0, 0);
+    for (size_type j = 0; j < polygon.size(); ++j)
+      plane_center += points_[polygon[(index_type)j]];
+    plane_center /= polygon.size();
+
+    // compute the volume of tetrahedron making by neighboring two points, the
+    // plane center and the reference point (zero) of the convex shape
+    const Vec3s& v3 = plane_center;
+    for (size_type j = 0; j < polygon.size(); ++j) {
+      index_type e_first = polygon[static_cast<index_type>(j)];
+      index_type e_second =
+          polygon[static_cast<index_type>((j + 1) % polygon.size())];
+      const Vec3s& v1 = points_[e_first];
+      const Vec3s& v2 = points_[e_second];
+      CoalScalar d_six_vol = (v1.cross(v2)).dot(v3);
+      vol += d_six_vol;
+      com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol;
+    }
+  }
+
+  return com / (vol * 4);  // here we choose zero as the reference
+}
+
+template <typename PolygonT>
+CoalScalar Convex<PolygonT>::computeVolume() const {
+  typedef typename PolygonT::size_type size_type;
+  typedef typename PolygonT::index_type index_type;
+
+  CoalScalar vol = 0;
+  if (!(points.get())) {
+    std::cerr << "Error in `Convex::computeVolume`! Convex has no vertices."
+              << std::endl;
+    return vol;
+  }
+  const std::vector<Vec3s>& points_ = *points;
+  if (!(polygons.get())) {
+    std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons."
+              << std::endl;
+    return vol;
+  }
+  const std::vector<PolygonT>& polygons_ = *polygons;
+  for (unsigned int i = 0; i < num_polygons; ++i) {
+    const PolygonT& polygon = polygons_[i];
+
+    // compute the center of the polygon
+    Vec3s plane_center(0, 0, 0);
+    for (size_type j = 0; j < polygon.size(); ++j)
+      plane_center += points_[polygon[(index_type)j]];
+    plane_center /= polygon.size();
+
+    // compute the volume of tetrahedron making by neighboring two points, the
+    // plane center and the reference point (zero point) of the convex shape
+    const Vec3s& v3 = plane_center;
+    for (size_type j = 0; j < polygon.size(); ++j) {
+      index_type e_first = polygon[static_cast<index_type>(j)];
+      index_type e_second =
+          polygon[static_cast<index_type>((j + 1) % polygon.size())];
+      const Vec3s& v1 = points_[e_first];
+      const Vec3s& v2 = points_[e_second];
+      CoalScalar d_six_vol = (v1.cross(v2)).dot(v3);
+      vol += d_six_vol;
+    }
+  }
+
+  return vol / 6;
+}
+
+template <typename PolygonT>
+void Convex<PolygonT>::fillNeighbors() {
+  neighbors.reset(new std::vector<Neighbors>(num_points));
+
+  typedef typename PolygonT::size_type size_type;
+  typedef typename PolygonT::index_type index_type;
+  std::vector<std::set<index_type>> nneighbors(num_points);
+  unsigned int c_nneighbors = 0;
+
+  if (!(polygons.get())) {
+    std::cerr << "Error in `Convex::fillNeighbors`! Convex has no polygons."
+              << std::endl;
+  }
+  const std::vector<PolygonT>& polygons_ = *polygons;
+  for (unsigned int l = 0; l < num_polygons; ++l) {
+    const PolygonT& polygon = polygons_[l];
+    const size_type n = polygon.size();
+
+    for (size_type j = 0; j < polygon.size(); ++j) {
+      size_type i = (j == 0) ? n - 1 : j - 1;
+      size_type k = (j == n - 1) ? 0 : j + 1;
+      index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j],
+                 pk = polygon[(index_type)k];
+      // Update neighbors of pj;
+      if (nneighbors[pj].count(pi) == 0) {
+        c_nneighbors++;
+        nneighbors[pj].insert(pi);
+      }
+      if (nneighbors[pj].count(pk) == 0) {
+        c_nneighbors++;
+        nneighbors[pj].insert(pk);
+      }
+    }
+  }
+
+  nneighbors_.reset(new std::vector<unsigned int>(c_nneighbors));
+
+  unsigned int* p_nneighbors = nneighbors_->data();
+  std::vector<Neighbors>& neighbors_ = *neighbors;
+  for (unsigned int i = 0; i < num_points; ++i) {
+    Neighbors& n = neighbors_[i];
+    if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
+      COAL_THROW_PRETTY("Too many neighbors.", std::logic_error);
+    n.count_ = (unsigned char)nneighbors[i].size();
+    n.n_ = p_nneighbors;
+    p_nneighbors =
+        std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
+  }
+  assert(p_nneighbors == nneighbors_->data() + c_nneighbors);
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/shape/geometric_shape_to_BVH_model.h b/include/coal/shape/geometric_shape_to_BVH_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f43e9d7d7db16a00b458dc26756e7e977d4d050
--- /dev/null
+++ b/include/coal/shape/geometric_shape_to_BVH_model.h
@@ -0,0 +1,354 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H
+#define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H
+
+#include "coal/shape/geometric_shapes.h"
+#include "coal/BVH/BVH_model.h"
+#include <boost/math/constants/constants.hpp>
+
+namespace coal {
+
+/// @brief Generate BVH model from box
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Box& shape,
+                      const Transform3s& pose) {
+  CoalScalar a = shape.halfSide[0];
+  CoalScalar b = shape.halfSide[1];
+  CoalScalar c = shape.halfSide[2];
+  std::vector<Vec3s> points(8);
+  std::vector<Triangle> tri_indices(12);
+  points[0] = Vec3s(a, -b, c);
+  points[1] = Vec3s(a, b, c);
+  points[2] = Vec3s(-a, b, c);
+  points[3] = Vec3s(-a, -b, c);
+  points[4] = Vec3s(a, -b, -c);
+  points[5] = Vec3s(a, b, -c);
+  points[6] = Vec3s(-a, b, -c);
+  points[7] = Vec3s(-a, -b, -c);
+
+  tri_indices[0].set(0, 4, 1);
+  tri_indices[1].set(1, 4, 5);
+  tri_indices[2].set(2, 6, 3);
+  tri_indices[3].set(3, 6, 7);
+  tri_indices[4].set(3, 0, 2);
+  tri_indices[5].set(2, 0, 1);
+  tri_indices[6].set(6, 5, 7);
+  tri_indices[7].set(7, 5, 4);
+  tri_indices[8].set(1, 5, 2);
+  tri_indices[9].set(2, 5, 6);
+  tri_indices[10].set(3, 7, 0);
+  tri_indices[11].set(0, 7, 4);
+
+  for (unsigned int i = 0; i < points.size(); ++i) {
+    points[i] = pose.transform(points[i]).eval();
+  }
+
+  model.beginModel();
+  model.addSubModel(points, tri_indices);
+  model.endModel();
+  model.computeLocalAABB();
+}
+
+/// @brief Generate BVH model from sphere, given the number of segments along
+/// longitude and number of rings along latitude.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
+                      const Transform3s& pose, unsigned int seg,
+                      unsigned int ring) {
+  std::vector<Vec3s> points;
+  std::vector<Triangle> tri_indices;
+
+  CoalScalar r = shape.radius;
+  CoalScalar phi, phid;
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
+  phid = pi * 2 / seg;
+  phi = 0;
+
+  CoalScalar theta, thetad;
+  thetad = pi / (ring + 1);
+  theta = 0;
+
+  for (unsigned int i = 0; i < ring; ++i) {
+    CoalScalar theta_ = theta + thetad * (i + 1);
+    for (unsigned int j = 0; j < seg; ++j) {
+      points.push_back(Vec3s(r * sin(theta_) * cos(phi + j * phid),
+                             r * sin(theta_) * sin(phi + j * phid),
+                             r * cos(theta_)));
+    }
+  }
+  points.push_back(Vec3s(0, 0, r));
+  points.push_back(Vec3s(0, 0, -r));
+
+  for (unsigned int i = 0; i < ring - 1; ++i) {
+    for (unsigned int j = 0; j < seg; ++j) {
+      unsigned int a, b, c, d;
+      a = i * seg + j;
+      b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
+      c = (i + 1) * seg + j;
+      d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
+      tri_indices.push_back(Triangle(a, c, b));
+      tri_indices.push_back(Triangle(b, c, d));
+    }
+  }
+
+  for (unsigned int j = 0; j < seg; ++j) {
+    unsigned int a, b;
+    a = j;
+    b = (j == seg - 1) ? 0 : (j + 1);
+    tri_indices.push_back(Triangle(ring * seg, a, b));
+
+    a = (ring - 1) * seg + j;
+    b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
+    tri_indices.push_back(Triangle(a, ring * seg + 1, b));
+  }
+
+  for (unsigned int i = 0; i < points.size(); ++i) {
+    points[i] = pose.transform(points[i]);
+  }
+
+  model.beginModel();
+  model.addSubModel(points, tri_indices);
+  model.endModel();
+  model.computeLocalAABB();
+}
+
+/// @brief Generate BVH model from sphere
+/// The difference between generateBVHModel is that it gives the number of
+/// triangles faces N for a sphere with unit radius. For sphere of radius r,
+/// then the number of triangles is r * r * N so that the area represented by a
+/// single triangle is approximately the same.s
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
+                      const Transform3s& pose,
+                      unsigned int n_faces_for_unit_sphere) {
+  CoalScalar r = shape.radius;
+  CoalScalar n_low_bound =
+      std::sqrt((CoalScalar)n_faces_for_unit_sphere / CoalScalar(2.)) * r * r;
+  unsigned int ring = (unsigned int)ceil(n_low_bound);
+  unsigned int seg = (unsigned int)ceil(n_low_bound);
+
+  generateBVHModel(model, shape, pose, seg, ring);
+}
+
+/// @brief Generate BVH model from cylinder, given the number of segments along
+/// circle and the number of segments along axis.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
+                      const Transform3s& pose, unsigned int tot,
+                      unsigned int h_num) {
+  std::vector<Vec3s> points;
+  std::vector<Triangle> tri_indices;
+
+  CoalScalar r = shape.radius;
+  CoalScalar h = shape.halfLength;
+  CoalScalar phi, phid;
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
+  phid = pi * 2 / tot;
+  phi = 0;
+
+  CoalScalar hd = 2 * h / h_num;
+
+  for (unsigned int i = 0; i < tot; ++i)
+    points.push_back(
+        Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
+
+  for (unsigned int i = 0; i < h_num - 1; ++i) {
+    for (unsigned int j = 0; j < tot; ++j) {
+      points.push_back(Vec3s(r * cos(phi + phid * j), r * sin(phi + phid * j),
+                             h - (i + 1) * hd));
+    }
+  }
+
+  for (unsigned int i = 0; i < tot; ++i)
+    points.push_back(
+        Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
+
+  points.push_back(Vec3s(0, 0, h));
+  points.push_back(Vec3s(0, 0, -h));
+
+  for (unsigned int i = 0; i < tot; ++i) {
+    Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
+    tri_indices.push_back(tmp);
+  }
+
+  for (unsigned int i = 0; i < tot; ++i) {
+    Triangle tmp((h_num + 1) * tot + 1,
+                 h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
+    tri_indices.push_back(tmp);
+  }
+
+  for (unsigned int i = 0; i < h_num; ++i) {
+    for (unsigned int j = 0; j < tot; ++j) {
+      unsigned int a, b, c, d;
+      a = j;
+      b = (j == tot - 1) ? 0 : (j + 1);
+      c = j + tot;
+      d = (j == tot - 1) ? tot : (j + 1 + tot);
+
+      unsigned int start = i * tot;
+      tri_indices.push_back(Triangle(start + b, start + a, start + c));
+      tri_indices.push_back(Triangle(start + b, start + c, start + d));
+    }
+  }
+
+  for (unsigned int i = 0; i < points.size(); ++i) {
+    points[i] = pose.transform(points[i]);
+  }
+
+  model.beginModel();
+  model.addSubModel(points, tri_indices);
+  model.endModel();
+  model.computeLocalAABB();
+}
+
+/// @brief Generate BVH model from cylinder
+/// Difference from generateBVHModel: is that it gives the circle split number
+/// tot for a cylinder with unit radius. For cylinder with larger radius, the
+/// number of circle split number is r * tot.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
+                      const Transform3s& pose,
+                      unsigned int tot_for_unit_cylinder) {
+  CoalScalar r = shape.radius;
+  CoalScalar h = 2 * shape.halfLength;
+
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
+  unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
+  CoalScalar phid = pi * 2 / tot;
+
+  CoalScalar circle_edge = phid * r;
+  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
+
+  generateBVHModel(model, shape, pose, tot, h_num);
+}
+
+/// @brief Generate BVH model from cone, given the number of segments along
+/// circle and the number of segments along axis.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
+                      const Transform3s& pose, unsigned int tot,
+                      unsigned int h_num) {
+  std::vector<Vec3s> points;
+  std::vector<Triangle> tri_indices;
+
+  CoalScalar r = shape.radius;
+  CoalScalar h = shape.halfLength;
+
+  CoalScalar phi, phid;
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
+  phid = pi * 2 / tot;
+  phi = 0;
+
+  CoalScalar hd = 2 * h / h_num;
+
+  for (unsigned int i = 0; i < h_num - 1; ++i) {
+    CoalScalar h_i = h - (i + 1) * hd;
+    CoalScalar rh = r * (0.5 - h_i / h / 2);
+    for (unsigned int j = 0; j < tot; ++j) {
+      points.push_back(
+          Vec3s(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
+    }
+  }
+
+  for (unsigned int i = 0; i < tot; ++i)
+    points.push_back(
+        Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
+
+  points.push_back(Vec3s(0, 0, h));
+  points.push_back(Vec3s(0, 0, -h));
+
+  for (unsigned int i = 0; i < tot; ++i) {
+    Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
+    tri_indices.push_back(tmp);
+  }
+
+  for (unsigned int i = 0; i < tot; ++i) {
+    Triangle tmp(h_num * tot + 1,
+                 (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
+                 (h_num - 1) * tot + i);
+    tri_indices.push_back(tmp);
+  }
+
+  for (unsigned int i = 0; i < h_num - 1; ++i) {
+    for (unsigned int j = 0; j < tot; ++j) {
+      unsigned int a, b, c, d;
+      a = j;
+      b = (j == tot - 1) ? 0 : (j + 1);
+      c = j + tot;
+      d = (j == tot - 1) ? tot : (j + 1 + tot);
+
+      unsigned int start = i * tot;
+      tri_indices.push_back(Triangle(start + b, start + a, start + c));
+      tri_indices.push_back(Triangle(start + b, start + c, start + d));
+    }
+  }
+
+  for (unsigned int i = 0; i < points.size(); ++i) {
+    points[i] = pose.transform(points[i]);
+  }
+
+  model.beginModel();
+  model.addSubModel(points, tri_indices);
+  model.endModel();
+  model.computeLocalAABB();
+}
+
+/// @brief Generate BVH model from cone
+/// Difference from generateBVHModel: is that it gives the circle split number
+/// tot for a cylinder with unit radius. For cone with larger radius, the number
+/// of circle split number is r * tot.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
+                      const Transform3s& pose, unsigned int tot_for_unit_cone) {
+  CoalScalar r = shape.radius;
+  CoalScalar h = 2 * shape.halfLength;
+
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
+  unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
+  CoalScalar phid = pi * 2 / tot;
+
+  CoalScalar circle_edge = phid * r;
+  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
+
+  generateBVHModel(model, shape, pose, tot, h_num);
+}
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h
new file mode 100644
index 0000000000000000000000000000000000000000..1050f692a6213034c3788587bfcfd6a95e381af5
--- /dev/null
+++ b/include/coal/shape/geometric_shapes.h
@@ -0,0 +1,1059 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_GEOMETRIC_SHAPES_H
+#define COAL_GEOMETRIC_SHAPES_H
+
+#include <vector>
+#include <memory>
+
+#include <boost/math/constants/constants.hpp>
+
+#include "coal/collision_object.h"
+#include "coal/data_types.h"
+
+#ifdef COAL_HAS_QHULL
+namespace orgQhull {
+class Qhull;
+}
+#endif
+
+namespace coal {
+
+/// @brief Base class for all basic geometric shapes
+class COAL_DLLAPI ShapeBase : public CollisionGeometry {
+ public:
+  ShapeBase() {}
+
+  ///  \brief Copy constructor
+  ShapeBase(const ShapeBase& other)
+      : CollisionGeometry(other),
+        m_swept_sphere_radius(other.m_swept_sphere_radius) {}
+
+  ShapeBase& operator=(const ShapeBase& other) = default;
+
+  virtual ~ShapeBase() {};
+
+  /// @brief Get object type: a geometric shape
+  OBJECT_TYPE getObjectType() const { return OT_GEOM; }
+
+  /// @brief Set radius of sphere swept around the shape.
+  /// Must be >= 0.
+  void setSweptSphereRadius(CoalScalar radius) {
+    if (radius < 0) {
+      COAL_THROW_PRETTY("Swept-sphere radius must be positive.",
+                        std::invalid_argument);
+    }
+    this->m_swept_sphere_radius = radius;
+  }
+
+  /// @brief Get radius of sphere swept around the shape.
+  /// This radius is always >= 0.
+  CoalScalar getSweptSphereRadius() const {
+    return this->m_swept_sphere_radius;
+  }
+
+ protected:
+  /// \brief Radius of the sphere swept around the shape.
+  /// Default value is 0.
+  /// Note: this property differs from `inflated` method of certain
+  /// derived classes (e.g. Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder)
+  /// in the sense that inflated returns a new shape which can be inflated but
+  /// also deflated.
+  /// Also, an inflated shape is not rounded. It simply has a different size.
+  /// Sweeping a shape with a sphere is a different operation (a Minkowski sum),
+  /// which rounds the sharp corners of a shape.
+  /// The swept sphere radius is a property of the shape itself and can be
+  /// manually updated between collision checks.
+  CoalScalar m_swept_sphere_radius{0};
+};
+
+/// @defgroup Geometric_Shapes Geometric shapes
+/// Classes of different types of geometric shapes.
+/// @{
+
+/// @brief Triangle stores the points instead of only indices of points
+class COAL_DLLAPI TriangleP : public ShapeBase {
+ public:
+  TriangleP() {};
+
+  TriangleP(const Vec3s& a_, const Vec3s& b_, const Vec3s& c_)
+      : ShapeBase(), a(a_), b(b_), c(c_) {}
+
+  TriangleP(const TriangleP& other)
+      : ShapeBase(other), a(other.a), b(other.b), c(other.c) {}
+
+  /// @brief Clone *this into a new TriangleP
+  virtual TriangleP* clone() const { return new TriangleP(*this); };
+
+  /// @brief virtual function of compute AABB in local coordinate
+  void computeLocalAABB();
+
+  NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
+
+  //  std::pair<ShapeBase*, Transform3s> inflated(const CoalScalar value) const
+  //  {
+  //    if (value == 0) return std::make_pair(new TriangleP(*this),
+  //    Transform3s()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize();
+  //    BC.normalize();
+  //    CA.normalize();
+  //
+  //    Vec3s new_a(a + value * Vec3s(-AB + CA).normalized());
+  //    Vec3s new_b(b + value * Vec3s(-BC + AB).normalized());
+  //    Vec3s new_c(c + value * Vec3s(-CA + BC).normalized());
+  //
+  //    return std::make_pair(new TriangleP(new_a, new_b, new_c),
+  //    Transform3s());
+  //  }
+  //
+  //  CoalScalar minInflationValue() const
+  //  {
+  //    return (std::numeric_limits<CoalScalar>::max)(); // TODO(jcarpent):
+  //    implement
+  //  }
+
+  Vec3s a, b, c;
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const TriangleP* other_ptr = dynamic_cast<const TriangleP*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const TriangleP& other = *other_ptr;
+
+    return a == other.a && b == other.b && c == other.c &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief Center at zero point, axis aligned box
+class COAL_DLLAPI Box : public ShapeBase {
+ public:
+  Box(CoalScalar x, CoalScalar y, CoalScalar z)
+      : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {}
+
+  Box(const Vec3s& side_) : ShapeBase(), halfSide(side_ / 2) {}
+
+  Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {}
+
+  Box& operator=(const Box& other) {
+    if (this == &other) return *this;
+
+    this->halfSide = other.halfSide;
+    return *this;
+  }
+
+  /// @brief Clone *this into a new Box
+  virtual Box* clone() const { return new Box(*this); };
+
+  /// @brief Default constructor
+  Box() {}
+
+  /// @brief box side half-length
+  Vec3s halfSide;
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a box
+  NODE_TYPE getNodeType() const { return GEOM_BOX; }
+
+  CoalScalar computeVolume() const { return 8 * halfSide.prod(); }
+
+  Matrix3s computeMomentofInertia() const {
+    CoalScalar V = computeVolume();
+    Vec3s s(halfSide.cwiseAbs2() * V);
+    return (Vec3s(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
+  }
+
+  CoalScalar minInflationValue() const { return -halfSide.minCoeff(); }
+
+  /// \brief Inflate the box by an amount given by `value`.
+  /// This value can be positive or negative but must always >=
+  /// `minInflationValue()`.
+  ///
+  /// \param[in] value of the shape inflation.
+  ///
+  /// \returns a new inflated box and the related transform to account for the
+  /// change of shape frame
+  std::pair<Box, Transform3s> inflated(const CoalScalar value) const {
+    if (value <= minInflationValue())
+      COAL_THROW_PRETTY("value (" << value << ") "
+                                  << "is two small. It should be at least: "
+                                  << minInflationValue(),
+                        std::invalid_argument);
+    return std::make_pair(Box(2 * (halfSide + Vec3s::Constant(value))),
+                          Transform3s());
+  }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const Box* other_ptr = dynamic_cast<const Box*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const Box& other = *other_ptr;
+
+    return halfSide == other.halfSide &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief Center at zero point sphere
+class COAL_DLLAPI Sphere : public ShapeBase {
+ public:
+  /// @brief Default constructor
+  Sphere() {}
+
+  explicit Sphere(CoalScalar radius_) : ShapeBase(), radius(radius_) {}
+
+  Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {}
+
+  /// @brief Clone *this into a new Sphere
+  virtual Sphere* clone() const { return new Sphere(*this); };
+
+  /// @brief Radius of the sphere
+  CoalScalar radius;
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a sphere
+  NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
+
+  Matrix3s computeMomentofInertia() const {
+    CoalScalar I = 0.4 * radius * radius * computeVolume();
+    return I * Matrix3s::Identity();
+  }
+
+  CoalScalar computeVolume() const {
+    return 4 * boost::math::constants::pi<CoalScalar>() * radius * radius *
+           radius / 3;
+  }
+
+  CoalScalar minInflationValue() const { return -radius; }
+
+  /// \brief Inflate the sphere by an amount given by `value`.
+  /// This value can be positive or negative but must always >=
+  /// `minInflationValue()`.
+  ///
+  /// \param[in] value of the shape inflation.
+  ///
+  /// \returns a new inflated sphere and the related transform to account for
+  /// the change of shape frame
+  std::pair<Sphere, Transform3s> inflated(const CoalScalar value) const {
+    if (value <= minInflationValue())
+      COAL_THROW_PRETTY("value (" << value
+                                  << ") is two small. It should be at least: "
+                                  << minInflationValue(),
+                        std::invalid_argument);
+    return std::make_pair(Sphere(radius + value), Transform3s());
+  }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const Sphere* other_ptr = dynamic_cast<const Sphere*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const Sphere& other = *other_ptr;
+
+    return radius == other.radius &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief Ellipsoid centered at point zero
+class COAL_DLLAPI Ellipsoid : public ShapeBase {
+ public:
+  /// @brief Default constructor
+  Ellipsoid() {}
+
+  Ellipsoid(CoalScalar rx, CoalScalar ry, CoalScalar rz)
+      : ShapeBase(), radii(rx, ry, rz) {}
+
+  explicit Ellipsoid(const Vec3s& radii) : radii(radii) {}
+
+  Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {}
+
+  /// @brief Clone *this into a new Ellipsoid
+  virtual Ellipsoid* clone() const { return new Ellipsoid(*this); };
+
+  /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2
+  /// + z^2/rz^2 = 1)
+  Vec3s radii;
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: an ellipsoid
+  NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; }
+
+  Matrix3s computeMomentofInertia() const {
+    CoalScalar V = computeVolume();
+    CoalScalar a2 = V * radii[0] * radii[0];
+    CoalScalar b2 = V * radii[1] * radii[1];
+    CoalScalar c2 = V * radii[2] * radii[2];
+    return (Matrix3s() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0,
+            0.2 * (a2 + b2))
+        .finished();
+  }
+
+  CoalScalar computeVolume() const {
+    return 4 * boost::math::constants::pi<CoalScalar>() * radii[0] * radii[1] *
+           radii[2] / 3;
+  }
+
+  CoalScalar minInflationValue() const { return -radii.minCoeff(); }
+
+  /// \brief Inflate the ellipsoid by an amount given by `value`.
+  /// This value can be positive or negative but must always >=
+  /// `minInflationValue()`.
+  ///
+  /// \param[in] value of the shape inflation.
+  ///
+  /// \returns a new inflated ellipsoid and the related transform to account for
+  /// the change of shape frame
+  std::pair<Ellipsoid, Transform3s> inflated(const CoalScalar value) const {
+    if (value <= minInflationValue())
+      COAL_THROW_PRETTY("value (" << value
+                                  << ") is two small. It should be at least: "
+                                  << minInflationValue(),
+                        std::invalid_argument);
+    return std::make_pair(Ellipsoid(radii + Vec3s::Constant(value)),
+                          Transform3s());
+  }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const Ellipsoid* other_ptr = dynamic_cast<const Ellipsoid*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const Ellipsoid& other = *other_ptr;
+
+    return radii == other.radii &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief Capsule
+/// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$
+/// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule
+/// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$.
+class COAL_DLLAPI Capsule : public ShapeBase {
+ public:
+  /// @brief Default constructor
+  Capsule() {}
+
+  Capsule(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) {
+    halfLength = lz_ / 2;
+  }
+
+  Capsule(const Capsule& other)
+      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
+
+  /// @brief Clone *this into a new Capsule
+  virtual Capsule* clone() const { return new Capsule(*this); };
+
+  /// @brief Radius of capsule
+  CoalScalar radius;
+
+  /// @brief Half Length along z axis
+  CoalScalar halfLength;
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a capsule
+  NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
+
+  CoalScalar computeVolume() const {
+    return boost::math::constants::pi<CoalScalar>() * radius * radius *
+           ((halfLength * 2) + radius * 4 / 3.0);
+  }
+
+  Matrix3s computeMomentofInertia() const {
+    CoalScalar v_cyl = radius * radius * (halfLength * 2) *
+                       boost::math::constants::pi<CoalScalar>();
+    CoalScalar v_sph = radius * radius * radius *
+                       boost::math::constants::pi<CoalScalar>() * 4 / 3.0;
+
+    CoalScalar h2 = halfLength * halfLength;
+    CoalScalar r2 = radius * radius;
+    CoalScalar ix = v_cyl * (h2 / 3. + r2 / 4.) +
+                    v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
+    CoalScalar iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
+
+    return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
+  }
+
+  CoalScalar minInflationValue() const { return -radius; }
+
+  /// \brief Inflate the capsule by an amount given by `value`.
+  /// This value can be positive or negative but must always >=
+  /// `minInflationValue()`.
+  ///
+  /// \param[in] value of the shape inflation.
+  ///
+  /// \returns a new inflated capsule and the related transform to account for
+  /// the change of shape frame
+  std::pair<Capsule, Transform3s> inflated(const CoalScalar value) const {
+    if (value <= minInflationValue())
+      COAL_THROW_PRETTY("value (" << value
+                                  << ") is two small. It should be at least: "
+                                  << minInflationValue(),
+                        std::invalid_argument);
+    return std::make_pair(Capsule(radius + value, 2 * halfLength),
+                          Transform3s());
+  }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const Capsule* other_ptr = dynamic_cast<const Capsule*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const Capsule& other = *other_ptr;
+
+    return radius == other.radius && halfLength == other.halfLength &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief Cone
+/// The base of the cone is at \f$ z = - halfLength \f$ and the top is at
+/// \f$ z = halfLength \f$.
+class COAL_DLLAPI Cone : public ShapeBase {
+ public:
+  /// @brief Default constructor
+  Cone() {}
+
+  Cone(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) {
+    halfLength = lz_ / 2;
+  }
+
+  Cone(const Cone& other)
+      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
+
+  /// @brief Clone *this into a new Cone
+  virtual Cone* clone() const { return new Cone(*this); };
+
+  /// @brief Radius of the cone
+  CoalScalar radius;
+
+  /// @brief Half Length along z axis
+  CoalScalar halfLength;
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a cone
+  NODE_TYPE getNodeType() const { return GEOM_CONE; }
+
+  CoalScalar computeVolume() const {
+    return boost::math::constants::pi<CoalScalar>() * radius * radius *
+           (halfLength * 2) / 3;
+  }
+
+  Matrix3s computeMomentofInertia() const {
+    CoalScalar V = computeVolume();
+    CoalScalar ix =
+        V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
+    CoalScalar iz = 0.3 * V * radius * radius;
+
+    return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
+  }
+
+  Vec3s computeCOM() const { return Vec3s(0, 0, -0.5 * halfLength); }
+
+  CoalScalar minInflationValue() const {
+    return -(std::min)(radius, halfLength);
+  }
+
+  /// \brief Inflate the cone by an amount given by `value`.
+  /// This value can be positive or negative but must always >=
+  /// `minInflationValue()`.
+  ///
+  /// \param[in] value of the shape inflation.
+  ///
+  /// \returns a new inflated cone and the related transform to account for the
+  /// change of shape frame
+  std::pair<Cone, Transform3s> inflated(const CoalScalar value) const {
+    if (value <= minInflationValue())
+      COAL_THROW_PRETTY("value (" << value
+                                  << ") is two small. It should be at least: "
+                                  << minInflationValue(),
+                        std::invalid_argument);
+
+    // tan(alpha) = 2*halfLength/radius;
+    const CoalScalar tan_alpha = 2 * halfLength / radius;
+    const CoalScalar sin_alpha =
+        tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha);
+    const CoalScalar top_inflation = value / sin_alpha;
+    const CoalScalar bottom_inflation = value;
+
+    const CoalScalar new_lz = 2 * halfLength + top_inflation + bottom_inflation;
+    const CoalScalar new_cz = (top_inflation + bottom_inflation) / 2.;
+    const CoalScalar new_radius = new_lz / tan_alpha;
+
+    return std::make_pair(Cone(new_radius, new_lz),
+                          Transform3s(Vec3s(0., 0., new_cz)));
+  }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const Cone* other_ptr = dynamic_cast<const Cone*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const Cone& other = *other_ptr;
+
+    return radius == other.radius && halfLength == other.halfLength &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief Cylinder along Z axis.
+/// The cylinder is defined at its centroid.
+class COAL_DLLAPI Cylinder : public ShapeBase {
+ public:
+  /// @brief Default constructor
+  Cylinder() {}
+
+  Cylinder(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) {
+    halfLength = lz_ / 2;
+  }
+
+  Cylinder(const Cylinder& other)
+      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
+
+  Cylinder& operator=(const Cylinder& other) {
+    if (this == &other) return *this;
+
+    this->radius = other.radius;
+    this->halfLength = other.halfLength;
+    return *this;
+  }
+
+  /// @brief Clone *this into a new Cylinder
+  virtual Cylinder* clone() const { return new Cylinder(*this); };
+
+  /// @brief Radius of the cylinder
+  CoalScalar radius;
+
+  /// @brief Half Length along z axis
+  CoalScalar halfLength;
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a cylinder
+  NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
+
+  CoalScalar computeVolume() const {
+    return boost::math::constants::pi<CoalScalar>() * radius * radius *
+           (halfLength * 2);
+  }
+
+  Matrix3s computeMomentofInertia() const {
+    CoalScalar V = computeVolume();
+    CoalScalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
+    CoalScalar iz = V * radius * radius / 2;
+    return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
+  }
+
+  CoalScalar minInflationValue() const {
+    return -(std::min)(radius, halfLength);
+  }
+
+  /// \brief Inflate the cylinder by an amount given by `value`.
+  /// This value can be positive or negative but must always >=
+  /// `minInflationValue()`.
+  ///
+  /// \param[in] value of the shape inflation.
+  ///
+  /// \returns a new inflated cylinder and the related transform to account for
+  /// the change of shape frame
+  std::pair<Cylinder, Transform3s> inflated(const CoalScalar value) const {
+    if (value <= minInflationValue())
+      COAL_THROW_PRETTY("value (" << value
+                                  << ") is two small. It should be at least: "
+                                  << minInflationValue(),
+                        std::invalid_argument);
+    return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)),
+                          Transform3s());
+  }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const Cylinder* other_ptr = dynamic_cast<const Cylinder*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const Cylinder& other = *other_ptr;
+
+    return radius == other.radius && halfLength == other.halfLength &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief Base for convex polytope.
+/// @note Inherited classes are responsible for filling ConvexBase::neighbors;
+class COAL_DLLAPI ConvexBase : public ShapeBase {
+ public:
+  /// @brief Build a convex hull based on Qhull library
+  /// and store the vertices and optionally the triangles
+  /// \param points, num_points the points whose convex hull should be computed.
+  /// \param keepTriangles if \c true, returns a Convex<Triangle> object which
+  ///        contains the triangle of the shape.
+  /// \param qhullCommand the command sent to qhull.
+  ///        - if \c keepTriangles is \c true, this parameter should include
+  ///          "Qt". If \c NULL, "Qt" is passed to Qhull.
+  ///        - if \c keepTriangles is \c false, an empty string is passed to
+  ///          Qhull.
+  /// \note Coal must have been compiled with option \c COAL_HAS_QHULL set
+  ///       to \c ON.
+  static ConvexBase* convexHull(std::shared_ptr<std::vector<Vec3s>>& points,
+                                unsigned int num_points, bool keepTriangles,
+                                const char* qhullCommand = NULL);
+
+  // TODO(louis): put this method in private sometime in the future.
+  COAL_DEPRECATED static ConvexBase* convexHull(
+      const Vec3s* points, unsigned int num_points, bool keepTriangles,
+      const char* qhullCommand = NULL);
+
+  virtual ~ConvexBase();
+
+  /// @brief Clone (deep copy).
+  /// This method is consistent with BVHModel `clone` method.
+  /// The copy constructor is called, which duplicates the data.
+  virtual ConvexBase* clone() const { return new ConvexBase(*this); }
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a convex polytope
+  NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
+
+#ifdef COAL_HAS_QHULL
+  /// @brief Builds the double description of the convex polytope, i.e. the set
+  /// of hyperplanes which intersection form the polytope.
+  void buildDoubleDescription();
+#endif
+
+  struct COAL_DLLAPI Neighbors {
+    unsigned char count_;
+    unsigned int* n_;
+
+    unsigned char const& count() const { return count_; }
+    unsigned int& operator[](int i) {
+      assert(i < count_);
+      return n_[i];
+    }
+    unsigned int const& operator[](int i) const {
+      assert(i < count_);
+      return n_[i];
+    }
+
+    bool operator==(const Neighbors& other) const {
+      if (count_ != other.count_) return false;
+
+      for (int i = 0; i < count_; ++i) {
+        if (n_[i] != other.n_[i]) return false;
+      }
+
+      return true;
+    }
+
+    bool operator!=(const Neighbors& other) const { return !(*this == other); }
+  };
+
+  /// @brief Above this threshold, the convex polytope is considered large.
+  /// This influcences the way the support function is computed.
+  static constexpr size_t num_vertices_large_convex_threshold = 32;
+
+  /// @brief An array of the points of the polygon.
+  std::shared_ptr<std::vector<Vec3s>> points;
+  unsigned int num_points;
+
+  /// @brief An array of the normals of the polygon.
+  std::shared_ptr<std::vector<Vec3s>> normals;
+  /// @brief An array of the offsets to the normals of the polygon.
+  /// Note: there are as many offsets as normals.
+  std::shared_ptr<std::vector<double>> offsets;
+  unsigned int num_normals_and_offsets;
+
+  /// @brief Neighbors of each vertex.
+  /// It is an array of size num_points. For each vertex, it contains the number
+  /// of neighbors and a list of indices pointing to them.
+  std::shared_ptr<std::vector<Neighbors>> neighbors;
+
+  /// @brief center of the convex polytope, this is used for collision: center
+  /// is guaranteed in the internal of the polytope (as it is convex)
+  Vec3s center;
+
+  /// @brief The support warm start polytope contains certain points of `this`
+  /// which are support points in specific directions of space.
+  /// This struct is used to warm start the support function computation for
+  /// large meshes (`num_points` > 32).
+  struct SupportWarmStartPolytope {
+    /// @brief Array of support points to warm start the support function
+    /// computation.
+    std::vector<Vec3s> points;
+
+    /// @brief Indices of the support points warm starts.
+    /// These are the indices of the real convex, not the indices of points in
+    /// the warm start polytope.
+    std::vector<int> indices;
+  };
+
+  /// @brief Number of support warm starts.
+  static constexpr size_t num_support_warm_starts = 14;
+
+  /// @brief Support warm start polytopes.
+  SupportWarmStartPolytope support_warm_starts;
+
+ protected:
+  /// @brief Construct an uninitialized convex object
+  /// Initialization is done with ConvexBase::initialize.
+  ConvexBase()
+      : ShapeBase(),
+        num_points(0),
+        num_normals_and_offsets(0),
+        center(Vec3s::Zero()) {}
+
+  /// @brief Initialize the points of the convex shape
+  /// This also initializes the ConvexBase::center.
+  ///
+  /// \param ownStorage weither the ConvexBase owns the data.
+  /// \param points_ list of 3D points  ///
+  /// \param num_points_ number of 3D points
+  void initialize(std::shared_ptr<std::vector<Vec3s>> points_,
+                  unsigned int num_points_);
+
+  /// @brief Set the points of the convex shape.
+  ///
+  /// \param ownStorage weither the ConvexBase owns the data.
+  /// \param points_ list of 3D points  ///
+  /// \param num_points_ number of 3D points
+  void set(std::shared_ptr<std::vector<Vec3s>> points_,
+           unsigned int num_points_);
+
+  /// @brief Copy constructor
+  /// Only the list of neighbors is copied.
+  ConvexBase(const ConvexBase& other);
+
+#ifdef COAL_HAS_QHULL
+  void buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh);
+#endif
+
+  /// @brief Build the support points warm starts.
+  void buildSupportWarmStart();
+
+  /// @brief Array of indices of the neighbors of each vertex.
+  /// Since we don't know a priori the number of neighbors of each vertex, we
+  /// store the indices of the neighbors in a single array.
+  /// The `neighbors` attribute, an array of `Neighbors`, is used to point each
+  /// vertex to the right indices in the `nneighbors_` array.
+  std::shared_ptr<std::vector<unsigned int>> nneighbors_;
+
+ private:
+  void computeCenter();
+
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const ConvexBase* other_ptr = dynamic_cast<const ConvexBase*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const ConvexBase& other = *other_ptr;
+
+    if (num_points != other.num_points) return false;
+
+    if ((!(points.get()) && other.points.get()) ||
+        (points.get() && !(other.points.get())))
+      return false;
+    if (points.get() && other.points.get()) {
+      const std::vector<Vec3s>& points_ = *points;
+      const std::vector<Vec3s>& other_points_ = *(other.points);
+      for (unsigned int i = 0; i < num_points; ++i) {
+        if (points_[i] != (other_points_)[i]) return false;
+      }
+    }
+
+    if ((!(neighbors.get()) && other.neighbors.get()) ||
+        (neighbors.get() && !(other.neighbors.get())))
+      return false;
+    if (neighbors.get() && other.neighbors.get()) {
+      const std::vector<Neighbors>& neighbors_ = *neighbors;
+      const std::vector<Neighbors>& other_neighbors_ = *(other.neighbors);
+      for (unsigned int i = 0; i < num_points; ++i) {
+        if (neighbors_[i] != other_neighbors_[i]) return false;
+      }
+    }
+
+    if ((!(normals.get()) && other.normals.get()) ||
+        (normals.get() && !(other.normals.get())))
+      return false;
+    if (normals.get() && other.normals.get()) {
+      const std::vector<Vec3s>& normals_ = *normals;
+      const std::vector<Vec3s>& other_normals_ = *(other.normals);
+      for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
+        if (normals_[i] != other_normals_[i]) return false;
+      }
+    }
+
+    if ((!(offsets.get()) && other.offsets.get()) ||
+        (offsets.get() && !(other.offsets.get())))
+      return false;
+    if (offsets.get() && other.offsets.get()) {
+      const std::vector<double>& offsets_ = *offsets;
+      const std::vector<double>& other_offsets_ = *(other.offsets);
+      for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
+        if (offsets_[i] != other_offsets_[i]) return false;
+      }
+    }
+
+    if (this->support_warm_starts.points.size() !=
+            other.support_warm_starts.points.size() ||
+        this->support_warm_starts.indices.size() !=
+            other.support_warm_starts.indices.size()) {
+      return false;
+    }
+
+    for (size_t i = 0; i < this->support_warm_starts.points.size(); ++i) {
+      if (this->support_warm_starts.points[i] !=
+              other.support_warm_starts.points[i] ||
+          this->support_warm_starts.indices[i] !=
+              other.support_warm_starts.indices[i]) {
+        return false;
+      }
+    }
+
+    return center == other.center &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template <typename PolygonT>
+class Convex;
+
+/// @brief Half Space: this is equivalent to the Plane in ODE.
+/// A Half space has a priviledged direction: the direction of the normal.
+/// The separation plane is defined as n * x = d; Points in the negative side of
+/// the separation plane (i.e. {x | n * x < d}) are inside the half space and
+/// points in the positive side of the separation plane (i.e. {x | n * x > d})
+/// are outside the half space.
+/// Note: prefer using a Halfspace instead of a Plane if possible, it has better
+/// behavior w.r.t. collision detection algorithms.
+class COAL_DLLAPI Halfspace : public ShapeBase {
+ public:
+  /// @brief Construct a half space with normal direction and offset
+  Halfspace(const Vec3s& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) {
+    unitNormalTest();
+  }
+
+  /// @brief Construct a plane with normal direction and offset
+  Halfspace(CoalScalar a, CoalScalar b, CoalScalar c, CoalScalar d_)
+      : ShapeBase(), n(a, b, c), d(d_) {
+    unitNormalTest();
+  }
+
+  Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {}
+
+  Halfspace(const Halfspace& other)
+      : ShapeBase(other), n(other.n), d(other.d) {}
+
+  /// @brief operator =
+  Halfspace& operator=(const Halfspace& other) {
+    n = other.n;
+    d = other.d;
+    return *this;
+  }
+
+  /// @brief Clone *this into a new Halfspace
+  virtual Halfspace* clone() const { return new Halfspace(*this); };
+
+  CoalScalar signedDistance(const Vec3s& p) const {
+    return n.dot(p) - (d + this->getSweptSphereRadius());
+  }
+
+  CoalScalar distance(const Vec3s& p) const {
+    return std::abs(this->signedDistance(p));
+  }
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a half space
+  NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
+
+  CoalScalar minInflationValue() const {
+    return std::numeric_limits<CoalScalar>::lowest();
+  }
+
+  /// \brief Inflate the halfspace by an amount given by `value`.
+  /// This value can be positive or negative but must always >=
+  /// `minInflationValue()`.
+  ///
+  /// \param[in] value of the shape inflation.
+  ///
+  /// \returns a new inflated halfspace and the related transform to account for
+  /// the change of shape frame
+  std::pair<Halfspace, Transform3s> inflated(const CoalScalar value) const {
+    if (value <= minInflationValue())
+      COAL_THROW_PRETTY("value (" << value
+                                  << ") is two small. It should be at least: "
+                                  << minInflationValue(),
+                        std::invalid_argument);
+    return std::make_pair(Halfspace(n, d + value), Transform3s());
+  }
+
+  /// @brief Plane normal
+  Vec3s n;
+
+  /// @brief Plane offset
+  CoalScalar d;
+
+ protected:
+  /// @brief Turn non-unit normal into unit
+  void unitNormalTest();
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const Halfspace* other_ptr = dynamic_cast<const Halfspace*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const Halfspace& other = *other_ptr;
+
+    return n == other.n && d == other.d &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief Infinite plane.
+/// A plane can be viewed as two half spaces; it has no priviledged direction.
+/// Note: prefer using a Halfspace instead of a Plane if possible, it has better
+/// behavior w.r.t. collision detection algorithms.
+class COAL_DLLAPI Plane : public ShapeBase {
+ public:
+  /// @brief Construct a plane with normal direction and offset
+  Plane(const Vec3s& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) {
+    unitNormalTest();
+  }
+
+  /// @brief Construct a plane with normal direction and offset
+  Plane(CoalScalar a, CoalScalar b, CoalScalar c, CoalScalar d_)
+      : ShapeBase(), n(a, b, c), d(d_) {
+    unitNormalTest();
+  }
+
+  Plane() : ShapeBase(), n(1, 0, 0), d(0) {}
+
+  Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {}
+
+  /// @brief operator =
+  Plane& operator=(const Plane& other) {
+    n = other.n;
+    d = other.d;
+    return *this;
+  }
+
+  /// @brief Clone *this into a new Plane
+  virtual Plane* clone() const { return new Plane(*this); };
+
+  CoalScalar signedDistance(const Vec3s& p) const {
+    const CoalScalar dist = n.dot(p) - d;
+    CoalScalar signed_dist =
+        std::abs(n.dot(p) - d) - this->getSweptSphereRadius();
+    if (dist >= 0) {
+      return signed_dist;
+    }
+    if (signed_dist >= 0) {
+      return -signed_dist;
+    }
+    return signed_dist;
+  }
+
+  CoalScalar distance(const Vec3s& p) const {
+    return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius());
+  }
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a plane
+  NODE_TYPE getNodeType() const { return GEOM_PLANE; }
+
+  /// @brief Plane normal
+  Vec3s n;
+
+  /// @brief Plane offset
+  CoalScalar d;
+
+ protected:
+  /// @brief Turn non-unit normal into unit
+  void unitNormalTest();
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const Plane* other_ptr = dynamic_cast<const Plane*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const Plane& other = *other_ptr;
+
+    return n == other.n && d == other.d &&
+           getSweptSphereRadius() == other.getSweptSphereRadius();
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/shape/geometric_shapes_traits.h b/include/coal/shape/geometric_shapes_traits.h
new file mode 100644
index 0000000000000000000000000000000000000000..fcc769e66a4295fdb87f28887e53d8723d36df59
--- /dev/null
+++ b/include/coal/shape/geometric_shapes_traits.h
@@ -0,0 +1,158 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2015-2022, CNRS, Inria
+ *  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.
+ */
+
+#ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H
+#define COAL_GEOMETRIC_SHAPES_TRAITS_H
+
+#include "coal/shape/geometric_shapes.h"
+
+namespace coal {
+
+struct shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = false,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = false
+  };
+};
+
+template <typename Shape>
+struct shape_traits : shape_traits_base {};
+
+template <>
+struct shape_traits<TriangleP> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = true,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = false
+  };
+};
+
+template <>
+struct shape_traits<Box> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = true,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = false
+  };
+};
+
+template <>
+struct shape_traits<Sphere> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = true,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = true
+  };
+};
+
+template <>
+struct shape_traits<Ellipsoid> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = true,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = true
+  };
+};
+
+template <>
+struct shape_traits<Capsule> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = true,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = false
+  };
+};
+
+template <>
+struct shape_traits<Cone> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = true,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = false
+  };
+};
+
+template <>
+struct shape_traits<Cylinder> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = true,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = false
+  };
+};
+
+template <>
+struct shape_traits<ConvexBase> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = true,
+    IsInflatable = true,
+    HasInflatedSupportFunction = true,
+    IsStrictlyConvex = false
+  };
+};
+
+template <>
+struct shape_traits<Halfspace> : shape_traits_base {
+  enum {
+    NeedNormalizedDir = false,
+    NeedNesterovNormalizeHeuristic = false,
+    IsInflatable = true,
+    HasInflatedSupportFunction = false,
+    IsStrictlyConvex = false
+  };
+};
+
+}  // namespace coal
+
+#endif  // ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H
diff --git a/include/coal/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h
new file mode 100644
index 0000000000000000000000000000000000000000..76ac78ae85ae4a3038d3f6f6923801f7a5ab86a5
--- /dev/null
+++ b/include/coal/shape/geometric_shapes_utility.h
@@ -0,0 +1,261 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, 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 COAL_GEOMETRIC_SHAPES_UTILITY_H
+#define COAL_GEOMETRIC_SHAPES_UTILITY_H
+
+#include <vector>
+#include "coal/shape/geometric_shapes.h"
+#include "coal/BV/BV.h"
+#include "coal/internal/BV_fitter.h"
+
+namespace coal {
+
+/// @cond IGNORE
+namespace details {
+/// @brief get the vertices of some convex shape which can bound the given shape
+/// in a specific configuration
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Box& box,
+                                                const Transform3s& tf);
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Sphere& sphere,
+                                                const Transform3s& tf);
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Ellipsoid& ellipsoid,
+                                                const Transform3s& tf);
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Capsule& capsule,
+                                                const Transform3s& tf);
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Cone& cone,
+                                                const Transform3s& tf);
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Cylinder& cylinder,
+                                                const Transform3s& tf);
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const ConvexBase& convex,
+                                                const Transform3s& tf);
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const TriangleP& triangle,
+                                                const Transform3s& tf);
+}  // namespace details
+/// @endcond
+
+/// @brief calculate a bounding volume for a shape in a specific configuration
+template <typename BV, typename S>
+inline void computeBV(const S& s, const Transform3s& tf, BV& bv) {
+  if (s.getSweptSphereRadius() > 0) {
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
+  }
+  std::vector<Vec3s> convex_bound_vertices = details::getBoundVertices(s, tf);
+  fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(),
+      bv);
+}
+
+template <>
+COAL_DLLAPI void computeBV<AABB, Box>(const Box& s, const Transform3s& tf,
+                                      AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, Sphere>(const Sphere& s, const Transform3s& tf,
+                                         AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, Ellipsoid>(const Ellipsoid& e,
+                                            const Transform3s& tf, AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, Capsule>(const Capsule& s,
+                                          const Transform3s& tf, AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, Cone>(const Cone& s, const Transform3s& tf,
+                                       AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, Cylinder>(const Cylinder& s,
+                                           const Transform3s& tf, AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, ConvexBase>(const ConvexBase& s,
+                                             const Transform3s& tf, AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, TriangleP>(const TriangleP& s,
+                                            const Transform3s& tf, AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, Halfspace>(const Halfspace& s,
+                                            const Transform3s& tf, AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<AABB, Plane>(const Plane& s, const Transform3s& tf,
+                                        AABB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBB, Box>(const Box& s, const Transform3s& tf,
+                                     OBB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBB, Sphere>(const Sphere& s, const Transform3s& tf,
+                                        OBB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBB, Capsule>(const Capsule& s,
+                                         const Transform3s& tf, OBB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBB, Cone>(const Cone& s, const Transform3s& tf,
+                                      OBB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBB, Cylinder>(const Cylinder& s,
+                                          const Transform3s& tf, OBB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBB, ConvexBase>(const ConvexBase& s,
+                                            const Transform3s& tf, OBB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBB, Halfspace>(const Halfspace& s,
+                                           const Transform3s& tf, OBB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<RSS, Halfspace>(const Halfspace& s,
+                                           const Transform3s& tf, RSS& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBBRSS, Halfspace>(const Halfspace& s,
+                                              const Transform3s& tf,
+                                              OBBRSS& bv);
+
+template <>
+COAL_DLLAPI void computeBV<kIOS, Halfspace>(const Halfspace& s,
+                                            const Transform3s& tf, kIOS& bv);
+
+template <>
+COAL_DLLAPI void computeBV<KDOP<16>, Halfspace>(const Halfspace& s,
+                                                const Transform3s& tf,
+                                                KDOP<16>& bv);
+
+template <>
+COAL_DLLAPI void computeBV<KDOP<18>, Halfspace>(const Halfspace& s,
+                                                const Transform3s& tf,
+                                                KDOP<18>& bv);
+
+template <>
+COAL_DLLAPI void computeBV<KDOP<24>, Halfspace>(const Halfspace& s,
+                                                const Transform3s& tf,
+                                                KDOP<24>& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBB, Plane>(const Plane& s, const Transform3s& tf,
+                                       OBB& bv);
+
+template <>
+COAL_DLLAPI void computeBV<RSS, Plane>(const Plane& s, const Transform3s& tf,
+                                       RSS& bv);
+
+template <>
+COAL_DLLAPI void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3s& tf,
+                                          OBBRSS& bv);
+
+template <>
+COAL_DLLAPI void computeBV<kIOS, Plane>(const Plane& s, const Transform3s& tf,
+                                        kIOS& bv);
+
+template <>
+COAL_DLLAPI void computeBV<KDOP<16>, Plane>(const Plane& s,
+                                            const Transform3s& tf,
+                                            KDOP<16>& bv);
+
+template <>
+COAL_DLLAPI void computeBV<KDOP<18>, Plane>(const Plane& s,
+                                            const Transform3s& tf,
+                                            KDOP<18>& bv);
+
+template <>
+COAL_DLLAPI void computeBV<KDOP<24>, Plane>(const Plane& s,
+                                            const Transform3s& tf,
+                                            KDOP<24>& bv);
+
+/// @brief construct a box shape (with a configuration) from a given bounding
+/// volume
+COAL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const AABB& bv, const Transform3s& tf_bv,
+                              Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box,
+                              Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3s& tf_bv,
+                              Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const kIOS& bv, const Transform3s& tf_bv,
+                              Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box,
+                              Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv,
+                              Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv,
+                              Box& box, Transform3s& tf);
+
+COAL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv,
+                              Box& box, Transform3s& tf);
+
+COAL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3s& tf);
+
+COAL_DLLAPI Plane transform(const Plane& a, const Transform3s& tf);
+
+COAL_DLLAPI std::array<Halfspace, 2> transformToHalfspaces(
+    const Plane& a, const Transform3s& tf);
+
+}  // namespace coal
+
+#endif
diff --git a/include/coal/timings.h b/include/coal/timings.h
new file mode 100644
index 0000000000000000000000000000000000000000..c9de3e3ee7ea5ac432a3dd110b6e30048913f628
--- /dev/null
+++ b/include/coal/timings.h
@@ -0,0 +1,112 @@
+//
+// Copyright (c) 2021-2023 INRIA
+//
+
+#ifndef COAL_TIMINGS_FWD_H
+#define COAL_TIMINGS_FWD_H
+
+#include "coal/fwd.hh"
+
+#ifdef COAL_WITH_CXX11_SUPPORT
+#include <chrono>
+#endif
+
+namespace coal {
+
+struct CPUTimes {
+  double wall;
+  double user;
+  double system;
+
+  CPUTimes() : wall(0), user(0), system(0) {}
+
+  void clear() { wall = user = system = 0; }
+};
+
+///
+/// @brief This class mimics the way "boost/timer/timer.hpp" operates while
+/// using the modern std::chrono library.
+///        Importantly, this class will only have an effect for C++11 and more.
+///
+struct COAL_DLLAPI Timer {
+#ifdef COAL_WITH_CXX11_SUPPORT
+  typedef std::chrono::steady_clock clock_type;
+  typedef clock_type::duration duration_type;
+#endif
+
+  /// \brief Default constructor for the timer
+  ///
+  /// \param[in] start_on_construction if true, the timer will be run just after
+  /// the object is created
+  Timer(const bool start_on_construction = true) : m_is_stopped(true) {
+    if (start_on_construction) Timer::start();
+  }
+
+  CPUTimes elapsed() const {
+    if (m_is_stopped) return m_times;
+
+    CPUTimes current(m_times);
+#ifdef COAL_WITH_CXX11_SUPPORT
+    std::chrono::time_point<std::chrono::steady_clock> current_clock =
+        std::chrono::steady_clock::now();
+    current.user += static_cast<double>(
+                        std::chrono::duration_cast<std::chrono::nanoseconds>(
+                            current_clock - m_start)
+                            .count()) *
+                    1e-3;
+#endif
+    return current;
+  }
+
+#ifdef COAL_WITH_CXX11_SUPPORT
+  duration_type duration() const { return (m_end - m_start); }
+#endif
+
+  void start() {
+    if (m_is_stopped) {
+      m_is_stopped = false;
+      m_times.clear();
+
+#ifdef COAL_WITH_CXX11_SUPPORT
+      m_start = std::chrono::steady_clock::now();
+#endif
+    }
+  }
+
+  void stop() {
+    if (m_is_stopped) return;
+    m_is_stopped = true;
+
+#ifdef COAL_WITH_CXX11_SUPPORT
+    m_end = std::chrono::steady_clock::now();
+    m_times.user += static_cast<double>(
+                        std::chrono::duration_cast<std::chrono::nanoseconds>(
+                            m_end - m_start)
+                            .count()) *
+                    1e-3;
+#endif
+  }
+
+  void resume() {
+#ifdef COAL_WITH_CXX11_SUPPORT
+    if (m_is_stopped) {
+      m_start = std::chrono::steady_clock::now();
+      m_is_stopped = false;
+    }
+#endif
+  }
+
+  bool is_stopped() const { return m_is_stopped; }
+
+ protected:
+  CPUTimes m_times;
+  bool m_is_stopped;
+
+#ifdef COAL_WITH_CXX11_SUPPORT
+  std::chrono::time_point<std::chrono::steady_clock> m_start, m_end;
+#endif
+};
+
+}  // namespace coal
+
+#endif  // ifndef COAL_TIMINGS_FWD_H
diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index bdcc8de644e1809d6381ab5893c71bc0c41ad733..c76ec66b013c6c13ac71fd8fc1b92fd99f83840d 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -1,269 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_AABB_H
-#define HPP_FCL_AABB_H
-
-#include "hpp/fcl/data_types.h"
-
-namespace hpp {
-namespace fcl {
-
-struct CollisionRequest;
-class Plane;
-class Halfspace;
-
-/// @defgroup Bounding_Volume Bounding volumes
-/// Classes of different types of bounding volume.
-/// @{
-
-/// @brief A class describing the AABB collision structure, which is a box in 3D
-/// space determined by two diagonal points
-class HPP_FCL_DLLAPI AABB {
- public:
-  /// @brief The min point in the AABB
-  Vec3f min_;
-  /// @brief The max point in the AABB
-  Vec3f max_;
-
-  /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf)
-  AABB();
-
-  /// @brief Creating an AABB at position v with zero size
-  AABB(const Vec3f& v) : min_(v), max_(v) {}
-
-  /// @brief Creating an AABB with two endpoints a and b
-  AABB(const Vec3f& a, const Vec3f& b)
-      : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {}
-
-  /// @brief Creating an AABB centered as core and is of half-dimension delta
-  AABB(const AABB& core, const Vec3f& delta)
-      : min_(core.min_ - delta), max_(core.max_ + delta) {}
-
-  /// @brief Creating an AABB contains three points
-  AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c)
-      : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {}
-
-  AABB(const AABB& other) = default;
-
-  AABB& operator=(const AABB& other) = default;
-
-  AABB& update(const Vec3f& a, const Vec3f& b) {
-    min_ = a.cwiseMin(b);
-    max_ = a.cwiseMax(b);
-    return *this;
-  }
-
-  /// @brief Comparison operator
-  bool operator==(const AABB& other) const {
-    return min_ == other.min_ && max_ == other.max_;
-  }
-
-  bool operator!=(const AABB& other) const { return !(*this == other); }
-
-  /// @name Bounding volume API
-  /// Common API to BVs.
-  /// @{
-
-  /// @brief Check whether the AABB contains a point
-  inline bool contain(const Vec3f& p) const {
-    if (p[0] < min_[0] || p[0] > max_[0]) return false;
-    if (p[1] < min_[1] || p[1] > max_[1]) return false;
-    if (p[2] < min_[2] || p[2] > max_[2]) return false;
-
-    return true;
-  }
-
-  /// @brief Check whether two AABB are overlap
-  inline bool overlap(const AABB& other) const {
-    if (min_[0] > other.max_[0]) return false;
-    if (min_[1] > other.max_[1]) return false;
-    if (min_[2] > other.max_[2]) return false;
-
-    if (max_[0] < other.min_[0]) return false;
-    if (max_[1] < other.min_[1]) return false;
-    if (max_[2] < other.min_[2]) return false;
-
-    return true;
-  }
-
-  /// @brief Check whether AABB overlaps a plane
-  bool overlap(const Plane& p) const;
-
-  /// @brief Check whether AABB overlaps a halfspace
-  bool overlap(const Halfspace& hs) const;
-
-  /// @brief Check whether two AABB are overlap
-  bool overlap(const AABB& other, const CollisionRequest& request,
-               FCL_REAL& sqrDistLowerBound) const;
-
-  /// @brief Distance between two AABBs
-  FCL_REAL distance(const AABB& other) const;
-
-  /// @brief Distance between two AABBs; P and Q, should not be NULL, return the
-  /// nearest points
-  FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
-
-  /// @brief Merge the AABB and a point
-  inline AABB& operator+=(const Vec3f& p) {
-    min_ = min_.cwiseMin(p);
-    max_ = max_.cwiseMax(p);
-    return *this;
-  }
-
-  /// @brief Merge the AABB and another AABB
-  inline AABB& operator+=(const AABB& other) {
-    min_ = min_.cwiseMin(other.min_);
-    max_ = max_.cwiseMax(other.max_);
-    return *this;
-  }
-
-  /// @brief Return the merged AABB of current AABB and the other one
-  inline AABB operator+(const AABB& other) const {
-    AABB res(*this);
-    return res += other;
-  }
-
-  /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
-  inline FCL_REAL size() const { return (max_ - min_).squaredNorm(); }
-
-  /// @brief Center of the AABB
-  inline Vec3f center() const { return (min_ + max_) * 0.5; }
-
-  /// @brief Width of the AABB
-  inline FCL_REAL width() const { return max_[0] - min_[0]; }
-
-  /// @brief Height of the AABB
-  inline FCL_REAL height() const { return max_[1] - min_[1]; }
-
-  /// @brief Depth of the AABB
-  inline FCL_REAL depth() const { return max_[2] - min_[2]; }
-
-  /// @brief Volume of the AABB
-  inline FCL_REAL volume() const { return width() * height() * depth(); }
-
-  /// @}
-
-  /// @brief Check whether the AABB contains another AABB
-  inline bool contain(const AABB& other) const {
-    return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) &&
-           (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) &&
-           (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
-  }
-
-  /// @brief Check whether two AABB are overlap and return the overlap part
-  inline bool overlap(const AABB& other, AABB& overlap_part) const {
-    if (!overlap(other)) {
-      return false;
-    }
-
-    overlap_part.min_ = min_.cwiseMax(other.min_);
-    overlap_part.max_ = max_.cwiseMin(other.max_);
-    return true;
-  }
-
-  /// @brief Check whether two AABB are overlapped along specific axis
-  inline bool axisOverlap(const AABB& other, int axis_id) const {
-    if (min_[axis_id] > other.max_[axis_id]) return false;
-    if (max_[axis_id] < other.min_[axis_id]) return false;
-
-    return true;
-  }
-
-  /// @brief expand the half size of the AABB by delta, and keep the center
-  /// unchanged.
-  inline AABB& expand(const Vec3f& delta) {
-    min_ -= delta;
-    max_ += delta;
-    return *this;
-  }
-
-  /// @brief expand the half size of the AABB by a scalar delta, and keep the
-  /// center unchanged.
-  inline AABB& expand(const FCL_REAL delta) {
-    min_.array() -= delta;
-    max_.array() += delta;
-    return *this;
-  }
-
-  /// @brief expand the aabb by increase the thickness of the plate by a ratio
-  inline AABB& expand(const AABB& core, FCL_REAL ratio) {
-    min_ = min_ * ratio - core.min_;
-    max_ = max_ * ratio - core.max_;
-    return *this;
-  }
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief translate the center of AABB by t
-static inline AABB translate(const AABB& aabb, const Vec3f& t) {
-  AABB res(aabb);
-  res.min_ += t;
-  res.max_ += t;
-  return res;
-}
-
-static inline AABB rotate(const AABB& aabb, const Matrix3f& R) {
-  AABB res(R * aabb.min_);
-  Vec3f corner(aabb.min_);
-  const Eigen::DenseIndex bit[3] = {1, 2, 4};
-  for (Eigen::DenseIndex ic = 1; ic < 8;
-       ++ic) {  // ic = 0 corresponds to aabb.min_. Skip it.
-    for (Eigen::DenseIndex i = 0; i < 3; ++i) {
-      corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i];
-    }
-    res += R * corner;
-  }
-  return res;
-}
-
-/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
-/// and b2 is in identity.
-HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
-                            const AABB& b2);
-
-/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
-/// and b2 is in identity.
-HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
-                            const AABB& b2, const CollisionRequest& request,
-                            FCL_REAL& sqrDistLowerBound);
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BV/AABB.h>
diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h
index b42316ea0fb9d212b3d929ed2fdf4fef2f166040..cda871c3a23f9f4ea3211aacd0da5c65db5cdefd 100644
--- a/include/hpp/fcl/BV/BV.h
+++ b/include/hpp/fcl/BV/BV.h
@@ -1,292 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_BV_H
-#define HPP_FCL_BV_H
-
-#include <hpp/fcl/BV/kDOP.h>
-#include <hpp/fcl/BV/AABB.h>
-#include <hpp/fcl/BV/OBB.h>
-#include <hpp/fcl/BV/RSS.h>
-#include <hpp/fcl/BV/OBBRSS.h>
-#include <hpp/fcl/BV/kIOS.h>
-#include <hpp/fcl/math/transform.h>
-
-/** @brief Main namespace */
-namespace hpp {
-namespace fcl {
-
-/// @cond IGNORE
-namespace details {
-
-/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a
-/// bounding volume of type BV2 in I configuration.
-template <typename BV1, typename BV2>
-struct Converter {
-  static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2);
-  static void convert(const BV1& bv1, BV2& bv2);
-};
-
-/// @brief Convert from AABB to AABB, not very tight but is fast.
-template <>
-struct Converter<AABB, AABB> {
-  static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) {
-    const Vec3f& center = bv1.center();
-    FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
-    const Vec3f center2 = tf1.transform(center);
-    bv2.min_ = center2 - Vec3f::Constant(r);
-    bv2.max_ = center2 + Vec3f::Constant(r);
-  }
-
-  static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; }
-};
-
-template <>
-struct Converter<AABB, OBB> {
-  static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) {
-    bv2.To = tf1.transform(bv1.center());
-    bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
-    bv2.axes = tf1.getRotation();
-  }
-
-  static void convert(const AABB& bv1, OBB& bv2) {
-    bv2.To = bv1.center();
-    bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
-    bv2.axes.setIdentity();
-  }
-};
-
-template <>
-struct Converter<OBB, OBB> {
-  static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2) {
-    bv2.extent = bv1.extent;
-    bv2.To = tf1.transform(bv1.To);
-    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
-  }
-
-  static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; }
-};
-
-template <>
-struct Converter<OBBRSS, OBB> {
-  static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2) {
-    Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
-  }
-
-  static void convert(const OBBRSS& bv1, OBB& bv2) {
-    Converter<OBB, OBB>::convert(bv1.obb, bv2);
-  }
-};
-
-template <>
-struct Converter<RSS, OBB> {
-  static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) {
-    bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius,
-                       bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
-    bv2.To = tf1.transform(bv1.Tr);
-    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
-  }
-
-  static void convert(const RSS& bv1, OBB& bv2) {
-    bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius,
-                       bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
-    bv2.To = bv1.Tr;
-    bv2.axes = bv1.axes;
-  }
-};
-
-template <typename BV1>
-struct Converter<BV1, AABB> {
-  static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) {
-    const Vec3f& center = bv1.center();
-    FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
-    const Vec3f center2 = tf1.transform(center);
-    bv2.min_ = center2 - Vec3f::Constant(r);
-    bv2.max_ = center2 + Vec3f::Constant(r);
-  }
-
-  static void convert(const BV1& bv1, AABB& bv2) {
-    const Vec3f& center = bv1.center();
-    FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
-    bv2.min_ = center - Vec3f::Constant(r);
-    bv2.max_ = center + Vec3f::Constant(r);
-  }
-};
-
-template <typename BV1>
-struct Converter<BV1, OBB> {
-  static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2) {
-    AABB bv;
-    Converter<BV1, AABB>::convert(bv1, bv);
-    Converter<AABB, OBB>::convert(bv, tf1, bv2);
-  }
-
-  static void convert(const BV1& bv1, OBB& bv2) {
-    AABB bv;
-    Converter<BV1, AABB>::convert(bv1, bv);
-    Converter<AABB, OBB>::convert(bv, bv2);
-  }
-};
-
-template <>
-struct Converter<OBB, RSS> {
-  static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2) {
-    bv2.Tr = tf1.transform(bv1.To);
-    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
-
-    bv2.radius = bv1.extent[2];
-    bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
-    bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
-  }
-
-  static void convert(const OBB& bv1, RSS& bv2) {
-    bv2.Tr = bv1.To;
-    bv2.axes = bv1.axes;
-
-    bv2.radius = bv1.extent[2];
-    bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
-    bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
-  }
-};
-
-template <>
-struct Converter<RSS, RSS> {
-  static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2) {
-    bv2.Tr = tf1.transform(bv1.Tr);
-    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
-
-    bv2.radius = bv1.radius;
-    bv2.length[0] = bv1.length[0];
-    bv2.length[1] = bv1.length[1];
-  }
-
-  static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; }
-};
-
-template <>
-struct Converter<OBBRSS, RSS> {
-  static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2) {
-    Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
-  }
-
-  static void convert(const OBBRSS& bv1, RSS& bv2) {
-    Converter<RSS, RSS>::convert(bv1.rss, bv2);
-  }
-};
-
-template <>
-struct Converter<AABB, RSS> {
-  static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2) {
-    bv2.Tr = tf1.transform(bv1.center());
-
-    /// Sort the AABB edges so that AABB extents are ordered.
-    FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth()};
-    Eigen::DenseIndex id[3] = {0, 1, 2};
-
-    for (Eigen::DenseIndex i = 1; i < 3; ++i) {
-      for (Eigen::DenseIndex j = i; j > 0; --j) {
-        if (d[j] > d[j - 1]) {
-          {
-            FCL_REAL tmp = d[j];
-            d[j] = d[j - 1];
-            d[j - 1] = tmp;
-          }
-          {
-            Eigen::DenseIndex tmp = id[j];
-            id[j] = id[j - 1];
-            id[j - 1] = tmp;
-          }
-        }
-      }
-    }
-
-    const Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
-    bv2.radius = extent[id[2]];
-    bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
-    bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
-
-    const Matrix3f& R = tf1.getRotation();
-    const bool left_hand = (id[0] == (id[1] + 1) % 3);
-    if (left_hand)
-      bv2.axes.col(0) = -R.col(id[0]);
-    else
-      bv2.axes.col(0) = R.col(id[0]);
-    bv2.axes.col(1) = R.col(id[1]);
-    bv2.axes.col(2) = R.col(id[2]);
-  }
-
-  static void convert(const AABB& bv1, RSS& bv2) {
-    convert(bv1, Transform3f(), bv2);
-  }
-};
-
-template <>
-struct Converter<AABB, OBBRSS> {
-  static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2) {
-    Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
-    Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
-  }
-
-  static void convert(const AABB& bv1, OBBRSS& bv2) {
-    Converter<AABB, OBB>::convert(bv1, bv2.obb);
-    Converter<AABB, RSS>::convert(bv1, bv2.rss);
-  }
-};
-
-}  // namespace details
-
-/// @endcond
-
-/// @brief Convert a bounding volume of type BV1 in configuration tf1 to
-/// bounding volume of type BV2 in identity configuration.
-template <typename BV1, typename BV2>
-static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2) {
-  details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
-}
-
-/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2
-/// in identity configuration.
-template <typename BV1, typename BV2>
-static inline void convertBV(const BV1& bv1, BV2& bv2) {
-  details::Converter<BV1, BV2>::convert(bv1, bv2);
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BV/BV.h>
diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h
index 300b24ecc87ac55f48a4b4c335f51e4948e1a0a8..50e3d7adcb01459a53cd09c05666d24287413604 100644
--- a/include/hpp/fcl/BV/BV_node.h
+++ b/include/hpp/fcl/BV/BV_node.h
@@ -1,169 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_BV_NODE_H
-#define HPP_FCL_BV_NODE_H
-
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/BV/BV.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @defgroup Construction_Of_BVH Construction of BVHModel
-/// Classes which are used to build a BVHModel (Bounding Volume Hierarchy)
-/// @{
-
-/// @brief BVNodeBase encodes the tree structure for BVH
-struct HPP_FCL_DLLAPI BVNodeBase {
-  /// @brief An index for first child node or primitive
-  /// If the value is positive, it is the index of the first child bv node
-  /// If the value is negative, it is -(primitive index + 1)
-  /// Zero is not used.
-  int first_child;
-
-  /// @brief The start id the primitive belonging to the current node. The index
-  /// is referred to the primitive_indices in BVHModel and from that we can
-  /// obtain the primitive's index in original data indirectly.
-  unsigned int first_primitive;
-
-  /// @brief The number of primitives belonging to the current node
-  unsigned int num_primitives;
-
-  /// @brief Default constructor
-  BVNodeBase()
-      : first_child(0),
-        first_primitive(
-            (std::numeric_limits<unsigned int>::max)())  // value we should help
-                                                         // to raise an issue
-        ,
-        num_primitives(0) {}
-
-  /// @brief Equality operator
-  bool operator==(const BVNodeBase& other) const {
-    return first_child == other.first_child &&
-           first_primitive == other.first_primitive &&
-           num_primitives == other.num_primitives;
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const BVNodeBase& other) const { return !(*this == other); }
-
-  /// @brief Whether current node is a leaf node (i.e. contains a primitive
-  /// index
-  inline bool isLeaf() const { return first_child < 0; }
-
-  /// @brief Return the primitive index. The index is referred to the original
-  /// data (i.e. vertices or tri_indices) in BVHModel
-  inline int primitiveId() const { return -(first_child + 1); }
-
-  /// @brief Return the index of the first child. The index is referred to the
-  /// bounding volume array (i.e. bvs) in BVHModel
-  inline int leftChild() const { return first_child; }
-
-  /// @brief Return the index of the second child. The index is referred to the
-  /// bounding volume array (i.e. bvs) in BVHModel
-  inline int rightChild() const { return first_child + 1; }
-};
-
-/// @brief A class describing a bounding volume node. It includes the tree
-/// structure providing in BVNodeBase and also the geometry data provided in BV
-/// template parameter.
-template <typename BV>
-struct HPP_FCL_DLLAPI BVNode : public BVNodeBase {
-  typedef BVNodeBase Base;
-
-  /// @brief bounding volume storing the geometry
-  BV bv;
-
-  /// @brief Equality operator
-  bool operator==(const BVNode& other) const {
-    return Base::operator==(other) && bv == other.bv;
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const BVNode& other) const { return !(*this == other); }
-
-  /// @brief Check whether two BVNode collide
-  bool overlap(const BVNode& other) const { return bv.overlap(other.bv); }
-  /// @brief Check whether two BVNode collide
-  bool overlap(const BVNode& other, const CollisionRequest& request,
-               FCL_REAL& sqrDistLowerBound) const {
-    return bv.overlap(other.bv, request, sqrDistLowerBound);
-  }
-
-  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
-  /// the underlying BV supports distance, return the nearest points.
-  FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL,
-                    Vec3f* P2 = NULL) const {
-    return bv.distance(other.bv, P1, P2);
-  }
-
-  /// @brief Access to the center of the BV
-  Vec3f getCenter() const { return bv.center(); }
-
-  /// @brief Access to the orientation of the BV
-  const Matrix3f& getOrientation() const {
-    static const Matrix3f id3 = Matrix3f::Identity();
-    return id3;
-  }
-
-  /// \cond
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  /// \endcond
-};
-
-template <>
-inline const Matrix3f& BVNode<OBB>::getOrientation() const {
-  return bv.axes;
-}
-
-template <>
-inline const Matrix3f& BVNode<RSS>::getOrientation() const {
-  return bv.axes;
-}
-
-template <>
-inline const Matrix3f& BVNode<OBBRSS>::getOrientation() const {
-  return bv.obb.axes;
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BV/BV_node.h>
diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h
index a8e11a006817e22b7d83fb58a7225f36216c3e17..003da1750079fac19f8e606b02b81e207442e802 100644
--- a/include/hpp/fcl/BV/OBB.h
+++ b/include/hpp/fcl/BV/OBB.h
@@ -1,153 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_OBB_H
-#define HPP_FCL_OBB_H
-
-#include "hpp/fcl/data_types.h"
-
-namespace hpp {
-namespace fcl {
-
-struct CollisionRequest;
-
-/// @addtogroup Bounding_Volume
-/// @{
-
-/// @brief Oriented bounding box class
-struct HPP_FCL_DLLAPI OBB {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  /// @brief Orientation of OBB. axis[i] is the ith column of the orientation
-  /// matrix for the box; it is also the i-th principle direction of the box. We
-  /// assume that axis[0] corresponds to the axis with the longest box edge,
-  /// axis[1] corresponds to the shorter one and axis[2] corresponds to the
-  /// shortest one.
-  Matrix3f axes;
-
-  /// @brief Center of OBB
-  Vec3f To;
-
-  /// @brief Half dimensions of OBB
-  Vec3f extent;
-
-  OBB() : axes(Matrix3f::Zero()), To(Vec3f::Zero()), extent(Vec3f::Zero()) {}
-
-  /// @brief Equality operator
-  bool operator==(const OBB& other) const {
-    return axes == other.axes && To == other.To && extent == other.extent;
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const OBB& other) const { return !(*this == other); }
-
-  /// @brief Check whether the OBB contains a point.
-  bool contain(const Vec3f& p) const;
-
-  /// Check collision between two OBB
-  /// @return true if collision happens.
-  bool overlap(const OBB& other) const;
-
-  /// Check collision between two OBB
-  /// @return true if collision happens.
-  /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
-  ///         they do not overlap.
-  bool overlap(const OBB& other, const CollisionRequest& request,
-               FCL_REAL& sqrDistLowerBound) const;
-
-  /// @brief Distance between two OBBs, not implemented.
-  FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-
-  /// @brief A simple way to merge the OBB and a point (the result is not
-  /// compact).
-  OBB& operator+=(const Vec3f& p);
-
-  /// @brief Merge the OBB and another OBB (the result is not compact).
-  OBB& operator+=(const OBB& other) {
-    *this = *this + other;
-    return *this;
-  }
-
-  /// @brief Return the merged OBB of current OBB and the other one (the result
-  /// is not compact).
-  OBB operator+(const OBB& other) const;
-
-  /// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
-  inline FCL_REAL size() const { return extent.squaredNorm(); }
-
-  /// @brief Center of the OBB
-  inline const Vec3f& center() const { return To; }
-
-  /// @brief Width of the OBB.
-  inline FCL_REAL width() const { return 2 * extent[0]; }
-
-  /// @brief Height of the OBB.
-  inline FCL_REAL height() const { return 2 * extent[1]; }
-
-  /// @brief Depth of the OBB
-  inline FCL_REAL depth() const { return 2 * extent[2]; }
-
-  /// @brief Volume of the OBB
-  inline FCL_REAL volume() const { return width() * height() * depth(); }
-};
-
-/// @brief Translate the OBB bv
-HPP_FCL_DLLAPI OBB translate(const OBB& bv, const Vec3f& t);
-
-/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
-/// b2 is in identity.
-HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
-                            const OBB& b2);
-
-/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
-/// b2 is in identity.
-HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
-                            const OBB& b2, const CollisionRequest& request,
-                            FCL_REAL& sqrDistLowerBound);
-
-/// Check collision between two boxes
-/// @param B, T orientation and position of first box,
-/// @param a half dimensions of first box,
-/// @param b half dimensions of second box.
-/// The second box is in identity configuration.
-HPP_FCL_DLLAPI bool obbDisjoint(const Matrix3f& B, const Vec3f& T,
-                                const Vec3f& a, const Vec3f& b);
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BV/OBB.h>
diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h
index d224ee1622b483c7b920a4e90416727965fba358..16b21dbda3dfc905674f6a27aa710b33b1be9b44 100644
--- a/include/hpp/fcl/BV/OBBRSS.h
+++ b/include/hpp/fcl/BV/OBBRSS.h
@@ -1,162 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_OBBRSS_H
-#define HPP_FCL_OBBRSS_H
-
-#include "hpp/fcl/BV/OBB.h"
-#include "hpp/fcl/BV/RSS.h"
-
-namespace hpp {
-namespace fcl {
-
-struct CollisionRequest;
-
-/// @addtogroup Bounding_Volume
-/// @{
-
-/// @brief Class merging the OBB and RSS, can handle collision and distance
-/// simultaneously
-struct HPP_FCL_DLLAPI OBBRSS {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  /// @brief OBB member, for rotation
-  OBB obb;
-
-  /// @brief RSS member, for distance
-  RSS rss;
-
-  /// @brief Equality operator
-  bool operator==(const OBBRSS& other) const {
-    return obb == other.obb && rss == other.rss;
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const OBBRSS& other) const { return !(*this == other); }
-
-  /// @brief Check whether the OBBRSS contains a point
-  inline bool contain(const Vec3f& p) const { return obb.contain(p); }
-
-  /// @brief Check collision between two OBBRSS
-  bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); }
-
-  /// Check collision between two OBBRSS
-  /// @retval sqrDistLowerBound squared lower bound on distance between
-  ///         objects if they do not overlap.
-  bool overlap(const OBBRSS& other, const CollisionRequest& request,
-               FCL_REAL& sqrDistLowerBound) const {
-    return obb.overlap(other.obb, request, sqrDistLowerBound);
-  }
-
-  /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the
-  /// nearest points
-  FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL,
-                    Vec3f* Q = NULL) const {
-    return rss.distance(other.rss, P, Q);
-  }
-
-  /// @brief Merge the OBBRSS and a point
-  OBBRSS& operator+=(const Vec3f& p) {
-    obb += p;
-    rss += p;
-    return *this;
-  }
-
-  /// @brief Merge two OBBRSS
-  OBBRSS& operator+=(const OBBRSS& other) {
-    *this = *this + other;
-    return *this;
-  }
-
-  /// @brief Merge two OBBRSS
-  OBBRSS operator+(const OBBRSS& other) const {
-    OBBRSS result;
-    result.obb = obb + other.obb;
-    result.rss = rss + other.rss;
-    return result;
-  }
-
-  /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
-  inline FCL_REAL size() const { return obb.size(); }
-
-  /// @brief Center of the OBBRSS
-  inline const Vec3f& center() const { return obb.center(); }
-
-  /// @brief Width of the OBRSS
-  inline FCL_REAL width() const { return obb.width(); }
-
-  /// @brief Height of the OBBRSS
-  inline FCL_REAL height() const { return obb.height(); }
-
-  /// @brief Depth of the OBBRSS
-  inline FCL_REAL depth() const { return obb.depth(); }
-
-  /// @brief Volume of the OBBRSS
-  inline FCL_REAL volume() const { return obb.volume(); }
-};
-
-/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0)
-/// and b2 is in indentity
-inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
-                    const OBBRSS& b2) {
-  return overlap(R0, T0, b1.obb, b2.obb);
-}
-
-/// Check collision between two OBBRSS
-/// @param  R0, T0 configuration of b1
-/// @param  b1 first OBBRSS in configuration (R0, T0)
-/// @param  b2 second OBBRSS in identity position
-/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do
-/// not overlap.
-inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
-                    const OBBRSS& b2, const CollisionRequest& request,
-                    FCL_REAL& sqrDistLowerBound) {
-  return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
-}
-
-/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0)
-/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points
-inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
-                         const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL) {
-  return distance(R0, T0, b1.rss, b2.rss, P, Q);
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BV/OBBRSS.h>
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index 734e20a7460c55f6842c7a5da0130623ca9638c5..cc6089bab7162ff9839156abea5a9adea40e134f 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -1,176 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_RSS_H
-#define HPP_FCL_RSS_H
-
-#include "hpp/fcl/data_types.h"
-
-#include <boost/math/constants/constants.hpp>
-
-namespace hpp {
-namespace fcl {
-
-struct CollisionRequest;
-
-/// @addtogroup Bounding_Volume
-/// @{
-
-/// @brief A class for rectangle sphere-swept bounding volume
-struct HPP_FCL_DLLAPI RSS {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  /// @brief Orientation of RSS. axis[i] is the ith column of the orientation
-  /// matrix for the RSS; it is also the i-th principle direction of the RSS. We
-  /// assume that axis[0] corresponds to the axis with the longest length,
-  /// axis[1] corresponds to the shorter one and axis[2] corresponds to the
-  /// shortest one.
-  Matrix3f axes;
-
-  /// @brief Origin of the rectangle in RSS
-  Vec3f Tr;
-
-  /// @brief Side lengths of rectangle
-  FCL_REAL length[2];
-
-  /// @brief Radius of sphere summed with rectangle to form RSS
-  FCL_REAL radius;
-
-  ///  @brief Default constructor with default values
-  RSS() : axes(Matrix3f::Zero()), Tr(Vec3f::Zero()), radius(-1) {
-    length[0] = 0;
-    length[1] = 0;
-  }
-
-  /// @brief Equality operator
-  bool operator==(const RSS& other) const {
-    return axes == other.axes && Tr == other.Tr &&
-           length[0] == other.length[0] && length[1] == other.length[1] &&
-           radius == other.radius;
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const RSS& other) const { return !(*this == other); }
-
-  /// @brief Check whether the RSS contains a point
-  bool contain(const Vec3f& p) const;
-
-  /// @brief Check collision between two RSS
-  bool overlap(const RSS& other) const;
-
-  /// Not implemented
-  bool overlap(const RSS& other, const CollisionRequest&,
-               FCL_REAL& sqrDistLowerBound) const {
-    sqrDistLowerBound = sqrt(-1);
-    return overlap(other);
-  }
-
-  /// @brief the distance between two RSS; P and Q, if not NULL, return the
-  /// nearest points
-  FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-
-  /// @brief A simple way to merge the RSS and a point, not compact.
-  /// @todo This function may have some bug.
-  RSS& operator+=(const Vec3f& p);
-
-  /// @brief Merge the RSS and another RSS
-  inline RSS& operator+=(const RSS& other) {
-    *this = *this + other;
-    return *this;
-  }
-
-  /// @brief Return the merged RSS of current RSS and the other one
-  RSS operator+(const RSS& other) const;
-
-  /// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
-  inline FCL_REAL size() const {
-    return (std::sqrt(length[0] * length[0] + length[1] * length[1]) +
-            2 * radius);
-  }
-
-  /// @brief The RSS center
-  inline const Vec3f& center() const { return Tr; }
-
-  /// @brief Width of the RSS
-  inline FCL_REAL width() const { return length[0] + 2 * radius; }
-
-  /// @brief Height of the RSS
-  inline FCL_REAL height() const { return length[1] + 2 * radius; }
-
-  /// @brief Depth of the RSS
-  inline FCL_REAL depth() const { return 2 * radius; }
-
-  /// @brief Volume of the RSS
-  inline FCL_REAL volume() const {
-    return (length[0] * length[1] * 2 * radius +
-            4 * boost::math::constants::pi<FCL_REAL>() * radius * radius *
-                radius);
-  }
-
-  /// @brief Check collision between two RSS and return the overlap part.
-  /// For RSS, we return nothing, as the overlap part of two RSSs usually is not
-  /// a RSS.
-  bool overlap(const RSS& other, RSS& /*overlap_part*/) const {
-    return overlap(other);
-  }
-};
-
-/// @brief distance between two RSS bounding volumes
-/// P and Q (optional return values) are the closest points in the rectangles,
-/// not the RSS. But the direction P - Q is the correct direction for cloest
-/// points Notice that P and Q are both in the local frame of the first RSS (not
-/// global frame and not even the local frame of object 1)
-HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0,
-                                 const RSS& b1, const RSS& b2, Vec3f* P = NULL,
-                                 Vec3f* Q = NULL);
-
-/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
-/// b2 is in identity.
-HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1,
-                            const RSS& b2);
-
-/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
-/// b2 is in identity.
-HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1,
-                            const RSS& b2, const CollisionRequest& request,
-                            FCL_REAL& sqrDistLowerBound);
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BV/RSS.h>
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 879bff1f1a0627e5834bc21ebcacdf95c338bb1d..51a7c44d2b38a50edaddfa04facf6af40173ed58 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -1,193 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_KDOP_H
-#define HPP_FCL_KDOP_H
-
-#include "hpp/fcl/fwd.hh"
-#include "hpp/fcl/data_types.h"
-
-namespace hpp {
-namespace fcl {
-
-struct CollisionRequest;
-
-/// @addtogroup Bounding_Volume
-/// @{
-
-/// @brief KDOP class describes the KDOP collision structures. K is set as the
-/// template parameter, which should be 16, 18, or 24
-///  The KDOP structure is defined by some pairs of parallel planes defined by
-///  some axes.
-/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off
-/// some space of the edges:
-/// (-1,0,0) and (1,0,0)  -> indices 0 and 8
-/// (0,-1,0) and (0,1,0)  -> indices 1 and 9
-/// (0,0,-1) and (0,0,1)  -> indices 2 and 10
-/// (-1,-1,0) and (1,1,0) -> indices 3 and 11
-/// (-1,0,-1) and (1,0,1) -> indices 4 and 12
-/// (0,-1,-1) and (0,1,1) -> indices 5 and 13
-/// (-1,1,0) and (1,-1,0) -> indices 6 and 14
-/// (-1,0,1) and (1,0,-1) -> indices 7 and 15
-/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off
-/// some space of the edges:
-/// (-1,0,0) and (1,0,0)  -> indices 0 and 9
-/// (0,-1,0) and (0,1,0)  -> indices 1 and 10
-/// (0,0,-1) and (0,0,1)  -> indices 2 and 11
-/// (-1,-1,0) and (1,1,0) -> indices 3 and 12
-/// (-1,0,-1) and (1,0,1) -> indices 4 and 13
-/// (0,-1,-1) and (0,1,1) -> indices 5 and 14
-/// (-1,1,0) and (1,-1,0) -> indices 6 and 15
-/// (-1,0,1) and (1,0,-1) -> indices 7 and 16
-/// (0,-1,1) and (0,1,-1) -> indices 8 and 17
-/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off
-/// some space of the edges:
-/// (-1,0,0) and (1,0,0)  -> indices 0 and 12
-/// (0,-1,0) and (0,1,0)  -> indices 1 and 13
-/// (0,0,-1) and (0,0,1)  -> indices 2 and 14
-/// (-1,-1,0) and (1,1,0) -> indices 3 and 15
-/// (-1,0,-1) and (1,0,1) -> indices 4 and 16
-/// (0,-1,-1) and (0,1,1) -> indices 5 and 17
-/// (-1,1,0) and (1,-1,0) -> indices 6 and 18
-/// (-1,0,1) and (1,0,-1) -> indices 7 and 19
-/// (0,-1,1) and (0,1,-1) -> indices 8 and 20
-/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21
-/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22
-/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23
-template <short N>
-class HPP_FCL_DLLAPI KDOP {
- protected:
-  /// @brief Origin's distances to N KDOP planes
-  Eigen::Array<FCL_REAL, N, 1> dist_;
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  /// @brief Creating kDOP containing nothing
-  KDOP();
-
-  /// @brief Creating kDOP containing only one point
-  KDOP(const Vec3f& v);
-
-  /// @brief Creating kDOP containing two points
-  KDOP(const Vec3f& a, const Vec3f& b);
-
-  /// @brief Equality operator
-  bool operator==(const KDOP& other) const {
-    return (dist_ == other.dist_).all();
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const KDOP& other) const {
-    return (dist_ != other.dist_).any();
-  }
-
-  /// @brief Check whether two KDOPs overlap.
-  bool overlap(const KDOP<N>& other) const;
-
-  /// @brief Check whether two KDOPs overlap.
-  /// @return true if collision happens.
-  /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
-  ///         they do not overlap.
-  bool overlap(const KDOP<N>& other, const CollisionRequest& request,
-               FCL_REAL& sqrDistLowerBound) const;
-
-  /// @brief The distance between two KDOP<N>. Not implemented.
-  FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL,
-                    Vec3f* Q = NULL) const;
-
-  /// @brief Merge the point and the KDOP
-  KDOP<N>& operator+=(const Vec3f& p);
-
-  /// @brief Merge two KDOPs
-  KDOP<N>& operator+=(const KDOP<N>& other);
-
-  /// @brief Create a KDOP by mergin two KDOPs
-  KDOP<N> operator+(const KDOP<N>& other) const;
-
-  /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
-  inline FCL_REAL size() const {
-    return width() * width() + height() * height() + depth() * depth();
-  }
-
-  /// @brief The (AABB) center
-  inline Vec3f center() const {
-    return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2;
-  }
-
-  /// @brief The (AABB) width
-  inline FCL_REAL width() const { return dist_[N / 2] - dist_[0]; }
-
-  /// @brief The (AABB) height
-  inline FCL_REAL height() const { return dist_[N / 2 + 1] - dist_[1]; }
-
-  /// @brief The (AABB) depth
-  inline FCL_REAL depth() const { return dist_[N / 2 + 2] - dist_[2]; }
-
-  /// @brief The (AABB) volume
-  inline FCL_REAL volume() const { return width() * height() * depth(); }
-
-  inline FCL_REAL dist(short i) const { return dist_[i]; }
-
-  inline FCL_REAL& dist(short i) { return dist_[i]; }
-
-  //// @brief Check whether one point is inside the KDOP
-  bool inside(const Vec3f& p) const;
-};
-
-template <short N>
-bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
-             const KDOP<N>& /*b2*/) {
-  HPP_FCL_THROW_PRETTY("not implemented", std::logic_error);
-}
-
-template <short N>
-bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
-             const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/,
-             FCL_REAL& /*sqrDistLowerBound*/) {
-  HPP_FCL_THROW_PRETTY("not implemented", std::logic_error);
-}
-
-/// @brief translate the KDOP BV
-template <short N>
-HPP_FCL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BV/kDOP.h>
diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h
index 7234b7740ae48ef073ed200ebde3b25763ea8556..cc24725ddec0840d4650d3849688b0f7dedb6c19 100644
--- a/include/hpp/fcl/BV/kIOS.h
+++ b/include/hpp/fcl/BV/kIOS.h
@@ -1,195 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_KIOS_H
-#define HPP_FCL_KIOS_H
-
-#include "hpp/fcl/BV/OBB.h"
-
-namespace hpp {
-namespace fcl {
-
-struct CollisionRequest;
-
-/// @addtogroup Bounding_Volume
-/// @{
-
-/// @brief A class describing the kIOS collision structure, which is a set of
-/// spheres.
-class HPP_FCL_DLLAPI kIOS {
-  /// @brief One sphere in kIOS
-  struct HPP_FCL_DLLAPI kIOS_Sphere {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    Vec3f o;
-    FCL_REAL r;
-
-    bool operator==(const kIOS_Sphere& other) const {
-      return o == other.o && r == other.r;
-    }
-
-    bool operator!=(const kIOS_Sphere& other) const {
-      return !(*this == other);
-    }
-  };
-
-  /// @brief generate one sphere enclosing two spheres
-  static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0,
-                                   const kIOS_Sphere& s1) {
-    Vec3f d = s1.o - s0.o;
-    FCL_REAL dist2 = d.squaredNorm();
-    FCL_REAL diff_r = s1.r - s0.r;
-
-    /** The sphere with the larger radius encloses the other */
-    if (diff_r * diff_r >= dist2) {
-      if (s1.r > s0.r)
-        return s1;
-      else
-        return s0;
-    } else /** spheres partially overlapping or disjoint */
-    {
-      float dist = (float)std::sqrt(dist2);
-      kIOS_Sphere s;
-      s.r = dist + s0.r + s1.r;
-      if (dist > 0)
-        s.o = s0.o + d * ((s.r - s0.r) / dist);
-      else
-        s.o = s0.o;
-      return s;
-    }
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  /// @brief Equality operator
-  bool operator==(const kIOS& other) const {
-    bool res = obb == other.obb && num_spheres == other.num_spheres;
-    if (!res) return false;
-
-    for (size_t k = 0; k < num_spheres; ++k) {
-      if (spheres[k] != other.spheres[k]) return false;
-    }
-
-    return true;
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const kIOS& other) const { return !(*this == other); }
-
-  static constexpr size_t max_num_spheres = 5;
-
-  /// @brief The (at most) five spheres for intersection
-  kIOS_Sphere spheres[max_num_spheres];
-
-  /// @brief The number of spheres, no larger than 5
-  unsigned int num_spheres;
-
-  /// @ OBB related with kIOS
-  OBB obb;
-
-  /// @brief Check whether the kIOS contains a point
-  bool contain(const Vec3f& p) const;
-
-  /// @brief Check collision between two kIOS
-  bool overlap(const kIOS& other) const;
-
-  /// @brief Check collision between two kIOS
-  bool overlap(const kIOS& other, const CollisionRequest&,
-               FCL_REAL& sqrDistLowerBound) const;
-
-  /// @brief The distance between two kIOS
-  FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-
-  /// @brief A simple way to merge the kIOS and a point
-  kIOS& operator+=(const Vec3f& p);
-
-  /// @brief Merge the kIOS and another kIOS
-  kIOS& operator+=(const kIOS& other) {
-    *this = *this + other;
-    return *this;
-  }
-
-  /// @brief Return the merged kIOS of current kIOS and the other one
-  kIOS operator+(const kIOS& other) const;
-
-  /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
-  FCL_REAL size() const;
-
-  /// @brief Center of the kIOS
-  const Vec3f& center() const { return spheres[0].o; }
-
-  /// @brief Width of the kIOS
-  FCL_REAL width() const;
-
-  /// @brief Height of the kIOS
-  FCL_REAL height() const;
-
-  /// @brief Depth of the kIOS
-  FCL_REAL depth() const;
-
-  /// @brief Volume of the kIOS
-  FCL_REAL volume() const;
-};
-
-/// @brief Translate the kIOS BV
-HPP_FCL_DLLAPI kIOS translate(const kIOS& bv, const Vec3f& t);
-
-/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
-/// and b2 is in identity.
-/// @todo Not efficient
-HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
-                            const kIOS& b2);
-
-/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
-/// and b2 is in identity.
-/// @todo Not efficient
-HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
-                            const kIOS& b2, const CollisionRequest& request,
-                            FCL_REAL& sqrDistLowerBound);
-
-/// @brief Approximate distance between two kIOS bounding volumes
-/// @todo P and Q is not returned, need implementation
-HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0,
-                                 const kIOS& b1, const kIOS& b2,
-                                 Vec3f* P = NULL, Vec3f* Q = NULL);
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BV/kIOS.h>
diff --git a/include/hpp/fcl/BVH/BVH_front.h b/include/hpp/fcl/BVH/BVH_front.h
index 9a3a99eea1d436ef37d9b3714eeeb21d3774f0be..0d567079d2226a1428a5e3b9ddcd6649a193a81e 100644
--- a/include/hpp/fcl/BVH/BVH_front.h
+++ b/include/hpp/fcl/BVH/BVH_front.h
@@ -1,79 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_BVH_FRONT_H
-#define HPP_FCL_BVH_FRONT_H
-
-#include <list>
-
-#include <hpp/fcl/config.hh>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Front list acceleration for collision
-/// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where
-/// the traversal terminates while performing a query during a given time
-/// instance. The front list reflects the subset of a BVTT that is traversed for
-/// that particular proximity query.
-struct HPP_FCL_DLLAPI BVHFrontNode {
-  /// @brief The nodes to start in the future, i.e. the wave front of the
-  /// traversal tree.
-  unsigned int left, right;
-
-  /// @brief The front node is not valid when collision is detected on the front
-  /// node.
-  bool valid;
-
-  BVHFrontNode(unsigned int left_, unsigned int right_)
-      : left(left_), right(right_), valid(true) {}
-};
-
-/// @brief BVH front list is a list of front nodes.
-typedef std::list<BVHFrontNode> BVHFrontList;
-
-/// @brief Add new front node into the front list
-inline void updateFrontList(BVHFrontList* front_list, unsigned int b1,
-                            unsigned int b2) {
-  if (front_list) front_list->push_back(BVHFrontNode(b1, b2));
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BVH/BVH_front.h>
diff --git a/include/hpp/fcl/BVH/BVH_internal.h b/include/hpp/fcl/BVH/BVH_internal.h
index 407a430cab154dc3ee4023b40864cce6078dd4c0..7b24ec33a5bf0bec8d8c732ee8aa3f39f1255dd2 100644
--- a/include/hpp/fcl/BVH/BVH_internal.h
+++ b/include/hpp/fcl/BVH/BVH_internal.h
@@ -1,90 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_BVH_INTERNAL_H
-#define HPP_FCL_BVH_INTERNAL_H
-
-#include <hpp/fcl/data_types.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief States for BVH construction
-/// empty->begun->processed ->replace_begun->processed -> ......
-///                        |
-///                        |-> update_begun -> updated -> .....
-enum BVHBuildState {
-  BVH_BUILD_STATE_EMPTY,      /// empty state, immediately after constructor
-  BVH_BUILD_STATE_BEGUN,      /// after beginModel(), state for adding geometry
-                              /// primitives
-  BVH_BUILD_STATE_PROCESSED,  /// after tree has been build, ready for cd use
-  BVH_BUILD_STATE_UPDATE_BEGUN,  /// after beginUpdateModel(), state for
-                                 /// updating geometry primitives
-  BVH_BUILD_STATE_UPDATED,  /// after tree has been build for updated geometry,
-                            /// ready for ccd use
-  BVH_BUILD_STATE_REPLACE_BEGUN  /// after beginReplaceModel(), state for
-                                 /// replacing geometry primitives
-};
-
-/// @brief Error code for BVH
-enum BVHReturnCode {
-  BVH_OK = 0,  /// BVH is valid
-  BVH_ERR_MODEL_OUT_OF_MEMORY =
-      -1,  /// Cannot allocate memory for vertices and triangles
-  BVH_ERR_BUILD_OUT_OF_SEQUENCE =
-      -2,  /// BVH construction does not follow correct sequence
-  BVH_ERR_BUILD_EMPTY_MODEL = -3,  /// BVH geometry is not prepared
-  BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME =
-      -4,  /// BVH geometry in previous frame is not prepared
-  BVH_ERR_UNSUPPORTED_FUNCTION = -5,  /// BVH funtion is not supported
-  BVH_ERR_UNUPDATED_MODEL = -6,       /// BVH model update failed
-  BVH_ERR_INCORRECT_DATA = -7,        /// BVH data is not valid
-  BVH_ERR_UNKNOWN = -8                /// Unknown failure
-};
-
-/// @brief BVH model type
-enum BVHModelType {
-  BVH_MODEL_UNKNOWN,    /// @brief unknown model type
-  BVH_MODEL_TRIANGLES,  /// @brief triangle model
-  BVH_MODEL_POINTCLOUD  /// @brief point cloud model
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BVH/BVH_internal.h>
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index 34186d0a642e9328c0a2e334f3b38ea7a54071bb..a069ea961003cdf0e0f256d76c8aec67b2903248 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -1,542 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2020-2022, INRIA
- *  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 HPP_FCL_BVH_MODEL_H
-#define HPP_FCL_BVH_MODEL_H
-
-#include "hpp/fcl/fwd.hh"
-#include "hpp/fcl/collision_object.h"
-#include "hpp/fcl/BVH/BVH_internal.h"
-#include "hpp/fcl/BV/BV_node.h"
-
-#include <vector>
-#include <memory>
-#include <iostream>
-
-namespace hpp {
-namespace fcl {
-
-/// @addtogroup Construction_Of_BVH
-/// @{
-
-class ConvexBase;
-
-template <typename BV>
-class BVFitter;
-template <typename BV>
-class BVSplitter;
-
-/// @brief A base class describing the bounding hierarchy of a mesh model or a
-/// point cloud model (which is viewed as a degraded version of mesh)
-class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
- public:
-  /// @brief Geometry point data
-  std::shared_ptr<std::vector<Vec3f>> vertices;
-
-  /// @brief Geometry triangle index data, will be NULL for point clouds
-  std::shared_ptr<std::vector<Triangle>> tri_indices;
-
-  /// @brief Geometry point data in previous frame
-  std::shared_ptr<std::vector<Vec3f>> prev_vertices;
-
-  /// @brief Number of triangles
-  unsigned int num_tris;
-
-  /// @brief Number of points
-  unsigned int num_vertices;
-
-  /// @brief The state of BVH building process
-  BVHBuildState build_state;
-
-  /// @brief Convex<Triangle> representation of this object
-  shared_ptr<ConvexBase> convex;
-
-  /// @brief Model type described by the instance
-  BVHModelType getModelType() const {
-    if (num_tris && num_vertices)
-      return BVH_MODEL_TRIANGLES;
-    else if (num_vertices)
-      return BVH_MODEL_POINTCLOUD;
-    else
-      return BVH_MODEL_UNKNOWN;
-  }
-
-  /// @brief Constructing an empty BVH
-  BVHModelBase();
-
-  /// @brief copy from another BVH
-  BVHModelBase(const BVHModelBase& other);
-
-  /// @brief deconstruction, delete mesh data related.
-  virtual ~BVHModelBase() {}
-
-  /// @brief Get the object type: it is a BVH
-  OBJECT_TYPE getObjectType() const { return OT_BVH; }
-
-  /// @brief Compute the AABB for the BVH, used for broad-phase collision
-  void computeLocalAABB();
-
-  /// @brief Begin a new BVH model
-  int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
-
-  /// @brief Add one point in the new BVH model
-  int addVertex(const Vec3f& p);
-
-  /// @brief Add points in the new BVH model
-  int addVertices(const Matrixx3f& points);
-
-  /// @brief Add triangles in the new BVH model
-  int addTriangles(const Matrixx3i& triangles);
-
-  /// @brief Add one triangle in the new BVH model
-  int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
-
-  /// @brief Add a set of triangles in the new BVH model
-  int addSubModel(const std::vector<Vec3f>& ps,
-                  const std::vector<Triangle>& ts);
-
-  /// @brief Add a set of points in the new BVH model
-  int addSubModel(const std::vector<Vec3f>& ps);
-
-  /// @brief End BVH model construction, will build the bounding volume
-  /// hierarchy
-  int endModel();
-
-  /// @brief Replace the geometry information of current frame (i.e. should have
-  /// the same mesh topology with the previous frame)
-  int beginReplaceModel();
-
-  /// @brief Replace one point in the old BVH model
-  int replaceVertex(const Vec3f& p);
-
-  /// @brief Replace one triangle in the old BVH model
-  int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
-
-  /// @brief Replace a set of points in the old BVH model
-  int replaceSubModel(const std::vector<Vec3f>& ps);
-
-  /// @brief End BVH model replacement, will also refit or rebuild the bounding
-  /// volume hierarchy
-  int endReplaceModel(bool refit = true, bool bottomup = true);
-
-  /// @brief Replace the geometry information of current frame (i.e. should have
-  /// the same mesh topology with the previous frame). The current frame will be
-  /// saved as the previous frame in prev_vertices.
-  int beginUpdateModel();
-
-  /// @brief Update one point in the old BVH model
-  int updateVertex(const Vec3f& p);
-
-  /// @brief Update one triangle in the old BVH model
-  int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
-
-  /// @brief Update a set of points in the old BVH model
-  int updateSubModel(const std::vector<Vec3f>& ps);
-
-  /// @brief End BVH model update, will also refit or rebuild the bounding
-  /// volume hierarchy
-  int endUpdateModel(bool refit = true, bool bottomup = true);
-
-  /// @brief Build this \ref Convex "Convex<Triangle>" representation of this
-  /// model. The result is stored in attribute \ref convex. \note this only
-  /// takes the points of this model. It does not check that the
-  ///       object is convex. It does not compute a convex hull.
-  void buildConvexRepresentation(bool share_memory);
-
-  /// @brief Build a convex hull
-  /// and store it in attribute \ref convex.
-  /// \param keepTriangle whether the convex should be triangulated.
-  /// \param qhullCommand see \ref ConvexBase::convexHull.
-  /// \return \c true if this object is convex, hence the convex hull represents
-  ///         the same object.
-  /// \sa ConvexBase::convexHull
-  /// \warning At the moment, the return value only checks whether there are as
-  ///          many points in the convex hull as in the original object. This is
-  ///          neither necessary (duplicated vertices get merged) nor sufficient
-  ///          (think of a U with 4 vertices and 3 edges).
-  bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
-
-  virtual int memUsage(const bool msg = false) const = 0;
-
-  /// @brief This is a special acceleration: BVH_model default stores the BV's
-  /// transform in world coordinate. However, we can also store each BV's
-  /// transform related to its parent BV node. When traversing the BVH, this can
-  /// save one matrix transformation.
-  virtual void makeParentRelative() = 0;
-
-  Vec3f computeCOM() const {
-    FCL_REAL vol = 0;
-    Vec3f com(0, 0, 0);
-    if (!(vertices.get())) {
-      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
-                   "vertices."
-                << std::endl;
-      return com;
-    }
-    const std::vector<Vec3f>& vertices_ = *vertices;
-    if (!(tri_indices.get())) {
-      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
-                   "triangles."
-                << std::endl;
-      return com;
-    }
-    const std::vector<Triangle>& tri_indices_ = *tri_indices;
-
-    for (unsigned int i = 0; i < num_tris; ++i) {
-      const Triangle& tri = tri_indices_[i];
-      FCL_REAL d_six_vol =
-          (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
-      vol += d_six_vol;
-      com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) *
-             d_six_vol;
-    }
-
-    return com / (vol * 4);
-  }
-
-  FCL_REAL computeVolume() const {
-    FCL_REAL vol = 0;
-    if (!(vertices.get())) {
-      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
-                   "vertices."
-                << std::endl;
-      return vol;
-    }
-    const std::vector<Vec3f>& vertices_ = *vertices;
-    if (!(tri_indices.get())) {
-      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
-                   "triangles."
-                << std::endl;
-      return vol;
-    }
-    const std::vector<Triangle>& tri_indices_ = *tri_indices;
-    for (unsigned int i = 0; i < num_tris; ++i) {
-      const Triangle& tri = tri_indices_[i];
-      FCL_REAL d_six_vol =
-          (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
-      vol += d_six_vol;
-    }
-
-    return vol / 6;
-  }
-
-  Matrix3f computeMomentofInertia() const {
-    Matrix3f C = Matrix3f::Zero();
-
-    Matrix3f C_canonical;
-    C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0,
-        1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0;
-
-    if (!(vertices.get())) {
-      std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
-                   "not contain vertices."
-                << std::endl;
-      return C;
-    }
-    const std::vector<Vec3f>& vertices_ = *vertices;
-    if (!(vertices.get())) {
-      std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
-                   "not contain vertices."
-                << std::endl;
-      return C;
-    }
-    const std::vector<Triangle>& tri_indices_ = *tri_indices;
-    for (unsigned int i = 0; i < num_tris; ++i) {
-      const Triangle& tri = tri_indices_[i];
-      const Vec3f& v1 = vertices_[tri[0]];
-      const Vec3f& v2 = vertices_[tri[1]];
-      const Vec3f& v3 = vertices_[tri[2]];
-      Matrix3f A;
-      A << v1.transpose(), v2.transpose(), v3.transpose();
-      C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
-    }
-
-    return C.trace() * Matrix3f::Identity() - C;
-  }
-
- protected:
-  virtual void deleteBVs() = 0;
-  virtual bool allocateBVs() = 0;
-
-  /// @brief Build the bounding volume hierarchy
-  virtual int buildTree() = 0;
-
-  /// @brief Refit the bounding volume hierarchy
-  virtual int refitTree(bool bottomup) = 0;
-
-  unsigned int num_tris_allocated;
-  unsigned int num_vertices_allocated;
-  unsigned int num_vertex_updated;  /// for ccd vertex update
-
- protected:
-  /// \brief Comparison operators
-  virtual bool isEqual(const CollisionGeometry& other) const;
-};
-
-/// @brief A class describing the bounding hierarchy of a mesh model or a point
-/// cloud model (which is viewed as a degraded version of mesh) \tparam BV one
-/// of the bounding volume class in \ref Bounding_Volume.
-template <typename BV>
-class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
-  typedef BVHModelBase Base;
-
- public:
-  using bv_node_vector_t =
-      std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;
-
-  /// @brief Split rule to split one BV node into two children
-  shared_ptr<BVSplitter<BV>> bv_splitter;
-
-  /// @brief Fitting rule to fit a BV node to a set of geometry primitives
-  shared_ptr<BVFitter<BV>> bv_fitter;
-
-  /// @brief Default constructor to build an empty BVH
-  BVHModel();
-
-  /// @brief Copy constructor from another BVH
-  ///
-  /// \param[in] other BVHModel to copy.
-  ///
-  BVHModel(const BVHModel& other);
-
-  /// @brief Clone *this into a new BVHModel
-  virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
-
-  /// @brief deconstruction, delete mesh data related.
-  ~BVHModel() {}
-
-  /// @brief We provide getBV() and getNumBVs() because BVH may be compressed
-  /// (in future), so we must provide some flexibility here
-
-  /// @brief Access the bv giving the its index
-  const BVNode<BV>& getBV(unsigned int i) const {
-    assert(i < num_bvs);
-    return (*bvs)[i];
-  }
-
-  /// @brief Access the bv giving the its index
-  BVNode<BV>& getBV(unsigned int i) {
-    assert(i < num_bvs);
-    return (*bvs)[i];
-  }
-
-  /// @brief Get the number of bv in the BVH
-  unsigned int getNumBVs() const { return num_bvs; }
-
-  /// @brief Get the BV type: default is unknown
-  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
-
-  /// @brief Check the number of memory used
-  int memUsage(const bool msg) const;
-
-  /// @brief This is a special acceleration: BVH_model default stores the BV's
-  /// transform in world coordinate. However, we can also store each BV's
-  /// transform related to its parent BV node. When traversing the BVH, this can
-  /// save one matrix transformation.
-  void makeParentRelative() {
-    Matrix3f I(Matrix3f::Identity());
-    makeParentRelativeRecurse(0, I, Vec3f::Zero());
-  }
-
- protected:
-  void deleteBVs();
-  bool allocateBVs();
-
-  unsigned int num_bvs_allocated;
-  std::shared_ptr<std::vector<unsigned int>> primitive_indices;
-
-  /// @brief Bounding volume hierarchy
-  std::shared_ptr<bv_node_vector_t> bvs;
-
-  /// @brief Number of BV nodes in bounding volume hierarchy
-  unsigned int num_bvs;
-
-  /// @brief Build the bounding volume hierarchy
-  int buildTree();
-
-  /// @brief Refit the bounding volume hierarchy
-  int refitTree(bool bottomup);
-
-  /// @brief Refit the bounding volume hierarchy in a top-down way (slow but
-  /// more compact)
-  int refitTree_topdown();
-
-  /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but
-  /// less compact)
-  int refitTree_bottomup();
-
-  /// @brief Recursive kernel for hierarchy construction
-  int recursiveBuildTree(int bv_id, unsigned int first_primitive,
-                         unsigned int num_primitives);
-
-  /// @brief Recursive kernel for bottomup refitting
-  int recursiveRefitTree_bottomup(int bv_id);
-
-  /// @ recursively compute each bv's transform related to its parent. For
-  /// default BV, only the translation works. For oriented BV (OBB, RSS,
-  /// OBBRSS), special implementation is provided.
-  void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
-                                 const Vec3f& parent_c) {
-    bv_node_vector_t& bvs_ = *bvs;
-    if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
-      makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
-                                parent_axes,
-                                bvs_[static_cast<size_t>(bv_id)].getCenter());
-
-      makeParentRelativeRecurse(
-          bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
-          bvs_[static_cast<size_t>(bv_id)].getCenter());
-    }
-
-    bvs_[static_cast<size_t>(bv_id)].bv =
-        translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c);
-  }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const BVHModel& other = *other_ptr;
-
-    bool res = Base::isEqual(other);
-    if (!res) return false;
-
-    // unsigned int other_num_primitives = 0;
-    // if(other.primitive_indices)
-    // {
-
-    //   switch(other.getModelType())
-    //   {
-    //     case BVH_MODEL_TRIANGLES:
-    //       other_num_primitives = num_tris;
-    //       break;
-    //     case BVH_MODEL_POINTCLOUD:
-    //       other_num_primitives = num_vertices;
-    //       break;
-    //     default:
-    //       ;
-    //   }
-    // }
-
-    //    unsigned int num_primitives = 0;
-    //    if(primitive_indices)
-    //    {
-    //
-    //      switch(other.getModelType())
-    //      {
-    //        case BVH_MODEL_TRIANGLES:
-    //          num_primitives = num_tris;
-    //          break;
-    //        case BVH_MODEL_POINTCLOUD:
-    //          num_primitives = num_vertices;
-    //          break;
-    //        default:
-    //          ;
-    //      }
-    //    }
-    //
-    //    if(num_primitives != other_num_primitives)
-    //      return false;
-    //
-    //    for(int k = 0; k < num_primitives; ++k)
-    //    {
-    //      if(primitive_indices[k] != other.primitive_indices[k])
-    //        return false;
-    //    }
-
-    if (num_bvs != other.num_bvs) return false;
-
-    if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
-      return false;
-    if (bvs.get() && other.bvs.get()) {
-      const bv_node_vector_t& bvs_ = *bvs;
-      const bv_node_vector_t& other_bvs_ = *(other.bvs);
-      for (unsigned int k = 0; k < num_bvs; ++k) {
-        if (bvs_[k] != other_bvs_[k]) return false;
-      }
-    }
-
-    return true;
-  }
-};
-
-/// @}
-
-template <>
-void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
-                                              const Vec3f& parent_c);
-
-template <>
-void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
-                                              const Vec3f& parent_c);
-
-template <>
-void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
-                                                 Matrix3f& parent_axes,
-                                                 const Vec3f& parent_c);
-
-/// @brief Specialization of getNodeType() for BVHModel with different BV types
-template <>
-NODE_TYPE BVHModel<AABB>::getNodeType() const;
-
-template <>
-NODE_TYPE BVHModel<OBB>::getNodeType() const;
-
-template <>
-NODE_TYPE BVHModel<RSS>::getNodeType() const;
-
-template <>
-NODE_TYPE BVHModel<kIOS>::getNodeType() const;
-
-template <>
-NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
-
-template <>
-NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const;
-
-template <>
-NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const;
-
-template <>
-NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const;
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BVH/BVH_model.h>
diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/hpp/fcl/BVH/BVH_utility.h
index 508af2fec83408ba33d958a5b223c49eadb8223a..275a171e10ef2d8c0cb18e4db212c745b6683b6a 100644
--- a/include/hpp/fcl/BVH/BVH_utility.h
+++ b/include/hpp/fcl/BVH/BVH_utility.h
@@ -1,119 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_BVH_UTILITY_H
-#define HPP_FCL_BVH_UTILITY_H
-
-#include <hpp/fcl/BVH/BVH_model.h>
-
-namespace hpp {
-namespace fcl {
-/// @brief Extract the part of the BVHModel that is inside an AABB.
-/// A triangle in collision with the AABB is considered inside.
-template <typename BV>
-HPP_FCL_DLLAPI BVHModel<BV>* BVHExtract(const BVHModel<BV>& model,
-                                        const Transform3f& pose,
-                                        const AABB& aabb);
-
-template <>
-HPP_FCL_DLLAPI BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model,
-                                         const Transform3f& pose,
-                                         const AABB& aabb);
-template <>
-HPP_FCL_DLLAPI BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model,
-                                          const Transform3f& pose,
-                                          const AABB& aabb);
-template <>
-HPP_FCL_DLLAPI BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model,
-                                         const Transform3f& pose,
-                                         const AABB& aabb);
-template <>
-HPP_FCL_DLLAPI BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model,
-                                          const Transform3f& pose,
-                                          const AABB& aabb);
-template <>
-HPP_FCL_DLLAPI BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model,
-                                            const Transform3f& pose,
-                                            const AABB& aabb);
-template <>
-HPP_FCL_DLLAPI BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model,
-                                               const Transform3f& pose,
-                                               const AABB& aabb);
-template <>
-HPP_FCL_DLLAPI BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model,
-                                               const Transform3f& pose,
-                                               const AABB& aabb);
-template <>
-HPP_FCL_DLLAPI BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
-                                               const Transform3f& pose,
-                                               const AABB& aabb);
-
-/// @brief Compute the covariance matrix for a set or subset of points. if ts =
-/// null, then indices refer to points directly; otherwise refer to triangles
-HPP_FCL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts,
-                                  unsigned int* indices, unsigned int n,
-                                  Matrix3f& M);
-
-/// @brief Compute the RSS bounding volume parameters: radius, rectangle size
-/// and the origin, given the BV axises.
-HPP_FCL_DLLAPI void getRadiusAndOriginAndRectangleSize(
-    Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n,
-    const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
-
-/// @brief Compute the bounding volume extent and center for a set or subset of
-/// points, given the BV axises.
-HPP_FCL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts,
-                                       unsigned int* indices, unsigned int n,
-                                       Matrix3f& axes, Vec3f& center,
-                                       Vec3f& extent);
-
-/// @brief Compute the center and radius for a triangle's circumcircle
-HPP_FCL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b,
-                                            const Vec3f& c, Vec3f& center,
-                                            FCL_REAL& radius);
-
-/// @brief Compute the maximum distance from a given center point to a point
-/// cloud
-HPP_FCL_DLLAPI FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts,
-                                        unsigned int* indices, unsigned int n,
-                                        const Vec3f& query);
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/BVH/BVH_utility.h>
diff --git a/include/hpp/fcl/broadphase/broadphase.h b/include/hpp/fcl/broadphase/broadphase.h
index 762792de3c0c257fd2bc778f9cae127d29fc02e2..7834fbe4c98b6d3fa53773ad8eb316c096f190f1 100644
--- a/include/hpp/fcl/broadphase/broadphase.h
+++ b/include/hpp/fcl/broadphase/broadphase.h
@@ -1,48 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2022, Inria
- *  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.
- */
-
-#ifndef HPP_FCL_BROADPHASE_BROADPHASE_H
-#define HPP_FCL_BROADPHASE_BROADPHASE_H
-
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
-#include "hpp/fcl/broadphase/broadphase_bruteforce.h"
-#include "hpp/fcl/broadphase/broadphase_SaP.h"
-#include "hpp/fcl/broadphase/broadphase_SSaP.h"
-#include "hpp/fcl/broadphase/broadphase_interval_tree.h"
-#include "hpp/fcl/broadphase/broadphase_spatialhash.h"
-
-#include "hpp/fcl/broadphase/default_broadphase_callbacks.h"
-
-#endif  // ifndef HPP_FCL_BROADPHASE_BROADPHASE_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_SSaP.h b/include/hpp/fcl/broadphase/broadphase_SSaP.h
index b99eb90c81d2807c917970396099dd6f494bb5bc..23dda8ef324804ce936b34c87fe04669b59557c4 100644
--- a/include/hpp/fcl/broadphase/broadphase_SSaP.h
+++ b/include/hpp/fcl/broadphase/broadphase_SSaP.h
@@ -1,148 +1,2 @@
-/*
- * 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 HPP_FCL_BROAD_PHASE_SSAP_H
-#define HPP_FCL_BROAD_PHASE_SSAP_H
-
-#include <vector>
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Simple SAP collision manager
-class HPP_FCL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager {
- public:
-  typedef BroadPhaseCollisionManager Base;
-  using Base::getObjects;
-
-  SSaPCollisionManager();
-
-  /// @brief remove one object from the manager
-  void registerObject(CollisionObject* obj);
-
-  /// @brief add one object to the manager
-  void unregisterObject(CollisionObject* obj);
-
-  /// @brief initialize the manager, related with the specific type of manager
-  void setup();
-
-  /// @brief update the condition of manager
-  virtual void update();
-
-  /// @brief clear the manager
-  void clear();
-
-  /// @brief return the objects managed by the manager
-  void getObjects(std::vector<CollisionObject*>& objs) const;
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging to the manager
-  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e., N^2 self collision)
-  void collide(CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  void distance(DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test with objects belonging to another manager
-  void collide(BroadPhaseCollisionManager* other_manager,
-               CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test with objects belonging to another manager
-  void distance(BroadPhaseCollisionManager* other_manager,
-                DistanceCallBackBase* callback) const;
-
-  /// @brief whether the manager is empty
-  bool empty() const;
-
-  /// @brief the number of objects managed by the manager
-  size_t size() const;
-
- protected:
-  /// @brief check collision between one object and a list of objects, return
-  /// value is whether stop is possible
-  bool checkColl(
-      typename std::vector<CollisionObject*>::const_iterator pos_start,
-      typename std::vector<CollisionObject*>::const_iterator pos_end,
-      CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief check distance between one object and a list of objects, return
-  /// value is whether stop is possible
-  bool checkDis(
-      typename std::vector<CollisionObject*>::const_iterator pos_start,
-      typename std::vector<CollisionObject*>::const_iterator pos_end,
-      CollisionObject* obj, DistanceCallBackBase* callback,
-      FCL_REAL& min_dist) const;
-
-  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
-                 FCL_REAL& min_dist) const;
-
-  static int selectOptimalAxis(
-      const std::vector<CollisionObject*>& objs_x,
-      const std::vector<CollisionObject*>& objs_y,
-      const std::vector<CollisionObject*>& objs_z,
-      typename std::vector<CollisionObject*>::const_iterator& it_beg,
-      typename std::vector<CollisionObject*>::const_iterator& it_end);
-
-  /// @brief Objects sorted according to lower x value
-  std::vector<CollisionObject*> objs_x;
-
-  /// @brief Objects sorted according to lower y value
-  std::vector<CollisionObject*> objs_y;
-
-  /// @brief Objects sorted according to lower z value
-  std::vector<CollisionObject*> objs_z;
-
-  /// @brief tag about whether the environment is maintained suitably (i.e., the
-  /// objs_x, objs_y, objs_z are sorted correctly
-  bool setup_;
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_SSaP.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/hpp/fcl/broadphase/broadphase_SaP.h
index a71f78ad44864940617085bc54d122b82fae569b..0cdef682203edee36ce4566201bf62d205cc5f8c 100644
--- a/include/hpp/fcl/broadphase/broadphase_SaP.h
+++ b/include/hpp/fcl/broadphase/broadphase_SaP.h
@@ -1,226 +1,2 @@
-/*
- * 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 HPP_FCL_BROAD_PHASE_SAP_H
-#define HPP_FCL_BROAD_PHASE_SAP_H
-
-#include <map>
-#include <list>
-
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Rigorous SAP collision manager
-class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager {
- public:
-  typedef BroadPhaseCollisionManager Base;
-  using Base::getObjects;
-
-  SaPCollisionManager();
-
-  ~SaPCollisionManager();
-
-  /// @brief add objects to the manager
-  void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
-  /// @brief remove one object from the manager
-  void registerObject(CollisionObject* obj);
-
-  /// @brief add one object to the manager
-  void unregisterObject(CollisionObject* obj);
-
-  /// @brief initialize the manager, related with the specific type of manager
-  void setup();
-
-  /// @brief update the condition of manager
-  virtual void update();
-
-  /// @brief update the manager by explicitly given the object updated
-  void update(CollisionObject* updated_obj);
-
-  /// @brief update the manager by explicitly given the set of objects update
-  void update(const std::vector<CollisionObject*>& updated_objs);
-
-  /// @brief clear the manager
-  void clear();
-
-  /// @brief return the objects managed by the manager
-  void getObjects(std::vector<CollisionObject*>& objs) const;
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging to the manager
-  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e., N^2 self collision)
-  void collide(CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  void distance(DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test with objects belonging to another manager
-  void collide(BroadPhaseCollisionManager* other_manager,
-               CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test with objects belonging to another manager
-  void distance(BroadPhaseCollisionManager* other_manager,
-                DistanceCallBackBase* callback) const;
-
-  /// @brief whether the manager is empty
-  bool empty() const;
-
-  /// @brief the number of objects managed by the manager
-  size_t size() const;
-
- protected:
-  struct EndPoint;
-
-  /// @brief SAP interval for one object
-  struct SaPAABB {
-    /// @brief object
-    CollisionObject* obj;
-
-    /// @brief lower bound end point of the interval
-    EndPoint* lo;
-
-    /// @brief higher bound end point of the interval
-    EndPoint* hi;
-
-    /// @brief cached AABB value
-    AABB cached;
-  };
-
-  /// @brief End point for an interval
-  struct EndPoint {
-    /// @brief tag for whether it is a lower bound or higher bound of an
-    /// interval, 0 for lo, and 1 for hi
-    char minmax;
-
-    /// @brief back pointer to SAP interval
-    SaPAABB* aabb;
-
-    /// @brief the previous end point in the end point list
-    EndPoint* prev[3];
-
-    /// @brief the next end point in the end point list
-    EndPoint* next[3];
-
-    /// @brief get the value of the end point
-    const Vec3f& getVal() const;
-
-    /// @brief set the value of the end point
-    Vec3f& getVal();
-
-    FCL_REAL getVal(int i) const;
-
-    FCL_REAL& getVal(int i);
-  };
-
-  /// @brief A pair of objects that are not culling away and should further
-  /// check collision
-  struct SaPPair {
-    SaPPair(CollisionObject* a, CollisionObject* b);
-
-    CollisionObject* obj1;
-    CollisionObject* obj2;
-
-    bool operator==(const SaPPair& other) const;
-  };
-
-  /// @brief Functor to help unregister one object
-  class HPP_FCL_DLLAPI isUnregistered {
-    CollisionObject* obj;
-
-   public:
-    isUnregistered(CollisionObject* obj_);
-
-    bool operator()(const SaPPair& pair) const;
-  };
-
-  /// @brief Functor to help remove collision pairs no longer valid (i.e.,
-  /// should be culled away)
-  class HPP_FCL_DLLAPI isNotValidPair {
-    CollisionObject* obj1;
-    CollisionObject* obj2;
-
-   public:
-    isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_);
-
-    bool operator()(const SaPPair& pair);
-  };
-
-  void update_(SaPAABB* updated_aabb);
-
-  void updateVelist();
-
-  /// @brief End point list for x, y, z coordinates
-  EndPoint* elist[3];
-
-  /// @brief vector version of elist, for acceleration
-  std::vector<EndPoint*> velist[3];
-
-  /// @brief SAP interval list
-  std::list<SaPAABB*> AABB_arr;
-
-  /// @brief The pair of objects that should further check for collision
-  std::list<SaPPair> overlap_pairs;
-
-  int optimal_axis;
-
-  std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
-
-  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
-                 FCL_REAL& min_dist) const;
-
-  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  void addToOverlapPairs(const SaPPair& p);
-
-  void removeFromOverlapPairs(const SaPPair& p);
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_SaP.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_bruteforce.h b/include/hpp/fcl/broadphase/broadphase_bruteforce.h
index 734c74fb787bd565555b0c12556992d3167ff47e..5a4811ce1d08670a39c7066f0ff8354a236af41c 100644
--- a/include/hpp/fcl/broadphase/broadphase_bruteforce.h
+++ b/include/hpp/fcl/broadphase/broadphase_bruteforce.h
@@ -1,115 +1,2 @@
-/*
- * 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 HPP_FCL_BROAD_PHASE_BRUTE_FORCE_H
-#define HPP_FCL_BROAD_PHASE_BRUTE_FORCE_H
-
-#include <list>
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Brute force N-body collision manager
-class HPP_FCL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager {
- public:
-  typedef BroadPhaseCollisionManager Base;
-  using Base::getObjects;
-
-  NaiveCollisionManager();
-
-  /// @brief add objects to the manager
-  void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
-  /// @brief add one object to the manager
-  void registerObject(CollisionObject* obj);
-
-  /// @brief remove one object from the manager
-  void unregisterObject(CollisionObject* obj);
-
-  /// @brief initialize the manager, related with the specific type of manager
-  void setup();
-
-  /// @brief update the condition of manager
-  virtual void update();
-
-  /// @brief clear the manager
-  void clear();
-
-  /// @brief return the objects managed by the manager
-  void getObjects(std::vector<CollisionObject*>& objs) const;
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging to the manager
-  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e., N^2 self collision)
-  void collide(CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  void distance(DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test with objects belonging to another manager
-  void collide(BroadPhaseCollisionManager* other_manager,
-               CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test with objects belonging to another manager
-  void distance(BroadPhaseCollisionManager* other_manager,
-                DistanceCallBackBase* callback) const;
-
-  /// @brief whether the manager is empty
-  bool empty() const;
-
-  /// @brief the number of objects managed by the manager
-  size_t size() const;
-
- protected:
-  /// @brief objects belonging to the manager are stored in a list structure
-  std::list<CollisionObject*> objs;
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_bruteforce.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_callbacks.h b/include/hpp/fcl/broadphase/broadphase_callbacks.h
index 8a7d6b0ff0a0f4bb0df1ec7288adb3c4882d2f81..40b5b0a6c797642853e4d5392a03d105cda5df65 100644
--- a/include/hpp/fcl/broadphase/broadphase_callbacks.h
+++ b/include/hpp/fcl/broadphase/broadphase_callbacks.h
@@ -1,98 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2022, INRIA
- *  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 the copyright holder 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 Justin Carpentier (justin.carpentier@inria.fr) */
-
-#ifndef HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
-#define HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
-
-#include "hpp/fcl/fwd.hh"
-#include "hpp/fcl/data_types.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Base callback class for collision queries.
-/// This class can be supersed by child classes to provide desired behaviors
-/// according to the application (e.g, only listing the potential
-/// CollisionObjects in collision).
-struct HPP_FCL_DLLAPI CollisionCallBackBase {
-  /// @brief Initialization of the callback before running the collision
-  /// broadphase manager.
-  virtual void init() {};
-
-  /// @brief Collision evaluation between two objects in collision.
-  ///        This callback will cause the broadphase evaluation to stop if it
-  ///        returns true.
-  ///
-  /// @param[in] o1 Collision object #1.
-  /// @param[in] o2 Collision object #2.
-  virtual bool collide(CollisionObject* o1, CollisionObject* o2) = 0;
-
-  /// @brief Functor call associated to the collide operation.
-  virtual bool operator()(CollisionObject* o1, CollisionObject* o2) {
-    return collide(o1, o2);
-  }
-};
-
-/// @brief Base callback class for distance queries.
-/// This class can be supersed by child classes to provide desired behaviors
-/// according to the application (e.g, only listing the potential
-/// CollisionObjects in collision).
-struct HPP_FCL_DLLAPI DistanceCallBackBase {
-  /// @brief Initialization of the callback before running the collision
-  /// broadphase manager.
-  virtual void init() {};
-
-  /// @brief Distance evaluation between two objects in collision.
-  ///        This callback will cause the broadphase evaluation to stop if it
-  ///        returns true.
-  ///
-  /// @param[in] o1 Collision object #1.
-  /// @param[in] o2 Collision object #2.
-  /// @param[out] dist Distance between the two collision geometries.
-  virtual bool distance(CollisionObject* o1, CollisionObject* o2,
-                        FCL_REAL& dist) = 0;
-
-  /// @brief Functor call associated to the distance operation.
-  virtual bool operator()(CollisionObject* o1, CollisionObject* o2,
-                          FCL_REAL& dist) {
-    return distance(o1, o2, dist);
-  }
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_callbacks.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_collision_manager.h
index 750907b28e256dd871d05a07215f80231bb08610..fe49da052299e9c864d95eef54a7a980ffc983ae 100644
--- a/include/hpp/fcl/broadphase/broadphase_collision_manager.h
+++ b/include/hpp/fcl/broadphase/broadphase_collision_manager.h
@@ -1,141 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
-#define HPP_FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
-
-#include <set>
-#include <vector>
-#include <boost/function.hpp>
-
-#include "hpp/fcl/collision_object.h"
-#include "hpp/fcl/broadphase/broadphase_callbacks.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Base class for broad phase collision. It helps to accelerate the
-/// collision/distance between N objects. Also support self collision, self
-/// distance and collision/distance with another M objects.
-class HPP_FCL_DLLAPI BroadPhaseCollisionManager {
- public:
-  BroadPhaseCollisionManager();
-
-  virtual ~BroadPhaseCollisionManager();
-
-  /// @brief add objects to the manager
-  virtual void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
-  /// @brief add one object to the manager
-  virtual void registerObject(CollisionObject* obj) = 0;
-
-  /// @brief remove one object from the manager
-  virtual void unregisterObject(CollisionObject* obj) = 0;
-
-  /// @brief initialize the manager, related with the specific type of manager
-  virtual void setup() = 0;
-
-  /// @brief update the condition of manager
-  virtual void update() = 0;
-
-  /// @brief update the manager by explicitly given the object updated
-  virtual void update(CollisionObject* updated_obj);
-
-  /// @brief update the manager by explicitly given the set of objects update
-  virtual void update(const std::vector<CollisionObject*>& updated_objs);
-
-  /// @brief clear the manager
-  virtual void clear() = 0;
-
-  /// @brief return the objects managed by the manager
-  virtual void getObjects(std::vector<CollisionObject*>& objs) const = 0;
-
-  /// @brief return the objects managed by the manager
-  virtual std::vector<CollisionObject*> getObjects() const {
-    std::vector<CollisionObject*> res(size());
-    getObjects(res);
-    return res;
-  };
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  virtual void collide(CollisionObject* obj,
-                       CollisionCallBackBase* callback) const = 0;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging to the manager
-  virtual void distance(CollisionObject* obj,
-                        DistanceCallBackBase* callback) const = 0;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e., N^2 self collision)
-  virtual void collide(CollisionCallBackBase* callback) const = 0;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  virtual void distance(DistanceCallBackBase* callback) const = 0;
-
-  /// @brief perform collision test with objects belonging to another manager
-  virtual void collide(BroadPhaseCollisionManager* other_manager,
-                       CollisionCallBackBase* callback) const = 0;
-
-  /// @brief perform distance test with objects belonging to another manager
-  virtual void distance(BroadPhaseCollisionManager* other_manager,
-                        DistanceCallBackBase* callback) const = 0;
-
-  /// @brief whether the manager is empty
-  virtual bool empty() const = 0;
-
-  /// @brief the number of objects managed by the manager
-  virtual size_t size() const = 0;
-
- protected:
-  /// @brief tools help to avoid repeating collision or distance callback for
-  /// the pairs of objects tested before. It can be useful for some of the
-  /// broadphase algorithms.
-  mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
-  mutable bool enable_tested_set_;
-
-  bool inTestedSet(CollisionObject* a, CollisionObject* b) const;
-
-  void insertTestedSet(CollisionObject* a, CollisionObject* b) const;
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_collision_manager.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h
index 93691b200a403e41916ba7b58d047bccdf01d165..5ee4f15b0d5a254984cc37bf78ddd0ca45882e38 100644
--- a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h
+++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h
@@ -1,87 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
-#define HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
-
-#include "hpp/fcl/broadphase/broadphase_continuous_collision_manager.h"
-#include <algorithm>
-
-namespace hpp {
-namespace fcl {
-
-//==============================================================================
-BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() {
-  // Do nothing
-}
-
-//==============================================================================
-
-BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() {
-  // Do nothing
-}
-
-//==============================================================================
-
-void BroadPhaseContinuousCollisionManager::registerObjects(
-    const std::vector<ContinuousCollisionObject*>& other_objs) {
-  for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]);
-}
-
-//==============================================================================
-
-void BroadPhaseContinuousCollisionManager::update(
-    ContinuousCollisionObject* updated_obj) {
-  HPP_FCL_UNUSED_VARIABLE(updated_obj);
-
-  update();
-}
-
-//==============================================================================
-
-void BroadPhaseContinuousCollisionManager::update(
-    const std::vector<ContinuousCollisionObject*>& updated_objs) {
-  HPP_FCL_UNUSED_VARIABLE(updated_objs);
-
-  update();
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_continuous_collision_manager-inl.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h
index 79bfe6aa6db981979ef9fd5034878d9202f195cc..a2f73c0a100e56bea8148986cfefb055f2fd664a 100644
--- a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h
+++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h
@@ -1,146 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
-#define HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
-
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
-#include "hpp/fcl/collision_object.h"
-#include "hpp/fcl/narrowphase/continuous_collision_object.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Callback for continuous collision between two objects. Return value
-/// is whether can stop now.
-template <typename S>
-using ContinuousCollisionCallBack = bool (*)(ContinuousCollisionObject* o1,
-                                             ContinuousCollisionObject* o2,
-                                             void* cdata);
-
-/// @brief Callback for continuous distance between two objects, Return value is
-/// whether can stop now, also return the minimum distance till now.
-template <typename S>
-using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1,
-                                            ContinuousCollisionObject* o2,
-                                            S& dist);
-
-/// @brief Base class for broad phase continuous collision. It helps to
-/// accelerate the continuous collision/distance between N objects. Also support
-/// self collision, self distance and collision/distance with another M objects.
-template <typename S>
-class HPP_FCL_DLLAPI BroadPhaseContinuousCollisionManager {
- public:
-  BroadPhaseContinuousCollisionManager();
-
-  virtual ~BroadPhaseContinuousCollisionManager();
-
-  /// @brief add objects to the manager
-  virtual void registerObjects(
-      const std::vector<ContinuousCollisionObject*>& other_objs);
-
-  /// @brief add one object to the manager
-  virtual void registerObject(ContinuousCollisionObject* obj) = 0;
-
-  /// @brief remove one object from the manager
-  virtual void unregisterObject(ContinuousCollisionObject* obj) = 0;
-
-  /// @brief initialize the manager, related with the specific type of manager
-  virtual void setup() = 0;
-
-  /// @brief update the condition of manager
-  virtual void update() = 0;
-
-  /// @brief update the manager by explicitly given the object updated
-  virtual void update(ContinuousCollisionObject* updated_obj);
-
-  /// @brief update the manager by explicitly given the set of objects update
-  virtual void update(
-      const std::vector<ContinuousCollisionObject*>& updated_objs);
-
-  /// @brief clear the manager
-  virtual void clear() = 0;
-
-  /// @brief return the objects managed by the manager
-  virtual void getObjects(
-      std::vector<ContinuousCollisionObject*>& objs) const = 0;
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  virtual void collide(ContinuousCollisionObject* obj,
-                       CollisionCallBackBase* callback) const = 0;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging to the manager
-  virtual void distance(ContinuousCollisionObject* obj,
-                        DistanceCallBackBase* callback) const = 0;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e., N^2 self collision)
-  virtual void collide(CollisionCallBackBase* callback) const = 0;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  virtual void distance(DistanceCallBackBase* callback) const = 0;
-
-  /// @brief perform collision test with objects belonging to another manager
-  virtual void collide(BroadPhaseContinuousCollisionManager* other_manager,
-                       CollisionCallBackBase* callback) const = 0;
-
-  /// @brief perform distance test with objects belonging to another manager
-  virtual void distance(BroadPhaseContinuousCollisionManager* other_manager,
-                        DistanceCallBackBase* callback) const = 0;
-
-  /// @brief whether the manager is empty
-  virtual bool empty() const = 0;
-
-  /// @brief the number of objects managed by the manager
-  virtual size_t size() const = 0;
-};
-
-using BroadPhaseContinuousCollisionManagerf =
-    BroadPhaseContinuousCollisionManager<float>;
-using BroadPhaseContinuousCollisionManagerd =
-    BroadPhaseContinuousCollisionManager<FCL_REAL>;
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_continuous_collision_manager.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h
index 0ef013214e6fe47f91de1d9b242f06ef526b28d7..167ec3adcda0cfe9bcbbc700c3291886b1886b63 100644
--- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h
+++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h
@@ -1,236 +1,2 @@
-/*
- * 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 HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
-#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
-
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
-
-#include <limits>
-
-#if HPP_FCL_HAVE_OCTOMAP
-#include "hpp/fcl/octree.h"
-#endif
-
-#include "hpp/fcl/BV/BV.h"
-#include "hpp/fcl/shape/geometric_shapes_utility.h"
-
-namespace hpp {
-namespace fcl {
-namespace detail {
-
-namespace dynamic_AABB_tree {
-
-#if HPP_FCL_HAVE_OCTOMAP
-
-//==============================================================================
-template <typename Derived>
-bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
-                       const OcTree* tree2, const OcTree::OcTreeNode* root2,
-                       const AABB& root2_bv,
-                       const Eigen::MatrixBase<Derived>& translation2,
-                       CollisionCallBackBase* callback) {
-  if (!root2) {
-    if (root1->isLeaf()) {
-      CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
-      if (!obj1->collisionGeometry()->isFree()) {
-        const AABB& root2_bv_t = translate(root2_bv, translation2);
-        if (root1->bv.overlap(root2_bv_t)) {
-          Box* box = new Box();
-          Transform3f box_tf;
-          Transform3f tf2 = Transform3f::Identity();
-          tf2.translation() = translation2;
-          constructBox(root2_bv, tf2, *box, box_tf);
-
-          box->cost_density =
-              tree2->getOccupancyThres();  // thresholds are 0, 1, so uncertain
-
-          CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
-          return (*callback)(obj1, &obj2);
-        }
-      }
-    } else {
-      if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv,
-                            translation2, callback))
-        return true;
-      if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv,
-                            translation2, callback))
-        return true;
-    }
-
-    return false;
-  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
-    CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
-    if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
-      const AABB& root2_bv_t = translate(root2_bv, translation2);
-      if (root1->bv.overlap(root2_bv_t)) {
-        Box* box = new Box();
-        Transform3f box_tf;
-        Transform3f tf2 = Transform3f::Identity();
-        tf2.translation() = translation2;
-        constructBox(root2_bv, tf2, *box, box_tf);
-
-        box->cost_density = root2->getOccupancy();
-        box->threshold_occupied = tree2->getOccupancyThres();
-
-        CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
-        return (*callback)(obj1, &obj2);
-      } else
-        return false;
-    } else
-      return false;
-  }
-
-  const AABB& root2_bv_t = translate(root2_bv, translation2);
-  if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
-
-  if (!tree2->nodeHasChildren(root2) ||
-      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
-    if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv,
-                          translation2, callback))
-      return true;
-    if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv,
-                          translation2, callback))
-      return true;
-  } else {
-    for (unsigned int i = 0; i < 8; ++i) {
-      if (tree2->nodeChildExists(root2, i)) {
-        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
-        AABB child_bv;
-        computeChildBV(root2_bv, i, child_bv);
-
-        if (collisionRecurse_(root1, tree2, child, child_bv, translation2,
-                              callback))
-          return true;
-      } else {
-        AABB child_bv;
-        computeChildBV(root2_bv, i, child_bv);
-        if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2,
-                              callback))
-          return true;
-      }
-    }
-  }
-  return false;
-}
-
-//==============================================================================
-template <typename Derived>
-bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
-                      const OcTree* tree2, const OcTree::OcTreeNode* root2,
-                      const AABB& root2_bv,
-                      const Eigen::MatrixBase<Derived>& translation2,
-                      DistanceCallBackBase* callback, FCL_REAL& min_dist) {
-  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
-    if (tree2->isNodeOccupied(root2)) {
-      Box* box = new Box();
-      Transform3f box_tf;
-      Transform3f tf2 = Transform3f::Identity();
-      tf2.translation() = translation2;
-      constructBox(root2_bv, tf2, *box, box_tf);
-      CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
-      return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
-                         min_dist);
-    } else
-      return false;
-  }
-
-  if (!tree2->isNodeOccupied(root2)) return false;
-
-  if (!tree2->nodeHasChildren(root2) ||
-      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
-    const AABB& aabb2 = translate(root2_bv, translation2);
-    FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
-    FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
-
-    if (d2 < d1) {
-      if (d2 < min_dist) {
-        if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
-                             translation2, callback, min_dist))
-          return true;
-      }
-
-      if (d1 < min_dist) {
-        if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
-                             translation2, callback, min_dist))
-          return true;
-      }
-    } else {
-      if (d1 < min_dist) {
-        if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
-                             translation2, callback, min_dist))
-          return true;
-      }
-
-      if (d2 < min_dist) {
-        if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
-                             translation2, callback, min_dist))
-          return true;
-      }
-    }
-  } else {
-    for (unsigned int i = 0; i < 8; ++i) {
-      if (tree2->nodeChildExists(root2, i)) {
-        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
-        AABB child_bv;
-        computeChildBV(root2_bv, i, child_bv);
-        const AABB& aabb2 = translate(child_bv, translation2);
-
-        FCL_REAL d = root1->bv.distance(aabb2);
-
-        if (d < min_dist) {
-          if (distanceRecurse_(root1, tree2, child, child_bv, translation2,
-                               callback, min_dist))
-            return true;
-        }
-      }
-    }
-  }
-
-  return false;
-}
-
-#endif
-
-}  // namespace dynamic_AABB_tree
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_dynamic_AABB_tree-inl.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
index 10b4341c3badb9d98dc2c3a30f0f7d949cc23ee8..5ce7c99fca05af6caee71edbf2761c30ddd4386f 100644
--- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
+++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
@@ -1,153 +1,2 @@
-/*
- * 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 HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
-#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
-
-#include <unordered_map>
-#include <functional>
-
-#include "hpp/fcl/fwd.hh"
-// #include "hpp/fcl/BV/utility.h"
-#include "hpp/fcl/shape/geometric_shapes.h"
-// #include "hpp/fcl/geometry/shape/utility.h"
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
-#include "hpp/fcl/broadphase/detail/hierarchy_tree.h"
-
-namespace hpp {
-namespace fcl {
-
-class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager
-    : public BroadPhaseCollisionManager {
- public:
-  typedef BroadPhaseCollisionManager Base;
-  using Base::getObjects;
-
-  using DynamicAABBNode = detail::NodeBase<AABB>;
-  using DynamicAABBTable =
-      std::unordered_map<CollisionObject*, DynamicAABBNode*>;
-
-  int max_tree_nonbalanced_level;
-  int tree_incremental_balance_pass;
-  int* tree_topdown_balance_threshold{nullptr};
-  int* tree_topdown_level{nullptr};
-  int tree_init_level;
-
-  bool octree_as_geometry_collide;
-  bool octree_as_geometry_distance;
-
-  DynamicAABBTreeCollisionManager();
-
-  /// @brief add objects to the manager
-  void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
-  /// @brief add one object to the manager
-  void registerObject(CollisionObject* obj);
-
-  /// @brief remove one object from the manager
-  void unregisterObject(CollisionObject* obj);
-
-  /// @brief initialize the manager, related with the specific type of manager
-  void setup();
-
-  /// @brief update the condition of manager
-  virtual void update();
-
-  /// @brief update the manager by explicitly given the object updated
-  void update(CollisionObject* updated_obj);
-
-  /// @brief update the manager by explicitly given the set of objects update
-  void update(const std::vector<CollisionObject*>& updated_objs);
-
-  /// @brief clear the manager
-  void clear();
-
-  /// @brief return the objects managed by the manager
-  void getObjects(std::vector<CollisionObject*>& objs) const;
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging to the manager
-  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e., N^2 self collision)
-  void collide(CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  void distance(DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test with objects belonging to another manager
-  void collide(BroadPhaseCollisionManager* other_manager_,
-               CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test with objects belonging to another manager
-  void distance(BroadPhaseCollisionManager* other_manager_,
-                DistanceCallBackBase* callback) const;
-
-  /// @brief whether the manager is empty
-  bool empty() const;
-
-  /// @brief the number of objects managed by the manager
-  size_t size() const;
-
-  /// @brief returns the AABB tree structure.
-  const detail::HierarchyTree<AABB>& getTree() const;
-
-  /// @brief returns the AABB tree structure.
-  detail::HierarchyTree<AABB>& getTree();
-
- private:
-  detail::HierarchyTree<AABB> dtree{};
-  std::unordered_map<CollisionObject*, DynamicAABBNode*> table;
-
-  bool setup_;
-
-  void update_(CollisionObject* updated_obj);
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_dynamic_AABB_tree.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
index 439f93b11b1d09de4c027f382339733ccdaf32a2..5e67f2525fa6cbf109a5ef66eb3fa3a12f44f105 100644
--- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
+++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
@@ -1,235 +1,2 @@
-/*
- * 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 HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
-#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
-
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
-#include "hpp/fcl/shape/geometric_shapes_utility.h"
-
-#if HPP_FCL_HAVE_OCTOMAP
-#include "hpp/fcl/octree.h"
-#endif
-
-namespace hpp {
-namespace fcl {
-namespace detail {
-namespace dynamic_AABB_tree_array {
-
-#if HPP_FCL_HAVE_OCTOMAP
-
-//==============================================================================
-template <typename Derived>
-bool collisionRecurse_(
-    DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
-    size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
-    const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
-    CollisionCallBackBase* callback) {
-  DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
-      nodes1 + root1_id;
-  if (!root2) {
-    if (root1->isLeaf()) {
-      CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
-      if (!obj1->collisionGeometry()->isFree()) {
-        const AABB& root_bv_t = translate(root2_bv, translation2);
-        if (root1->bv.overlap(root_bv_t)) {
-          Box* box = new Box();
-          Transform3f box_tf;
-          Transform3f tf2 = Transform3f::Identity();
-          tf2.translation() = translation2;
-          constructBox(root2_bv, tf2, *box, box_tf);
-
-          box->cost_density = tree2->getDefaultOccupancy();
-
-          CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
-          return (*callback)(obj1, &obj2);
-        }
-      }
-    } else {
-      if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr,
-                            root2_bv, translation2, callback))
-        return true;
-      if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr,
-                            root2_bv, translation2, callback))
-        return true;
-    }
-
-    return false;
-  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
-    CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-    if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
-      const AABB& root_bv_t = translate(root2_bv, translation2);
-      if (root1->bv.overlap(root_bv_t)) {
-        Box* box = new Box();
-        Transform3f box_tf;
-        Transform3f tf2 = Transform3f::Identity();
-        tf2.translation() = translation2;
-        constructBox(root2_bv, tf2, *box, box_tf);
-
-        box->cost_density = root2->getOccupancy();
-        box->threshold_occupied = tree2->getOccupancyThres();
-
-        CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
-        return (*callback)(obj1, &obj2);
-      } else
-        return false;
-    } else
-      return false;
-  }
-
-  const AABB& root_bv_t = translate(root2_bv, translation2);
-  if (tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false;
-
-  if (!tree2->nodeHasChildren(root2) ||
-      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
-    if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
-                          translation2, callback))
-      return true;
-    if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
-                          translation2, callback))
-      return true;
-  } else {
-    for (unsigned int i = 0; i < 8; ++i) {
-      if (tree2->nodeChildExists(root2, i)) {
-        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
-        AABB child_bv;
-        computeChildBV(root2_bv, i, child_bv);
-
-        if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv,
-                              translation2, callback))
-          return true;
-      } else {
-        AABB child_bv;
-        computeChildBV(root2_bv, i, child_bv);
-        if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv,
-                              translation2, callback))
-          return true;
-      }
-    }
-  }
-
-  return false;
-}
-
-//==============================================================================
-template <typename Derived>
-bool distanceRecurse_(
-    DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
-    size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
-    const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
-    DistanceCallBackBase* callback, FCL_REAL& min_dist) {
-  DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
-      nodes1 + root1_id;
-  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
-    if (tree2->isNodeOccupied(root2)) {
-      Box* box = new Box();
-      Transform3f box_tf;
-      Transform3f tf2 = Transform3f::Identity();
-      tf2.translation() = translation2;
-      constructBox(root2_bv, tf2, *box, box_tf);
-      CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
-      return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
-                         min_dist);
-    } else
-      return false;
-  }
-
-  if (!tree2->isNodeOccupied(root2)) return false;
-
-  if (!tree2->nodeHasChildren(root2) ||
-      (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
-    const AABB& aabb2 = translate(root2_bv, translation2);
-
-    FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
-    FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
-
-    if (d2 < d1) {
-      if (d2 < min_dist) {
-        if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
-                             translation2, callback, min_dist))
-          return true;
-      }
-
-      if (d1 < min_dist) {
-        if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
-                             translation2, callback, min_dist))
-          return true;
-      }
-    } else {
-      if (d1 < min_dist) {
-        if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
-                             translation2, callback, min_dist))
-          return true;
-      }
-
-      if (d2 < min_dist) {
-        if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
-                             translation2, callback, min_dist))
-          return true;
-      }
-    }
-  } else {
-    for (unsigned int i = 0; i < 8; ++i) {
-      if (tree2->nodeChildExists(root2, i)) {
-        const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
-        AABB child_bv;
-        computeChildBV(root2_bv, i, child_bv);
-
-        const AABB& aabb2 = translate(child_bv, translation2);
-        FCL_REAL d = root1->bv.distance(aabb2);
-
-        if (d < min_dist) {
-          if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv,
-                               translation2, callback, min_dist))
-            return true;
-        }
-      }
-    }
-  }
-
-  return false;
-}
-
-#endif
-
-}  // namespace dynamic_AABB_tree_array
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
index e4756300b459b6c525dd1d3d60453edebd94f27c..5923fd17769ec209ebd1313f50ab48e2ef254008 100644
--- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
+++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
@@ -1,149 +1,2 @@
-/*
- * 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 HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
-#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
-
-#include <unordered_map>
-#include <functional>
-#include <limits>
-
-#include "hpp/fcl/fwd.hh"
-// #include "hpp/fcl/BV/utility.h"
-#include "hpp/fcl/shape/geometric_shapes.h"
-// #include "hpp/fcl/geometry/shape/utility.h"
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
-#include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h"
-
-namespace hpp {
-namespace fcl {
-
-class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager
-    : public BroadPhaseCollisionManager {
- public:
-  typedef BroadPhaseCollisionManager Base;
-  using Base::getObjects;
-
-  using DynamicAABBNode = detail::implementation_array::NodeBase<AABB>;
-  using DynamicAABBTable = std::unordered_map<CollisionObject*, size_t>;
-
-  int max_tree_nonbalanced_level;
-  int tree_incremental_balance_pass;
-  int* tree_topdown_balance_threshold{nullptr};
-  int* tree_topdown_level{nullptr};
-  int tree_init_level;
-
-  bool octree_as_geometry_collide;
-  bool octree_as_geometry_distance;
-
-  DynamicAABBTreeArrayCollisionManager();
-
-  /// @brief add objects to the manager
-  void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
-  /// @brief add one object to the manager
-  void registerObject(CollisionObject* obj);
-
-  /// @brief remove one object from the manager
-  void unregisterObject(CollisionObject* obj);
-
-  /// @brief initialize the manager, related with the specific type of manager
-  void setup();
-
-  /// @brief update the condition of manager
-  virtual void update();
-
-  /// @brief update the manager by explicitly given the object updated
-  void update(CollisionObject* updated_obj);
-
-  /// @brief update the manager by explicitly given the set of objects update
-  void update(const std::vector<CollisionObject*>& updated_objs);
-
-  /// @brief clear the manager
-  void clear();
-
-  /// @brief return the objects managed by the manager
-  void getObjects(std::vector<CollisionObject*>& objs) const;
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging to the manager
-  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e., N^2 self collision)
-  void collide(CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  void distance(DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test with objects belonging to another manager
-  void collide(BroadPhaseCollisionManager* other_manager_,
-               CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test with objects belonging to another manager
-  void distance(BroadPhaseCollisionManager* other_manager_,
-                DistanceCallBackBase* callback) const;
-
-  /// @brief whether the manager is empty
-  bool empty() const;
-
-  /// @brief the number of objects managed by the manager
-  size_t size() const;
-
-  const detail::implementation_array::HierarchyTree<AABB>& getTree() const;
-
- private:
-  detail::implementation_array::HierarchyTree<AABB> dtree{};
-  std::unordered_map<CollisionObject*, size_t> table;
-
-  bool setup_;
-
-  void update_(CollisionObject* updated_obj);
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_dynamic_AABB_tree_array.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_interval_tree.h b/include/hpp/fcl/broadphase/broadphase_interval_tree.h
index 78b1037d059e36d5558d635597c5104ea449c26e..d00b1f6d3f0bf837b9117034e9e140a8846b8dc5 100644
--- a/include/hpp/fcl/broadphase/broadphase_interval_tree.h
+++ b/include/hpp/fcl/broadphase/broadphase_interval_tree.h
@@ -1,172 +1,2 @@
-/*
- * 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 HPP_FCL_BROAD_PHASE_INTERVAL_TREE_H
-#define HPP_FCL_BROAD_PHASE_INTERVAL_TREE_H
-
-#include <deque>
-#include <map>
-
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
-#include "hpp/fcl/broadphase/detail/interval_tree.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Collision manager based on interval tree
-class HPP_FCL_DLLAPI IntervalTreeCollisionManager
-    : public BroadPhaseCollisionManager {
- public:
-  typedef BroadPhaseCollisionManager Base;
-  using Base::getObjects;
-
-  IntervalTreeCollisionManager();
-
-  ~IntervalTreeCollisionManager();
-
-  /// @brief remove one object from the manager
-  void registerObject(CollisionObject* obj);
-
-  /// @brief add one object to the manager
-  void unregisterObject(CollisionObject* obj);
-
-  /// @brief initialize the manager, related with the specific type of manager
-  void setup();
-
-  /// @brief update the condition of manager
-  virtual void update();
-
-  /// @brief update the manager by explicitly given the object updated
-  void update(CollisionObject* updated_obj);
-
-  /// @brief update the manager by explicitly given the set of objects update
-  void update(const std::vector<CollisionObject*>& updated_objs);
-
-  /// @brief clear the manager
-  void clear();
-
-  /// @brief return the objects managed by the manager
-  void getObjects(std::vector<CollisionObject*>& objs) const;
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging to the manager
-  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e., N^2 self collision)
-  void collide(CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  void distance(DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test with objects belonging to another manager
-  void collide(BroadPhaseCollisionManager* other_manager,
-               CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test with objects belonging to another manager
-  void distance(BroadPhaseCollisionManager* other_manager,
-                DistanceCallBackBase* callback) const;
-
-  /// @brief whether the manager is empty
-  bool empty() const;
-
-  /// @brief the number of objects managed by the manager
-  size_t size() const;
-
- protected:
-  /// @brief SAP end point
-  /// @brief SAP end point
-  struct HPP_FCL_DLLAPI EndPoint {
-    /// @brief object related with the end point
-    CollisionObject* obj;
-
-    /// @brief end point value
-    FCL_REAL value;
-
-    /// @brief tag for whether it is a lower bound or higher bound of an
-    /// interval, 0 for lo, and 1 for hi
-    char minmax;
-
-    bool operator<(const EndPoint& p) const;
-  };
-
-  /// @brief Extention interval tree's interval to SAP interval, adding more
-  /// information
-  struct HPP_FCL_DLLAPI SAPInterval : public detail::SimpleInterval {
-    CollisionObject* obj;
-
-    SAPInterval(FCL_REAL low_, FCL_REAL high_, CollisionObject* obj_);
-  };
-
-  bool checkColl(
-      typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
-      typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
-      CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  bool checkDist(
-      typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
-      typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
-      CollisionObject* obj, DistanceCallBackBase* callback,
-      FCL_REAL& min_dist) const;
-
-  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
-                 FCL_REAL& min_dist) const;
-
-  /// @brief vector stores all the end points
-  std::vector<EndPoint> endpoints[3];
-
-  /// @brief  interval tree manages the intervals
-  detail::IntervalTree* interval_trees[3];
-
-  std::map<CollisionObject*, SAPInterval*> obj_interval_maps[3];
-
-  /// @brief tag for whether the interval tree is maintained suitably
-  bool setup_;
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_interval_tree.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h
index 29c462c3e65a7c982d55f57c9c43b6e863af4037..31917d0a65a149ed9366ed023e8046188a9b04ec 100644
--- a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h
+++ b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h
@@ -1,542 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
-#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
-
-#include "hpp/fcl/broadphase/broadphase_spatialhash.h"
-
-namespace hpp {
-namespace fcl {
-
-//==============================================================================
-template <typename HashTable>
-SpatialHashingCollisionManager<HashTable>::SpatialHashingCollisionManager(
-    FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max,
-    unsigned int default_table_size)
-    : scene_limit(AABB(scene_min, scene_max)),
-      hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
-  hash_table->init(default_table_size);
-}
-
-//==============================================================================
-template <typename HashTable>
-SpatialHashingCollisionManager<HashTable>::~SpatialHashingCollisionManager() {
-  delete hash_table;
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::registerObject(
-    CollisionObject* obj) {
-  objs.push_back(obj);
-
-  const AABB& obj_aabb = obj->getAABB();
-  AABB overlap_aabb;
-
-  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
-    if (!scene_limit.contain(obj_aabb))
-      objs_partially_penetrating_scene_limit.push_back(obj);
-
-    hash_table->insert(overlap_aabb, obj);
-  } else {
-    objs_outside_scene_limit.push_back(obj);
-  }
-
-  obj_aabb_map[obj] = obj_aabb;
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::unregisterObject(
-    CollisionObject* obj) {
-  objs.remove(obj);
-
-  const AABB& obj_aabb = obj->getAABB();
-  AABB overlap_aabb;
-
-  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
-    if (!scene_limit.contain(obj_aabb))
-      objs_partially_penetrating_scene_limit.remove(obj);
-
-    hash_table->remove(overlap_aabb, obj);
-  } else {
-    objs_outside_scene_limit.remove(obj);
-  }
-
-  obj_aabb_map.erase(obj);
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::setup() {
-  // Do nothing
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::update() {
-  hash_table->clear();
-  objs_partially_penetrating_scene_limit.clear();
-  objs_outside_scene_limit.clear();
-
-  for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) {
-    CollisionObject* obj = *it;
-    const AABB& obj_aabb = obj->getAABB();
-    AABB overlap_aabb;
-
-    if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
-      if (!scene_limit.contain(obj_aabb))
-        objs_partially_penetrating_scene_limit.push_back(obj);
-
-      hash_table->insert(overlap_aabb, obj);
-    } else {
-      objs_outside_scene_limit.push_back(obj);
-    }
-
-    obj_aabb_map[obj] = obj_aabb;
-  }
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::update(
-    CollisionObject* updated_obj) {
-  const AABB& new_aabb = updated_obj->getAABB();
-  const AABB& old_aabb = obj_aabb_map[updated_obj];
-
-  AABB old_overlap_aabb;
-  const auto is_old_aabb_overlapping =
-      scene_limit.overlap(old_aabb, old_overlap_aabb);
-  if (is_old_aabb_overlapping)
-    hash_table->remove(old_overlap_aabb, updated_obj);
-
-  AABB new_overlap_aabb;
-  const auto is_new_aabb_overlapping =
-      scene_limit.overlap(new_aabb, new_overlap_aabb);
-  if (is_new_aabb_overlapping)
-    hash_table->insert(new_overlap_aabb, updated_obj);
-
-  ObjectStatus old_status;
-  if (is_old_aabb_overlapping) {
-    if (scene_limit.contain(old_aabb))
-      old_status = Inside;
-    else
-      old_status = PartiallyPenetrating;
-  } else {
-    old_status = Outside;
-  }
-
-  if (is_new_aabb_overlapping) {
-    if (scene_limit.contain(new_aabb)) {
-      if (old_status == PartiallyPenetrating) {
-        // Status change: PartiallyPenetrating --> Inside
-        // Required action(s):
-        // - remove object from "objs_partially_penetrating_scene_limit"
-
-        auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
-                                 objs_partially_penetrating_scene_limit.end(),
-                                 updated_obj);
-        objs_partially_penetrating_scene_limit.erase(find_it);
-      } else if (old_status == Outside) {
-        // Status change: Outside --> Inside
-        // Required action(s):
-        // - remove object from "objs_outside_scene_limit"
-
-        auto find_it = std::find(objs_outside_scene_limit.begin(),
-                                 objs_outside_scene_limit.end(), updated_obj);
-        objs_outside_scene_limit.erase(find_it);
-      }
-    } else {
-      if (old_status == Inside) {
-        // Status change: Inside --> PartiallyPenetrating
-        // Required action(s):
-        // - add object to "objs_partially_penetrating_scene_limit"
-
-        objs_partially_penetrating_scene_limit.push_back(updated_obj);
-      } else if (old_status == Outside) {
-        // Status change: Outside --> PartiallyPenetrating
-        // Required action(s):
-        // - remove object from "objs_outside_scene_limit"
-        // - add object to "objs_partially_penetrating_scene_limit"
-
-        auto find_it = std::find(objs_outside_scene_limit.begin(),
-                                 objs_outside_scene_limit.end(), updated_obj);
-        objs_outside_scene_limit.erase(find_it);
-
-        objs_partially_penetrating_scene_limit.push_back(updated_obj);
-      }
-    }
-  } else {
-    if (old_status == Inside) {
-      // Status change: Inside --> Outside
-      // Required action(s):
-      // - add object to "objs_outside_scene_limit"
-
-      objs_outside_scene_limit.push_back(updated_obj);
-    } else if (old_status == PartiallyPenetrating) {
-      // Status change: PartiallyPenetrating --> Outside
-      // Required action(s):
-      // - remove object from "objs_partially_penetrating_scene_limit"
-      // - add object to "objs_outside_scene_limit"
-
-      auto find_it =
-          std::find(objs_partially_penetrating_scene_limit.begin(),
-                    objs_partially_penetrating_scene_limit.end(), updated_obj);
-      objs_partially_penetrating_scene_limit.erase(find_it);
-
-      objs_outside_scene_limit.push_back(updated_obj);
-    }
-  }
-
-  obj_aabb_map[updated_obj] = new_aabb;
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::update(
-    const std::vector<CollisionObject*>& updated_objs) {
-  for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::clear() {
-  objs.clear();
-  hash_table->clear();
-  objs_outside_scene_limit.clear();
-  obj_aabb_map.clear();
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::getObjects(
-    std::vector<CollisionObject*>& objs_) const {
-  objs_.resize(objs.size());
-  std::copy(objs.begin(), objs.end(), objs_.begin());
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::collide(
-    CollisionObject* obj, CollisionCallBackBase* callback) const {
-  if (size() == 0) return;
-  collide_(obj, callback);
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::distance(
-    CollisionObject* obj, DistanceCallBackBase* callback) const {
-  if (size() == 0) return;
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
-  distance_(obj, callback, min_dist);
-}
-
-//==============================================================================
-template <typename HashTable>
-bool SpatialHashingCollisionManager<HashTable>::collide_(
-    CollisionObject* obj, CollisionCallBackBase* callback) const {
-  const auto& obj_aabb = obj->getAABB();
-  AABB overlap_aabb;
-
-  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
-    const auto query_result = hash_table->query(overlap_aabb);
-    for (const auto& obj2 : query_result) {
-      if (obj == obj2) continue;
-
-      if ((*callback)(obj, obj2)) return true;
-    }
-
-    if (!scene_limit.contain(obj_aabb)) {
-      for (const auto& obj2 : objs_outside_scene_limit) {
-        if (obj == obj2) continue;
-
-        if ((*callback)(obj, obj2)) return true;
-      }
-    }
-  } else {
-    for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
-      if (obj == obj2) continue;
-
-      if ((*callback)(obj, obj2)) return true;
-    }
-
-    for (const auto& obj2 : objs_outside_scene_limit) {
-      if (obj == obj2) continue;
-
-      if ((*callback)(obj, obj2)) return true;
-    }
-  }
-
-  return false;
-}
-
-//==============================================================================
-template <typename HashTable>
-bool SpatialHashingCollisionManager<HashTable>::distance_(
-    CollisionObject* obj, DistanceCallBackBase* callback,
-    FCL_REAL& min_dist) const {
-  auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
-  auto aabb = obj->getAABB();
-  if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) {
-    Vec3f min_dist_delta(min_dist, min_dist, min_dist);
-    aabb.expand(min_dist_delta);
-  }
-
-  AABB overlap_aabb;
-
-  auto status = 1;
-  FCL_REAL old_min_distance;
-
-  while (1) {
-    old_min_distance = min_dist;
-
-    if (scene_limit.overlap(aabb, overlap_aabb)) {
-      if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
-                                  callback, min_dist)) {
-        return true;
-      }
-
-      if (!scene_limit.contain(aabb)) {
-        if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
-                                    min_dist)) {
-          return true;
-        }
-      }
-    } else {
-      if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
-                                  callback, min_dist)) {
-        return true;
-      }
-
-      if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
-                                  min_dist)) {
-        return true;
-      }
-    }
-
-    if (status == 1) {
-      if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)()) {
-        break;
-      } else {
-        if (min_dist < old_min_distance) {
-          Vec3f min_dist_delta(min_dist, min_dist, min_dist);
-          aabb = AABB(obj->getAABB(), min_dist_delta);
-          status = 0;
-        } else {
-          if (aabb == obj->getAABB())
-            aabb.expand(delta);
-          else
-            aabb.expand(obj->getAABB(), 2.0);
-        }
-      }
-    } else if (status == 0) {
-      break;
-    }
-  }
-
-  return false;
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::collide(
-    CollisionCallBackBase* callback) const {
-  if (size() == 0) return;
-
-  for (const auto& obj1 : objs) {
-    const auto& obj_aabb = obj1->getAABB();
-    AABB overlap_aabb;
-
-    if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
-      auto query_result = hash_table->query(overlap_aabb);
-      for (const auto& obj2 : query_result) {
-        if (obj1 < obj2) {
-          if ((*callback)(obj1, obj2)) return;
-        }
-      }
-
-      if (!scene_limit.contain(obj_aabb)) {
-        for (const auto& obj2 : objs_outside_scene_limit) {
-          if (obj1 < obj2) {
-            if ((*callback)(obj1, obj2)) return;
-          }
-        }
-      }
-    } else {
-      for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
-        if (obj1 < obj2) {
-          if ((*callback)(obj1, obj2)) return;
-        }
-      }
-
-      for (const auto& obj2 : objs_outside_scene_limit) {
-        if (obj1 < obj2) {
-          if ((*callback)(obj1, obj2)) return;
-        }
-      }
-    }
-  }
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::distance(
-    DistanceCallBackBase* callback) const {
-  if (size() == 0) return;
-
-  this->enable_tested_set_ = true;
-  this->tested_set.clear();
-
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
-
-  for (const auto& obj : objs) {
-    if (distance_(obj, callback, min_dist)) break;
-  }
-
-  this->enable_tested_set_ = false;
-  this->tested_set.clear();
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::collide(
-    BroadPhaseCollisionManager* other_manager_,
-    CollisionCallBackBase* callback) const {
-  auto* other_manager =
-      static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
-
-  if ((size() == 0) || (other_manager->size() == 0)) return;
-
-  if (this == other_manager) {
-    collide(callback);
-    return;
-  }
-
-  if (this->size() < other_manager->size()) {
-    for (const auto& obj : objs) {
-      if (other_manager->collide_(obj, callback)) return;
-    }
-  } else {
-    for (const auto& obj : other_manager->objs) {
-      if (collide_(obj, callback)) return;
-    }
-  }
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::distance(
-    BroadPhaseCollisionManager* other_manager_,
-    DistanceCallBackBase* callback) const {
-  auto* other_manager =
-      static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
-
-  if ((size() == 0) || (other_manager->size() == 0)) return;
-
-  if (this == other_manager) {
-    distance(callback);
-    return;
-  }
-
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
-
-  if (this->size() < other_manager->size()) {
-    for (const auto& obj : objs)
-      if (other_manager->distance_(obj, callback, min_dist)) return;
-  } else {
-    for (const auto& obj : other_manager->objs)
-      if (distance_(obj, callback, min_dist)) return;
-  }
-}
-
-//==============================================================================
-template <typename HashTable>
-bool SpatialHashingCollisionManager<HashTable>::empty() const {
-  return objs.empty();
-}
-
-//==============================================================================
-template <typename HashTable>
-size_t SpatialHashingCollisionManager<HashTable>::size() const {
-  return objs.size();
-}
-
-//==============================================================================
-template <typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::computeBound(
-    std::vector<CollisionObject*>& objs, Vec3f& l, Vec3f& u) {
-  AABB bound;
-  for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
-
-  l = bound.min_;
-  u = bound.max_;
-}
-
-//==============================================================================
-template <typename HashTable>
-template <typename Container>
-bool SpatialHashingCollisionManager<HashTable>::distanceObjectToObjects(
-    CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback,
-    FCL_REAL& min_dist) const {
-  for (auto& obj2 : objs) {
-    if (obj == obj2) continue;
-
-    if (!this->enable_tested_set_) {
-      if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
-        if ((*callback)(obj, obj2, min_dist)) return true;
-      }
-    } else {
-      if (!this->inTestedSet(obj, obj2)) {
-        if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
-          if ((*callback)(obj, obj2, min_dist)) return true;
-        }
-
-        this->insertTestedSet(obj, obj2);
-      }
-    }
-  }
-
-  return false;
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_spatialhash-inl.h>
diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash.h b/include/hpp/fcl/broadphase/broadphase_spatialhash.h
index 606b82ada32ec907fac56c57665b61cb6797532c..5ad5260778aec64627e1ea9a12de1b22ab007160 100644
--- a/include/hpp/fcl/broadphase/broadphase_spatialhash.h
+++ b/include/hpp/fcl/broadphase/broadphase_spatialhash.h
@@ -1,170 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_H
-#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_H
-
-#include <list>
-#include <map>
-#include "hpp/fcl/BV/AABB.h"
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
-#include "hpp/fcl/broadphase/detail/simple_hash_table.h"
-#include "hpp/fcl/broadphase/detail/sparse_hash_table.h"
-#include "hpp/fcl/broadphase/detail/spatial_hash.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief spatial hashing collision mananger
-template <typename HashTable = detail::SimpleHashTable<AABB, CollisionObject*,
-                                                       detail::SpatialHash> >
-class SpatialHashingCollisionManager : public BroadPhaseCollisionManager {
- public:
-  typedef BroadPhaseCollisionManager Base;
-  using Base::getObjects;
-
-  SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min,
-                                 const Vec3f& scene_max,
-                                 unsigned int default_table_size = 1000);
-
-  ~SpatialHashingCollisionManager();
-
-  /// @brief add one object to the manager
-  void registerObject(CollisionObject* obj);
-
-  /// @brief remove one object from the manager
-  void unregisterObject(CollisionObject* obj);
-
-  /// @brief initialize the manager, related with the specific type of manager
-  void setup();
-
-  /// @brief update the condition of manager
-  virtual void update();
-
-  /// @brief update the manager by explicitly given the object updated
-  void update(CollisionObject* updated_obj);
-
-  /// @brief update the manager by explicitly given the set of objects update
-  void update(const std::vector<CollisionObject*>& updated_objs);
-
-  /// @brief clear the manager
-  void clear();
-
-  /// @brief return the objects managed by the manager
-  void getObjects(std::vector<CollisionObject*>& objs) const;
-
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging ot the manager
-  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test for the objects belonging to the manager
-  /// (i.e, N^2 self collision)
-  void collide(CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test for the objects belonging to the manager
-  /// (i.e., N^2 self distance)
-  void distance(DistanceCallBackBase* callback) const;
-
-  /// @brief perform collision test with objects belonging to another manager
-  void collide(BroadPhaseCollisionManager* other_manager,
-               CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance test with objects belonging to another manager
-  void distance(BroadPhaseCollisionManager* other_manager,
-                DistanceCallBackBase* callback) const;
-
-  /// @brief whether the manager is empty
-  bool empty() const;
-
-  /// @brief the number of objects managed by the manager
-  size_t size() const;
-
-  /// @brief compute the bound for the environent
-  static void computeBound(std::vector<CollisionObject*>& objs, Vec3f& l,
-                           Vec3f& u);
-
- protected:
-  /// @brief perform collision test between one object and all the objects
-  /// belonging to the manager
-  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
-
-  /// @brief perform distance computation between one object and all the objects
-  /// belonging ot the manager
-  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
-                 FCL_REAL& min_dist) const;
-
-  /// @brief all objects in the scene
-  std::list<CollisionObject*> objs;
-
-  /// @brief objects partially penetrating (not totally inside nor outside) the
-  /// scene limit are in another list
-  std::list<CollisionObject*> objs_partially_penetrating_scene_limit;
-
-  /// @brief objects outside the scene limit are in another list
-  std::list<CollisionObject*> objs_outside_scene_limit;
-
-  /// @brief the size of the scene
-  AABB scene_limit;
-
-  /// @brief store the map between objects and their aabbs. will make update
-  /// more convenient
-  std::map<CollisionObject*, AABB> obj_aabb_map;
-
-  /// @brief objects in the scene limit (given by scene_min and scene_max) are
-  /// in the spatial hash table
-  HashTable* hash_table;
-
- private:
-  enum ObjectStatus { Inside, PartiallyPenetrating, Outside };
-
-  template <typename Container>
-  bool distanceObjectToObjects(CollisionObject* obj, const Container& objs,
-                               DistanceCallBackBase* callback,
-                               FCL_REAL& min_dist) const;
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/broadphase_spatialhash-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/broadphase_spatialhash.h>
diff --git a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h
index b984226dac29108e2d1e61978e8cdf14c69442d3..7d78f9532a990f2d135c601ab23a5f62a2d930ba 100644
--- a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h
+++ b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h
@@ -1,258 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2020, Toyota Research Institute
- *  Copyright (c) 2022-2023, INRIA
- *  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 the copyright holder 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 Sean Curtis (sean@tri.global) */
-/** @author Justin Carpentier (justin.carpentier@inria.fr) */
-
-#ifndef HPP_FCL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
-#define HPP_FCL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
-
-#include "hpp/fcl/broadphase/broadphase_callbacks.h"
-#include "hpp/fcl/collision.h"
-#include "hpp/fcl/distance.h"
-// #include "hpp/fcl/narrowphase/continuous_collision.h"
-// #include "hpp/fcl/narrowphase/continuous_collision_request.h"
-// #include "hpp/fcl/narrowphase/continuous_collision_result.h"
-// #include "hpp/fcl/narrowphase/distance_request.h"
-// #include "hpp/fcl/narrowphase/distance_result.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Collision data stores the collision request and the result given by
-/// collision algorithm.
-struct CollisionData {
-  CollisionData() { done = false; }
-
-  /// @brief Collision request
-  CollisionRequest request;
-
-  /// @brief Collision result
-  CollisionResult result;
-
-  /// @brief Whether the collision iteration can stop
-  bool done;
-
-  /// @brief Clears the CollisionData
-  void clear() {
-    result.clear();
-    done = false;
-  }
-};
-
-/// @brief Distance data stores the distance request and the result given by
-/// distance algorithm.
-struct DistanceData {
-  DistanceData() { done = false; }
-
-  /// @brief Distance request
-  DistanceRequest request;
-
-  /// @brief Distance result
-  DistanceResult result;
-
-  /// @brief Whether the distance iteration can stop
-  bool done;
-
-  /// @brief Clears the DistanceData
-  void clear() {
-    result.clear();
-    done = false;
-  }
-};
-
-/// @brief Provides a simple callback for the collision query in the
-/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
-/// points to an instance of CollisionData. It simply invokes the
-/// `collide()` method on the culled pair of geometries and stores the results
-/// in the data's CollisionResult instance.
-///
-/// This callback will cause the broadphase evaluation to stop if:
-///   - the collision requests _disables_ cost _and_
-///   - the collide() reports a collision for the culled pair, _and_
-///   - we've reported the number of contacts requested in the CollisionRequest.
-///
-/// For a given instance of CollisionData, if broadphase evaluation has
-/// already terminated (i.e., defaultCollisionFunction() returned `true`),
-/// subsequent invocations with the same instance of CollisionData will
-/// return immediately, requesting termination of broadphase evaluation (i.e.,
-/// return `true`).
-///
-/// @param o1   The first object in the culled pair.
-/// @param o2   The second object in the culled pair.
-/// @param data A non-null pointer to a CollisionData instance.
-/// @return `true` if the broadphase evaluation should stop.
-/// @tparam S   The scalar type with which the computation will be performed.
-bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
-                              void* data);
-
-/// @brief Collision data for use with the DefaultContinuousCollisionFunction.
-/// It stores the collision request and the result given by the collision
-/// algorithm (and stores the conclusion of whether further evaluation of the
-/// broadphase collision manager has been deemed unnecessary).
-// struct DefaultContinuousCollisionData {
-//   ContinuousCollisionRequest request;
-//   ContinuousCollisionResult result;
-//
-//   /// If `true`, requests that the broadphase evaluation stop.
-//   bool done{false};
-// };
-
-/// @brief Provides a simple callback for the continuous collision query in the
-/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
-/// points to an instance of DefaultContinuousCollisionData. It simply invokes
-/// the `collide()` method on the culled pair of geometries and stores the
-/// results in the data's ContinuousCollisionResult instance.
-///
-/// This callback will never cause the broadphase evaluation to terminate early.
-/// However, if the `done` member of the DefaultContinuousCollisionData is set
-/// to true, this method will simply return without doing any computation.
-///
-/// For a given instance of DefaultContinuousCollisionData, if broadphase
-/// evaluation has already terminated (i.e.,
-/// DefaultContinuousCollisionFunction() returned `true`), subsequent
-/// invocations with the same instance of CollisionData will return
-/// immediately, requesting termination of broadphase evaluation (i.e., return
-/// `true`).
-///
-/// @param o1   The first object in the culled pair.
-/// @param o2   The second object in the culled pair.
-/// @param data A non-null pointer to a CollisionData instance.
-/// @return True if the broadphase evaluation should stop.
-/// @tparam S   The scalar type with which the computation will be performed.
-// bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1,
-//                                         ContinuousCollisionObject* o2,
-//                                         void* data) {
-//   assert(data != nullptr);
-//   auto* cdata = static_cast<DefaultContinuousCollisionData*>(data);
-//
-//   if (cdata->done) return true;
-//
-//   const ContinuousCollisionRequest& request = cdata->request;
-//   ContinuousCollisionResult& result = cdata->result;
-//   collide(o1, o2, request, result);
-//
-//   return cdata->done;
-// }
-
-/// @brief Provides a simple callback for the distance query in the
-/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
-/// points to an instance of DistanceData. It simply invokes the
-/// `distance()` method on the culled pair of geometries and stores the results
-/// in the data's DistanceResult instance.
-///
-/// This callback will cause the broadphase evaluation to stop if:
-///   - The distance is less than or equal to zero (i.e., the pair is in
-///     contact).
-///
-/// For a given instance of DistanceData, if broadphase evaluation has
-/// already terminated (i.e., defaultDistanceFunction() returned `true`),
-/// subsequent invocations with the same instance of DistanceData will
-/// simply report the previously reported minimum distance and request
-/// termination of broadphase evaluation (i.e., return `true`).
-///
-/// @param o1     The first object in the culled pair.
-/// @param o2     The second object in the culled pair.
-/// @param data   A non-null pointer to a DistanceData instance.
-/// @param dist   The distance computed by distance().
-/// @return `true` if the broadphase evaluation should stop.
-/// @tparam S   The scalar type with which the computation will be performed.
-bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
-                             void* data, FCL_REAL& dist);
-
-/// @brief Default collision callback to check collision between collision
-/// objects.
-struct HPP_FCL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase {
-  /// @brief Initialize the callback.
-  /// Clears the collision result and sets the done boolean to false.
-  void init() { data.clear(); }
-
-  bool collide(CollisionObject* o1, CollisionObject* o2);
-
-  CollisionData data;
-
-  virtual ~CollisionCallBackDefault() {};
-};
-
-/// @brief Default distance callback to check collision between collision
-/// objects.
-struct HPP_FCL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase {
-  /// @brief Initialize the callback.
-  /// Clears the distance result and sets the done boolean to false.
-  void init() { data.clear(); }
-
-  bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist);
-
-  DistanceData data;
-
-  virtual ~DistanceCallBackDefault() {};
-};
-
-/// @brief Collision callback to collect collision pairs potentially in contacts
-struct HPP_FCL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase {
-  typedef std::pair<CollisionObject*, CollisionObject*> CollisionPair;
-
-  /// @brief Default constructor.
-  CollisionCallBackCollect(const size_t max_size);
-
-  bool collide(CollisionObject* o1, CollisionObject* o2);
-
-  /// @brief Returns the number of registered collision pairs
-  size_t numCollisionPairs() const;
-
-  /// @brief Returns a const reference to the active collision_pairs to check
-  const std::vector<CollisionPair>& getCollisionPairs() const;
-
-  /// @brief Reset the callback
-  void init();
-
-  /// @brief Check whether a collision pair exists
-  bool exist(const CollisionPair& pair) const;
-
-  /// @brief Check whether a collision pair exists
-  bool exist(CollisionObject* o1, CollisionObject* o2) const;
-
-  virtual ~CollisionCallBackCollect() {};
-
- protected:
-  std::vector<CollisionPair> collision_pairs;
-  size_t max_size;
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif  // HPP_FCL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/default_broadphase_callbacks.h>
diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h
index 36d8df358b75847e5e769bcbb22d4e6489a1696f..54b709d7973e806ed0e2b29016e2d0878eb801de 100644
--- a/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h
+++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h
@@ -1,1000 +1,2 @@
-/*
- * 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 HPP_FCL_HIERARCHY_TREE_INL_H
-#define HPP_FCL_HIERARCHY_TREE_INL_H
-
-#include "hpp/fcl/broadphase/detail/hierarchy_tree.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-//==============================================================================
-template <typename BV>
-HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
-  root_node = nullptr;
-  n_leaves = 0;
-  free_node = nullptr;
-  max_lookahead_level = -1;
-  opath = 0;
-  bu_threshold = bu_threshold_;
-  topdown_level = topdown_level_;
-}
-
-//==============================================================================
-template <typename BV>
-HierarchyTree<BV>::~HierarchyTree() {
-  clear();
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init(std::vector<Node*>& leaves, int level) {
-  switch (level) {
-    case 0:
-      init_0(leaves);
-      break;
-    case 1:
-      init_1(leaves);
-      break;
-    case 2:
-      init_2(leaves);
-      break;
-    case 3:
-      init_3(leaves);
-      break;
-    default:
-      init_0(leaves);
-  }
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::insert(const BV& bv,
-                                                            void* data) {
-  Node* leaf = createNode(nullptr, bv, data);
-  insertLeaf(root_node, leaf);
-  ++n_leaves;
-  return leaf;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::remove(Node* leaf) {
-  removeLeaf(leaf);
-  deleteNode(leaf);
-  --n_leaves;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::clear() {
-  if (root_node) recurseDeleteNode(root_node);
-  n_leaves = 0;
-  delete free_node;
-  free_node = nullptr;
-  max_lookahead_level = -1;
-  opath = 0;
-}
-
-//==============================================================================
-template <typename BV>
-bool HierarchyTree<BV>::empty() const {
-  return (nullptr == root_node);
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::update(Node* leaf, int lookahead_level) {
-  // TODO(DamrongGuoy): Since we update a leaf node by removing and
-  //  inserting the same leaf node, it is likely to change the structure of
-  //  the tree even if no object's pose has changed. In the future,
-  //  find a way to preserve the structure of the tree to solve this issue:
-  //  https://github.com/flexible-collision-library/fcl/issues/368
-
-  // First we remove the leaf node pointed by `leaf` variable from the tree.
-  // The `sub_root` variable is the root of the subtree containing nodes
-  // whose BVs were adjusted due to node removal.
-  typename HierarchyTree<BV>::Node* sub_root = removeLeaf(leaf);
-  if (sub_root) {
-    if (lookahead_level > 0) {
-      // For positive `lookahead_level`, we move the `sub_root` variable up.
-      for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i)
-        sub_root = sub_root->parent;
-    } else
-      // By default, lookahead_level = -1, and we reset the `sub_root` variable
-      // to the root of the entire tree.
-      sub_root = root_node;
-  }
-  // Then we insert the node pointed by `leaf` variable back into the
-  // subtree rooted at `sub_root` variable.
-  insertLeaf(sub_root, leaf);
-}
-
-//==============================================================================
-template <typename BV>
-bool HierarchyTree<BV>::update(Node* leaf, const BV& bv) {
-  if (leaf->bv.contain(bv)) return false;
-  update_(leaf, bv);
-  return true;
-}
-
-//==============================================================================
-template <typename S, typename BV>
-struct UpdateImpl {
-  static bool run(const HierarchyTree<BV>& tree,
-                  typename HierarchyTree<BV>::Node* leaf, const BV& bv,
-                  const Vec3f& /*vel*/, FCL_REAL /*margin*/) {
-    if (leaf->bv.contain(bv)) return false;
-    tree.update_(leaf, bv);
-    return true;
-  }
-
-  static bool run(const HierarchyTree<BV>& tree,
-                  typename HierarchyTree<BV>::Node* leaf, const BV& bv,
-                  const Vec3f& /*vel*/) {
-    if (leaf->bv.contain(bv)) return false;
-    tree.update_(leaf, bv);
-    return true;
-  }
-};
-
-//==============================================================================
-template <typename BV>
-bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel,
-                               FCL_REAL margin) {
-  return UpdateImpl<FCL_REAL, BV>::run(*this, leaf, bv, vel, margin);
-}
-
-//==============================================================================
-template <typename BV>
-bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel) {
-  return UpdateImpl<FCL_REAL, BV>::run(*this, leaf, bv, vel);
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::getMaxHeight() const {
-  if (!root_node) return 0;
-  return getMaxHeight(root_node);
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::getMaxDepth() const {
-  if (!root_node) return 0;
-
-  size_t max_depth;
-  getMaxDepth(root_node, 0, max_depth);
-  return max_depth;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::balanceBottomup() {
-  if (root_node) {
-    std::vector<Node*> leaves;
-    leaves.reserve(n_leaves);
-    fetchLeaves(root_node, leaves);
-    bottomup(leaves.begin(), leaves.end());
-    root_node = leaves[0];
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::balanceTopdown() {
-  if (root_node) {
-    std::vector<Node*> leaves;
-    leaves.reserve(n_leaves);
-    fetchLeaves(root_node, leaves);
-    root_node = topdown(leaves.begin(), leaves.end());
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::balanceIncremental(int iterations) {
-  if (iterations < 0) iterations = (int)n_leaves;
-  if (root_node && (iterations > 0)) {
-    for (int i = 0; i < iterations; ++i) {
-      Node* node = root_node;
-      unsigned int bit = 0;
-      while (!node->isLeaf()) {
-        node = sort(node, root_node)->children[(opath >> bit) & 1];
-        bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
-      }
-      update(node);
-      ++opath;
-    }
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::refit() {
-  if (root_node) recurseRefit(root_node);
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::extractLeaves(const Node* root,
-                                      std::vector<Node*>& leaves) const {
-  if (!root->isLeaf()) {
-    extractLeaves(root->children[0], leaves);
-    extractLeaves(root->children[1], leaves);
-  } else
-    leaves.push_back(root);
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::size() const {
-  return n_leaves;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getRoot() const {
-  return root_node;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node*& HierarchyTree<BV>::getRoot() {
-  return root_node;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::print(Node* root, int depth) {
-  for (int i = 0; i < depth; ++i) std::cout << " ";
-  std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", "
-            << root->bv.min_[2] << "; " << root->bv.max_[0] << ", "
-            << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
-  if (root->isLeaf()) {
-  } else {
-    print(root->children[0], depth + 1);
-    print(root->children[1], depth + 1);
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::bottomup(const NodeVecIterator lbeg,
-                                 const NodeVecIterator lend) {
-  NodeVecIterator lcur_end = lend;
-  while (lbeg < lcur_end - 1) {
-    NodeVecIterator min_it1 = lbeg;
-    NodeVecIterator min_it2 = lbeg + 1;
-    FCL_REAL min_size = (std::numeric_limits<FCL_REAL>::max)();
-    for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) {
-      for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) {
-        FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size();
-        if (cur_size < min_size) {
-          min_size = cur_size;
-          min_it1 = it1;
-          min_it2 = it2;
-        }
-      }
-    }
-
-    Node* n[2] = {*min_it1, *min_it2};
-    Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr);
-    p->children[0] = n[0];
-    p->children[1] = n[1];
-    n[0]->parent = p;
-    n[1]->parent = p;
-    *min_it1 = p;
-    Node* tmp = *min_it2;
-    lcur_end--;
-    *min_it2 = *lcur_end;
-    *lcur_end = tmp;
-  }
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown(
-    const NodeVecIterator lbeg, const NodeVecIterator lend) {
-  switch (topdown_level) {
-    case 0:
-      return topdown_0(lbeg, lend);
-      break;
-    case 1:
-      return topdown_1(lbeg, lend);
-      break;
-    default:
-      return topdown_0(lbeg, lend);
-  }
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::getMaxHeight(Node* node) const {
-  if (!node->isLeaf()) {
-    size_t height1 = getMaxHeight(node->children[0]);
-    size_t height2 = getMaxHeight(node->children[1]);
-    return std::max(height1, height2) + 1;
-  } else
-    return 0;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::getMaxDepth(Node* node, size_t depth,
-                                    size_t& max_depth) const {
-  if (!node->isLeaf()) {
-    getMaxDepth(node->children[0], depth + 1, max_depth);
-    getMaxDepth(node->children[1], depth + 1, max_depth);
-  } else
-    max_depth = std::max(max_depth, depth);
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_0(
-    const NodeVecIterator lbeg, const NodeVecIterator lend) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    if (num_leaves > bu_threshold) {
-      BV vol = (*lbeg)->bv;
-      for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv;
-
-      int best_axis = 0;
-      FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
-      if (extent[1] > extent[0]) best_axis = 1;
-      if (extent[2] > extent[best_axis]) best_axis = 2;
-
-      // compute median
-      NodeVecIterator lcenter = lbeg + num_leaves / 2;
-      std::nth_element(lbeg, lcenter, lend,
-                       std::bind(&nodeBaseLess<BV>, std::placeholders::_1,
-                                 std::placeholders::_2, std::ref(best_axis)));
-
-      Node* node = createNode(nullptr, vol, nullptr);
-      node->children[0] = topdown_0(lbeg, lcenter);
-      node->children[1] = topdown_0(lcenter, lend);
-      node->children[0]->parent = node;
-      node->children[1]->parent = node;
-      return node;
-    } else {
-      bottomup(lbeg, lend);
-      return *lbeg;
-    }
-  }
-  return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_1(
-    const NodeVecIterator lbeg, const NodeVecIterator lend) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    if (num_leaves > bu_threshold) {
-      Vec3f split_p = (*lbeg)->bv.center();
-      BV vol = (*lbeg)->bv;
-      NodeVecIterator it;
-      for (it = lbeg + 1; it < lend; ++it) {
-        split_p += (*it)->bv.center();
-        vol += (*it)->bv;
-      }
-      split_p /= static_cast<FCL_REAL>(num_leaves);
-      int best_axis = -1;
-      long bestmidp = num_leaves;
-      int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
-      for (it = lbeg; it < lend; ++it) {
-        Vec3f x = (*it)->bv.center() - split_p;
-        for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
-      }
-
-      for (int i = 0; i < 3; ++i) {
-        if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
-          long midp = std::abs(splitcount[i][0] - splitcount[i][1]);
-          if (midp < bestmidp) {
-            best_axis = i;
-            bestmidp = midp;
-          }
-        }
-      }
-
-      if (best_axis < 0) best_axis = 0;
-
-      FCL_REAL split_value = split_p[best_axis];
-      NodeVecIterator lcenter = lbeg;
-      for (it = lbeg; it < lend; ++it) {
-        if ((*it)->bv.center()[best_axis] < split_value) {
-          Node* temp = *it;
-          *it = *lcenter;
-          *lcenter = temp;
-          ++lcenter;
-        }
-      }
-
-      Node* node = createNode(nullptr, vol, nullptr);
-      node->children[0] = topdown_1(lbeg, lcenter);
-      node->children[1] = topdown_1(lcenter, lend);
-      node->children[0]->parent = node;
-      node->children[1]->parent = node;
-      return node;
-    } else {
-      bottomup(lbeg, lend);
-      return *lbeg;
-    }
-  }
-  return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init_0(std::vector<Node*>& leaves) {
-  clear();
-  root_node = topdown(leaves.begin(), leaves.end());
-  n_leaves = leaves.size();
-  max_lookahead_level = -1;
-  opath = 0;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init_1(std::vector<Node*>& leaves) {
-  clear();
-
-  BV bound_bv;
-  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
-  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
-
-  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
-  for (size_t i = 0; i < leaves.size(); ++i)
-    leaves[i]->code = coder(leaves[i]->bv.center());
-
-  std::sort(leaves.begin(), leaves.end(), SortByMorton());
-
-  root_node = mortonRecurse_0(leaves.begin(), leaves.end(),
-                              (1 << (coder.bits() - 1)), coder.bits() - 1);
-
-  refit();
-  n_leaves = leaves.size();
-  max_lookahead_level = -1;
-  opath = 0;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init_2(std::vector<Node*>& leaves) {
-  clear();
-
-  BV bound_bv;
-  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
-  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
-
-  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
-  for (size_t i = 0; i < leaves.size(); ++i)
-    leaves[i]->code = coder(leaves[i]->bv.center());
-
-  std::sort(leaves.begin(), leaves.end(), SortByMorton());
-
-  root_node = mortonRecurse_1(leaves.begin(), leaves.end(),
-                              (1 << (coder.bits() - 1)), coder.bits() - 1);
-
-  refit();
-  n_leaves = leaves.size();
-  max_lookahead_level = -1;
-  opath = 0;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init_3(std::vector<Node*>& leaves) {
-  clear();
-
-  BV bound_bv;
-  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
-  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
-
-  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
-  for (size_t i = 0; i < leaves.size(); ++i)
-    leaves[i]->code = coder(leaves[i]->bv.center());
-
-  std::sort(leaves.begin(), leaves.end(), SortByMorton());
-
-  root_node = mortonRecurse_2(leaves.begin(), leaves.end());
-
-  refit();
-  n_leaves = leaves.size();
-  max_lookahead_level = -1;
-  opath = 0;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_0(
-    const NodeVecIterator lbeg, const NodeVecIterator lend,
-    const uint32_t& split, int bits) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    if (bits > 0) {
-      Node dummy;
-      dummy.code = split;
-      NodeVecIterator lcenter =
-          std::lower_bound(lbeg, lend, &dummy, SortByMorton());
-
-      if (lcenter == lbeg) {
-        uint32_t split2 = split | (1 << (bits - 1));
-        return mortonRecurse_0(lbeg, lend, split2, bits - 1);
-      } else if (lcenter == lend) {
-        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
-        return mortonRecurse_0(lbeg, lend, split1, bits - 1);
-      } else {
-        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
-        uint32_t split2 = split | (1 << (bits - 1));
-
-        Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
-        Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
-        Node* node = createNode(nullptr, nullptr);
-        node->children[0] = child1;
-        node->children[1] = child2;
-        child1->parent = node;
-        child2->parent = node;
-        return node;
-      }
-    } else {
-      Node* node = topdown(lbeg, lend);
-      return node;
-    }
-  } else
-    return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_1(
-    const NodeVecIterator lbeg, const NodeVecIterator lend,
-    const uint32_t& split, int bits) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    if (bits > 0) {
-      Node dummy;
-      dummy.code = split;
-      NodeVecIterator lcenter =
-          std::lower_bound(lbeg, lend, &dummy, SortByMorton());
-
-      if (lcenter == lbeg) {
-        uint32_t split2 = split | (1 << (bits - 1));
-        return mortonRecurse_1(lbeg, lend, split2, bits - 1);
-      } else if (lcenter == lend) {
-        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
-        return mortonRecurse_1(lbeg, lend, split1, bits - 1);
-      } else {
-        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
-        uint32_t split2 = split | (1 << (bits - 1));
-
-        Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
-        Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
-        Node* node = createNode(nullptr, nullptr);
-        node->children[0] = child1;
-        node->children[1] = child2;
-        child1->parent = node;
-        child2->parent = node;
-        return node;
-      }
-    } else {
-      Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
-      Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
-      Node* node = createNode(nullptr, nullptr);
-      node->children[0] = child1;
-      node->children[1] = child2;
-      child1->parent = node;
-      child2->parent = node;
-      return node;
-    }
-  } else
-    return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_2(
-    const NodeVecIterator lbeg, const NodeVecIterator lend) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
-    Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
-    Node* node = createNode(nullptr, nullptr);
-    node->children[0] = child1;
-    node->children[1] = child2;
-    child1->parent = node;
-    child2->parent = node;
-    return node;
-  } else
-    return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::update_(Node* leaf, const BV& bv) {
-  Node* root = removeLeaf(leaf);
-  if (root) {
-    if (max_lookahead_level >= 0) {
-      for (int i = 0; (i < max_lookahead_level) && root->parent; ++i)
-        root = root->parent;
-    } else
-      root = root_node;
-  }
-
-  leaf->bv = bv;
-  insertLeaf(root, leaf);
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::sort(Node* n, Node*& r) {
-  Node* p = n->parent;
-  if (p > n) {
-    size_t i = indexOf(n);
-    size_t j = 1 - i;
-    Node* s = p->children[j];
-    Node* q = p->parent;
-    if (q)
-      q->children[indexOf(p)] = n;
-    else
-      r = n;
-    s->parent = n;
-    p->parent = n;
-    n->parent = q;
-    p->children[0] = n->children[0];
-    p->children[1] = n->children[1];
-    n->children[0]->parent = p;
-    n->children[1]->parent = p;
-    n->children[i] = p;
-    n->children[j] = s;
-    std::swap(p->bv, n->bv);
-    return p;
-  }
-  return n;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::insertLeaf(Node* const sub_root, Node* const leaf)
-// Attempts to insert `leaf` into a subtree rooted at `sub_root`.
-// 1. If the whole tree is empty, then `leaf` simply becomes the tree.
-// 2. Otherwise, a leaf node called `sibling` of the subtree rooted at
-//    `sub_root` is selected (the selection criteria is a black box for this
-//    algorithm), and the `sibling` leaf is replaced by an internal node with
-//    two children, `sibling` and `leaf`. The bounding volumes are updated as
-//    necessary.
-{
-  if (!root_node) {
-    // If the entire tree is empty, the node pointed by `leaf` variable will
-    // become the root of the tree.
-    root_node = leaf;
-    leaf->parent = nullptr;
-    return;
-  }
-  // Traverse the tree from the given `sub_root` down to an existing leaf
-  // node that we call `sibling`. The `sibling` node will eventually become
-  // the sibling of the given `leaf` node.
-  Node* sibling = sub_root;
-  while (!sibling->isLeaf()) {
-    sibling = sibling->children[select(*leaf, *(sibling->children[0]),
-                                       *(sibling->children[1]))];
-  }
-  Node* prev = sibling->parent;
-  // Create a new `node` that later will become the new parent of both the
-  // `sibling` and the given `leaf`.
-  Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr);
-  if (prev)
-  // If the parent `prev` of the `sibling` is an interior node, we will
-  // replace the `sibling` with the subtree {node {`sibling`, leaf}} like
-  // this:
-  //        Before                After
-  //
-  //          ╱                     ╱
-  //        prev                  prev
-  //        ╱  ╲        ─>        ╱  ╲
-  //  sibling  ...             node  ...
-  //                           ╱  ╲
-  //                     sibling  leaf
-  {
-    prev->children[indexOf(sibling)] = node;
-    node->children[0] = sibling;
-    sibling->parent = node;
-    node->children[1] = leaf;
-    leaf->parent = node;
-    // Now that we've inserted `leaf` some of the existing bounding
-    // volumes might not fully enclose their children. Walk up the tree
-    // looking for parents that don't already enclose their children
-    // and create a new tight-fitting bounding volume for those.
-    do {
-      if (!prev->bv.contain(node->bv))
-        prev->bv = prev->children[0]->bv + prev->children[1]->bv;
-      else
-        break;
-      node = prev;
-    } while (nullptr != (prev = node->parent));
-  } else
-  // If the `sibling` has no parent, i.e., the tree is a singleton,
-  // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like
-  // this:
-  //
-  //        node
-  //        ╱  ╲
-  //  sibling  leaf
-  {
-    node->children[0] = sibling;
-    sibling->parent = node;
-    node->children[1] = leaf;
-    leaf->parent = node;
-    root_node = node;
-  }
-
-  // Note that the above algorithm always adds the new `leaf` node as the right
-  // child, i.e., children[1].  Calling removeLeaf(l) followed by calling
-  // this function insertLeaf(l) where l is a left child will result in
-  // switching l and its sibling even if no object's pose has changed.
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::removeLeaf(
-    Node* const leaf) {
-  // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling,
-  // and its parent with just its sibling. It then "tightens" the ancestor
-  // bounding volumes. Returns a pointer to the parent of the highest node whose
-  // BV changed due to the removal.
-  if (leaf == root_node) {
-    // If the `leaf` node is the only node in the tree, the tree becomes empty.
-    root_node = nullptr;
-    return nullptr;
-  }
-  Node* parent = leaf->parent;
-  Node* prev = parent->parent;
-  Node* sibling = parent->children[1 - indexOf(leaf)];
-  if (prev) {
-    // If the parent node is not the root node, the sibling node will
-    // replace the parent node like this:
-    //
-    //            Before              After
-    //             ...                 ...
-    //             ╱                   ╱
-    //           prev                prev
-    //          ╱   ╲               ╱   ╲
-    //     parent   ...    ─>  sibling  ...
-    //      ╱  ╲                 ╱╲
-    //  leaf  sibling           ...
-    //           ╱╲
-    //          ...
-    //
-    // Step 1: replace the subtree {parent {leaf, sibling {...}}} with
-    // {sibling {...}}.
-    prev->children[indexOf(parent)] = sibling;
-    sibling->parent = prev;
-    deleteNode(parent);
-    // Step 2: tighten up the BVs of the ancestor nodes.
-    while (prev) {
-      BV new_bv = prev->children[0]->bv + prev->children[1]->bv;
-      if (!(new_bv == prev->bv)) {
-        prev->bv = new_bv;
-        prev = prev->parent;
-      } else
-        break;
-    }
-
-    return prev ? prev : root_node;
-  } else {
-    // If the parent node is the root node, the sibling node will become the
-    // root of the whole tree like this:
-    //
-    //     Before                   After
-    //
-    //     parent
-    //      ╱  ╲
-    //  leaf  sibling     ─>       sibling
-    //           ╱╲                  ╱╲
-    //          ...                 ...
-    root_node = sibling;
-    sibling->parent = nullptr;
-    deleteNode(parent);
-    return root_node;
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::fetchLeaves(Node* root, std::vector<Node*>& leaves,
-                                    int depth) {
-  if ((!root->isLeaf()) && depth) {
-    fetchLeaves(root->children[0], leaves, depth - 1);
-    fetchLeaves(root->children[1], leaves, depth - 1);
-    deleteNode(root);
-  } else {
-    leaves.push_back(root);
-  }
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::indexOf(Node* node) {
-  // node cannot be nullptr
-  return (node->parent->children[1] == node);
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
-                                                                const BV& bv,
-                                                                void* data) {
-  Node* node = createNode(parent, data);
-  node->bv = bv;
-  return node;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
-                                                                const BV& bv1,
-                                                                const BV& bv2,
-                                                                void* data) {
-  Node* node = createNode(parent, data);
-  node->bv = bv1 + bv2;
-  return node;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
-                                                                void* data) {
-  Node* node = nullptr;
-  if (free_node) {
-    node = free_node;
-    free_node = nullptr;
-  } else
-    node = new Node();
-  node->parent = parent;
-  node->data = data;
-  node->children[1] = 0;
-  return node;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::deleteNode(Node* node) {
-  if (free_node != node) {
-    delete free_node;
-    free_node = node;
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::recurseDeleteNode(Node* node) {
-  if (!node->isLeaf()) {
-    recurseDeleteNode(node->children[0]);
-    recurseDeleteNode(node->children[1]);
-  }
-
-  if (node == root_node) root_node = nullptr;
-  deleteNode(node);
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::recurseRefit(Node* node) {
-  if (!node->isLeaf()) {
-    recurseRefit(node->children[0]);
-    recurseRefit(node->children[1]);
-    node->bv = node->children[0]->bv + node->children[1]->bv;
-  } else
-    return;
-}
-
-//==============================================================================
-template <typename BV>
-bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d) {
-  if (a->bv.center()[d] < b->bv.center()[d]) return true;
-  return false;
-}
-
-//==============================================================================
-template <typename S, typename BV>
-struct SelectImpl {
-  static std::size_t run(const NodeBase<BV>& /*query*/,
-                         const NodeBase<BV>& /*node1*/,
-                         const NodeBase<BV>& /*node2*/) {
-    return 0;
-  }
-
-  static std::size_t run(const BV& /*query*/, const NodeBase<BV>& /*node1*/,
-                         const NodeBase<BV>& /*node2*/) {
-    return 0;
-  }
-};
-
-//==============================================================================
-template <typename BV>
-size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
-              const NodeBase<BV>& node2) {
-  return SelectImpl<FCL_REAL, BV>::run(query, node1, node2);
-}
-
-//==============================================================================
-template <typename BV>
-size_t select(const BV& query, const NodeBase<BV>& node1,
-              const NodeBase<BV>& node2) {
-  return SelectImpl<FCL_REAL, BV>::run(query, node1, node2);
-}
-
-//==============================================================================
-template <typename S>
-struct SelectImpl<S, AABB> {
-  static std::size_t run(const NodeBase<AABB>& node,
-                         const NodeBase<AABB>& node1,
-                         const NodeBase<AABB>& node2) {
-    const AABB& bv = node.bv;
-    const AABB& bv1 = node1.bv;
-    const AABB& bv2 = node2.bv;
-    Vec3f v = bv.min_ + bv.max_;
-    Vec3f v1 = v - (bv1.min_ + bv1.max_);
-    Vec3f v2 = v - (bv2.min_ + bv2.max_);
-    FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
-    FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
-    return (d1 < d2) ? 0 : 1;
-  }
-
-  static std::size_t run(const AABB& query, const NodeBase<AABB>& node1,
-                         const NodeBase<AABB>& node2) {
-    const AABB& bv = query;
-    const AABB& bv1 = node1.bv;
-    const AABB& bv2 = node2.bv;
-    Vec3f v = bv.min_ + bv.max_;
-    Vec3f v1 = v - (bv1.min_ + bv1.max_);
-    Vec3f v2 = v - (bv2.min_ + bv2.max_);
-    FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
-    FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
-    return (d1 < d2) ? 0 : 1;
-  }
-};
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/hierarchy_tree-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h
index 54cddd07939a284dff2cc65890b4b89a2047f8b7..ebacd648db5f6ec9fc533fdb18320634b4e188a6 100644
--- a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h
+++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h
@@ -1,294 +1,2 @@
-/*
- * 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 HPP_FCL_HIERARCHY_TREE_H
-#define HPP_FCL_HIERARCHY_TREE_H
-
-#include <vector>
-#include <map>
-#include <functional>
-#include <iostream>
-#include "hpp/fcl/warning.hh"
-#include "hpp/fcl/BV/AABB.h"
-#include "hpp/fcl/broadphase/detail/morton.h"
-#include "hpp/fcl/broadphase/detail/node_base.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-/// @brief Class for hierarchy tree structure
-template <typename BV>
-class HierarchyTree {
- public:
-  typedef NodeBase<BV> Node;
-
-  /// @brief Create hierarchy tree with suitable setting.
-  /// bu_threshold decides the height of tree node to start bottom-up
-  /// construction / optimization; topdown_level decides different methods to
-  /// construct tree in topdown manner. lower level method constructs tree with
-  /// better quality but is slower.
-  HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
-
-  ~HierarchyTree();
-
-  /// @brief Initialize the tree by a set of leaves using algorithm with a given
-  /// level.
-  void init(std::vector<Node*>& leaves, int level = 0);
-
-  /// @brief Insest a node
-  Node* insert(const BV& bv, void* data);
-
-  /// @brief Remove a leaf node
-  void remove(Node* leaf);
-
-  /// @brief Clear the tree
-  void clear();
-
-  /// @brief Whether the tree is empty
-  bool empty() const;
-
-  /// @brief Updates a `leaf` node. A use case is when the bounding volume
-  /// of an object changes. Ensure every parent node has its bounding volume
-  /// expand or shrink to fit the bounding volumes of its children.
-  /// @note Strangely the structure of the tree may change even if the
-  ///       bounding volume of the `leaf` node does not change. It is just
-  ///       another valid tree of the same set of objects.  This is because
-  ///       update() works by first removing `leaf` and then inserting `leaf`
-  ///       back. The structural change could be as simple as switching the
-  ///       order of two leaves if the sibling of the `leaf` is also a leaf.
-  ///       Or it could be more complicated if the sibling is an internal
-  ///       node.
-  void update(Node* leaf, int lookahead_level = -1);
-
-  /// @brief update the tree when the bounding volume of a given leaf has
-  /// changed
-  bool update(Node* leaf, const BV& bv);
-
-  /// @brief update one leaf's bounding volume, with prediction
-  bool update(Node* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin);
-
-  /// @brief update one leaf's bounding volume, with prediction
-  bool update(Node* leaf, const BV& bv, const Vec3f& vel);
-
-  /// @brief get the max height of the tree
-  size_t getMaxHeight() const;
-
-  /// @brief get the max depth of the tree
-  size_t getMaxDepth() const;
-
-  /// @brief balance the tree from bottom
-  void balanceBottomup();
-
-  /// @brief balance the tree from top
-  void balanceTopdown();
-
-  /// @brief balance the tree in an incremental way
-  void balanceIncremental(int iterations);
-
-  /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
-  /// update the entire tree in a bottom-up manner
-  void refit();
-
-  /// @brief extract all the leaves of the tree
-  void extractLeaves(const Node* root, std::vector<Node*>& leaves) const;
-
-  /// @brief number of leaves in the tree
-  size_t size() const;
-
-  /// @brief get the root of the tree
-  Node* getRoot() const;
-
-  Node*& getRoot();
-
-  /// @brief print the tree in a recursive way
-  void print(Node* root, int depth);
-
- private:
-  typedef typename std::vector<NodeBase<BV>*>::iterator NodeVecIterator;
-  typedef
-      typename std::vector<NodeBase<BV>*>::const_iterator NodeVecConstIterator;
-
-  struct SortByMorton {
-    bool operator()(const Node* a, const Node* b) const {
-      return a->code < b->code;
-    }
-  };
-
-  /// @brief construct a tree for a set of leaves from bottom -- very heavy way
-  void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
-
-  /// @brief construct a tree for a set of leaves from top
-  Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
-
-  /// @brief compute the maximum height of a subtree rooted from a given node
-  size_t getMaxHeight(Node* node) const;
-
-  /// @brief compute the maximum depth of a subtree rooted from a given node
-  void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const;
-
-  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
-  /// topdown manner. During construction, first compute the best split axis as
-  /// the axis along with the longest AABB edge. Then compute the median of all
-  /// nodes' center projection onto the axis and using it as the split
-  /// threshold.
-  Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
-
-  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
-  /// topdown manner. During construction, first compute the best split
-  /// thresholds for different axes as the average of all nodes' center. Then
-  /// choose the split axis as the axis whose threshold can divide the nodes
-  /// into two parts with almost similar size. This construction is more
-  /// expensive then topdown_0, but also can provide tree with better quality.
-  Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
-
-  /// @brief init tree from leaves in the topdown manner (topdown_0 or
-  /// topdown_1)
-  void init_0(std::vector<Node*>& leaves);
-
-  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
-  /// for nodes which is of depth more than the maximum bits of the morton code,
-  /// we use bottomup method to construct the subtree, which is slow but can
-  /// construct tree with high quality.
-  void init_1(std::vector<Node*>& leaves);
-
-  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
-  /// for nodes which is of depth more than the maximum bits of the morton code,
-  /// we split the leaves into two parts with the same size simply using the
-  /// node index.
-  void init_2(std::vector<Node*>& leaves);
-
-  /// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
-  /// for all nodes, we simply divide the leaves into parts with the same size
-  /// simply using the node index.
-  void init_3(std::vector<Node*>& leaves);
-
-  Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend,
-                        const uint32_t& split, int bits);
-
-  Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend,
-                        const uint32_t& split, int bits);
-
-  Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
-
-  /// @brief update one leaf node's bounding volume
-  void update_(Node* leaf, const BV& bv);
-
-  /// @brief sort node n and its parent according to their memory position
-  Node* sort(Node* n, Node*& r);
-
-  /// @brief Insert a leaf node and also update its ancestors. Maintain the
-  /// tree as a full binary tree (every interior node has exactly two children).
-  /// Furthermore, adjust the BV of interior nodes so that each parent's BV
-  /// tightly fits its children's BVs.
-  /// @param sub_root The root of the subtree into which we will insert the
-  //                  leaf node.
-  void insertLeaf(Node* const sub_root, Node* const leaf);
-
-  /// @brief Remove a leaf. Maintain the tree as a full binary tree (every
-  /// interior node has exactly two children). Furthermore, adjust the BV of
-  /// interior nodes so that each parent's BV tightly fits its children's BVs.
-  /// @note The leaf node itself is not deleted yet, but all the unnecessary
-  ///       internal nodes are deleted.
-  /// @returns the root of the subtree containing the nodes whose BVs were
-  //           adjusted.
-  Node* removeLeaf(Node* const leaf);
-
-  /// @brief Delete all internal nodes and return all leaves nodes with given
-  /// depth from root
-  void fetchLeaves(Node* root, std::vector<Node*>& leaves, int depth = -1);
-
-  static size_t indexOf(Node* node);
-
-  /// @brief create one node (leaf or internal)
-  Node* createNode(Node* parent, const BV& bv, void* data);
-
-  Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data);
-
-  Node* createNode(Node* parent, void* data);
-
-  void deleteNode(Node* node);
-
-  void recurseDeleteNode(Node* node);
-
-  void recurseRefit(Node* node);
-
- protected:
-  Node* root_node;
-
-  size_t n_leaves;
-
-  unsigned int opath;
-
-  /// This is a one Node cache, the reason is that we need to remove a node and
-  /// then add it again frequently.
-  Node* free_node;
-
-  int max_lookahead_level;
-
- public:
-  /// @brief decide which topdown algorithm to use
-  int topdown_level;
-
-  /// @brief decide the depth to use expensive bottom-up algorithm
-  int bu_threshold;
-};
-
-/// @brief Compare two nodes accoording to the d-th dimension of node center
-template <typename BV>
-bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d);
-
-/// @brief select from node1 and node2 which is close to a given query. 0 for
-/// node1 and 1 for node2
-template <typename BV>
-size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
-              const NodeBase<BV>& node2);
-
-/// @brief select from node1 and node2 which is close to a given query bounding
-/// volume. 0 for node1 and 1 for node2
-template <typename BV>
-size_t select(const BV& query, const NodeBase<BV>& node1,
-              const NodeBase<BV>& node2);
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/detail/hierarchy_tree-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/hierarchy_tree.h>
diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h
index 8bb715fe956c1c787bafd03b2143cffb7bbd7469..a6923fbb7d901d5a05c2d1b5777e70418d807ea9 100644
--- a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h
+++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h
@@ -1,980 +1,2 @@
-/*
- * 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 HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H
-#define HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H
-
-#include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h"
-
-#include <algorithm>
-#include <iostream>
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-namespace implementation_array {
-
-//==============================================================================
-template <typename BV>
-HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
-  root_node = NULL_NODE;
-  n_nodes = 0;
-  n_nodes_alloc = 16;
-  nodes = new Node[n_nodes_alloc];
-  for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
-  nodes[n_nodes_alloc - 1].next = NULL_NODE;
-  n_leaves = 0;
-  freelist = 0;
-  opath = 0;
-  max_lookahead_level = -1;
-  bu_threshold = bu_threshold_;
-  topdown_level = topdown_level_;
-}
-
-//==============================================================================
-template <typename BV>
-HierarchyTree<BV>::~HierarchyTree() {
-  delete[] nodes;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init(Node* leaves, int n_leaves_, int level) {
-  switch (level) {
-    case 0:
-      init_0(leaves, n_leaves_);
-      break;
-    case 1:
-      init_1(leaves, n_leaves_);
-      break;
-    case 2:
-      init_2(leaves, n_leaves_);
-      break;
-    case 3:
-      init_3(leaves, n_leaves_);
-      break;
-    default:
-      init_0(leaves, n_leaves_);
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init_0(Node* leaves, int n_leaves_) {
-  clear();
-
-  n_leaves = (size_t)n_leaves_;
-  root_node = NULL_NODE;
-  nodes = new Node[n_leaves * 2];
-  std::copy(leaves, leaves + n_leaves, nodes);
-  freelist = n_leaves;
-  n_nodes = n_leaves;
-  n_nodes_alloc = 2 * n_leaves;
-  for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
-  nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-  size_t* ids = new size_t[n_leaves];
-  for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
-
-  root_node = topdown(ids, ids + n_leaves);
-  delete[] ids;
-
-  opath = 0;
-  max_lookahead_level = -1;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init_1(Node* leaves, int n_leaves_) {
-  clear();
-
-  n_leaves = (size_t)n_leaves_;
-  root_node = NULL_NODE;
-  nodes = new Node[n_leaves * 2];
-  std::copy(leaves, leaves + n_leaves, nodes);
-  freelist = n_leaves;
-  n_nodes = n_leaves;
-  n_nodes_alloc = 2 * n_leaves;
-  for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
-  nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-  BV bound_bv;
-  if (n_leaves > 0) bound_bv = nodes[0].bv;
-  for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
-
-  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
-  for (size_t i = 0; i < n_leaves; ++i)
-    nodes[i].code = coder(nodes[i].bv.center());
-
-  size_t* ids = new size_t[n_leaves];
-  for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
-
-  const SortByMorton comp{nodes};
-  std::sort(ids, ids + n_leaves, comp);
-  root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
-                              coder.bits() - 1);
-  delete[] ids;
-
-  refit();
-
-  opath = 0;
-  max_lookahead_level = -1;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init_2(Node* leaves, int n_leaves_) {
-  clear();
-
-  n_leaves = (size_t)n_leaves_;
-  root_node = NULL_NODE;
-  nodes = new Node[n_leaves * 2];
-  std::copy(leaves, leaves + n_leaves, nodes);
-  freelist = n_leaves;
-  n_nodes = n_leaves;
-  n_nodes_alloc = 2 * n_leaves;
-  for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
-  nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-  BV bound_bv;
-  if (n_leaves > 0) bound_bv = nodes[0].bv;
-  for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
-
-  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
-  for (size_t i = 0; i < n_leaves; ++i)
-    nodes[i].code = coder(nodes[i].bv.center());
-
-  size_t* ids = new size_t[n_leaves];
-  for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
-
-  const SortByMorton comp{nodes};
-  std::sort(ids, ids + n_leaves, comp);
-  root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
-                              coder.bits() - 1);
-  delete[] ids;
-
-  refit();
-
-  opath = 0;
-  max_lookahead_level = -1;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::init_3(Node* leaves, int n_leaves_) {
-  clear();
-
-  n_leaves = (size_t)n_leaves_;
-  root_node = NULL_NODE;
-  nodes = new Node[n_leaves * 2];
-  std::copy(leaves, leaves + n_leaves, nodes);
-  freelist = n_leaves;
-  n_nodes = n_leaves;
-  n_nodes_alloc = 2 * n_leaves;
-  for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
-  nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-  BV bound_bv;
-  if (n_leaves > 0) bound_bv = nodes[0].bv;
-  for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
-
-  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
-  for (size_t i = 0; i < n_leaves; ++i)
-    nodes[i].code = coder(nodes[i].bv.center());
-
-  size_t* ids = new size_t[n_leaves];
-  for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
-
-  const SortByMorton comp{nodes};
-  std::sort(ids, ids + n_leaves, comp);
-  root_node = mortonRecurse_2(ids, ids + n_leaves);
-  delete[] ids;
-
-  refit();
-
-  opath = 0;
-  max_lookahead_level = -1;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::insert(const BV& bv, void* data) {
-  size_t node = createNode(NULL_NODE, bv, data);
-  insertLeaf(root_node, node);
-  ++n_leaves;
-  return node;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::remove(size_t leaf) {
-  removeLeaf(leaf);
-  deleteNode(leaf);
-  --n_leaves;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::clear() {
-  delete[] nodes;
-  root_node = NULL_NODE;
-  n_nodes = 0;
-  n_nodes_alloc = 16;
-  nodes = new Node[n_nodes_alloc];
-  for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
-  nodes[n_nodes_alloc - 1].next = NULL_NODE;
-  n_leaves = 0;
-  freelist = 0;
-  opath = 0;
-  max_lookahead_level = -1;
-}
-
-//==============================================================================
-template <typename BV>
-bool HierarchyTree<BV>::empty() const {
-  return (n_nodes == 0);
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::update(size_t leaf, int lookahead_level) {
-  size_t root = removeLeaf(leaf);
-  if (root != NULL_NODE) {
-    if (lookahead_level > 0) {
-      for (int i = 0;
-           (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
-        root = nodes[root].parent;
-    } else
-      root = root_node;
-  }
-  insertLeaf(root, leaf);
-}
-
-//==============================================================================
-template <typename BV>
-bool HierarchyTree<BV>::update(size_t leaf, const BV& bv) {
-  if (nodes[leaf].bv.contain(bv)) return false;
-  update_(leaf, bv);
-  return true;
-}
-
-//==============================================================================
-template <typename BV>
-bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel,
-                               FCL_REAL margin) {
-  HPP_FCL_UNUSED_VARIABLE(bv);
-  HPP_FCL_UNUSED_VARIABLE(vel);
-  HPP_FCL_UNUSED_VARIABLE(margin);
-
-  if (nodes[leaf].bv.contain(bv)) return false;
-  update_(leaf, bv);
-  return true;
-}
-
-//==============================================================================
-template <typename BV>
-bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel) {
-  HPP_FCL_UNUSED_VARIABLE(vel);
-
-  if (nodes[leaf].bv.contain(bv)) return false;
-  update_(leaf, bv);
-  return true;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::getMaxHeight() const {
-  if (root_node == NULL_NODE) return 0;
-
-  return getMaxHeight(root_node);
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::getMaxDepth() const {
-  if (root_node == NULL_NODE) return 0;
-
-  size_t max_depth;
-  getMaxDepth(root_node, 0, max_depth);
-  return max_depth;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::balanceBottomup() {
-  if (root_node != NULL_NODE) {
-    Node* leaves = new Node[n_leaves];
-    Node* leaves_ = leaves;
-    extractLeaves(root_node, leaves_);
-    root_node = NULL_NODE;
-    std::copy(leaves, leaves + n_leaves, nodes);
-    freelist = n_leaves;
-    n_nodes = n_leaves;
-    for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
-    nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-    size_t* ids = new size_t[n_leaves];
-    for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
-
-    bottomup(ids, ids + n_leaves);
-    root_node = *ids;
-
-    delete[] ids;
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::balanceTopdown() {
-  if (root_node != NULL_NODE) {
-    Node* leaves = new Node[n_leaves];
-    Node* leaves_ = leaves;
-    extractLeaves(root_node, leaves_);
-    root_node = NULL_NODE;
-    std::copy(leaves, leaves + n_leaves, nodes);
-    freelist = n_leaves;
-    n_nodes = n_leaves;
-    for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
-    nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-    size_t* ids = new size_t[n_leaves];
-    for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
-
-    root_node = topdown(ids, ids + n_leaves);
-    delete[] ids;
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::balanceIncremental(int iterations) {
-  if (iterations < 0) iterations = (int)n_leaves;
-  if ((root_node != NULL_NODE) && (iterations > 0)) {
-    for (int i = 0; i < iterations; ++i) {
-      size_t node = root_node;
-      unsigned int bit = 0;
-      while (!nodes[node].isLeaf()) {
-        node = nodes[node].children[(opath >> bit) & 1];
-        bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
-      }
-      update(node);
-      ++opath;
-    }
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::refit() {
-  if (root_node != NULL_NODE) recurseRefit(root_node);
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::extractLeaves(size_t root, Node*& leaves) const {
-  if (!nodes[root].isLeaf()) {
-    extractLeaves(nodes[root].children[0], leaves);
-    extractLeaves(nodes[root].children[1], leaves);
-  } else {
-    *leaves = nodes[root];
-    leaves++;
-  }
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::size() const {
-  return n_leaves;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::getRoot() const {
-  return root_node;
-}
-
-//==============================================================================
-template <typename BV>
-typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getNodes() const {
-  return nodes;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::print(size_t root, int depth) {
-  for (int i = 0; i < depth; ++i) std::cout << " ";
-  Node* n = nodes + root;
-  std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", "
-            << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1]
-            << ", " << n->bv.max_[2] << ")" << std::endl;
-  if (n->isLeaf()) {
-  } else {
-    print(n->children[0], depth + 1);
-    print(n->children[1], depth + 1);
-  }
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::getMaxHeight(size_t node) const {
-  if (!nodes[node].isLeaf()) {
-    size_t height1 = getMaxHeight(nodes[node].children[0]);
-    size_t height2 = getMaxHeight(nodes[node].children[1]);
-    return std::max(height1, height2) + 1;
-  } else
-    return 0;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::getMaxDepth(size_t node, size_t depth,
-                                    size_t& max_depth) const {
-  if (!nodes[node].isLeaf()) {
-    getMaxDepth(nodes[node].children[0], depth + 1, max_depth);
-    getmaxDepth(nodes[node].children[1], depth + 1, max_depth);
-  } else
-    max_depth = std::max(max_depth, depth);
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend) {
-  size_t* lcur_end = lend;
-  while (lbeg < lcur_end - 1) {
-    size_t *min_it1 = nullptr, *min_it2 = nullptr;
-    FCL_REAL min_size = (std::numeric_limits<FCL_REAL>::max)();
-    for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) {
-      for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) {
-        FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
-        if (cur_size < min_size) {
-          min_size = cur_size;
-          min_it1 = it1;
-          min_it2 = it2;
-        }
-      }
-    }
-
-    size_t p =
-        createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr);
-    nodes[p].children[0] = *min_it1;
-    nodes[p].children[1] = *min_it2;
-    nodes[*min_it1].parent = p;
-    nodes[*min_it2].parent = p;
-    *min_it1 = p;
-    size_t tmp = *min_it2;
-    lcur_end--;
-    *min_it2 = *lcur_end;
-    *lcur_end = tmp;
-  }
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::topdown(size_t* lbeg, size_t* lend) {
-  switch (topdown_level) {
-    case 0:
-      return topdown_0(lbeg, lend);
-      break;
-    case 1:
-      return topdown_1(lbeg, lend);
-      break;
-    default:
-      return topdown_0(lbeg, lend);
-  }
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::topdown_0(size_t* lbeg, size_t* lend) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    if (num_leaves > bu_threshold) {
-      BV vol = nodes[*lbeg].bv;
-      for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv;
-
-      size_t best_axis = 0;
-      FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
-      if (extent[1] > extent[0]) best_axis = 1;
-      if (extent[2] > extent[best_axis]) best_axis = 2;
-
-      nodeBaseLess<BV> comp(nodes, best_axis);
-      size_t* lcenter = lbeg + num_leaves / 2;
-      std::nth_element(lbeg, lcenter, lend, comp);
-
-      size_t node = createNode(NULL_NODE, vol, nullptr);
-      nodes[node].children[0] = topdown_0(lbeg, lcenter);
-      nodes[node].children[1] = topdown_0(lcenter, lend);
-      nodes[nodes[node].children[0]].parent = node;
-      nodes[nodes[node].children[1]].parent = node;
-      return node;
-    } else {
-      bottomup(lbeg, lend);
-      return *lbeg;
-    }
-  }
-  return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    if (num_leaves > bu_threshold) {
-      Vec3f split_p = nodes[*lbeg].bv.center();
-      BV vol = nodes[*lbeg].bv;
-      for (size_t* i = lbeg + 1; i < lend; ++i) {
-        split_p += nodes[*i].bv.center();
-        vol += nodes[*i].bv;
-      }
-      split_p /= static_cast<FCL_REAL>(num_leaves);
-      int best_axis = -1;
-      int bestmidp = (int)num_leaves;
-      int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
-      for (size_t* i = lbeg; i < lend; ++i) {
-        Vec3f x = nodes[*i].bv.center() - split_p;
-        for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
-      }
-
-      for (size_t i = 0; i < 3; ++i) {
-        if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
-          int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
-          if (midp < bestmidp) {
-            best_axis = (int)i;
-            bestmidp = midp;
-          }
-        }
-      }
-
-      if (best_axis < 0) best_axis = 0;
-
-      FCL_REAL split_value = split_p[best_axis];
-      size_t* lcenter = lbeg;
-      for (size_t* i = lbeg; i < lend; ++i) {
-        if (nodes[*i].bv.center()[best_axis] < split_value) {
-          size_t temp = *i;
-          *i = *lcenter;
-          *lcenter = temp;
-          ++lcenter;
-        }
-      }
-
-      size_t node = createNode(NULL_NODE, vol, nullptr);
-      nodes[node].children[0] = topdown_1(lbeg, lcenter);
-      nodes[node].children[1] = topdown_1(lcenter, lend);
-      nodes[nodes[node].children[0]].parent = node;
-      nodes[nodes[node].children[1]].parent = node;
-      return node;
-    } else {
-      bottomup(lbeg, lend);
-      return *lbeg;
-    }
-  }
-  return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::mortonRecurse_0(size_t* lbeg, size_t* lend,
-                                          const uint32_t& split, int bits) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    if (bits > 0) {
-      const SortByMorton comp{nodes, split};
-      size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
-
-      if (lcenter == lbeg) {
-        uint32_t split2 = split | (1 << (bits - 1));
-        return mortonRecurse_0(lbeg, lend, split2, bits - 1);
-      } else if (lcenter == lend) {
-        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
-        return mortonRecurse_0(lbeg, lend, split1, bits - 1);
-      } else {
-        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
-        uint32_t split2 = split | (1 << (bits - 1));
-
-        size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
-        size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
-        size_t node = createNode(NULL_NODE, nullptr);
-        nodes[node].children[0] = child1;
-        nodes[node].children[1] = child2;
-        nodes[child1].parent = node;
-        nodes[child2].parent = node;
-        return node;
-      }
-    } else {
-      size_t node = topdown(lbeg, lend);
-      return node;
-    }
-  } else
-    return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::mortonRecurse_1(size_t* lbeg, size_t* lend,
-                                          const uint32_t& split, int bits) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    if (bits > 0) {
-      const SortByMorton comp{nodes, split};
-      size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
-
-      if (lcenter == lbeg) {
-        uint32_t split2 = split | (1 << (bits - 1));
-        return mortonRecurse_1(lbeg, lend, split2, bits - 1);
-      } else if (lcenter == lend) {
-        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
-        return mortonRecurse_1(lbeg, lend, split1, bits - 1);
-      } else {
-        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
-        uint32_t split2 = split | (1 << (bits - 1));
-
-        size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
-        size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
-        size_t node = createNode(NULL_NODE, nullptr);
-        nodes[node].children[0] = child1;
-        nodes[node].children[1] = child2;
-        nodes[child1].parent = node;
-        nodes[child2].parent = node;
-        return node;
-      }
-    } else {
-      size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
-      size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
-      size_t node = createNode(NULL_NODE, nullptr);
-      nodes[node].children[0] = child1;
-      nodes[node].children[1] = child2;
-      nodes[child1].parent = node;
-      nodes[child2].parent = node;
-      return node;
-    }
-  } else
-    return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::mortonRecurse_2(size_t* lbeg, size_t* lend) {
-  long num_leaves = lend - lbeg;
-  if (num_leaves > 1) {
-    size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
-    size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
-    size_t node = createNode(NULL_NODE, nullptr);
-    nodes[node].children[0] = child1;
-    nodes[node].children[1] = child2;
-    nodes[child1].parent = node;
-    nodes[child2].parent = node;
-    return node;
-  } else
-    return *lbeg;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::insertLeaf(size_t root, size_t leaf) {
-  if (root_node == NULL_NODE) {
-    root_node = leaf;
-    nodes[leaf].parent = NULL_NODE;
-  } else {
-    if (!nodes[root].isLeaf()) {
-      do {
-        root = nodes[root].children[select(leaf, nodes[root].children[0],
-                                           nodes[root].children[1], nodes)];
-      } while (!nodes[root].isLeaf());
-    }
-
-    size_t prev = nodes[root].parent;
-    size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr);
-    if (prev != NULL_NODE) {
-      nodes[prev].children[indexOf(root)] = node;
-      nodes[node].children[0] = root;
-      nodes[root].parent = node;
-      nodes[node].children[1] = leaf;
-      nodes[leaf].parent = node;
-      do {
-        if (!nodes[prev].bv.contain(nodes[node].bv))
-          nodes[prev].bv = nodes[nodes[prev].children[0]].bv +
-                           nodes[nodes[prev].children[1]].bv;
-        else
-          break;
-        node = prev;
-      } while (NULL_NODE != (prev = nodes[node].parent));
-    } else {
-      nodes[node].children[0] = root;
-      nodes[root].parent = node;
-      nodes[node].children[1] = leaf;
-      nodes[leaf].parent = node;
-      root_node = node;
-    }
-  }
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::removeLeaf(size_t leaf) {
-  if (leaf == root_node) {
-    root_node = NULL_NODE;
-    return NULL_NODE;
-  } else {
-    size_t parent = nodes[leaf].parent;
-    size_t prev = nodes[parent].parent;
-    size_t sibling = nodes[parent].children[1 - indexOf(leaf)];
-
-    if (prev != NULL_NODE) {
-      nodes[prev].children[indexOf(parent)] = sibling;
-      nodes[sibling].parent = prev;
-      deleteNode(parent);
-      while (prev != NULL_NODE) {
-        BV new_bv = nodes[nodes[prev].children[0]].bv +
-                    nodes[nodes[prev].children[1]].bv;
-        if (!(new_bv == nodes[prev].bv)) {
-          nodes[prev].bv = new_bv;
-          prev = nodes[prev].parent;
-        } else
-          break;
-      }
-
-      return (prev != NULL_NODE) ? prev : root_node;
-    } else {
-      root_node = sibling;
-      nodes[sibling].parent = NULL_NODE;
-      deleteNode(parent);
-      return root_node;
-    }
-  }
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::update_(size_t leaf, const BV& bv) {
-  size_t root = removeLeaf(leaf);
-  if (root != NULL_NODE) {
-    if (max_lookahead_level >= 0) {
-      for (int i = 0;
-           (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
-        root = nodes[root].parent;
-    }
-
-    nodes[leaf].bv = bv;
-    insertLeaf(root, leaf);
-  }
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::indexOf(size_t node) {
-  return (nodes[nodes[node].parent].children[1] == node);
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::allocateNode() {
-  if (freelist == NULL_NODE) {
-    Node* old_nodes = nodes;
-    n_nodes_alloc *= 2;
-    nodes = new Node[n_nodes_alloc];
-    std::copy(old_nodes, old_nodes + n_nodes, nodes);
-    delete[] old_nodes;
-
-    for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
-    nodes[n_nodes_alloc - 1].next = NULL_NODE;
-    freelist = n_nodes;
-  }
-
-  size_t node_id = freelist;
-  freelist = nodes[node_id].next;
-  nodes[node_id].parent = NULL_NODE;
-  nodes[node_id].children[0] = NULL_NODE;
-  nodes[node_id].children[1] = NULL_NODE;
-  ++n_nodes;
-  return node_id;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv1,
-                                     const BV& bv2, void* data) {
-  size_t node = allocateNode();
-  nodes[node].parent = parent;
-  nodes[node].data = data;
-  nodes[node].bv = bv1 + bv2;
-  return node;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv, void* data) {
-  size_t node = allocateNode();
-  nodes[node].parent = parent;
-  nodes[node].data = data;
-  nodes[node].bv = bv;
-  return node;
-}
-
-//==============================================================================
-template <typename BV>
-size_t HierarchyTree<BV>::createNode(size_t parent, void* data) {
-  size_t node = allocateNode();
-  nodes[node].parent = parent;
-  nodes[node].data = data;
-  return node;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::deleteNode(size_t node) {
-  nodes[node].next = freelist;
-  freelist = node;
-  --n_nodes;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::recurseRefit(size_t node) {
-  if (!nodes[node].isLeaf()) {
-    recurseRefit(nodes[node].children[0]);
-    recurseRefit(nodes[node].children[1]);
-    nodes[node].bv =
-        nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv;
-  } else
-    return;
-}
-
-//==============================================================================
-template <typename BV>
-void HierarchyTree<BV>::fetchLeaves(size_t root, Node*& leaves, int depth) {
-  if ((!nodes[root].isLeaf()) && depth) {
-    fetchLeaves(nodes[root].children[0], leaves, depth - 1);
-    fetchLeaves(nodes[root].children[1], leaves, depth - 1);
-    deleteNode(root);
-  } else {
-    *leaves = nodes[root];
-    leaves++;
-  }
-}
-
-//==============================================================================
-template <typename BV>
-nodeBaseLess<BV>::nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_)
-    : nodes(nodes_), d(d_) {
-  // Do nothing
-}
-
-//==============================================================================
-template <typename BV>
-bool nodeBaseLess<BV>::operator()(size_t i, size_t j) const {
-  if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true;
-
-  return false;
-}
-
-//==============================================================================
-template <typename S, typename BV>
-struct SelectImpl {
-  static bool run(size_t query, size_t node1, size_t node2,
-                  NodeBase<BV>* nodes) {
-    HPP_FCL_UNUSED_VARIABLE(query);
-    HPP_FCL_UNUSED_VARIABLE(node1);
-    HPP_FCL_UNUSED_VARIABLE(node2);
-    HPP_FCL_UNUSED_VARIABLE(nodes);
-
-    return 0;
-  }
-
-  static bool run(const BV& query, size_t node1, size_t node2,
-                  NodeBase<BV>* nodes) {
-    HPP_FCL_UNUSED_VARIABLE(query);
-    HPP_FCL_UNUSED_VARIABLE(node1);
-    HPP_FCL_UNUSED_VARIABLE(node2);
-    HPP_FCL_UNUSED_VARIABLE(nodes);
-
-    return 0;
-  }
-};
-
-//==============================================================================
-template <typename BV>
-size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes) {
-  return SelectImpl<FCL_REAL, BV>::run(query, node1, node2, nodes);
-}
-
-//==============================================================================
-template <typename BV>
-size_t select(const BV& query, size_t node1, size_t node2,
-              NodeBase<BV>* nodes) {
-  return SelectImpl<FCL_REAL, BV>::run(query, node1, node2, nodes);
-}
-
-//==============================================================================
-template <typename S>
-struct SelectImpl<S, AABB> {
-  static bool run(size_t query, size_t node1, size_t node2,
-                  NodeBase<AABB>* nodes) {
-    const AABB& bv = nodes[query].bv;
-    const AABB& bv1 = nodes[node1].bv;
-    const AABB& bv2 = nodes[node2].bv;
-    Vec3f v = bv.min_ + bv.max_;
-    Vec3f v1 = v - (bv1.min_ + bv1.max_);
-    Vec3f v2 = v - (bv2.min_ + bv2.max_);
-    FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
-    FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
-    return (d1 < d2) ? 0 : 1;
-  }
-
-  static bool run(const AABB& query, size_t node1, size_t node2,
-                  NodeBase<AABB>* nodes) {
-    const AABB& bv = query;
-    const AABB& bv1 = nodes[node1].bv;
-    const AABB& bv2 = nodes[node2].bv;
-    Vec3f v = bv.min_ + bv.max_;
-    Vec3f v1 = v - (bv1.min_ + bv1.max_);
-    Vec3f v2 = v - (bv2.min_ + bv2.max_);
-    FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
-    FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
-    return (d1 < d2) ? 0 : 1;
-  }
-};
-
-}  // namespace implementation_array
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/hierarchy_tree_array-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h
index 8709a848be952069a5c1533efadb84e964cbba12..e0c9a90a1d3f9d211e8fbb8021ea15f6de14e769 100644
--- a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h
+++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h
@@ -1,302 +1,2 @@
-/*
- * 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 HPP_FCL_HIERARCHY_TREE_ARRAY_H
-#define HPP_FCL_HIERARCHY_TREE_ARRAY_H
-
-#include <vector>
-#include <map>
-#include <functional>
-
-#include "hpp/fcl/fwd.hh"
-#include "hpp/fcl/BV/AABB.h"
-#include "hpp/fcl/broadphase/detail/morton.h"
-#include "hpp/fcl/broadphase/detail/node_base_array.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-namespace implementation_array {
-
-/// @brief Class for hierarchy tree structure
-template <typename BV>
-class HierarchyTree {
- public:
-  typedef NodeBase<BV> Node;
-
- private:
-  struct SortByMorton {
-    SortByMorton(Node* nodes_in) : nodes(nodes_in) {}
-    SortByMorton(Node* nodes_in, uint32_t split_in)
-        : nodes(nodes_in), split(split_in) {}
-    bool operator()(size_t a, size_t b) const {
-      if ((a != NULL_NODE) && (b != NULL_NODE))
-        return nodes[a].code < nodes[b].code;
-      else if (a == NULL_NODE)
-        return split < nodes[b].code;
-      else if (b == NULL_NODE)
-        return nodes[a].code < split;
-
-      return false;
-    }
-
-    Node* nodes{};
-    uint32_t split{};
-  };
-
- public:
-  /// @brief Create hierarchy tree with suitable setting.
-  /// bu_threshold decides the height of tree node to start bottom-up
-  /// construction / optimization; topdown_level decides different methods to
-  /// construct tree in topdown manner. lower level method constructs tree with
-  /// better quality but is slower.
-  HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
-
-  ~HierarchyTree();
-
-  /// @brief Initialize the tree by a set of leaves using algorithm with a given
-  /// level.
-  void init(Node* leaves, int n_leaves_, int level = 0);
-
-  /// @brief Initialize the tree by a set of leaves using algorithm with a given
-  /// level.
-  size_t insert(const BV& bv, void* data);
-
-  /// @brief Remove a leaf node
-  void remove(size_t leaf);
-
-  /// @brief Clear the tree
-  void clear();
-
-  /// @brief Whether the tree is empty
-  bool empty() const;
-
-  /// @brief update one leaf node
-  void update(size_t leaf, int lookahead_level = -1);
-
-  /// @brief update the tree when the bounding volume of a given leaf has
-  /// changed
-  bool update(size_t leaf, const BV& bv);
-
-  /// @brief update one leaf's bounding volume, with prediction
-  bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin);
-
-  /// @brief update one leaf's bounding volume, with prediction
-  bool update(size_t leaf, const BV& bv, const Vec3f& vel);
-
-  /// @brief get the max height of the tree
-  size_t getMaxHeight() const;
-
-  /// @brief get the max depth of the tree
-  size_t getMaxDepth() const;
-
-  /// @brief balance the tree from bottom
-  void balanceBottomup();
-
-  /// @brief balance the tree from top
-  void balanceTopdown();
-
-  /// @brief balance the tree in an incremental way
-  void balanceIncremental(int iterations);
-
-  /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
-  /// update the entire tree in a bottom-up manner
-  void refit();
-
-  /// @brief extract all the leaves of the tree
-  void extractLeaves(size_t root, Node*& leaves) const;
-
-  /// @brief number of leaves in the tree
-  size_t size() const;
-
-  /// @brief get the root of the tree
-  size_t getRoot() const;
-
-  /// @brief get the pointer to the nodes array
-  Node* getNodes() const;
-
-  /// @brief print the tree in a recursive way
-  void print(size_t root, int depth);
-
- private:
-  /// @brief construct a tree for a set of leaves from bottom -- very heavy way
-  void bottomup(size_t* lbeg, size_t* lend);
-
-  /// @brief construct a tree for a set of leaves from top
-  size_t topdown(size_t* lbeg, size_t* lend);
-
-  /// @brief compute the maximum height of a subtree rooted from a given node
-  size_t getMaxHeight(size_t node) const;
-
-  /// @brief compute the maximum depth of a subtree rooted from a given node
-  void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const;
-
-  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
-  /// topdown manner. During construction, first compute the best split axis as
-  /// the axis along with the longest AABB edge. Then compute the median of all
-  /// nodes' center projection onto the axis and using it as the split
-  /// threshold.
-  size_t topdown_0(size_t* lbeg, size_t* lend);
-
-  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
-  /// topdown manner. During construction, first compute the best split
-  /// thresholds for different axes as the average of all nodes' center. Then
-  /// choose the split axis as the axis whose threshold can divide the nodes
-  /// into two parts with almost similar size. This construction is more
-  /// expensive then topdown_0, but also can provide tree with better quality.
-  size_t topdown_1(size_t* lbeg, size_t* lend);
-
-  /// @brief init tree from leaves in the topdown manner (topdown_0 or
-  /// topdown_1)
-  void init_0(Node* leaves, int n_leaves_);
-
-  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
-  /// for nodes which is of depth more than the maximum bits of the morton code,
-  /// we use bottomup method to construct the subtree, which is slow but can
-  /// construct tree with high quality.
-  void init_1(Node* leaves, int n_leaves_);
-
-  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
-  /// for nodes which is of depth more than the maximum bits of the morton code,
-  /// we split the leaves into two parts with the same size simply using the
-  /// node index.
-  void init_2(Node* leaves, int n_leaves_);
-
-  /// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
-  /// for all nodes, we simply divide the leaves into parts with the same size
-  /// simply using the node index.
-  void init_3(Node* leaves, int n_leaves_);
-
-  size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split,
-                         int bits);
-
-  size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split,
-                         int bits);
-
-  size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
-
-  /// @brief update one leaf node's bounding volume
-  void update_(size_t leaf, const BV& bv);
-
-  /// @brief Insert a leaf node and also update its ancestors
-  void insertLeaf(size_t root, size_t leaf);
-
-  /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the
-  /// unnecessary internal nodes are deleted. return the node with the smallest
-  /// depth and is influenced by the remove operation
-  size_t removeLeaf(size_t leaf);
-
-  /// @brief Delete all internal nodes and return all leaves nodes with given
-  /// depth from root
-  void fetchLeaves(size_t root, Node*& leaves, int depth = -1);
-
-  size_t indexOf(size_t node);
-
-  size_t allocateNode();
-
-  /// @brief create one node (leaf or internal)
-  size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data);
-
-  size_t createNode(size_t parent, const BV& bv, void* data);
-
-  size_t createNode(size_t parent, void* data);
-
-  void deleteNode(size_t node);
-
-  void recurseRefit(size_t node);
-
- protected:
-  size_t root_node;
-  Node* nodes;
-  size_t n_nodes;
-  size_t n_nodes_alloc;
-
-  size_t n_leaves;
-  size_t freelist;
-  unsigned int opath;
-
-  int max_lookahead_level;
-
- public:
-  /// @brief decide which topdown algorithm to use
-  int topdown_level;
-
-  /// @brief decide the depth to use expensive bottom-up algorithm
-  int bu_threshold;
-
- public:
-  static const size_t NULL_NODE = std::numeric_limits<size_t>::max();
-};
-
-template <typename BV>
-const size_t HierarchyTree<BV>::NULL_NODE;
-
-/// @brief Functor comparing two nodes
-template <typename BV>
-struct nodeBaseLess {
-  nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_);
-
-  bool operator()(size_t i, size_t j) const;
-
- private:
-  /// @brief the nodes array
-  const NodeBase<BV>* nodes;
-
-  /// @brief the dimension to compare
-  size_t d;
-};
-
-/// @brief select the node from node1 and node2 which is close to the query-th
-/// node in the nodes. 0 for node1 and 1 for node2.
-template <typename BV>
-size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes);
-
-/// @brief select the node from node1 and node2 which is close to the query
-/// AABB. 0 for node1 and 1 for node2.
-template <typename BV>
-size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes);
-
-}  // namespace implementation_array
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/hierarchy_tree_array.h>
diff --git a/include/hpp/fcl/broadphase/detail/interval_tree.h b/include/hpp/fcl/broadphase/detail/interval_tree.h
index bbcb1fec85dac1911a188b39b4baad1062b1b7e1..5480fc00fed8240de0ca9fb306c202ae560dac44 100644
--- a/include/hpp/fcl/broadphase/detail/interval_tree.h
+++ b/include/hpp/fcl/broadphase/detail/interval_tree.h
@@ -1,129 +1,2 @@
-/*
- * 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 HPP_FCL_INTERVAL_TREE_H
-#define HPP_FCL_INTERVAL_TREE_H
-
-#include <deque>
-#include <limits>
-#include <cstdlib>
-
-#include "hpp/fcl/broadphase/detail/interval_tree_node.h"
-
-namespace hpp {
-namespace fcl {
-namespace detail {
-
-/// @brief Class describes the information needed when we take the
-/// right branch in searching for intervals but possibly come back
-/// and check the left branch as well.
-struct HPP_FCL_DLLAPI it_recursion_node {
- public:
-  IntervalTreeNode* start_node;
-
-  unsigned int parent_index;
-
-  bool try_right_branch;
-};
-
-/// @brief Interval tree
-class HPP_FCL_DLLAPI IntervalTree {
- public:
-  IntervalTree();
-
-  ~IntervalTree();
-
-  /// @brief Print the whole interval tree
-  void print() const;
-
-  /// @brief Delete one node of the interval tree
-  SimpleInterval* deleteNode(IntervalTreeNode* node);
-
-  /// @brief delete node stored a given interval
-  void deleteNode(SimpleInterval* ivl);
-
-  /// @brief Insert one node of the interval tree
-  IntervalTreeNode* insert(SimpleInterval* new_interval);
-
-  /// @brief get the predecessor of a given node
-  IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const;
-
-  /// @brief Get the successor of a given node
-  IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const;
-
-  /// @brief Return result for a given query
-  std::deque<SimpleInterval*> query(FCL_REAL low, FCL_REAL high);
-
- protected:
-  IntervalTreeNode* root;
-
-  IntervalTreeNode* invalid_node;
-
-  /// @brief left rotation of tree node
-  void leftRotate(IntervalTreeNode* node);
-
-  /// @brief right rotation of tree node
-  void rightRotate(IntervalTreeNode* node);
-
-  /// @brief Inserts node into the tree as if it were a regular binary tree
-  void recursiveInsert(IntervalTreeNode* node);
-
-  /// @brief recursively print a subtree
-  void recursivePrint(IntervalTreeNode* node) const;
-
-  /// @brief recursively find the node corresponding to the interval
-  IntervalTreeNode* recursiveSearch(IntervalTreeNode* node,
-                                    SimpleInterval* ivl) const;
-
-  /// @brief Travels up to the root fixing the max_high fields after an
-  /// insertion or deletion
-  void fixupMaxHigh(IntervalTreeNode* node);
-
-  void deleteFixup(IntervalTreeNode* node);
-
- private:
-  unsigned int recursion_node_stack_size;
-  it_recursion_node* recursion_node_stack;
-  unsigned int current_parent;
-  unsigned int recursion_node_stack_top;
-};
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/interval_tree.h>
diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node.h b/include/hpp/fcl/broadphase/detail/interval_tree_node.h
index e8a653c157bf8a4dd5e883e09c0f8a4e105b3f79..531898a41d83f66b3de663133489d821c44a81e9 100644
--- a/include/hpp/fcl/broadphase/detail/interval_tree_node.h
+++ b/include/hpp/fcl/broadphase/detail/interval_tree_node.h
@@ -1,92 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_H
-#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_H
-
-#include "hpp/fcl/broadphase/detail/simple_interval.h"
-#include "hpp/fcl/fwd.hh"
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-class HPP_FCL_DLLAPI IntervalTree;
-
-/// @brief The node for interval tree
-class HPP_FCL_DLLAPI IntervalTreeNode {
- public:
-  friend class IntervalTree;
-
-  /// @brief Create an empty node
-  IntervalTreeNode();
-
-  /// @brief Create an node storing the interval
-  IntervalTreeNode(SimpleInterval* new_interval);
-
-  ~IntervalTreeNode();
-
-  /// @brief Print the interval node information: set left = invalid_node and
-  /// right = root
-  void print(IntervalTreeNode* left, IntervalTreeNode* right) const;
-
- protected:
-  /// @brief interval stored in the node
-  SimpleInterval* stored_interval;
-
-  FCL_REAL key;
-
-  FCL_REAL high;
-
-  FCL_REAL max_high;
-
-  /// @brief red or black node: if red = false then the node is black
-  bool red;
-
-  IntervalTreeNode* left;
-
-  IntervalTreeNode* right;
-
-  IntervalTreeNode* parent;
-};
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/interval_tree_node.h>
diff --git a/include/hpp/fcl/broadphase/detail/morton-inl.h b/include/hpp/fcl/broadphase/detail/morton-inl.h
index 53d7d90f77d6a58f314933b2e5bbea333b5595f9..b3ef57cbaccae56153bbbbc891564a782b9b8f38 100644
--- a/include/hpp/fcl/broadphase/detail/morton-inl.h
+++ b/include/hpp/fcl/broadphase/detail/morton-inl.h
@@ -1,153 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2016, Open Source Robotics Foundation
- *  Copyright (c) 2016, Toyota Research Institute
- *  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 HPP_FCL_MORTON_INL_H
-#define HPP_FCL_MORTON_INL_H
-
-#include "hpp/fcl/broadphase/detail/morton.h"
-
-namespace hpp {
-namespace fcl {  /// @cond IGNORE
-namespace detail {
-
-//==============================================================================
-template <typename S>
-uint32_t quantize(S x, uint32_t n) {
-  return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0));
-}
-
-//==============================================================================
-template <typename S>
-morton_functor<S, uint32_t>::morton_functor(const AABB& bbox)
-    : base(bbox.min_),
-      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
-          1.0 / (bbox.max_[1] - bbox.min_[1]),
-          1.0 / (bbox.max_[2] - bbox.min_[2])) {
-  // Do nothing
-}
-
-//==============================================================================
-template <typename S>
-uint32_t morton_functor<S, uint32_t>::operator()(const Vec3f& point) const {
-  uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
-  uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
-  uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
-
-  return detail::morton_code(x, y, z);
-}
-
-//==============================================================================
-template <typename S>
-morton_functor<S, uint64_t>::morton_functor(const AABB& bbox)
-    : base(bbox.min_),
-      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
-          1.0 / (bbox.max_[1] - bbox.min_[1]),
-          1.0 / (bbox.max_[2] - bbox.min_[2])) {
-  // Do nothing
-}
-
-//==============================================================================
-template <typename S>
-uint64_t morton_functor<S, uint64_t>::operator()(const Vec3f& point) const {
-  uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
-  uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
-  uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20);
-
-  return detail::morton_code60(x, y, z);
-}
-
-//==============================================================================
-template <typename S>
-constexpr size_t morton_functor<S, uint64_t>::bits() {
-  return 60;
-}
-
-//==============================================================================
-template <typename S>
-constexpr size_t morton_functor<S, uint32_t>::bits() {
-  return 30;
-}
-
-//==============================================================================
-template <typename S, size_t N>
-morton_functor<S, std::bitset<N>>::morton_functor(const AABB& bbox)
-    : base(bbox.min_),
-      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
-          1.0 / (bbox.max_[1] - bbox.min_[1]),
-          1.0 / (bbox.max_[2] - bbox.min_[2])) {
-  // Do nothing
-}
-
-//==============================================================================
-template <typename S, size_t N>
-std::bitset<N> morton_functor<S, std::bitset<N>>::operator()(
-    const Vec3f& point) const {
-  S x = (point[0] - base[0]) * inv[0];
-  S y = (point[1] - base[1]) * inv[1];
-  S z = (point[2] - base[2]) * inv[2];
-  int start_bit = bits() - 1;
-  std::bitset<N> bset;
-
-  x *= 2;
-  y *= 2;
-  z *= 2;
-
-  for (size_t i = 0; i < bits() / 3; ++i) {
-    bset[start_bit--] = ((z < 1) ? 0 : 1);
-    bset[start_bit--] = ((y < 1) ? 0 : 1);
-    bset[start_bit--] = ((x < 1) ? 0 : 1);
-    x = ((x >= 1) ? 2 * (x - 1) : 2 * x);
-    y = ((y >= 1) ? 2 * (y - 1) : 2 * y);
-    z = ((z >= 1) ? 2 * (z - 1) : 2 * z);
-  }
-
-  return bset;
-}
-
-//==============================================================================
-template <typename S, size_t N>
-constexpr size_t morton_functor<S, std::bitset<N>>::bits() {
-  return N;
-}
-
-}  // namespace detail
-/// @endcond
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/morton-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/morton.h b/include/hpp/fcl/broadphase/detail/morton.h
index cf9968af49aedd32c405add78c08a66b1ed0846c..c0a89c6b24d1dc838d0403eba3b921977547d62a 100644
--- a/include/hpp/fcl/broadphase/detail/morton.h
+++ b/include/hpp/fcl/broadphase/detail/morton.h
@@ -1,122 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2016, Open Source Robotics Foundation
- *  Copyright (c) 2016, Toyota Research Institute
- *  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 HPP_FCL_MORTON_H
-#define HPP_FCL_MORTON_H
-
-#include "hpp/fcl/BV/AABB.h"
-#include <cstdint>
-#include <bitset>
-
-namespace hpp {
-namespace fcl {
-
-/// @cond IGNORE
-namespace detail {
-
-template <typename S>
-uint32_t quantize(S x, uint32_t n);
-
-/// @brief compute 30 bit morton code
-HPP_FCL_DLLAPI
-uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z);
-
-/// @brief compute 60 bit morton code
-HPP_FCL_DLLAPI
-uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z);
-
-/// @brief Functor to compute the morton code for a given AABB
-/// This is specialized for 32- and 64-bit unsigned integers giving
-/// a 30- or 60-bit code, respectively, and for `std::bitset<N>` where
-/// N is the length of the code and must be a multiple of 3.
-template <typename S, typename T>
-struct morton_functor {};
-
-/// @brief Functor to compute 30 bit morton code for a given AABB
-template <typename S>
-struct morton_functor<S, uint32_t> {
-  morton_functor(const AABB& bbox);
-
-  uint32_t operator()(const Vec3f& point) const;
-
-  const Vec3f base;
-  const Vec3f inv;
-
-  static constexpr size_t bits();
-};
-
-using morton_functoru32f = morton_functor<float, uint32_t>;
-using morton_functoru32d = morton_functor<FCL_REAL, uint32_t>;
-
-/// @brief Functor to compute 60 bit morton code for a given AABB
-template <typename S>
-struct morton_functor<S, uint64_t> {
-  morton_functor(const AABB& bbox);
-
-  uint64_t operator()(const Vec3f& point) const;
-
-  const Vec3f base;
-  const Vec3f inv;
-
-  static constexpr size_t bits();
-};
-
-/// @brief Functor to compute N bit morton code for a given AABB
-/// N must be a multiple of 3.
-template <typename S, size_t N>
-struct morton_functor<S, std::bitset<N>> {
-  static_assert(N % 3 == 0, "Number of bits must be a multiple of 3");
-
-  morton_functor(const AABB& bbox);
-
-  std::bitset<N> operator()(const Vec3f& point) const;
-
-  const Vec3f base;
-  const Vec3f inv;
-
-  static constexpr size_t bits();
-};
-
-}  // namespace detail
-/// @endcond
-}  // namespace fcl
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/detail/morton-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/morton.h>
diff --git a/include/hpp/fcl/broadphase/detail/node_base-inl.h b/include/hpp/fcl/broadphase/detail/node_base-inl.h
index d7a69680cd1e81239e34f6fab4551e1e14258854..32602004003896146c978e34ff19654a8f83b077 100644
--- a/include/hpp/fcl/broadphase/detail/node_base-inl.h
+++ b/include/hpp/fcl/broadphase/detail/node_base-inl.h
@@ -1,77 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_DETAIL_NODEBASE_INL_H
-#define HPP_FCL_BROADPHASE_DETAIL_NODEBASE_INL_H
-
-#include "hpp/fcl/broadphase/detail/node_base.h"
-
-namespace hpp {
-namespace fcl {
-namespace detail {
-
-//============================================================================//
-//                                                                            //
-//                              Implementations                               //
-//                                                                            //
-//============================================================================//
-
-//==============================================================================
-template <typename BV>
-bool NodeBase<BV>::isLeaf() const {
-  return (children[1] == nullptr);
-}
-
-//==============================================================================
-template <typename BV>
-bool NodeBase<BV>::isInternal() const {
-  return !isLeaf();
-}
-
-//==============================================================================
-template <typename BV>
-NodeBase<BV>::NodeBase() {
-  parent = nullptr;
-  children[0] = nullptr;
-  children[1] = nullptr;
-}
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/node_base-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/node_base.h b/include/hpp/fcl/broadphase/detail/node_base.h
index 5937b9cefb47d1a540293d7e99ed46b47f37c0c6..b0c8206e3738a7475be3673bfc1fec82b25dcac6 100644
--- a/include/hpp/fcl/broadphase/detail/node_base.h
+++ b/include/hpp/fcl/broadphase/detail/node_base.h
@@ -1,81 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_DETAIL_NODEBASE_H
-#define HPP_FCL_BROADPHASE_DETAIL_NODEBASE_H
-
-#include "hpp/fcl/data_types.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-/// @brief dynamic AABB tree node
-template <typename BV>
-struct NodeBase {
-  /// @brief the bounding volume for the node
-  BV bv;
-
-  /// @brief pointer to parent node
-  NodeBase<BV>* parent;
-
-  /// @brief whether is a leaf
-  bool isLeaf() const;
-
-  /// @brief whether is internal node
-  bool isInternal() const;
-
-  union {
-    /// @brief for leaf node, children nodes
-    NodeBase<BV>* children[2];
-    void* data;
-  };
-
-  /// @brief morton code for current BV
-  uint32_t code;
-
-  NodeBase();
-};
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/detail/node_base-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/node_base.h>
diff --git a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h
index 0ee3e5f050f58efefb6385b7310b8b5e436938d1..e66c59cdd7da2fb4f1deed366503ba34fef16690 100644
--- a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h
+++ b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h
@@ -1,67 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
-#define HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
-
-#include "hpp/fcl/broadphase/detail/node_base_array.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-namespace implementation_array {
-
-//==============================================================================
-template <typename BV>
-bool NodeBase<BV>::isLeaf() const {
-  return (children[1] == (size_t)(-1));
-}
-
-//==============================================================================
-template <typename BV>
-bool NodeBase<BV>::isInternal() const {
-  return !isLeaf();
-}
-
-}  // namespace implementation_array
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/node_base_array-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/node_base_array.h b/include/hpp/fcl/broadphase/detail/node_base_array.h
index ecf8dc01d42c47c531a4ef13c6a888ac7ee0cdad..8ad4c1ed36344a93205d97f2ae95913ff8adef96 100644
--- a/include/hpp/fcl/broadphase/detail/node_base_array.h
+++ b/include/hpp/fcl/broadphase/detail/node_base_array.h
@@ -1,77 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_H
-#define HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_H
-
-#include "hpp/fcl/data_types.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-namespace implementation_array {
-
-template <typename BV>
-struct NodeBase {
-  BV bv;
-
-  union {
-    size_t parent;
-    size_t next;
-  };
-
-  union {
-    size_t children[2];
-    void* data;
-  };
-
-  uint32_t code;
-
-  bool isLeaf() const;
-  bool isInternal() const;
-};
-
-}  // namespace implementation_array
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/detail/node_base_array-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/node_base_array.h>
diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h
index de51472a23dcb62cf63d114290eb6beae382671e..712c72ec39c71a03932a47dfdebc16f8e354c8f6 100644
--- a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h
+++ b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h
@@ -1,113 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_INL_H
-#define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_INL_H
-
-#include "hpp/fcl/broadphase/detail/simple_hash_table.h"
-
-#include <iterator>
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc>
-SimpleHashTable<Key, Data, HashFnc>::SimpleHashTable(const HashFnc& h) : h_(h) {
-  // Do nothing
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc>
-void SimpleHashTable<Key, Data, HashFnc>::init(size_t size) {
-  if (size == 0) {
-    HPP_FCL_THROW_PRETTY("SimpleHashTable must have non-zero size.",
-                         std::logic_error);
-  }
-
-  table_.resize(size);
-  table_size_ = size;
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc>
-void SimpleHashTable<Key, Data, HashFnc>::insert(Key key, Data value) {
-  std::vector<unsigned int> indices = h_(key);
-  size_t range = table_.size();
-  for (size_t i = 0; i < indices.size(); ++i)
-    table_[indices[i] % range].push_back(value);
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc>
-std::vector<Data> SimpleHashTable<Key, Data, HashFnc>::query(Key key) const {
-  size_t range = table_.size();
-  std::vector<unsigned int> indices = h_(key);
-  std::set<Data> result;
-  for (size_t i = 0; i < indices.size(); ++i) {
-    size_t index = indices[i] % range;
-    std::copy(table_[index].begin(), table_[index].end(),
-              std::inserter(result, result.end()));
-  }
-
-  return std::vector<Data>(result.begin(), result.end());
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc>
-void SimpleHashTable<Key, Data, HashFnc>::remove(Key key, Data value) {
-  size_t range = table_.size();
-  std::vector<unsigned int> indices = h_(key);
-  for (size_t i = 0; i < indices.size(); ++i) {
-    size_t index = indices[i] % range;
-    table_[index].remove(value);
-  }
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc>
-void SimpleHashTable<Key, Data, HashFnc>::clear() {
-  table_.clear();
-  table_.resize(table_size_);
-}
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/simple_hash_table-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table.h b/include/hpp/fcl/broadphase/detail/simple_hash_table.h
index ed1f0fcd827da13b39af7f48e4d30449f6348a85..678cd34c81f57569322cf313c1cfdd8f7c1783a6 100644
--- a/include/hpp/fcl/broadphase/detail/simple_hash_table.h
+++ b/include/hpp/fcl/broadphase/detail/simple_hash_table.h
@@ -1,89 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H
-#define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H
-
-#include <set>
-#include <vector>
-#include <list>
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-/// @brief A simple hash table implemented as multiple buckets. HashFnc is any
-/// extended hash function: HashFnc(key) = {index1, index2, ..., }
-template <typename Key, typename Data, typename HashFnc>
-class SimpleHashTable {
- protected:
-  typedef std::list<Data> Bin;
-
-  std::vector<Bin> table_;
-
-  HashFnc h_;
-
-  size_t table_size_;
-
- public:
-  SimpleHashTable(const HashFnc& h);
-
-  /// @brief Init the number of bins in the hash table
-  void init(size_t size);
-
-  //// @brief Insert a key-value pair into the table
-  void insert(Key key, Data value);
-
-  /// @brief Find the elements in the hash table whose key is the same as query
-  /// key.
-  std::vector<Data> query(Key key) const;
-
-  /// @brief remove the key-value pair from the table
-  void remove(Key key, Data value);
-
-  /// @brief clear the hash table
-  void clear();
-};
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/detail/simple_hash_table-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/simple_hash_table.h>
diff --git a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h
index 78719156c9f83e9ae72fd3cdd2d2d5d1ac4013bb..b6e1fbd6c71d6c94a5503e5c004b65c02f7ea127 100644
--- a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h
+++ b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h
@@ -1,62 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
-#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
-
-#include "hpp/fcl/broadphase/detail/simple_interval.h"
-#include <algorithm>
-
-namespace hpp {
-namespace fcl {
-namespace detail {
-
-//==============================================================================
-SimpleInterval::~SimpleInterval() {
-  // Do nothing
-}
-
-//==============================================================================
-void SimpleInterval::print() {
-  // Do nothing
-}
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/simple_interval-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/simple_interval.h b/include/hpp/fcl/broadphase/detail/simple_interval.h
index 0f1976ebf05cc7cb1c1dd78be4129b70f30ee5c8..f0c26e038a1ab55027f325951e5f1a377ab3925e 100644
--- a/include/hpp/fcl/broadphase/detail/simple_interval.h
+++ b/include/hpp/fcl/broadphase/detail/simple_interval.h
@@ -1,64 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H
-#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H
-
-#include "hpp/fcl/fwd.hh"
-#include "hpp/fcl/data_types.h"
-
-namespace hpp {
-namespace fcl {
-namespace detail {
-
-/// @brief Interval trees implemented using red-black-trees as described in
-/// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest.
-struct HPP_FCL_DLLAPI SimpleInterval {
- public:
-  virtual ~SimpleInterval();
-
-  virtual void print();
-
-  /// @brief interval is defined as [low, high]
-  FCL_REAL low, high;
-};
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/simple_interval.h>
diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h
index b0668d989d8eed3b2d531959627e96240c120362..cabe7e8fb43775af24a01ce0f7e1dbe5f95c361e 100644
--- a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h
+++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h
@@ -1,113 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_SPARSEHASHTABLE_INL_H
-#define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_INL_H
-
-#include "hpp/fcl/broadphase/detail/sparse_hash_table.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc,
-          template <typename, typename> class TableT>
-SparseHashTable<Key, Data, HashFnc, TableT>::SparseHashTable(const HashFnc& h)
-    : h_(h) {
-  // Do nothing
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc,
-          template <typename, typename> class TableT>
-void SparseHashTable<Key, Data, HashFnc, TableT>::init(size_t) {
-  table_.clear();
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc,
-          template <typename, typename> class TableT>
-void SparseHashTable<Key, Data, HashFnc, TableT>::insert(Key key, Data value) {
-  std::vector<unsigned int> indices = h_(key);
-  for (size_t i = 0; i < indices.size(); ++i)
-    table_[indices[i]].push_back(value);
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc,
-          template <typename, typename> class TableT>
-std::vector<Data> SparseHashTable<Key, Data, HashFnc, TableT>::query(
-    Key key) const {
-  std::vector<unsigned int> indices = h_(key);
-  std::set<Data> result;
-  for (size_t i = 0; i < indices.size(); ++i) {
-    unsigned int index = indices[i];
-    typename Table::const_iterator p = table_.find(index);
-    if (p != table_.end()) {
-      std::copy((*p).second.begin(), (*p).second.end(),
-                std ::inserter(result, result.end()));
-    }
-  }
-
-  return std::vector<Data>(result.begin(), result.end());
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc,
-          template <typename, typename> class TableT>
-void SparseHashTable<Key, Data, HashFnc, TableT>::remove(Key key, Data value) {
-  std::vector<unsigned int> indices = h_(key);
-  for (size_t i = 0; i < indices.size(); ++i) {
-    unsigned int index = indices[i];
-    table_[index].remove(value);
-  }
-}
-
-//==============================================================================
-template <typename Key, typename Data, typename HashFnc,
-          template <typename, typename> class TableT>
-void SparseHashTable<Key, Data, HashFnc, TableT>::clear() {
-  table_.clear();
-}
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/sparse_hash_table-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h
index 5c28ccf3fa7d404d09722a78f811290be93ef741..9f585fac5006af47da54a2ae7134803568c11ce7 100644
--- a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h
+++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h
@@ -1,95 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H
-#define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H
-
-#include <set>
-#include <vector>
-#include <list>
-#include <unordered_map>
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-template <typename U, typename V>
-class unordered_map_hash_table : public std::unordered_map<U, V> {
-  typedef std::unordered_map<U, V> Base;
-
- public:
-  unordered_map_hash_table() : Base() {};
-};
-
-/// @brief A hash table implemented using unordered_map
-template <typename Key, typename Data, typename HashFnc,
-          template <typename, typename> class TableT = unordered_map_hash_table>
-class SparseHashTable {
- protected:
-  HashFnc h_;
-  typedef std::list<Data> Bin;
-  typedef TableT<size_t, Bin> Table;
-
-  Table table_;
-
- public:
-  SparseHashTable(const HashFnc& h);
-
-  /// @brief Init the hash table. The bucket size is dynamically decided.
-  void init(size_t);
-
-  /// @brief insert one key-value pair into the hash table
-  void insert(Key key, Data value);
-
-  /// @brief find the elements whose key is the same as the query
-  std::vector<Data> query(Key key) const;
-
-  /// @brief remove one key-value pair from the hash table
-  void remove(Key key, Data value);
-
-  /// @brief clear the hash table
-  void clear();
-};
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#include "hpp/fcl/broadphase/detail/sparse_hash_table-inl.h"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/sparse_hash_table.h>
diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h
index 1248b9eae351508c3098ad91ae57a86bbd628bc7..be5a1d873388d9edfcdb505c444d38a8fdb8e6c9 100644
--- a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h
+++ b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h
@@ -1,82 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_SPATIALHASH_INL_H
-#define HPP_FCL_BROADPHASE_SPATIALHASH_INL_H
-
-#include "hpp/fcl/broadphase/detail/spatial_hash.h"
-#include <algorithm>
-
-namespace hpp {
-namespace fcl {
-namespace detail {
-
-//==============================================================================
-SpatialHash::SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_)
-    : cell_size(cell_size_), scene_limit(scene_limit_) {
-  width[0] = std::ceil(scene_limit.width() / cell_size);
-  width[1] = std::ceil(scene_limit.height() / cell_size);
-  width[2] = std::ceil(scene_limit.depth() / cell_size);
-}
-
-//==============================================================================
-std::vector<unsigned int> SpatialHash::operator()(const AABB& aabb) const {
-  int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size);
-  int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size);
-  int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size);
-  int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size);
-  int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size);
-  int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size);
-
-  std::vector<unsigned int> keys((max_x - min_x) * (max_y - min_y) *
-                                 (max_z - min_z));
-  int id = 0;
-  for (int x = min_x; x < max_x; ++x) {
-    for (int y = min_y; y < max_y; ++y) {
-      for (int z = min_z; z < max_z; ++z) {
-        keys[id++] = x + y * width[0] + z * width[0] * width[1];
-      }
-    }
-  }
-  return keys;
-}
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/spatial_hash-inl.h>
diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash.h b/include/hpp/fcl/broadphase/detail/spatial_hash.h
index a7b711ea2b1d69cdb1a9623bd06be7a6222b0ac0..f4f10af83524058b64f3a90c929f25eab3b6bbfb 100644
--- a/include/hpp/fcl/broadphase/detail/spatial_hash.h
+++ b/include/hpp/fcl/broadphase/detail/spatial_hash.h
@@ -1,65 +1,2 @@
-/*
- * 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 HPP_FCL_BROADPHASE_SPATIALHASH_H
-#define HPP_FCL_BROADPHASE_SPATIALHASH_H
-
-#include "hpp/fcl/BV/AABB.h"
-#include <vector>
-
-namespace hpp {
-namespace fcl {
-
-namespace detail {
-
-/// @brief Spatial hash function: hash an AABB to a set of integer values
-struct HPP_FCL_DLLAPI SpatialHash {
-  SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_);
-
-  std::vector<unsigned int> operator()(const AABB& aabb) const;
-
- private:
-  FCL_REAL cell_size;
-  AABB scene_limit;
-  unsigned int width[3];
-};
-
-}  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/broadphase/detail/spatial_hash.h>
diff --git a/include/hpp/fcl/coal.hpp b/include/hpp/fcl/coal.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e4fdb758cdf868f6e2dc3cf1ddb1a488e528f63e
--- /dev/null
+++ b/include/hpp/fcl/coal.hpp
@@ -0,0 +1,19 @@
+#ifndef HPP_FCL_COAL_HPP
+#define HPP_FCL_COAL_HPP
+
+#include <coal/config.hh>
+#include <coal/deprecated.hh>
+
+#define COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL
+
+#ifdef _MSC_VER
+#pragma message COAL_DEPRECATED_HEADER( \
+    "Please update your includes from 'hpp/fcl' to 'coal'")
+#else
+#warning "Please update your includes from 'hpp/fcl' to 'coal'"
+#endif
+
+#define HPP_FCL_VERSION_AT_LEAST(major, minor, patch) \
+  COAL_VERSION_AT_LEAST(major, minor, patch)
+
+#endif  // COAL_FWD_HPP
diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h
index cbc0bd562571530230bd87cd5fc888a05785d062..099670f2591df815b1eb06df14adc27261452874 100644
--- a/include/hpp/fcl/collision.h
+++ b/include/hpp/fcl/collision.h
@@ -1,122 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2021, INRIA
- *  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 HPP_FCL_COLLISION_H
-#define HPP_FCL_COLLISION_H
-
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/collision_func_matrix.h>
-#include <hpp/fcl/timings.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Main collision interface: given two collision objects, and the
-/// requirements for contacts, including num of max contacts, whether perform
-/// exhaustive collision (i.e., returning returning all the contact points),
-/// whether return detailed contact information (i.e., normal, contact point,
-/// depth; otherwise only contact primitive id is returned), this function
-/// performs the collision between them.
-/// Return value is the number of contacts generated between the two objects.
-HPP_FCL_DLLAPI std::size_t collide(const CollisionObject* o1,
-                                   const CollisionObject* o2,
-                                   const CollisionRequest& request,
-                                   CollisionResult& result);
-
-/// @copydoc collide(const CollisionObject*, const CollisionObject*, const
-/// CollisionRequest&, CollisionResult&)
-HPP_FCL_DLLAPI std::size_t collide(const CollisionGeometry* o1,
-                                   const Transform3f& tf1,
-                                   const CollisionGeometry* o2,
-                                   const Transform3f& tf2,
-                                   const CollisionRequest& request,
-                                   CollisionResult& result);
-
-/// @brief This class reduces the cost of identifying the geometry pair.
-/// This is mostly useful for repeated shape-shape queries.
-///
-/// \code
-///   ComputeCollision calc_collision (o1, o2);
-///   std::size_t ncontacts = calc_collision(tf1, tf2, request, result);
-/// \endcode
-class HPP_FCL_DLLAPI ComputeCollision {
- public:
-  /// @brief Default constructor from two Collision Geometries.
-  ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2);
-
-  std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
-                         const CollisionRequest& request,
-                         CollisionResult& result) const;
-
-  bool operator==(const ComputeCollision& other) const {
-    return o1 == other.o1 && o2 == other.o2 && solver == other.solver;
-  }
-
-  bool operator!=(const ComputeCollision& other) const {
-    return !(*this == other);
-  }
-
-  virtual ~ComputeCollision() {};
-
- protected:
-  // These pointers are made mutable to let the derived classes to update
-  // their values when updating the collision geometry (e.g. creating a new
-  // one). This feature should be used carefully to avoid any mis usage (e.g,
-  // changing the type of the collision geometry should be avoided).
-  mutable const CollisionGeometry* o1;
-  mutable const CollisionGeometry* o2;
-
-  mutable GJKSolver solver;
-
-  CollisionFunctionMatrix::CollisionFunc func;
-  bool swap_geoms;
-
-  virtual std::size_t run(const Transform3f& tf1, const Transform3f& tf2,
-                          const CollisionRequest& request,
-                          CollisionResult& result) const;
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/collision.h>
diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h
index ee03e35b16d71997f426f6a4ff9e74bb8feb8312..185424ac7953fe0a10ca35d978579d999e039268 100644
--- a/include/hpp/fcl/collision_data.h
+++ b/include/hpp/fcl/collision_data.h
@@ -1,1241 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2024, INRIA
- *  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 HPP_FCL_COLLISION_DATA_H
-#define HPP_FCL_COLLISION_DATA_H
-
-#include <vector>
-#include <array>
-#include <set>
-#include <limits>
-#include <numeric>
-
-#include "hpp/fcl/collision_object.h"
-#include "hpp/fcl/config.hh"
-#include "hpp/fcl/data_types.h"
-#include "hpp/fcl/timings.h"
-#include "hpp/fcl/narrowphase/narrowphase_defaults.h"
-#include "hpp/fcl/logging.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Contact information returned by collision
-struct HPP_FCL_DLLAPI Contact {
-  /// @brief collision object 1
-  const CollisionGeometry* o1;
-
-  /// @brief collision object 2
-  const CollisionGeometry* o2;
-
-  /// @brief contact primitive in object 1
-  /// if object 1 is mesh or point cloud, it is the triangle or point id
-  /// if object 1 is geometry shape, it is NONE (-1),
-  /// if object 1 is octree, it is the id of the cell
-  int b1;
-
-  /// @brief contact primitive in object 2
-  /// if object 2 is mesh or point cloud, it is the triangle or point id
-  /// if object 2 is geometry shape, it is NONE (-1),
-  /// if object 2 is octree, it is the id of the cell
-  int b2;
-
-  /// @brief contact normal, pointing from o1 to o2.
-  /// The normal defined as the normalized separation vector:
-  /// normal = (p2 - p1) / dist(o1, o2), where p1 = nearest_points[0]
-  /// belongs to o1 and p2 = nearest_points[1] belongs to o2 and dist(o1, o2) is
-  /// the **signed** distance between o1 and o2. The normal always points from
-  /// o1 to o2.
-  /// @note The separation vector is the smallest vector such that if o1 is
-  /// translated by it, o1 and o2 are in touching contact (they share at least
-  /// one contact point but have a zero intersection volume). If the shapes
-  /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) =
-  /// (p2-p1).norm().
-  Vec3f normal;
-
-  /// @brief nearest points associated to this contact.
-  /// @note Also referred as "witness points" in other collision libraries.
-  /// The points p1 = nearest_points[0] and p2 = nearest_points[1] verify the
-  /// property that dist(o1, o2) * (p1 - p2) is the separation vector between o1
-  /// and o2, with dist(o1, o2) being the **signed** distance separating o1 from
-  /// o2. See \ref DistanceResult::normal for the definition of the separation
-  /// vector. If o1 and o2 have multiple contacts, the nearest_points are
-  /// associated with the contact which has the greatest penetration depth.
-  /// TODO (louis): rename `nearest_points` to `witness_points`.
-  std::array<Vec3f, 2> nearest_points;
-
-  /// @brief contact position, in world space
-  Vec3f pos;
-
-  /// @brief penetration depth
-  FCL_REAL penetration_depth;
-
-  /// @brief invalid contact primitive information
-  static const int NONE = -1;
-
-  /// @brief Default constructor
-  Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
-    penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
-    nearest_points[0] = nearest_points[1] = normal = pos =
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
-  }
-
-  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
-          int b2_)
-      : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
-    penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
-    nearest_points[0] = nearest_points[1] = normal = pos =
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
-  }
-
-  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
-          int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_)
-      : o1(o1_),
-        o2(o2_),
-        b1(b1_),
-        b2(b2_),
-        normal(normal_),
-        nearest_points{pos_ - (depth_ * normal_ / 2),
-                       pos_ + (depth_ * normal_ / 2)},
-        pos(pos_),
-        penetration_depth(depth_) {}
-
-  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
-          int b2_, const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_,
-          FCL_REAL depth_)
-      : o1(o1_),
-        o2(o2_),
-        b1(b1_),
-        b2(b2_),
-        normal(normal_),
-        nearest_points{p1, p2},
-        pos((p1 + p2) / 2),
-        penetration_depth(depth_) {}
-
-  bool operator<(const Contact& other) const {
-    if (b1 == other.b1) return b2 < other.b2;
-    return b1 < other.b1;
-  }
-
-  bool operator==(const Contact& other) const {
-    return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 &&
-           b2 == other.b2 && normal == other.normal && pos == other.pos &&
-           nearest_points[0] == other.nearest_points[0] &&
-           nearest_points[1] == other.nearest_points[1] &&
-           penetration_depth == other.penetration_depth;
-  }
-
-  bool operator!=(const Contact& other) const { return !(*this == other); }
-
-  FCL_REAL getDistanceToCollision(const CollisionRequest& request) const;
-};
-
-struct QueryResult;
-
-/// @brief base class for all query requests
-struct HPP_FCL_DLLAPI QueryRequest {
-  // @brief Initial guess to use for the GJK algorithm
-  GJKInitialGuess gjk_initial_guess;
-
-  /// @brief whether enable gjk initial guess
-  /// @Deprecated Use gjk_initial_guess instead
-  HPP_FCL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
-  bool enable_cached_gjk_guess;
-
-  /// @brief the gjk initial guess set by user
-  mutable Vec3f cached_gjk_guess;
-
-  /// @brief the support function initial guess set by user
-  mutable support_func_guess_t cached_support_func_guess;
-
-  /// @brief maximum iteration for the GJK algorithm
-  size_t gjk_max_iterations;
-
-  /// @brief tolerance for the GJK algorithm.
-  /// Note: This tolerance determines the precision on the estimated distance
-  /// between two geometries which are not in collision.
-  /// It is recommended to not set this tolerance to less than 1e-6.
-  FCL_REAL gjk_tolerance;
-
-  /// @brief whether to enable the Nesterov accleration of GJK
-  GJKVariant gjk_variant;
-
-  /// @brief convergence criterion used to stop GJK
-  GJKConvergenceCriterion gjk_convergence_criterion;
-
-  /// @brief convergence criterion used to stop GJK
-  GJKConvergenceCriterionType gjk_convergence_criterion_type;
-
-  /// @brief max number of iterations for EPA
-  size_t epa_max_iterations;
-
-  /// @brief tolerance for EPA.
-  /// Note: This tolerance determines the precision on the estimated distance
-  /// between two geometries which are in collision.
-  /// It is recommended to not set this tolerance to less than 1e-6.
-  /// Also, setting EPA's tolerance to less than GJK's is not recommended.
-  FCL_REAL epa_tolerance;
-
-  /// @brief enable timings when performing collision/distance request
-  bool enable_timings;
-
-  /// @brief threshold below which a collision is considered.
-  FCL_REAL collision_distance_threshold;
-
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  /// @brief Default constructor.
-  QueryRequest()
-      : gjk_initial_guess(GJKInitialGuess::DefaultGuess),
-        enable_cached_gjk_guess(false),
-        cached_gjk_guess(1, 0, 0),
-        cached_support_func_guess(support_func_guess_t::Zero()),
-        gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
-        gjk_tolerance(GJK_DEFAULT_TOLERANCE),
-        gjk_variant(GJKVariant::DefaultGJK),
-        gjk_convergence_criterion(GJKConvergenceCriterion::Default),
-        gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
-        epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
-        epa_tolerance(EPA_DEFAULT_TOLERANCE),
-        enable_timings(false),
-        collision_distance_threshold(
-            Eigen::NumTraits<FCL_REAL>::dummy_precision()) {}
-
-  /// @brief Copy  constructor.
-  QueryRequest(const QueryRequest& other) = default;
-
-  /// @brief Copy  assignment operator.
-  QueryRequest& operator=(const QueryRequest& other) = default;
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-
-  /// @brief Updates the guess for the internal GJK algorithm in order to
-  /// warm-start it when reusing this collision request on the same collision
-  /// pair.
-  /// @note The option `gjk_initial_guess` must be set to
-  /// `GJKInitialGuess::CachedGuess` for this to work.
-  void updateGuess(const QueryResult& result) const;
-
-  /// @brief whether two QueryRequest are the same or not
-  inline bool operator==(const QueryRequest& other) const {
-    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-    return gjk_initial_guess == other.gjk_initial_guess &&
-           enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
-           gjk_variant == other.gjk_variant &&
-           gjk_convergence_criterion == other.gjk_convergence_criterion &&
-           gjk_convergence_criterion_type ==
-               other.gjk_convergence_criterion_type &&
-           gjk_tolerance == other.gjk_tolerance &&
-           gjk_max_iterations == other.gjk_max_iterations &&
-           cached_gjk_guess == other.cached_gjk_guess &&
-           cached_support_func_guess == other.cached_support_func_guess &&
-           epa_max_iterations == other.epa_max_iterations &&
-           epa_tolerance == other.epa_tolerance &&
-           enable_timings == other.enable_timings &&
-           collision_distance_threshold == other.collision_distance_threshold;
-    HPP_FCL_COMPILER_DIAGNOSTIC_POP
-  }
-};
-
-/// @brief base class for all query results
-struct HPP_FCL_DLLAPI QueryResult {
-  /// @brief stores the last GJK ray when relevant.
-  Vec3f cached_gjk_guess;
-
-  /// @brief stores the last support function vertex index, when relevant.
-  support_func_guess_t cached_support_func_guess;
-
-  /// @brief timings for the given request
-  CPUTimes timings;
-
-  QueryResult()
-      : cached_gjk_guess(Vec3f::Zero()),
-        cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
-};
-
-inline void QueryRequest::updateGuess(const QueryResult& result) const {
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
-      enable_cached_gjk_guess) {
-    cached_gjk_guess = result.cached_gjk_guess;
-    cached_support_func_guess = result.cached_support_func_guess;
-  }
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-}
-
-struct CollisionResult;
-
-/// @brief flag declaration for specifying required params in CollisionResult
-enum CollisionRequestFlag {
-  CONTACT = 0x00001,
-  DISTANCE_LOWER_BOUND = 0x00002,
-  NO_REQUEST = 0x01000
-};
-
-/// @brief request to the collision algorithm
-struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest {
-  /// @brief The maximum number of contacts that can be returned
-  size_t num_max_contacts;
-
-  /// @brief whether the contact information (normal, penetration depth and
-  /// contact position) will return.
-  bool enable_contact;
-
-  /// Whether a lower bound on distance is returned when objects are disjoint
-  HPP_FCL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.)
-  bool enable_distance_lower_bound;
-
-  /// @brief Distance below which objects are considered in collision.
-  /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation
-  /// @note If set to -inf, the objects tested for collision are considered
-  ///       as collision free and no test is actually performed by functions
-  ///       hpp::fcl::collide of class hpp::fcl::ComputeCollision.
-  FCL_REAL security_margin;
-
-  /// @brief Distance below which bounding volumes are broken down.
-  /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation
-  FCL_REAL break_distance;
-
-  /// @brief Distance above which GJK solver makes an early stopping.
-  /// GJK stops searching for the closest points when it proves that the
-  /// distance between two geometries is above this threshold.
-  ///
-  /// @remarks Consequently, the closest points might be incorrect, but allows
-  /// to save computational resources.
-  FCL_REAL distance_upper_bound;
-
-  /// @brief Constructor from a flag and a maximal number of contacts.
-  ///
-  /// @param[in] flag Collision request flag
-  /// @param[in] num_max_contacts  Maximal number of allowed contacts
-  ///
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
-      : num_max_contacts(num_max_contacts_),
-        enable_contact(flag & CONTACT),
-        enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND),
-        security_margin(0),
-        break_distance(1e-3),
-        distance_upper_bound((std::numeric_limits<FCL_REAL>::max)()) {}
-
-  /// @brief Default constructor.
-  CollisionRequest()
-      : num_max_contacts(1),
-        enable_contact(true),
-        enable_distance_lower_bound(false),
-        security_margin(0),
-        break_distance(1e-3),
-        distance_upper_bound((std::numeric_limits<FCL_REAL>::max)()) {}
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-
-  bool isSatisfied(const CollisionResult& result) const;
-
-  /// @brief whether two CollisionRequest are the same or not
-  inline bool operator==(const CollisionRequest& other) const {
-    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-    return QueryRequest::operator==(other) &&
-           num_max_contacts == other.num_max_contacts &&
-           enable_contact == other.enable_contact &&
-           enable_distance_lower_bound == other.enable_distance_lower_bound &&
-           security_margin == other.security_margin &&
-           break_distance == other.break_distance &&
-           distance_upper_bound == other.distance_upper_bound;
-    HPP_FCL_COMPILER_DIAGNOSTIC_POP
-  }
-};
-
-inline FCL_REAL Contact::getDistanceToCollision(
-    const CollisionRequest& request) const {
-  return penetration_depth - request.security_margin;
-}
-
-/// @brief collision result
-struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
- private:
-  /// @brief contact information
-  std::vector<Contact> contacts;
-
- public:
-  /// Lower bound on distance between objects if they are disjoint.
-  /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation
-  /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is
-  /// set to infinity, distance_lower_bound is the actual distance between the
-  /// shapes.
-  FCL_REAL distance_lower_bound;
-
-  /// @brief normal associated to nearest_points.
-  /// Same as `CollisionResult::nearest_points` but for the normal.
-  Vec3f normal;
-
-  /// @brief nearest points.
-  /// A `CollisionResult` can have multiple contacts.
-  /// The nearest points in `CollisionResults` correspond to the witness points
-  /// associated with the smallest distance i.e the `distance_lower_bound`.
-  /// For bounding volumes and BVHs, these nearest points are available
-  /// only when distance_lower_bound is inferior to
-  /// CollisionRequest::break_distance.
-  std::array<Vec3f, 2> nearest_points;
-
- public:
-  CollisionResult()
-      : distance_lower_bound((std::numeric_limits<FCL_REAL>::max)()) {
-    nearest_points[0] = nearest_points[1] = normal =
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
-  }
-
-  /// @brief Update the lower bound only if the distance is inferior.
-  inline void updateDistanceLowerBound(const FCL_REAL& distance_lower_bound_) {
-    if (distance_lower_bound_ < distance_lower_bound)
-      distance_lower_bound = distance_lower_bound_;
-  }
-
-  /// @brief add one contact into result structure
-  inline void addContact(const Contact& c) { contacts.push_back(c); }
-
-  /// @brief whether two CollisionResult are the same or not
-  inline bool operator==(const CollisionResult& other) const {
-    return contacts == other.contacts &&
-           distance_lower_bound == other.distance_lower_bound &&
-           nearest_points[0] == other.nearest_points[0] &&
-           nearest_points[1] == other.nearest_points[1] &&
-           normal == other.normal;
-  }
-
-  /// @brief return binary collision result
-  bool isCollision() const { return contacts.size() > 0; }
-
-  /// @brief number of contacts found
-  size_t numContacts() const { return contacts.size(); }
-
-  /// @brief get the i-th contact calculated
-  const Contact& getContact(size_t i) const {
-    if (contacts.size() == 0)
-      HPP_FCL_THROW_PRETTY(
-          "The number of contacts is zero. No Contact can be returned.",
-          std::invalid_argument);
-
-    if (i < contacts.size())
-      return contacts[i];
-    else
-      return contacts.back();
-  }
-
-  /// @brief set the i-th contact calculated
-  void setContact(size_t i, const Contact& c) {
-    if (contacts.size() == 0)
-      HPP_FCL_THROW_PRETTY(
-          "The number of contacts is zero. No Contact can be returned.",
-          std::invalid_argument);
-
-    if (i < contacts.size())
-      contacts[i] = c;
-    else
-      contacts.back() = c;
-  }
-
-  /// @brief get all the contacts
-  void getContacts(std::vector<Contact>& contacts_) const {
-    contacts_.resize(contacts.size());
-    std::copy(contacts.begin(), contacts.end(), contacts_.begin());
-  }
-
-  const std::vector<Contact>& getContacts() const { return contacts; }
-
-  /// @brief clear the results obtained
-  void clear() {
-    distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
-    contacts.clear();
-    timings.clear();
-    nearest_points[0] = nearest_points[1] = normal =
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
-  }
-
-  /// @brief reposition Contact objects when fcl inverts them
-  /// during their construction.
-  void swapObjects();
-};
-
-/// @brief This structure allows to encode contact patches.
-/// A contact patch is defined by a set of points belonging to a subset of a
-/// plane passing by `p` and supported by `n`, where `n = Contact::normal` and
-/// `p = Contact::pos`. If we denote by P this plane and by S1 and S2 the first
-/// and second shape of a collision pair, a contact patch is represented as a
-/// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the
-/// set-intersection. Since a contact patch is a subset of a plane supported by
-/// `n`, it has a preferred direction. In HPP-FCL, the `Contact::normal` points
-/// from S1 to S2. In the same way, a contact patch points by default from S1
-/// to S2.
-///
-/// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope),
-/// so the points of the set, forming the convex-hull of the polytope, are
-/// stored in a counter-clockwise fashion.
-/// @note If needed, the internal algorithms of hpp-fcl can easily be extended
-/// to compute a contact volume instead of a contact patch.
-struct HPP_FCL_DLLAPI ContactPatch {
- public:
-  using Polygon = std::vector<Vec2f>;
-
-  /// @brief Frame of the set, expressed in the world coordinates.
-  /// The z-axis of the frame's rotation is the contact patch normal.
-  Transform3f tf;
-
-  /// @brief Direction of ContactPatch.
-  /// When doing collision detection, the convention of HPP-FCL is that the
-  /// normal always points from the first to the second shape of the collision
-  /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`.
-  /// The PatchDirection enum allows to identify if the patch points from
-  /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted
-  /// type). The Inverted type should only be used for internal HPP-FCL
-  /// computations (it allows to properly define two separate contact patches in
-  /// the same frame).
-  enum PatchDirection { DEFAULT = 0, INVERTED = 1 };
-
-  /// @brief Direction of this contact patch.
-  PatchDirection direction;
-
-  /// @brief Penetration depth of the contact patch. This value corresponds to
-  /// the signed distance `d` between the shapes.
-  /// @note For each contact point `p` in the patch of normal `n`, `p1 = p -
-  /// 0.5*d*n` and `p2 = p + 0.5*d*n` define a pair of witness points. `p1`
-  /// belongs to the surface of the first shape and `p2` belongs to the surface
-  /// of the second shape. For any pair of witness points, we always have `p2 -
-  /// p1 = d * n`. The vector `d * n` is called a minimum separation vector:
-  /// if S1 is translated by it, S1 and S2 are not in collision anymore.
-  /// @note Although there may exist multiple minimum separation vectors between
-  /// two shapes, the term "minimum" comes from the fact that it's impossible to
-  /// find a different separation vector which has a smaller norm than `d * n`.
-  FCL_REAL penetration_depth;
-
-  /// @brief Default maximum size of the polygon representing the contact patch.
-  /// Used to pre-allocate memory for the patch.
-  static constexpr size_t default_preallocated_size = 12;
-
- protected:
-  /// @brief Container for the vertices of the set.
-  Polygon m_points;
-
- public:
-  /// @brief Default constructor.
-  /// Note: the preallocated size does not determine the maximum number of
-  /// points in the patch, it only serves as preallocation if the maximum size
-  /// of the patch is known in advance. HPP-FCL will automatically expand/shrink
-  /// the contact patch if needed.
-  explicit ContactPatch(size_t preallocated_size = default_preallocated_size)
-      : tf(Transform3f::Identity()),
-        direction(PatchDirection::DEFAULT),
-        penetration_depth(0) {
-    this->m_points.reserve(preallocated_size);
-  }
-
-  /// @brief Normal of the contact patch, expressed in the WORLD frame.
-  Vec3f getNormal() const {
-    if (this->direction == PatchDirection::INVERTED) {
-      return -this->tf.rotation().col(2);
-    }
-    return this->tf.rotation().col(2);
-  }
-
-  /// @brief Returns the number of points in the contact patch.
-  size_t size() const { return this->m_points.size(); }
-
-  /// @brief Add a 3D point to the set, expressed in the world frame.
-  /// @note This function takes a 3D point and expresses it in the local frame
-  /// of the set. It then takes only the x and y components of the vector,
-  /// effectively doing a projection onto the plane to which the set belongs.
-  /// TODO(louis): if necessary, we can store the offset to the plane (x, y).
-  void addPoint(const Vec3f& point_3d) {
-    const Vec3f point = this->tf.inverseTransform(point_3d);
-    this->m_points.emplace_back(point.template head<2>());
-  }
-
-  /// @brief Get the i-th point of the set, expressed in the 3D world frame.
-  Vec3f getPoint(const size_t i) const {
-    Vec3f point(0, 0, 0);
-    point.head<2>() = this->point(i);
-    point = tf.transform(point);
-    return point;
-  }
-
-  /// @brief Get the i-th point of the contact patch, projected back onto the
-  /// first shape of the collision pair. This point is expressed in the 3D
-  /// world frame.
-  Vec3f getPointShape1(const size_t i) const {
-    Vec3f point = this->getPoint(i);
-    point -= (this->penetration_depth / 2) * this->getNormal();
-    return point;
-  }
-
-  /// @brief Get the i-th point of the contact patch, projected back onto the
-  /// first shape of the collision pair. This 3D point is expressed in the world
-  /// frame.
-  Vec3f getPointShape2(const size_t i) const {
-    Vec3f point = this->getPoint(i);
-    point += (this->penetration_depth / 2) * this->getNormal();
-    return point;
-  }
-
-  /// @brief Getter for the 2D points in the set.
-  Polygon& points() { return this->m_points; }
-
-  /// @brief Const getter for the 2D points in the set.
-  const Polygon& points() const { return this->m_points; }
-
-  /// @brief Getter for the i-th 2D point in the set.
-  Vec2f& point(const size_t i) {
-    HPP_FCL_ASSERT(this->m_points.size() > 0, "Patch is empty.",
-                   std::logic_error);
-    if (i < this->m_points.size()) {
-      return this->m_points[i];
-    }
-    return this->m_points.back();
-  }
-
-  /// @brief Const getter for the i-th 2D point in the set.
-  const Vec2f& point(const size_t i) const {
-    HPP_FCL_ASSERT(this->m_points.size() > 0, "Patch is empty.",
-                   std::logic_error);
-    if (i < this->m_points.size()) {
-      return this->m_points[i];
-    }
-    return this->m_points.back();
-  }
-
-  /// @brief Clear the set.
-  void clear() {
-    this->m_points.clear();
-    this->tf.setIdentity();
-    this->penetration_depth = 0;
-  }
-
-  /// @brief Whether two contact patches are the same or not.
-  /// @note This compares the two sets terms by terms.
-  /// However, two contact patches can be identical, but have a different
-  /// order for their points. Use `isEqual` in this case.
-  bool operator==(const ContactPatch& other) const {
-    return this->tf == other.tf && this->direction == other.direction &&
-           this->penetration_depth == other.penetration_depth &&
-           this->points() == other.points();
-  }
-
-  /// @brief Whether two contact patches are the same or not.
-  /// Checks for different order of the points.
-  bool isSame(const ContactPatch& other,
-              const FCL_REAL tol =
-                  Eigen::NumTraits<FCL_REAL>::dummy_precision()) const {
-    // The x and y axis of the set are arbitrary, but the z axis is
-    // always the normal. The position of the origin of the frame is also
-    // arbitrary. So we only check if the normals are the same.
-    if (!this->getNormal().isApprox(other.getNormal(), tol)) {
-      return false;
-    }
-
-    if (std::abs(this->penetration_depth - other.penetration_depth) > tol) {
-      return false;
-    }
-
-    if (this->direction != other.direction) {
-      return false;
-    }
-
-    if (this->size() != other.size()) {
-      return false;
-    }
-
-    // Check all points of the contact patch.
-    for (size_t i = 0; i < this->size(); ++i) {
-      bool found = false;
-      const Vec3f pi = this->getPoint(i);
-      for (size_t j = 0; j < other.size(); ++j) {
-        const Vec3f other_pj = other.getPoint(j);
-        if (pi.isApprox(other_pj, tol)) {
-          found = true;
-        }
-      }
-      if (!found) {
-        return false;
-      }
-    }
-
-    return true;
-  }
-};
-
-/// @brief Construct a frame from a `Contact`'s position and normal.
-/// Because both `Contact`'s position and normal are expressed in the world
-/// frame, this frame is also expressed w.r.t the world frame.
-/// The origin of the frame is `contact.pos` and the z-axis of the frame is
-/// `contact.normal`.
-inline void constructContactPatchFrameFromContact(const Contact& contact,
-                                                  ContactPatch& contact_patch) {
-  contact_patch.penetration_depth = contact.penetration_depth;
-  contact_patch.tf.translation() = contact.pos;
-  contact_patch.tf.rotation() =
-      constructOrthonormalBasisFromVector(contact.normal);
-  contact_patch.direction = ContactPatch::PatchDirection::DEFAULT;
-}
-
-/// @brief Structure used for internal computations. A support set and a
-/// contact patch can be represented by the same structure. In fact, a contact
-/// patch is the intersection of two support sets, one with
-/// `PatchDirection::DEFAULT` and one with `PatchDirection::INVERTED`.
-/// @note A support set with `DEFAULT` direction is the support set of a shape
-/// in the direction given by `+n`, where `n` is the z-axis of the frame's
-/// patch rotation. An `INVERTED` support set is the support set of a shape in
-/// the direction `-n`.
-using SupportSet = ContactPatch;
-
-/// @brief Request for a contact patch computation.
-struct HPP_FCL_DLLAPI ContactPatchRequest {
-  /// @brief Maximum number of contact patches that will be computed.
-  size_t max_num_patch;
-
- protected:
-  /// @brief Maximum samples to compute the support sets of curved shapes,
-  /// i.e. when the normal is perpendicular to the base of a cylinder. For
-  /// now, only relevant for Cone and Cylinder. In the future this might be
-  /// extended to Sphere and Ellipsoid.
-  size_t m_num_samples_curved_shapes;
-
-  /// @brief Tolerance below which points are added to a contact patch.
-  /// In details, given two shapes S1 and S2, a contact patch is the triple
-  /// intersection between the separating plane (P) (passing by `Contact::pos`
-  /// and supported by `Contact::normal`), S1 and S2; i.e. a contact patch is
-  /// `P & S1 & S2` if we denote `&` the set intersection operator. If a point
-  /// p1 of S1 is at a distance below `patch_tolerance` from the separating
-  /// plane, it is taken into account in the computation of the contact patch.
-  /// Otherwise, it is not used for the computation.
-  /// @note Needs to be positive.
-  FCL_REAL m_patch_tolerance;
-
- public:
-  /// @brief Default constructor.
-  /// @param max_num_patch maximum number of contact patches per collision pair.
-  /// @param max_sub_patch_size maximum size of each sub contact patch. Each
-  /// contact patch contains an internal representation for an inscribed sub
-  /// contact patch. This allows physics simulation to always work with a
-  /// predetermined maximum size for each contact patch. A sub contact patch is
-  /// simply a subset of the vertices of a contact patch.
-  /// @param num_samples_curved_shapes for shapes like cones and cylinders,
-  /// which have smooth basis (circles in this case), we need to sample a
-  /// certain amount of point of this basis.
-  /// @param patch_tolerance the tolerance below which a point of a shape is
-  /// considered to belong to the support set of this shape in the direction of
-  /// the normal. Said otherwise, `patch_tolerance` determines the "thickness"
-  /// of the separating plane between shapes of a collision pair.
-  explicit ContactPatchRequest(size_t max_num_patch = 1,
-                               size_t num_samples_curved_shapes =
-                                   ContactPatch::default_preallocated_size,
-                               FCL_REAL patch_tolerance = 1e-3)
-      : max_num_patch(max_num_patch) {
-    this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
-    this->setPatchTolerance(patch_tolerance);
-  }
-
-  /// @brief Construct a contact patch request from a collision request.
-  explicit ContactPatchRequest(const CollisionRequest& collision_request,
-                               size_t num_samples_curved_shapes =
-                                   ContactPatch::default_preallocated_size,
-                               FCL_REAL patch_tolerance = 1e-3)
-      : max_num_patch(collision_request.num_max_contacts) {
-    this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
-    this->setPatchTolerance(patch_tolerance);
-  }
-
-  /// @copydoc m_num_samples_curved_shapes
-  void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) {
-    if (num_samples_curved_shapes < 3) {
-      HPP_FCL_LOG_WARNING(
-          "`num_samples_curved_shapes` cannot be lower than 3. Setting it to "
-          "3 to prevent bugs.");
-      this->m_num_samples_curved_shapes = 3;
-    } else {
-      this->m_num_samples_curved_shapes = num_samples_curved_shapes;
-    }
-  }
-
-  /// @copydoc m_num_samples_curved_shapes
-  size_t getNumSamplesCurvedShapes() const {
-    return this->m_num_samples_curved_shapes;
-  }
-
-  /// @copydoc m_patch_tolerance
-  void setPatchTolerance(const FCL_REAL patch_tolerance) {
-    if (patch_tolerance < 0) {
-      HPP_FCL_LOG_WARNING(
-          "`patch_tolerance` cannot be negative. Setting it to 0 to prevent "
-          "bugs.");
-      this->m_patch_tolerance = Eigen::NumTraits<FCL_REAL>::dummy_precision();
-    } else {
-      this->m_patch_tolerance = patch_tolerance;
-    }
-  }
-
-  /// @copydoc m_patch_tolerance
-  FCL_REAL getPatchTolerance() const { return this->m_patch_tolerance; }
-
-  /// @brief Whether two ContactPatchRequest are identical or not.
-  bool operator==(const ContactPatchRequest& other) const {
-    return this->max_num_patch == other.max_num_patch &&
-           this->getNumSamplesCurvedShapes() ==
-               other.getNumSamplesCurvedShapes() &&
-           this->getPatchTolerance() == other.getPatchTolerance();
-  }
-};
-
-/// @brief Result for a contact patch computation.
-struct HPP_FCL_DLLAPI ContactPatchResult {
-  using ContactPatchVector = std::vector<ContactPatch>;
-  using ContactPatchRef = std::reference_wrapper<ContactPatch>;
-  using ContactPatchRefVector = std::vector<ContactPatchRef>;
-
- protected:
-  /// @brief Data container for the vector of contact patches.
-  /// @note Contrary to `CollisionResult` or `DistanceResult`, which have a
-  /// very small memory footprint, contact patches can contain relatively
-  /// large polytopes. In order to reuse a `ContactPatchResult` while avoiding
-  /// successive mallocs, we have a data container and a vector which points
-  /// to the currently active patches in this data container.
-  ContactPatchVector m_contact_patches_data;
-
-  /// @brief Contact patches in `m_contact_patches_data` can have two
-  /// statuses: used or unused. This index tracks the first unused patch in
-  /// the `m_contact_patches_data` vector.
-  size_t m_id_available_patch;
-
-  /// @brief Vector of contact patches of the result.
-  ContactPatchRefVector m_contact_patches;
-
- public:
-  /// @brief Default constructor.
-  ContactPatchResult() : m_id_available_patch(0) {
-    const size_t max_num_patch = 1;
-    const ContactPatchRequest request(max_num_patch);
-    this->set(request);
-  }
-
-  /// @brief Constructor using a `ContactPatchRequest`.
-  explicit ContactPatchResult(const ContactPatchRequest& request)
-      : m_id_available_patch(0) {
-    this->set(request);
-  };
-
-  /// @brief Number of contact patches in the result.
-  size_t numContactPatches() const { return this->m_contact_patches.size(); }
-
-  /// @brief Returns a new unused contact patch from the internal data vector.
-  ContactPatchRef getUnusedContactPatch() {
-    if (this->m_id_available_patch >= this->m_contact_patches_data.size()) {
-      HPP_FCL_LOG_WARNING(
-          "Trying to get an unused contact patch but all contact patches are "
-          "used. Increasing size of contact patches vector, at the cost of a "
-          "copy. You should increase `max_num_patch` in the "
-          "`ContactPatchRequest`.");
-      this->m_contact_patches_data.emplace_back(
-          this->m_contact_patches_data.back());
-      this->m_contact_patches_data.back().clear();
-    }
-    ContactPatch& contact_patch =
-        this->m_contact_patches_data[this->m_id_available_patch];
-    contact_patch.clear();
-    this->m_contact_patches.emplace_back(contact_patch);
-    ++(this->m_id_available_patch);
-    return this->m_contact_patches.back();
-  }
-
-  /// @brief Const getter for the i-th contact patch of the result.
-  const ContactPatch& getContactPatch(const size_t i) const {
-    if (this->m_contact_patches.empty()) {
-      HPP_FCL_THROW_PRETTY(
-          "The number of contact patches is zero. No ContactPatch can be "
-          "returned.",
-          std::invalid_argument);
-    }
-    if (i < this->m_contact_patches.size()) {
-      return this->m_contact_patches[i];
-    }
-    return this->m_contact_patches.back();
-  }
-
-  /// @brief Getter for the i-th contact patch of the result.
-  ContactPatch& contactPatch(const size_t i) {
-    if (this->m_contact_patches.empty()) {
-      HPP_FCL_THROW_PRETTY(
-          "The number of contact patches is zero. No ContactPatch can be "
-          "returned.",
-          std::invalid_argument);
-    }
-    if (i < this->m_contact_patches.size()) {
-      return this->m_contact_patches[i];
-    }
-    return this->m_contact_patches.back();
-  }
-
-  /// @brief Clears the contact patch result.
-  void clear() {
-    this->m_contact_patches.clear();
-    this->m_id_available_patch = 0;
-    for (ContactPatch& patch : this->m_contact_patches_data) {
-      patch.clear();
-    }
-  }
-
-  /// @brief Set up a `ContactPatchResult` from a `ContactPatchRequest`
-  void set(const ContactPatchRequest& request) {
-    if (this->m_contact_patches_data.size() < request.max_num_patch) {
-      this->m_contact_patches_data.resize(request.max_num_patch);
-    }
-    for (ContactPatch& patch : this->m_contact_patches_data) {
-      patch.points().reserve(request.getNumSamplesCurvedShapes());
-    }
-    this->clear();
-  }
-
-  /// @brief Return true if this `ContactPatchResult` is aligned with the
-  /// `ContactPatchRequest` given as input.
-  bool check(const ContactPatchRequest& request) const {
-    assert(this->m_contact_patches_data.size() >= request.max_num_patch);
-    if (this->m_contact_patches_data.size() < request.max_num_patch) {
-      return false;
-    }
-
-    for (const ContactPatch& patch : this->m_contact_patches_data) {
-      if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) {
-        assert(patch.points().capacity() >=
-               request.getNumSamplesCurvedShapes());
-        return false;
-      }
-    }
-    return true;
-  }
-
-  /// @brief Whether two ContactPatchResult are identical or not.
-  bool operator==(const ContactPatchResult& other) const {
-    if (this->numContactPatches() != other.numContactPatches()) {
-      return false;
-    }
-
-    for (size_t i = 0; i < this->numContactPatches(); ++i) {
-      const ContactPatch& patch = this->getContactPatch(i);
-      const ContactPatch& other_patch = other.getContactPatch(i);
-      if (!(patch == other_patch)) {
-        return false;
-      }
-    }
-
-    return true;
-  }
-
-  /// @brief Repositions the ContactPatch when they get inverted during their
-  /// construction.
-  void swapObjects() {
-    // Create new transform: it's the reflection of `tf` along the z-axis.
-    // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis
-    // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis.
-    for (size_t i = 0; i < this->numContactPatches(); ++i) {
-      ContactPatch& patch = this->contactPatch(i);
-      patch.tf.rotation().col(0) *= -1.0;
-      patch.tf.rotation().col(2) *= -1.0;
-
-      for (size_t j = 0; j < patch.size(); ++j) {
-        patch.point(i)(0) *= -1.0;  // only invert the x-axis
-      }
-    }
-  }
-};
-
-struct DistanceResult;
-
-/// @brief request to the distance computation
-struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest {
-  /// @brief whether to return the nearest points.
-  /// Nearest points are always computed and are the points of the shapes that
-  /// achieve a distance of `DistanceResult::min_distance`.
-  HPP_FCL_DEPRECATED_MESSAGE(
-      Nearest points are always computed : they are the points of the shapes
-          that achieve a distance of
-      `DistanceResult::min_distance`
-              .\n Use `enable_signed_distance` if you want to compute a signed
-                  minimum distance(and thus its corresponding nearest points)
-              .)
-  bool enable_nearest_points;
-
-  /// @brief whether to compute the penetration depth when objects are in
-  /// collision.
-  /// Turning this off can save computation time if only the distance
-  /// when objects are disjoint is needed.
-  /// @note The minimum distance between the shapes is stored in
-  /// `DistanceResult::min_distance`.
-  /// If `enable_signed_distance` is off, `DistanceResult::min_distance`
-  /// is always positive.
-  /// If `enable_signed_distance` is on, `DistanceResult::min_distance`
-  /// can be positive or negative.
-  /// The nearest points are the points of the shapes that achieve
-  /// a distance of `DistanceResult::min_distance`.
-  bool enable_signed_distance;
-
-  /// @brief error threshold for approximate distance
-  FCL_REAL rel_err;  // relative error, between 0 and 1
-  FCL_REAL abs_err;  // absolute error
-
-  /// \param enable_nearest_points_ enables the nearest points computation.
-  /// \param enable_signed_distance_ allows to compute the penetration depth
-  /// \param rel_err_
-  /// \param abs_err_
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  DistanceRequest(bool enable_nearest_points_ = true,
-                  bool enable_signed_distance_ = true, FCL_REAL rel_err_ = 0.0,
-                  FCL_REAL abs_err_ = 0.0)
-      : enable_nearest_points(enable_nearest_points_),
-        enable_signed_distance(enable_signed_distance_),
-        rel_err(rel_err_),
-        abs_err(abs_err_) {}
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-
-  bool isSatisfied(const DistanceResult& result) const;
-
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  DistanceRequest& operator=(const DistanceRequest& other) = default;
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-
-  /// @brief whether two DistanceRequest are the same or not
-  inline bool operator==(const DistanceRequest& other) const {
-    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-    return QueryRequest::operator==(other) &&
-           enable_nearest_points == other.enable_nearest_points &&
-           enable_signed_distance == other.enable_signed_distance &&
-           rel_err == other.rel_err && abs_err == other.abs_err;
-    HPP_FCL_COMPILER_DIAGNOSTIC_POP
-  }
-};
-
-/// @brief distance result
-struct HPP_FCL_DLLAPI DistanceResult : QueryResult {
- public:
-  /// @brief minimum distance between two objects.
-  /// If two objects are in collision and
-  /// DistanceRequest::enable_signed_distance is activated, min_distance <= 0.
-  /// @note The nearest points are the points of the shapes that achieve a
-  /// distance of `DistanceResult::min_distance`.
-  FCL_REAL min_distance;
-
-  /// @brief normal.
-  Vec3f normal;
-
-  /// @brief nearest points.
-  /// See CollisionResult::nearest_points.
-  std::array<Vec3f, 2> nearest_points;
-
-  /// @brief collision object 1
-  const CollisionGeometry* o1;
-
-  /// @brief collision object 2
-  const CollisionGeometry* o2;
-
-  /// @brief information about the nearest point in object 1
-  /// if object 1 is mesh or point cloud, it is the triangle or point id
-  /// if object 1 is geometry shape, it is NONE (-1),
-  /// if object 1 is octree, it is the id of the cell
-  int b1;
-
-  /// @brief information about the nearest point in object 2
-  /// if object 2 is mesh or point cloud, it is the triangle or point id
-  /// if object 2 is geometry shape, it is NONE (-1),
-  /// if object 2 is octree, it is the id of the cell
-  int b2;
-
-  /// @brief invalid contact primitive information
-  static const int NONE = -1;
-
-  DistanceResult(
-      FCL_REAL min_distance_ = (std::numeric_limits<FCL_REAL>::max)())
-      : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
-    const Vec3f nan(
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
-    nearest_points[0] = nearest_points[1] = normal = nan;
-  }
-
-  /// @brief add distance information into the result
-  void update(FCL_REAL distance, const CollisionGeometry* o1_,
-              const CollisionGeometry* o2_, int b1_, int b2_) {
-    if (min_distance > distance) {
-      min_distance = distance;
-      o1 = o1_;
-      o2 = o2_;
-      b1 = b1_;
-      b2 = b2_;
-    }
-  }
-
-  /// @brief add distance information into the result
-  void update(FCL_REAL distance, const CollisionGeometry* o1_,
-              const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1,
-              const Vec3f& p2, const Vec3f& normal_) {
-    if (min_distance > distance) {
-      min_distance = distance;
-      o1 = o1_;
-      o2 = o2_;
-      b1 = b1_;
-      b2 = b2_;
-      nearest_points[0] = p1;
-      nearest_points[1] = p2;
-      normal = normal_;
-    }
-  }
-
-  /// @brief add distance information into the result
-  void update(const DistanceResult& other_result) {
-    if (min_distance > other_result.min_distance) {
-      min_distance = other_result.min_distance;
-      o1 = other_result.o1;
-      o2 = other_result.o2;
-      b1 = other_result.b1;
-      b2 = other_result.b2;
-      nearest_points[0] = other_result.nearest_points[0];
-      nearest_points[1] = other_result.nearest_points[1];
-      normal = other_result.normal;
-    }
-  }
-
-  /// @brief clear the result
-  void clear() {
-    const Vec3f nan(
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
-    min_distance = (std::numeric_limits<FCL_REAL>::max)();
-    o1 = NULL;
-    o2 = NULL;
-    b1 = NONE;
-    b2 = NONE;
-    nearest_points[0] = nearest_points[1] = normal = nan;
-    timings.clear();
-  }
-
-  /// @brief whether two DistanceResult are the same or not
-  inline bool operator==(const DistanceResult& other) const {
-    bool is_same = min_distance == other.min_distance &&
-                   nearest_points[0] == other.nearest_points[0] &&
-                   nearest_points[1] == other.nearest_points[1] &&
-                   normal == other.normal && o1 == other.o1 && o2 == other.o2 &&
-                   b1 == other.b1 && b2 == other.b2;
-
-    // TODO: check also that two GeometryObject are indeed equal.
-    if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
-    is_same &= (o1 == other.o1);
-    //    else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 ==
-    //    *other.o1;
-
-    if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
-    is_same &= (o2 == other.o2);
-    //    else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 ==
-    //    *other.o2;
-
-    return is_same;
-  }
-};
-
-namespace internal {
-inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
-                                           CollisionResult& res,
-                                           const FCL_REAL sqrDistLowerBound) {
-  // BV cannot find negative distance.
-  if (res.distance_lower_bound <= 0) return;
-  FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound);  // - req.security_margin;
-  if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
-}
-
-inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
-                                             CollisionResult& res,
-                                             const FCL_REAL& distance,
-                                             const Vec3f& p0, const Vec3f& p1,
-                                             const Vec3f& normal) {
-  if (distance < res.distance_lower_bound) {
-    res.distance_lower_bound = distance;
-    res.nearest_points[0] = p0;
-    res.nearest_points[1] = p1;
-    res.normal = normal;
-  }
-}
-}  // namespace internal
-
-inline CollisionRequestFlag operator~(CollisionRequestFlag a) {
-  return static_cast<CollisionRequestFlag>(~static_cast<int>(a));
-}
-
-inline CollisionRequestFlag operator|(CollisionRequestFlag a,
-                                      CollisionRequestFlag b) {
-  return static_cast<CollisionRequestFlag>(static_cast<int>(a) |
-                                           static_cast<int>(b));
-}
-
-inline CollisionRequestFlag operator&(CollisionRequestFlag a,
-                                      CollisionRequestFlag b) {
-  return static_cast<CollisionRequestFlag>(static_cast<int>(a) &
-                                           static_cast<int>(b));
-}
-
-inline CollisionRequestFlag operator^(CollisionRequestFlag a,
-                                      CollisionRequestFlag b) {
-  return static_cast<CollisionRequestFlag>(static_cast<int>(a) ^
-                                           static_cast<int>(b));
-}
-
-inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a,
-                                        CollisionRequestFlag b) {
-  return (CollisionRequestFlag&)((int&)(a) |= static_cast<int>(b));
-}
-
-inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a,
-                                        CollisionRequestFlag b) {
-  return (CollisionRequestFlag&)((int&)(a) &= static_cast<int>(b));
-}
-
-inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a,
-                                        CollisionRequestFlag b) {
-  return (CollisionRequestFlag&)((int&)(a) ^= static_cast<int>(b));
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/collision_data.h>
diff --git a/include/hpp/fcl/collision_func_matrix.h b/include/hpp/fcl/collision_func_matrix.h
index 57b78bdfaf571a4201234132febc8523c957dabb..c17b824125b5c240f9918e388cb496b43fb80d85 100644
--- a/include/hpp/fcl/collision_func_matrix.h
+++ b/include/hpp/fcl/collision_func_matrix.h
@@ -1,80 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_COLLISION_FUNC_MATRIX_H
-#define HPP_FCL_COLLISION_FUNC_MATRIX_H
-
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief collision matrix stores the functions for collision between different
-/// types of objects and provides a uniform call interface
-
-struct HPP_FCL_DLLAPI CollisionFunctionMatrix {
-  /// @brief the uniform call interface for collision: for collision, we need
-  /// know
-  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
-  /// and tf2;
-  /// 2. the solver for narrow phase collision, this is for the collision
-  /// between geometric shapes;
-  /// 3. the request setting for collision (e.g., whether need to return normal
-  /// information, whether need to compute cost);
-  /// 4. the structure to return collision result
-  typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1,
-                                       const Transform3f& tf1,
-                                       const CollisionGeometry* o2,
-                                       const Transform3f& tf2,
-                                       const GJKSolver* nsolver,
-                                       const CollisionRequest& request,
-                                       CollisionResult& result);
-
-  /// @brief each item in the collision matrix is a function to handle collision
-  /// between objects of type1 and type2
-  CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
-
-  CollisionFunctionMatrix();
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/collision_func_matrix.h>
diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h
index fae6514644394076b22f8c9f782996578c8a15ea..245da1c077367372312b2c600e47ba6dbe5b4e34 100644
--- a/include/hpp/fcl/collision_object.h
+++ b/include/hpp/fcl/collision_object.h
@@ -1,363 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_COLLISION_OBJECT_BASE_H
-#define HPP_FCL_COLLISION_OBJECT_BASE_H
-
-#include <limits>
-#include <typeinfo>
-
-#include <hpp/fcl/deprecated.hh>
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/BV/AABB.h>
-#include <hpp/fcl/math/transform.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief object type: BVH (mesh, points), basic geometry, octree
-enum OBJECT_TYPE {
-  OT_UNKNOWN,
-  OT_BVH,
-  OT_GEOM,
-  OT_OCTREE,
-  OT_HFIELD,
-  OT_COUNT
-};
-
-/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS,
-/// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone,
-/// cylinder, convex, plane, triangle), and octree
-enum NODE_TYPE {
-  BV_UNKNOWN,
-  BV_AABB,
-  BV_OBB,
-  BV_RSS,
-  BV_kIOS,
-  BV_OBBRSS,
-  BV_KDOP16,
-  BV_KDOP18,
-  BV_KDOP24,
-  GEOM_BOX,
-  GEOM_SPHERE,
-  GEOM_CAPSULE,
-  GEOM_CONE,
-  GEOM_CYLINDER,
-  GEOM_CONVEX,
-  GEOM_PLANE,
-  GEOM_HALFSPACE,
-  GEOM_TRIANGLE,
-  GEOM_OCTREE,
-  GEOM_ELLIPSOID,
-  HF_AABB,
-  HF_OBBRSS,
-  NODE_COUNT
-};
-
-/// @addtogroup Construction_Of_BVH
-/// @{
-
-/// @brief The geometry for the object for collision or distance computation
-class HPP_FCL_DLLAPI CollisionGeometry {
- public:
-  CollisionGeometry()
-      : aabb_center(Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)())),
-        aabb_radius(-1),
-        cost_density(1),
-        threshold_occupied(1),
-        threshold_free(0) {}
-
-  /// \brief Copy constructor
-  CollisionGeometry(const CollisionGeometry& other) = default;
-
-  virtual ~CollisionGeometry() {}
-
-  /// @brief Clone *this into a new CollisionGeometry
-  virtual CollisionGeometry* clone() const = 0;
-
-  /// \brief Equality operator
-  bool operator==(const CollisionGeometry& other) const {
-    return cost_density == other.cost_density &&
-           threshold_occupied == other.threshold_occupied &&
-           threshold_free == other.threshold_free &&
-           aabb_center == other.aabb_center &&
-           aabb_radius == other.aabb_radius && aabb_local == other.aabb_local &&
-           isEqual(other);
-  }
-
-  /// \brief Difference operator
-  bool operator!=(const CollisionGeometry& other) const {
-    return isNotEqual(other);
-  }
-
-  /// @brief get the type of the object
-  virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
-
-  /// @brief get the node type
-  virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
-
-  /// @brief compute the AABB for object in local coordinate
-  virtual void computeLocalAABB() = 0;
-
-  /// @brief get user data in geometry
-  void* getUserData() const { return user_data; }
-
-  /// @brief set user data in geometry
-  void setUserData(void* data) { user_data = data; }
-
-  /// @brief whether the object is completely occupied
-  inline bool isOccupied() const { return cost_density >= threshold_occupied; }
-
-  /// @brief whether the object is completely free
-  inline bool isFree() const { return cost_density <= threshold_free; }
-
-  /// @brief whether the object has some uncertainty
-  bool isUncertain() const;
-
-  /// @brief AABB center in local coordinate
-  Vec3f aabb_center;
-
-  /// @brief AABB radius
-  FCL_REAL aabb_radius;
-
-  /// @brief AABB in local coordinate, used for tight AABB when only translation
-  /// transform
-  AABB aabb_local;
-
-  /// @brief pointer to user defined data specific to this object
-  void* user_data;
-
-  /// @brief collision cost for unit volume
-  FCL_REAL cost_density;
-
-  /// @brief threshold for occupied ( >= is occupied)
-  FCL_REAL threshold_occupied;
-
-  /// @brief threshold for free (<= is free)
-  FCL_REAL threshold_free;
-
-  /// @brief compute center of mass
-  virtual Vec3f computeCOM() const { return Vec3f::Zero(); }
-
-  /// @brief compute the inertia matrix, related to the origin
-  virtual Matrix3f computeMomentofInertia() const {
-    return Matrix3f::Constant(NAN);
-  }
-
-  /// @brief compute the volume
-  virtual FCL_REAL computeVolume() const { return 0; }
-
-  /// @brief compute the inertia matrix, related to the com
-  virtual Matrix3f computeMomentofInertiaRelatedToCOM() const {
-    Matrix3f C = computeMomentofInertia();
-    Vec3f com = computeCOM();
-    FCL_REAL V = computeVolume();
-
-    return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
-            C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2],
-            C(1, 0) + V * com[1] * com[0],
-            C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
-            C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0],
-            C(2, 1) + V * com[2] * com[1],
-            C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]))
-        .finished();
-  }
-
- private:
-  /// @brief equal operator with another object of derived type.
-  virtual bool isEqual(const CollisionGeometry& other) const = 0;
-
-  /// @brief not equal operator with another object of derived type.
-  virtual bool isNotEqual(const CollisionGeometry& other) const {
-    return !(*this == other);
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief the object for collision or distance computation, contains the
-/// geometry and the transform information
-class HPP_FCL_DLLAPI CollisionObject {
- public:
-  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
-                  bool compute_local_aabb = true)
-      : cgeom(cgeom_), user_data(nullptr) {
-    init(compute_local_aabb);
-  }
-
-  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
-                  const Transform3f& tf, bool compute_local_aabb = true)
-      : cgeom(cgeom_), t(tf), user_data(nullptr) {
-    init(compute_local_aabb);
-  }
-
-  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
-                  const Matrix3f& R, const Vec3f& T,
-                  bool compute_local_aabb = true)
-      : cgeom(cgeom_), t(R, T), user_data(nullptr) {
-    init(compute_local_aabb);
-  }
-
-  bool operator==(const CollisionObject& other) const {
-    return cgeom == other.cgeom && t == other.t && user_data == other.user_data;
-  }
-
-  bool operator!=(const CollisionObject& other) const {
-    return !(*this == other);
-  }
-
-  ~CollisionObject() {}
-
-  /// @brief get the type of the object
-  OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); }
-
-  /// @brief get the node type
-  NODE_TYPE getNodeType() const { return cgeom->getNodeType(); }
-
-  /// @brief get the AABB in world space
-  const AABB& getAABB() const { return aabb; }
-
-  /// @brief get the AABB in world space
-  AABB& getAABB() { return aabb; }
-
-  /// @brief compute the AABB in world space
-  void computeAABB() {
-    if (t.getRotation().isIdentity()) {
-      aabb = translate(cgeom->aabb_local, t.getTranslation());
-    } else {
-      aabb.min_ = aabb.max_ = t.getTranslation();
-
-      Vec3f min_world, max_world;
-      for (int k = 0; k < 3; ++k) {
-        min_world.array() = t.getRotation().row(k).array() *
-                            cgeom->aabb_local.min_.transpose().array();
-        max_world.array() = t.getRotation().row(k).array() *
-                            cgeom->aabb_local.max_.transpose().array();
-
-        aabb.min_[k] += (min_world.array().min)(max_world.array()).sum();
-        aabb.max_[k] += (min_world.array().max)(max_world.array()).sum();
-      }
-    }
-  }
-
-  /// @brief get user data in object
-  void* getUserData() const { return user_data; }
-
-  /// @brief set user data in object
-  void setUserData(void* data) { user_data = data; }
-
-  /// @brief get translation of the object
-  inline const Vec3f& getTranslation() const { return t.getTranslation(); }
-
-  /// @brief get matrix rotation of the object
-  inline const Matrix3f& getRotation() const { return t.getRotation(); }
-
-  /// @brief get object's transform
-  inline const Transform3f& getTransform() const { return t; }
-
-  /// @brief set object's rotation matrix
-  void setRotation(const Matrix3f& R) { t.setRotation(R); }
-
-  /// @brief set object's translation
-  void setTranslation(const Vec3f& T) { t.setTranslation(T); }
-
-  /// @brief set object's transform
-  void setTransform(const Matrix3f& R, const Vec3f& T) { t.setTransform(R, T); }
-
-  /// @brief set object's transform
-  void setTransform(const Transform3f& tf) { t = tf; }
-
-  /// @brief whether the object is in local coordinate
-  bool isIdentityTransform() const { return t.isIdentity(); }
-
-  /// @brief set the object in local coordinate
-  void setIdentityTransform() { t.setIdentity(); }
-
-  /// @brief get shared pointer to collision geometry of the object instance
-  const shared_ptr<const CollisionGeometry> collisionGeometry() const {
-    return cgeom;
-  }
-
-  /// @brief get shared pointer to collision geometry of the object instance
-  const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; }
-
-  /// @brief get raw pointer to collision geometry of the object instance
-  const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); }
-
-  /// @brief get raw pointer to collision geometry of the object instance
-  CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); }
-
-  /// @brief Associate a new CollisionGeometry
-  ///
-  /// @param[in] collision_geometry The new CollisionGeometry
-  /// @param[in] compute_local_aabb Whether the local aabb of the input new has
-  /// to be computed.
-  ///
-  void setCollisionGeometry(
-      const shared_ptr<CollisionGeometry>& collision_geometry,
-      bool compute_local_aabb = true) {
-    if (collision_geometry.get() != cgeom.get()) {
-      cgeom = collision_geometry;
-      init(compute_local_aabb);
-    }
-  }
-
- protected:
-  void init(bool compute_local_aabb = true) {
-    if (cgeom) {
-      if (compute_local_aabb) cgeom->computeLocalAABB();
-      computeAABB();
-    }
-  }
-
-  shared_ptr<CollisionGeometry> cgeom;
-
-  Transform3f t;
-
-  /// @brief AABB in global coordinate
-  mutable AABB aabb;
-
-  /// @brief pointer to user defined data specific to this object
-  void* user_data;
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/collision_object.h>
diff --git a/include/hpp/fcl/collision_utility.h b/include/hpp/fcl/collision_utility.h
index b231973eccd8ff9db9ce645e4627c8a6ed493387..9043e1f8eee0a663253a49597bd8fbb31d8e8375 100644
--- a/include/hpp/fcl/collision_utility.h
+++ b/include/hpp/fcl/collision_utility.h
@@ -1,59 +1,2 @@
-// Copyright (c) 2017 CNRS
-// Copyright (c) 2022 INRIA
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-//
-// This file is part of hpp-fcl.
-// hpp-fcl is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-//
-// hpp-fcl is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-// General Lesser Public License for more details.  You should have
-// received a copy of the GNU Lesser General Public License along with
-// hpp-fcl. If not, see <http://www.gnu.org/licenses/>.
-
-#ifndef HPP_FCL_COLLISION_UTILITY_H
-#define HPP_FCL_COLLISION_UTILITY_H
-
-#include <hpp/fcl/collision_object.h>
-
-namespace hpp {
-namespace fcl {
-
-HPP_FCL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model,
-                                          const Transform3f& pose,
-                                          const AABB& aabb);
-
-/**
- * \brief Returns the name associated to a NODE_TYPE
- */
-inline const char* get_node_type_name(NODE_TYPE node_type) {
-  static const char* node_type_name_all[] = {
-      "BV_UNKNOWN",     "BV_AABB",       "BV_OBB",      "BV_RSS",
-      "BV_kIOS",        "BV_OBBRSS",     "BV_KDOP16",   "BV_KDOP18",
-      "BV_KDOP24",      "GEOM_BOX",      "GEOM_SPHERE", "GEOM_CAPSULE",
-      "GEOM_CONE",      "GEOM_CYLINDER", "GEOM_CONVEX", "GEOM_PLANE",
-      "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE", "GEOM_ELLIPSOID",
-      "HF_AABB",        "HF_OBBRSS",     "NODE_COUNT"};
-
-  return node_type_name_all[node_type];
-}
-
-/**
- * \brief Returns the name associated to a OBJECT_TYPE
- */
-inline const char* get_object_type_name(OBJECT_TYPE object_type) {
-  static const char* object_type_name_all[] = {
-      "OT_UNKNOWN", "OT_BVH", "OT_GEOM", "OT_OCTREE", "OT_HFIELD", "OT_COUNT"};
-
-  return object_type_name_all[object_type];
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif  // FCL_COLLISION_UTILITY_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/collision_utility.h>
diff --git a/include/hpp/fcl/config.hh b/include/hpp/fcl/config.hh
new file mode 100644
index 0000000000000000000000000000000000000000..7388d60876038fe60cd85fbdef79d32d500d8075
--- /dev/null
+++ b/include/hpp/fcl/config.hh
@@ -0,0 +1,2 @@
+#include <hpp/fcl/coal.hpp>
+#include <coal/config.hh>
diff --git a/include/hpp/fcl/contact_patch.h b/include/hpp/fcl/contact_patch.h
index e3dfa754787bcbffb4bfe5148234ec6d6d86f439..fef1ee0d4eff442ed64cceb9b3c6056322101f5d 100644
--- a/include/hpp/fcl/contact_patch.h
+++ b/include/hpp/fcl/contact_patch.h
@@ -1,124 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2024, INRIA
- *  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 INRIA 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 Louis Montaut */
-
-#ifndef HPP_FCL_CONTACT_PATCH_H
-#define HPP_FCL_CONTACT_PATCH_H
-
-#include "hpp/fcl/data_types.h"
-#include "hpp/fcl/collision_data.h"
-#include "hpp/fcl/contact_patch/contact_patch_solver.h"
-#include "hpp/fcl/contact_patch_func_matrix.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Main contact patch computation interface.
-/// @note Please see @ref ContactPatchRequest and @ref ContactPatchResult for
-/// more info on the content of the input/output of this function. Also, please
-/// read @ref ContactPatch if you want to fully understand what is meant by
-/// "contact patch".
-HPP_FCL_DLLAPI void computeContactPatch(const CollisionGeometry* o1,
-                                        const Transform3f& tf1,
-                                        const CollisionGeometry* o2,
-                                        const Transform3f& tf2,
-                                        const CollisionResult& collision_result,
-                                        const ContactPatchRequest& request,
-                                        ContactPatchResult& result);
-
-/// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3f&,
-// const CollisionGeometry*, const Transform3f&, const CollisionResult&, const
-// ContactPatchRequest&, ContactPatchResult&);
-HPP_FCL_DLLAPI void computeContactPatch(const CollisionObject* o1,
-                                        const CollisionObject* o2,
-                                        const CollisionResult& collision_result,
-                                        const ContactPatchRequest& request,
-                                        ContactPatchResult& result);
-
-/// @brief This class reduces the cost of identifying the geometry pair.
-/// This is usefull for repeated shape-shape queries.
-/// @note This needs to be called after `collide` or after `ComputeCollision`.
-///
-/// \code
-///   ComputeContactPatch calc_patch (o1, o2);
-///   calc_patch(tf1, tf2, collision_result, patch_request, patch_result);
-/// \endcode
-class HPP_FCL_DLLAPI ComputeContactPatch {
- public:
-  /// @brief Default constructor from two Collision Geometries.
-  ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2);
-
-  void operator()(const Transform3f& tf1, const Transform3f& tf2,
-                  const CollisionResult& collision_result,
-                  const ContactPatchRequest& request,
-                  ContactPatchResult& result) const;
-
-  bool operator==(const ComputeContactPatch& other) const {
-    return this->o1 == other.o1 && this->o2 == other.o2 &&
-           this->csolver == other.csolver;
-  }
-
-  bool operator!=(const ComputeContactPatch& other) const {
-    return !(*this == other);
-  }
-
-  virtual ~ComputeContactPatch() = default;
-
- protected:
-  // These pointers are made mutable to let the derived classes to update
-  // their values when updating the collision geometry (e.g. creating a new
-  // one). This feature should be used carefully to avoid any mis usage (e.g,
-  // changing the type of the collision geometry should be avoided).
-  mutable const CollisionGeometry* o1;
-  mutable const CollisionGeometry* o2;
-
-  mutable ContactPatchSolver csolver;
-
-  ContactPatchFunctionMatrix::ContactPatchFunc func;
-  bool swap_geoms;
-
-  virtual void run(const Transform3f& tf1, const Transform3f& tf2,
-                   const CollisionResult& collision_result,
-                   const ContactPatchRequest& request,
-                   ContactPatchResult& result) const;
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/contact_patch.h>
diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.h b/include/hpp/fcl/contact_patch/contact_patch_solver.h
index ed2ae94782d354e0ca1c96b68c3c3e2a2c3af6ad..f53a91507019235de35ace29bccb66043d2f7610 100644
--- a/include/hpp/fcl/contact_patch/contact_patch_solver.h
+++ b/include/hpp/fcl/contact_patch/contact_patch_solver.h
@@ -1,210 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2024, INRIA
- *  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 INRIA 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 Louis Montaut */
-
-#ifndef HPP_FCL_CONTACT_PATCH_SOLVER_H
-#define HPP_FCL_CONTACT_PATCH_SOLVER_H
-
-#include "hpp/fcl/collision_data.h"
-#include "hpp/fcl/logging.h"
-#include "hpp/fcl/narrowphase/gjk.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Solver to compute contact patches, i.e. the intersection between two
-/// contact surfaces projected onto the shapes' separating plane.
-/// Otherwise said, a contact patch is simply the intersection between two
-/// support sets: the support set of shape S1 in direction `n` and the support
-/// set of shape S2 in direction `-n`, where `n` is the contact normal
-/// (satisfying the optimality conditions of GJK/EPA).
-/// @note A contact patch is **not** the support set of the Minkowski Difference
-/// in the direction of the normal.
-/// A contact patch is actually the support set of the Minkowski difference in
-/// the direction of the normal, i.e. the instersection of the shapes support
-/// sets as mentioned above.
-///
-/// TODO(louis): algo improvement:
-/// - The clipping algo is currently n1 * n2; it can be done in n1 + n2.
-struct HPP_FCL_DLLAPI ContactPatchSolver {
-  // Note: `ContactPatch` is an alias for `SupportSet`.
-  // The two can be used interchangeably.
-  using ShapeSupportData = details::ShapeSupportData;
-  using SupportSetDirection = SupportSet::PatchDirection;
-
-  /// @brief Support set function for shape si.
-  /// @param[in] shape the shape.
-  /// @param[in/out] support_set a support set of the shape. A support set is
-  /// attached to a frame. All the points of the set computed by this function
-  /// will be expressed in the local frame of the support set. The support set
-  /// is computed in the direction of the positive z-axis if its direction is
-  /// DEFAULT, negative z-axis if its direction is INVERTED.
-  /// @param[in/out] hint for the support computation of ConvexBase shapes. Gets
-  /// updated after calling the function onto ConvexBase shapes.
-  /// @param[in/out] support_data for the support computation of ConvexBase
-  /// shapes. Gets updated with visited vertices after calling the function onto
-  /// ConvexBase shapes.
-  /// @param[in] num_sampled_supports for shapes like cone or cylinders which
-  /// have smooth non-strictly convex sides (their bases are circles), we need
-  /// to know how many supports we sample from these sides. For any other shape,
-  /// this parameter is not used.
-  /// @param[in] tol the "thickness" of the support plane. Any point v which
-  /// satisfies `max_{x in shape}(x.dot(dir)) - v.dot(dir) <= tol` is tol
-  /// distant from the support plane and is added to the support set.
-  typedef void (*SupportSetFunction)(const ShapeBase* shape,
-                                     SupportSet& support_set, int& hint,
-                                     ShapeSupportData& support_data,
-                                     size_t num_sampled_supports, FCL_REAL tol);
-
-  /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors.
-  static constexpr size_t default_num_preallocated_supports = 16;
-
-  /// @brief Number of points sampled for Cone and Cylinder when the normal is
-  /// orthogonal to the shapes' basis.
-  /// See @ref ContactPatchRequest::m_num_samples_curved_shapes for more
-  /// details.
-  size_t num_samples_curved_shapes;
-
-  /// @brief Tolerance below which points are added to the shapes support sets.
-  /// See @ref ContactPatchRequest::m_patch_tolerance for more details.
-  FCL_REAL patch_tolerance;
-
-  /// @brief Support set function for shape s1.
-  mutable SupportSetFunction supportFuncShape1;
-
-  /// @brief Support set function for shape s2.
-  mutable SupportSetFunction supportFuncShape2;
-
-  /// @brief Temporary data to compute the support sets on each shape.
-  mutable std::array<ShapeSupportData, 2> supports_data;
-
-  /// @brief Guess for the support sets computation.
-  mutable support_func_guess_t support_guess;
-
-  /// @brief Holder for support set of shape 1, used for internal computation.
-  /// After `computePatch` has been called, this support set is no longer valid.
-  mutable SupportSet support_set_shape1;
-
-  /// @brief Holder for support set of shape 2, used for internal computation.
-  /// After `computePatch` has been called, this support set is no longer valid.
-  mutable SupportSet support_set_shape2;
-
-  /// @brief Temporary support set used for the Sutherland-Hodgman algorithm.
-  mutable SupportSet support_set_buffer;
-
-  /// @brief Tracks which point of the Sutherland-Hodgman result have been added
-  /// to the contact patch. Only used if the post-processing step occurs, i.e.
-  /// if the result of Sutherland-Hodgman has a size bigger than
-  /// `max_patch_size`.
-  mutable std::vector<bool> added_to_patch;
-
-  /// @brief Default constructor.
-  explicit ContactPatchSolver() {
-    const size_t num_contact_patch = 1;
-    const size_t preallocated_patch_size =
-        ContactPatch::default_preallocated_size;
-    const FCL_REAL patch_tolerance = 1e-3;
-    const ContactPatchRequest request(num_contact_patch,
-                                      preallocated_patch_size, patch_tolerance);
-    this->set(request);
-  }
-
-  /// @brief Construct the solver with a `ContactPatchRequest`.
-  explicit ContactPatchSolver(const ContactPatchRequest& request) {
-    this->set(request);
-  }
-
-  /// @brief Set up the solver using a `ContactPatchRequest`.
-  void set(const ContactPatchRequest& request);
-
-  /// @brief Sets the support guess used during support set computation of
-  /// shapes s1 and s2.
-  void setSupportGuess(const support_func_guess_t guess) const {
-    this->support_guess = guess;
-  }
-
-  /// @brief Main API of the solver: compute a contact patch from a contact
-  /// between shapes s1 and s2.
-  /// The contact patch is the (triple) intersection between the separating
-  /// plane passing (by `contact.pos` and supported by `contact.normal`) and the
-  /// shapes s1 and s2.
-  template <typename ShapeType1, typename ShapeType2>
-  void computePatch(const ShapeType1& s1, const Transform3f& tf1,
-                    const ShapeType2& s2, const Transform3f& tf2,
-                    const Contact& contact, ContactPatch& contact_patch) const;
-
-  /// @brief Reset the internal quantities of the solver.
-  template <typename ShapeType1, typename ShapeType2>
-  void reset(const ShapeType1& shape1, const Transform3f& tf1,
-             const ShapeType2& shape2, const Transform3f& tf2,
-             const ContactPatch& contact_patch) const;
-
-  /// @brief Retrieve result, adds a post-processing step if result has bigger
-  /// size than `this->max_patch_size`.
-  void getResult(const Contact& contact, const ContactPatch::Polygon* result,
-                 ContactPatch& contact_patch) const;
-
-  /// @return the intersecting point between line defined by ray (a, b) and
-  /// the segment [c, d].
-  /// @note we make the following hypothesis:
-  /// 1) c != d (should be when creating initial polytopes)
-  /// 2) (c, d) is not parallel to ray -> if so, we return d.
-  static Vec2f computeLineSegmentIntersection(const Vec2f& a, const Vec2f& b,
-                                              const Vec2f& c, const Vec2f& d);
-
-  /// @brief Construct support set function for shape.
-  static SupportSetFunction makeSupportSetFunction(
-      const ShapeBase* shape, ShapeSupportData& support_data);
-
-  bool operator==(const ContactPatchSolver& other) const {
-    return this->num_samples_curved_shapes == other.num_samples_curved_shapes &&
-           this->patch_tolerance == other.patch_tolerance &&
-           this->support_guess == other.support_guess &&
-           this->support_set_shape1 == other.support_set_shape1 &&
-           this->support_set_shape2 == other.support_set_shape2 &&
-           this->support_set_buffer == other.support_set_buffer &&
-           this->added_to_patch == other.added_to_patch &&
-           this->supportFuncShape1 == other.supportFuncShape1 &&
-           this->supportFuncShape2 == other.supportFuncShape2;
-  }
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#include "hpp/fcl/contact_patch/contact_patch_solver.hxx"
-
-#endif  // HPP_FCL_CONTACT_PATCH_SOLVER_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/contact_patch/contact_patch_solver.h>
diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx
index 1960ff0acd644ec08d0d8ba83d308edc6acbd6c1..b73d4e935e78053192057eaee1712a57b71dcb36 100644
--- a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx
+++ b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx
@@ -1,427 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2024, INRIA
- *  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 INRIA 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 Louis Montaut */
-
-#ifndef HPP_FCL_CONTACT_PATCH_SOLVER_HXX
-#define HPP_FCL_CONTACT_PATCH_SOLVER_HXX
-
-#include "hpp/fcl/data_types.h"
-#include "hpp/fcl/shape/geometric_shapes_traits.h"
-
-namespace hpp {
-namespace fcl {
-
-// ============================================================================
-inline void ContactPatchSolver::set(const ContactPatchRequest& request) {
-  // Note: it's important for the number of pre-allocated Vec3f in
-  // `m_clipping_sets` to be larger than `request.max_size_patch`
-  // because we don't know in advance how many supports will be discarded to
-  // form the convex-hulls of the shapes supports which will serve as the
-  // input of the Sutherland-Hodgman algorithm.
-  size_t num_preallocated_supports = default_num_preallocated_supports;
-  if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) {
-    num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes();
-  }
-
-  // Used for support set computation of shape1 and for the first iterate of the
-  // Sutherland-Hodgman algo.
-  this->support_set_shape1.points().reserve(num_preallocated_supports);
-  this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
-
-  // Used for computing the next iterate of the Sutherland-Hodgman algo.
-  this->support_set_buffer.points().reserve(num_preallocated_supports);
-
-  // Used for support set computation of shape2 and acts as the "clipper" set in
-  // the Sutherland-Hodgman algo.
-  this->support_set_shape2.points().reserve(num_preallocated_supports);
-  this->support_set_shape2.direction = SupportSetDirection::INVERTED;
-
-  this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes();
-  this->patch_tolerance = request.getPatchTolerance();
-}
-
-// ============================================================================
-template <typename ShapeType1, typename ShapeType2>
-void ContactPatchSolver::computePatch(const ShapeType1& s1,
-                                      const Transform3f& tf1,
-                                      const ShapeType2& s2,
-                                      const Transform3f& tf2,
-                                      const Contact& contact,
-                                      ContactPatch& contact_patch) const {
-  // Note: `ContactPatch` is an alias for `SupportSet`.
-  // Step 1
-  constructContactPatchFrameFromContact(contact, contact_patch);
-  contact_patch.points().clear();
-  if ((bool)(shape_traits<ShapeType1>::IsStrictlyConvex) ||
-      (bool)(shape_traits<ShapeType2>::IsStrictlyConvex)) {
-    // If a shape is strictly convex, the support set in any direction is
-    // reduced to a single point. Thus, the contact point `contact.pos` is the
-    // only point belonging to the contact patch, and it has already been
-    // computed.
-    // TODO(louis): even for strictly convex shapes, we can sample the support
-    // function around the normal and return a pseudo support set. This would
-    // allow spheres and ellipsoids to have a contact surface, which does make
-    // sense in certain physics simulation cases.
-    // Do the same for strictly convex regions of non-strictly convex shapes
-    // like the ends of capsules.
-    contact_patch.addPoint(contact.pos);
-    return;
-  }
-
-  // Step 2 - Compute support set of each shape, in the direction of
-  // the contact's normal.
-  // The first shape's support set is called "current"; it will be the first
-  // iterate of the Sutherland-Hodgman algorithm. The second shape's support set
-  // is called "clipper"; it will be used to clip "current". The support set
-  // computation step computes a convex polygon; its vertices are ordered
-  // counter-clockwise. This is important as the Sutherland-Hodgman algorithm
-  // expects points to be ranked counter-clockwise.
-  this->reset(s1, tf1, s2, tf2, contact_patch);
-  assert(this->num_samples_curved_shapes > 3);
-
-  this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0],
-                          this->supports_data[0],
-                          this->num_samples_curved_shapes,
-                          this->patch_tolerance);
-
-  this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1],
-                          this->supports_data[1],
-                          this->num_samples_curved_shapes,
-                          this->patch_tolerance);
-
-  // We can immediatly return if one of the support set has only
-  // one point.
-  if (this->support_set_shape1.size() <= 1 ||
-      this->support_set_shape2.size() <= 1) {
-    contact_patch.addPoint(contact.pos);
-    return;
-  }
-
-  // `eps` is be used to check strict positivity of determinants.
-  const FCL_REAL eps = Eigen::NumTraits<FCL_REAL>::dummy_precision();
-  using Polygon = SupportSet::Polygon;
-
-  if ((this->support_set_shape1.size() == 2) &&
-      (this->support_set_shape2.size() == 2)) {
-    // Segment-Segment case
-    // We compute the determinant; if it is non-zero, the intersection
-    // has already been computed: it's `Contact::pos`.
-    const Polygon& pts1 = this->support_set_shape1.points();
-    const Vec2f& a = pts1[0];
-    const Vec2f& b = pts1[1];
-
-    const Polygon& pts2 = this->support_set_shape2.points();
-    const Vec2f& c = pts2[0];
-    const Vec2f& d = pts2[1];
-
-    const FCL_REAL det =
-        (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0));
-    if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) ||
-        ((b - a).squaredNorm() < eps)) {
-      contact_patch.addPoint(contact.pos);
-      return;
-    }
-
-    const Vec2f cd = (d - c);
-    const FCL_REAL l = cd.squaredNorm();
-    Polygon& patch = contact_patch.points();
-
-    // Project a onto [c, d]
-    FCL_REAL t1 = (a - c).dot(cd);
-    t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l));
-    const Vec2f p1 = c + t1 * cd;
-    patch.emplace_back(p1);
-
-    // Project b onto [c, d]
-    FCL_REAL t2 = (b - c).dot(cd);
-    t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l));
-    const Vec2f p2 = c + t2 * cd;
-    if ((p1 - p2).squaredNorm() >= eps) {
-      patch.emplace_back(p2);
-    }
-    return;
-  }
-
-  //
-  // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the
-  // "current" polygon. The resulting intersection is the contact patch of the
-  // contact between s1 and s2. "clipper" and "current" are the support sets of
-  // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to
-  // shape1 and current to shape2, depending on which case we are). Currently,
-  // to clip one polygon with the other, we use the Sutherland-Hodgman
-  // algorithm:
-  // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm
-  // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using
-  // another polygon of size >=3. However, it can be easily extended to handle
-  // the segment-polygon case.
-  //
-  // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 +
-  // n2 where n1 and n2 are the sizes of the first and second polygon.
-  const size_t max_result_size =
-      this->support_set_shape1.size() + this->support_set_shape2.size();
-  if (this->added_to_patch.size() < max_result_size) {
-    this->added_to_patch.assign(max_result_size, false);
-  }
-
-  const Polygon* clipper_ptr = nullptr;
-  Polygon* current_ptr = nullptr;
-  Polygon* previous_ptr = &(this->support_set_buffer.points());
-
-  // Let the clipper set be the one with the most vertices, to make sure it is
-  // at least a triangle.
-  if (this->support_set_shape1.size() < this->support_set_shape2.size()) {
-    current_ptr = &(this->support_set_shape1.points());
-    clipper_ptr = &(this->support_set_shape2.points());
-  } else {
-    current_ptr = &(this->support_set_shape2.points());
-    clipper_ptr = &(this->support_set_shape1.points());
-  }
-
-  const Polygon& clipper = *(clipper_ptr);
-  const size_t clipper_size = clipper.size();
-  for (size_t i = 0; i < clipper_size; ++i) {
-    // Swap `current` and `previous`.
-    // `previous` tracks the last iteration of the algorithm; `current` is
-    // filled by clipping `current` using `clipper`.
-    Polygon* tmp_ptr = previous_ptr;
-    previous_ptr = current_ptr;
-    current_ptr = tmp_ptr;
-
-    const Polygon& previous = *(previous_ptr);
-    Polygon& current = *(current_ptr);
-    current.clear();
-
-    const Vec2f& a = clipper[i];
-    const Vec2f& b = clipper[(i + 1) % clipper_size];
-    const Vec2f ab = b - a;
-
-    if (previous.size() == 2) {
-      //
-      // Segment-Polygon case
-      //
-      const Vec2f& p1 = previous[0];
-      const Vec2f& p2 = previous[1];
-
-      const Vec2f ap1 = p1 - a;
-      const Vec2f ap2 = p2 - a;
-
-      const FCL_REAL det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
-      const FCL_REAL det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
-
-      if (det1 < 0 && det2 < 0) {
-        // Both p1 and p2 are outside the clipping polygon, i.e. there is no
-        // intersection. The algorithm can stop.
-        break;
-      }
-
-      if (det1 >= 0 && det2 >= 0) {
-        // Both p1 and p2 are inside the clipping polygon, there is nothing to
-        // do; move to the next iteration.
-        current = previous;
-        continue;
-      }
-
-      // Compute the intersection between the line (a, b) and the segment
-      // [p1, p2].
-      if (det1 >= 0) {
-        if (det1 > eps) {
-          const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2);
-          current.emplace_back(p1);
-          current.emplace_back(p);
-          continue;
-        } else {
-          // p1 is the only point of current which is also a point of the
-          // clipper. We can exit.
-          current.emplace_back(p1);
-          break;
-        }
-      } else {
-        if (det2 > eps) {
-          const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2);
-          current.emplace_back(p2);
-          current.emplace_back(p);
-          continue;
-        } else {
-          // p2 is the only point of current which is also a point of the
-          // clipper. We can exit.
-          current.emplace_back(p2);
-          break;
-        }
-      }
-    } else {
-      //
-      // Polygon-Polygon case.
-      //
-      std::fill(this->added_to_patch.begin(),  //
-                this->added_to_patch.end(),    //
-                false);
-
-      const size_t previous_size = previous.size();
-      for (size_t j = 0; j < previous_size; ++j) {
-        const Vec2f& p1 = previous[j];
-        const Vec2f& p2 = previous[(j + 1) % previous_size];
-
-        const Vec2f ap1 = p1 - a;
-        const Vec2f ap2 = p2 - a;
-
-        const FCL_REAL det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
-        const FCL_REAL det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
-
-        if (det1 < 0 && det2 < 0) {
-          // No intersection. Continue to next segment of previous.
-          continue;
-        }
-
-        if (det1 >= 0 && det2 >= 0) {
-          // Both p1 and p2 are inside the clipping polygon, add p1 to current
-          // (only if it has not already been added).
-          if (!this->added_to_patch[j]) {
-            current.emplace_back(p1);
-            this->added_to_patch[j] = true;
-          }
-          // Continue to next segment of previous.
-          continue;
-        }
-
-        if (det1 >= 0) {
-          if (det1 > eps) {
-            if (!this->added_to_patch[j]) {
-              current.emplace_back(p1);
-              this->added_to_patch[j] = true;
-            }
-            const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2);
-            current.emplace_back(p);
-          } else {
-            // a, b and p1 are colinear; we add only p1.
-            if (!this->added_to_patch[j]) {
-              current.emplace_back(p1);
-              this->added_to_patch[j] = true;
-            }
-          }
-        } else {
-          if (det2 > eps) {
-            const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2);
-            current.emplace_back(p);
-          } else {
-            if (!this->added_to_patch[(j + 1) % previous.size()]) {
-              current.emplace_back(p2);
-              this->added_to_patch[(j + 1) % previous.size()] = true;
-            }
-          }
-        }
-      }
-    }
-    //
-    // End of iteration i of Sutherland-Hodgman.
-    if (current.size() <= 1) {
-      // No intersection or one point found, the algo can early stop.
-      break;
-    }
-  }
-
-  // Transfer the result of the Sutherland-Hodgman algorithm to the contact
-  // patch.
-  this->getResult(contact, current_ptr, contact_patch);
-}
-
-// ============================================================================
-inline void ContactPatchSolver::getResult(
-    const Contact& contact, const ContactPatch::Polygon* result_ptr,
-    ContactPatch& contact_patch) const {
-  if (result_ptr->size() <= 1) {
-    contact_patch.addPoint(contact.pos);
-    return;
-  }
-
-  const ContactPatch::Polygon& result = *(result_ptr);
-  ContactPatch::Polygon& patch = contact_patch.points();
-  patch = result;
-}
-
-// ============================================================================
-template <typename ShapeType1, typename ShapeType2>
-inline void ContactPatchSolver::reset(const ShapeType1& shape1,
-                                      const Transform3f& tf1,
-                                      const ShapeType2& shape2,
-                                      const Transform3f& tf2,
-                                      const ContactPatch& contact_patch) const {
-  // Reset internal quantities
-  this->support_set_shape1.clear();
-  this->support_set_shape2.clear();
-  this->support_set_buffer.clear();
-
-  // Get the support function of each shape
-  const Transform3f& tfc = contact_patch.tf;
-
-  this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
-  // Set the reference frame of the support set of the first shape to be the
-  // local frame of shape 1.
-  Transform3f& tf1c = this->support_set_shape1.tf;
-  tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation();
-  tf1c.translation().noalias() =
-      tf1.rotation().transpose() * (tfc.translation() - tf1.translation());
-  this->supportFuncShape1 =
-      this->makeSupportSetFunction(&shape1, this->supports_data[0]);
-
-  this->support_set_shape2.direction = SupportSetDirection::INVERTED;
-  // Set the reference frame of the support set of the second shape to be the
-  // local frame of shape 2.
-  Transform3f& tf2c = this->support_set_shape2.tf;
-  tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation();
-  tf2c.translation().noalias() =
-      tf2.rotation().transpose() * (tfc.translation() - tf2.translation());
-  this->supportFuncShape2 =
-      this->makeSupportSetFunction(&shape2, this->supports_data[1]);
-}
-
-// ==========================================================================
-inline Vec2f ContactPatchSolver::computeLineSegmentIntersection(
-    const Vec2f& a, const Vec2f& b, const Vec2f& c, const Vec2f& d) {
-  const Vec2f ab = b - a;
-  const Vec2f n(-ab(1), ab(0));
-  const FCL_REAL denominator = n.dot(c - d);
-  if (std::abs(denominator) < std::numeric_limits<double>::epsilon()) {
-    return d;
-  }
-  const FCL_REAL nominator = n.dot(a - d);
-  FCL_REAL alpha = nominator / denominator;
-  alpha = std::min<double>(1.0, std::max<FCL_REAL>(0.0, alpha));
-  return alpha * c + (1 - alpha) * d;
-}
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // HPP_FCL_CONTACT_PATCH_SOLVER_HXX
+#include <hpp/fcl/coal.hpp>
+#include <coal/contact_patch/contact_patch_solver.hxx>
diff --git a/include/hpp/fcl/contact_patch_func_matrix.h b/include/hpp/fcl/contact_patch_func_matrix.h
index 7476650a434ffb917039463f7c819ef2e1ca5a3d..9103090df5bbf7625db6e7c7e0b76be5c90f7f6e 100644
--- a/include/hpp/fcl/contact_patch_func_matrix.h
+++ b/include/hpp/fcl/contact_patch_func_matrix.h
@@ -1,87 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2024, INRIA
- *  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 INRIA 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 Louis Montaut */
-
-#ifndef HPP_FCL_CONTACT_PATCH_FUNC_MATRIX_H
-#define HPP_FCL_CONTACT_PATCH_FUNC_MATRIX_H
-
-#include "hpp/fcl/collision_data.h"
-#include "hpp/fcl/contact_patch/contact_patch_solver.h"
-#include "hpp/fcl/narrowphase/narrowphase.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief The contact patch matrix stores the functions for contact patches
-/// computation between different types of objects and provides a uniform call
-/// interface
-struct HPP_FCL_DLLAPI ContactPatchFunctionMatrix {
-  /// @brief the uniform call interface for computing contact patches: we need
-  /// know
-  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
-  ///    and tf2;
-  /// 2. the collision result that generated contact patches candidates
-  ///    (`hpp::fcl::Contact`), from which contact patches will be expanded;
-  /// 3. the solver for computation of contact patches;
-  /// 4. the request setting for contact patches (e.g. maximum amount of
-  ///    patches, patch tolerance etc.)
-  /// 5. the structure to return contact patches
-  ///    (`hpp::fcl::ContactPatchResult`).
-  ///
-  /// Note: we pass a GJKSolver, because it allows to reuse internal computation
-  /// that was made during the narrow phase. It also allows to experiment with
-  /// different ways to compute contact patches. We could, for example, perturb
-  /// tf1 and tf2 and make multiple calls to the GJKSolver (although this is not
-  /// the approach done by default).
-  typedef void (*ContactPatchFunc)(const CollisionGeometry* o1,
-                                   const Transform3f& tf1,
-                                   const CollisionGeometry* o2,
-                                   const Transform3f& tf2,
-                                   const CollisionResult& collision_result,
-                                   const ContactPatchSolver* csolver,
-                                   const ContactPatchRequest& request,
-                                   ContactPatchResult& result);
-
-  /// @brief Each item in the contact patch matrix is a function to handle
-  /// contact patch computation between objects of type1 and type2.
-  ContactPatchFunc contact_patch_matrix[NODE_COUNT][NODE_COUNT];
-
-  ContactPatchFunctionMatrix();
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/contact_patch_func_matrix.h>
diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h
index bcbb3c9a34b3c5cf291ec7770489ae099bad7fc6..690f179c6fa46616209004a2cb521438d26acb12 100644
--- a/include/hpp/fcl/data_types.h
+++ b/include/hpp/fcl/data_types.h
@@ -1,189 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2023, Inria
- *  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 HPP_FCL_DATA_TYPES_H
-#define HPP_FCL_DATA_TYPES_H
-
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
-#include <hpp/fcl/config.hh>
-
-namespace hpp {
-
-#ifdef HPP_FCL_HAS_OCTOMAP
-#define OCTOMAP_VERSION_AT_LEAST(x, y, z) \
-  (OCTOMAP_MAJOR_VERSION > x ||           \
-   (OCTOMAP_MAJOR_VERSION >= x &&         \
-    (OCTOMAP_MINOR_VERSION > y ||         \
-     (OCTOMAP_MINOR_VERSION >= y && OCTOMAP_PATCH_VERSION >= z))))
-
-#define OCTOMAP_VERSION_AT_MOST(x, y, z) \
-  (OCTOMAP_MAJOR_VERSION < x ||          \
-   (OCTOMAP_MAJOR_VERSION <= x &&        \
-    (OCTOMAP_MINOR_VERSION < y ||        \
-     (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z))))
-#endif  // HPP_FCL_HAS_OCTOMAP
-}  // namespace hpp
-
-namespace hpp {
-namespace fcl {
-typedef double FCL_REAL;
-typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
-typedef Eigen::Matrix<FCL_REAL, 2, 1> Vec2f;
-typedef Eigen::Matrix<FCL_REAL, 6, 1> Vec6f;
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> VecXf;
-typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3, Eigen::RowMajor> Matrixx3f;
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 2, Eigen::RowMajor> Matrixx2f;
-typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor>
-    Matrixx3i;
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
-typedef Eigen::Vector2i support_func_guess_t;
-
-/// @brief Initial guess to use for the GJK algorithm
-/// DefaultGuess: Vec3f(1, 0, 0)
-/// CachedGuess: previous vector found by GJK or guess cached by the user
-/// BoundingVolumeGuess: guess using the centers of the shapes' AABB
-/// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called
-/// on the two shapes.
-enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess };
-
-/// @brief Variant to use for the GJK algorithm
-enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration };
-
-/// @brief Which convergence criterion is used to stop the algorithm (when the
-/// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and
-/// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe
-/// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG.
-enum GJKConvergenceCriterion { Default, DualityGap, Hybrid };
-
-/// @brief Wether the convergence criterion is scaled on the norm of the
-/// solution or not
-enum GJKConvergenceCriterionType { Relative, Absolute };
-
-/// @brief Triangle with 3 indices for points
-class HPP_FCL_DLLAPI Triangle {
- public:
-  typedef std::size_t index_type;
-  typedef int size_type;
-
-  /// @brief Default constructor
-  Triangle() {}
-
-  /// @brief Create a triangle with given vertex indices
-  Triangle(index_type p1, index_type p2, index_type p3) { set(p1, p2, p3); }
-
-  /// @brief Set the vertex indices of the triangle
-  inline void set(index_type p1, index_type p2, index_type p3) {
-    vids[0] = p1;
-    vids[1] = p2;
-    vids[2] = p3;
-  }
-
-  /// @brief Access the triangle index
-  inline index_type operator[](index_type i) const { return vids[i]; }
-
-  inline index_type& operator[](index_type i) { return vids[i]; }
-
-  static inline size_type size() { return 3; }
-
-  bool operator==(const Triangle& other) const {
-    return vids[0] == other.vids[0] && vids[1] == other.vids[1] &&
-           vids[2] == other.vids[2];
-  }
-
-  bool operator!=(const Triangle& other) const { return !(*this == other); }
-
-  bool isValid() const {
-    return vids[0] != (std::numeric_limits<index_type>::max)() &&
-           vids[1] != (std::numeric_limits<index_type>::max)() &&
-           vids[2] != (std::numeric_limits<index_type>::max)();
-  }
-
- private:
-  /// @brief indices for each vertex of triangle
-  index_type vids[3] = {(std::numeric_limits<index_type>::max)(),
-                        (std::numeric_limits<index_type>::max)(),
-                        (std::numeric_limits<index_type>::max)()};
-};
-
-/// @brief Quadrilateral with 4 indices for points
-struct HPP_FCL_DLLAPI Quadrilateral {
-  typedef std::size_t index_type;
-  typedef int size_type;
-
-  Quadrilateral() {}
-
-  Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3) {
-    set(p0, p1, p2, p3);
-  }
-
-  /// @brief Set the vertex indices of the quadrilateral
-  inline void set(index_type p0, index_type p1, index_type p2, index_type p3) {
-    vids[0] = p0;
-    vids[1] = p1;
-    vids[2] = p2;
-    vids[3] = p3;
-  }
-
-  /// @access the quadrilateral index
-  inline index_type operator[](index_type i) const { return vids[i]; }
-
-  inline index_type& operator[](index_type i) { return vids[i]; }
-
-  static inline size_type size() { return 4; }
-
-  bool operator==(const Quadrilateral& other) const {
-    return vids[0] == other.vids[0] && vids[1] == other.vids[1] &&
-           vids[2] == other.vids[2] && vids[3] == other.vids[3];
-  }
-
-  bool operator!=(const Quadrilateral& other) const {
-    return !(*this == other);
-  }
-
- private:
-  index_type vids[4];
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/data_types.h>
diff --git a/include/hpp/fcl/deprecated.hh b/include/hpp/fcl/deprecated.hh
new file mode 100644
index 0000000000000000000000000000000000000000..513abdf65a3cd05c42fb7b5689f9951efaa04ca4
--- /dev/null
+++ b/include/hpp/fcl/deprecated.hh
@@ -0,0 +1,2 @@
+#include <hpp/fcl/coal.hpp>
+#include <coal/deprecated.hh>
diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h
index c0c4564943cde3e1467391ffe67df09ea2cb1192..a8301218b689fefe93112d88dfcfcc19f1f56d55 100644
--- a/include/hpp/fcl/distance.h
+++ b/include/hpp/fcl/distance.h
@@ -1,117 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_DISTANCE_H
-#define HPP_FCL_DISTANCE_H
-
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/distance_func_matrix.h>
-#include <hpp/fcl/timings.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Main distance interface: given two collision objects, and the
-/// requirements for contacts, including whether return the nearest points, this
-/// function performs the distance between them. Return value is the minimum
-/// distance generated between the two objects.
-HPP_FCL_DLLAPI FCL_REAL distance(const CollisionObject* o1,
-                                 const CollisionObject* o2,
-                                 const DistanceRequest& request,
-                                 DistanceResult& result);
-
-/// @copydoc distance(const CollisionObject*, const CollisionObject*, const
-/// DistanceRequest&, DistanceResult&)
-HPP_FCL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1,
-                                 const Transform3f& tf1,
-                                 const CollisionGeometry* o2,
-                                 const Transform3f& tf2,
-                                 const DistanceRequest& request,
-                                 DistanceResult& result);
-
-/// This class reduces the cost of identifying the geometry pair.
-/// This is mostly useful for repeated shape-shape queries.
-///
-/// \code
-///   ComputeDistance calc_distance (o1, o2);
-///   FCL_REAL distance = calc_distance(tf1, tf2, request, result);
-/// \endcode
-class HPP_FCL_DLLAPI ComputeDistance {
- public:
-  ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2);
-
-  FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2,
-                      const DistanceRequest& request,
-                      DistanceResult& result) const;
-
-  bool operator==(const ComputeDistance& other) const {
-    return o1 == other.o1 && o2 == other.o2 && swap_geoms == other.swap_geoms &&
-           solver == other.solver && func == other.func;
-  }
-
-  bool operator!=(const ComputeDistance& other) const {
-    return !(*this == other);
-  }
-
-  virtual ~ComputeDistance() {};
-
- protected:
-  // These pointers are made mutable to let the derived classes to update
-  // their values when updating the collision geometry (e.g. creating a new
-  // one). This feature should be used carefully to avoid any mis usage (e.g,
-  // changing the type of the collision geometry should be avoided).
-  mutable const CollisionGeometry* o1;
-  mutable const CollisionGeometry* o2;
-
-  mutable GJKSolver solver;
-
-  DistanceFunctionMatrix::DistanceFunc func;
-  bool swap_geoms;
-
-  virtual FCL_REAL run(const Transform3f& tf1, const Transform3f& tf2,
-                       const DistanceRequest& request,
-                       DistanceResult& result) const;
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/distance.h>
diff --git a/include/hpp/fcl/distance_func_matrix.h b/include/hpp/fcl/distance_func_matrix.h
index cf51cb9067da7ba12cd24c78d0b515c96af2ad0a..5bc9be51c55984fbf7869ad73271c718209ef740 100644
--- a/include/hpp/fcl/distance_func_matrix.h
+++ b/include/hpp/fcl/distance_func_matrix.h
@@ -1,77 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_DISTANCE_FUNC_MATRIX_H
-#define HPP_FCL_DISTANCE_FUNC_MATRIX_H
-
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief distance matrix stores the functions for distance between different
-/// types of objects and provides a uniform call interface
-struct HPP_FCL_DLLAPI DistanceFunctionMatrix {
-  /// @brief the uniform call interface for distance: for distance, we need know
-  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
-  /// and tf2;
-  /// 2. the solver for narrow phase collision, this is for distance computation
-  /// between geometric shapes;
-  /// 3. the request setting for distance (e.g., whether need to return nearest
-  /// points);
-  typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1,
-                                   const Transform3f& tf1,
-                                   const CollisionGeometry* o2,
-                                   const Transform3f& tf2,
-                                   const GJKSolver* nsolver,
-                                   const DistanceRequest& request,
-                                   DistanceResult& result);
-
-  /// @brief each item in the distance matrix is a function to handle distance
-  /// between objects of type1 and type2
-  DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
-
-  DistanceFunctionMatrix();
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/distance_func_matrix.h>
diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh
index cb756a84ea8b658c5e16a3f97f8cf9fe78aa2e8e..1bf07ff6c8d2ac489944e63ed4d502a937ba113d 100644
--- a/include/hpp/fcl/fwd.hh
+++ b/include/hpp/fcl/fwd.hh
@@ -1,151 +1,2 @@
-//
-// Software License Agreement (BSD License)
-//
-//  Copyright (c) 2014, CNRS-LAAS
-//  Copyright (c) 2022-2024, Inria
-//  Author: Florent Lamiraux
-//
-//  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 CNRS-LAAS 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.
-//
-
-#ifndef HPP_FCL_FWD_HH
-#define HPP_FCL_FWD_HH
-
-#include <cassert>
-#include <memory>
-#include <sstream>
-#include <stdexcept>
-
-#include <hpp/fcl/config.hh>
-#include <hpp/fcl/deprecated.hh>
-#include <hpp/fcl/warning.hh>
-
-#if _WIN32
-#define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__
-#else
-#define HPP_FCL_PRETTY_FUNCTION __PRETTY_FUNCTION__
-#endif
-
-#define HPP_FCL_UNUSED_VARIABLE(var) (void)(var)
-
-#ifdef NDEBUG
-#define HPP_FCL_ONLY_USED_FOR_DEBUG(var) HPP_FCL_UNUSED_VARIABLE(var)
-#else
-#define HPP_FCL_ONLY_USED_FOR_DEBUG(var)
-#endif
-
-#define HPP_FCL_THROW_PRETTY(message, exception)              \
-  {                                                           \
-    std::stringstream ss;                                     \
-    ss << "From file: " << __FILE__ << "\n";                  \
-    ss << "in function: " << HPP_FCL_PRETTY_FUNCTION << "\n"; \
-    ss << "at line: " << __LINE__ << "\n";                    \
-    ss << "message: " << message << "\n";                     \
-    throw exception(ss.str());                                \
-  }
-
-#ifdef HPP_FCL_TURN_ASSERT_INTO_EXCEPTION
-#define HPP_FCL_ASSERT(check, message, exception) \
-  do {                                            \
-    if (!(check)) {                               \
-      HPP_FCL_THROW_PRETTY(message, exception);   \
-    }                                             \
-  } while (0)
-#else
-#define HPP_FCL_ASSERT(check, message, exception) \
-  {                                               \
-    HPP_FCL_UNUSED_VARIABLE(exception(message));  \
-    assert((check) && message);                   \
-  }
-#endif
-
-#if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
-#define HPP_FCL_WITH_CXX11_SUPPORT
-#endif
-
-#if defined(__GNUC__) || defined(__clang__)
-#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push")
-#define HPP_FCL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop")
-#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
-  _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"")
-
-// GCC version 4.6 and higher supports -Wmaybe-uninitialized
-#if (defined(__GNUC__) && \
-     ((__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)))
-#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
-  _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"")
-// Use __has_warning with clang. Clang introduced it in 2024 (3.5+)
-#elif (defined(__clang__) && defined(__has_warning) && \
-       __has_warning("-Wmaybe-uninitialized"))
-#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
-  _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"")
-#else
-#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
-#endif
-#elif defined(WIN32)
-#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)")
-#define HPP_FCL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)")
-#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
-  _Pragma("warning(disable : 4996)")
-#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
-  _Pragma("warning(disable : 4700)")
-#else
-#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-#define HPP_FCL_COMPILER_DIAGNOSTIC_POP
-#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
-#endif  // __GNUC__
-
-namespace hpp {
-namespace fcl {
-using std::dynamic_pointer_cast;
-using std::make_shared;
-using std::shared_ptr;
-
-class CollisionObject;
-typedef shared_ptr<CollisionObject> CollisionObjectPtr_t;
-typedef shared_ptr<const CollisionObject> CollisionObjectConstPtr_t;
-class CollisionGeometry;
-typedef shared_ptr<CollisionGeometry> CollisionGeometryPtr_t;
-typedef shared_ptr<const CollisionGeometry> CollisionGeometryConstPtr_t;
-class Transform3f;
-
-class AABB;
-
-class BVHModelBase;
-typedef shared_ptr<BVHModelBase> BVHModelPtr_t;
-
-class OcTree;
-typedef shared_ptr<OcTree> OcTreePtr_t;
-typedef shared_ptr<const OcTree> OcTreeConstPtr_t;
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // HPP_FCL_FWD_HH
+#include <hpp/fcl/coal.hpp>
+#include <coal/fwd.hh>
diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h
index 18ab9e5a17e45c42e38efb23d0b290cfd58e90ea..4d462776aeb4f08ed76ff744d76139c88b9af858 100644
--- a/include/hpp/fcl/hfield.h
+++ b/include/hpp/fcl/hfield.h
@@ -1,545 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2021-2024, INRIA
- *  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 Justin Carpentier */
-
-#ifndef HPP_FCL_HEIGHT_FIELD_H
-#define HPP_FCL_HEIGHT_FIELD_H
-
-#include "hpp/fcl/fwd.hh"
-#include "hpp/fcl/data_types.h"
-#include "hpp/fcl/collision_object.h"
-#include "hpp/fcl/BV/BV_node.h"
-#include "hpp/fcl/BVH/BVH_internal.h"
-
-#include <vector>
-
-namespace hpp {
-namespace fcl {
-
-/// @addtogroup Construction_Of_HeightField
-/// @{
-
-struct HPP_FCL_DLLAPI HFNodeBase {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  enum class FaceOrientation {
-    TOP = 1,
-    BOTTOM = 1,
-    NORTH = 2,
-    EAST = 4,
-    SOUTH = 8,
-    WEST = 16
-  };
-
-  /// @brief An index for first child node or primitive
-  /// If the value is positive, it is the index of the first child bv node
-  /// If the value is negative, it is -(primitive index + 1)
-  /// Zero is not used.
-  size_t first_child;
-
-  Eigen::DenseIndex x_id, x_size;
-  Eigen::DenseIndex y_id, y_size;
-
-  FCL_REAL max_height;
-  int contact_active_faces;
-
-  /// @brief Default constructor
-  HFNodeBase()
-      : first_child(0),
-        x_id(-1),
-        x_size(0),
-        y_id(-1),
-        y_size(0),
-        max_height(std::numeric_limits<FCL_REAL>::lowest()),
-        contact_active_faces(0) {}
-
-  /// @brief Comparison operator
-  bool operator==(const HFNodeBase& other) const {
-    return first_child == other.first_child && x_id == other.x_id &&
-           x_size == other.x_size && y_id == other.y_id &&
-           y_size == other.y_size && max_height == other.max_height &&
-           contact_active_faces == other.contact_active_faces;
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const HFNodeBase& other) const { return !(*this == other); }
-
-  /// @brief Whether current node is a leaf node (i.e. contains a primitive
-  /// index)
-  inline bool isLeaf() const { return x_size == 1 && y_size == 1; }
-
-  /// @brief Return the index of the first child. The index is referred to the
-  /// bounding volume array (i.e. bvs) in BVHModel
-  inline size_t leftChild() const { return first_child; }
-
-  /// @brief Return the index of the second child. The index is referred to the
-  /// bounding volume array (i.e. bvs) in BVHModel
-  inline size_t rightChild() const { return first_child + 1; }
-
-  inline Eigen::Vector2i leftChildIndexes() const {
-    return Eigen::Vector2i(x_id, y_id);
-  }
-  inline Eigen::Vector2i rightChildIndexes() const {
-    return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2);
-  }
-};
-
-inline HFNodeBase::FaceOrientation operator&(HFNodeBase::FaceOrientation a,
-                                             HFNodeBase::FaceOrientation b) {
-  return HFNodeBase::FaceOrientation(int(a) & int(b));
-}
-
-inline int operator&(int a, HFNodeBase::FaceOrientation b) {
-  return a & int(b);
-}
-
-template <typename BV>
-struct HPP_FCL_DLLAPI HFNode : public HFNodeBase {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  typedef HFNodeBase Base;
-
-  /// @brief bounding volume storing the geometry
-  BV bv;
-
-  /// @brief Equality operator
-  bool operator==(const HFNode& other) const {
-    return Base::operator==(other) && bv == other.bv;
-  }
-
-  /// @brief Difference operator
-  bool operator!=(const HFNode& other) const { return !(*this == other); }
-
-  /// @brief Check whether two BVNode collide
-  bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }
-  /// @brief Check whether two BVNode collide
-  bool overlap(const HFNode& other, const CollisionRequest& request,
-               FCL_REAL& sqrDistLowerBound) const {
-    return bv.overlap(other.bv, request, sqrDistLowerBound);
-  }
-
-  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
-  /// the underlying BV supports distance, return the nearest points.
-  FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL,
-                    Vec3f* P2 = NULL) const {
-    return bv.distance(other.bv, P1, P2);
-  }
-
-  /// @brief Access to the center of the BV
-  Vec3f getCenter() const { return bv.center(); }
-
-  /// @brief Access to the orientation of the BV
-  hpp::fcl::Matrix3f::IdentityReturnType getOrientation() const {
-    return Matrix3f::Identity();
-  }
-
-  virtual ~HFNode() {}
-};
-
-namespace details {
-
-template <typename BV>
-struct UpdateBoundingVolume {
-  static void run(const Vec3f& pointA, const Vec3f& pointB, BV& bv) {
-    AABB bv_aabb(pointA, pointB);
-    //      AABB bv_aabb;
-    //      bv_aabb.update(pointA,pointB);
-    convertBV(bv_aabb, bv);
-  }
-};
-
-template <>
-struct UpdateBoundingVolume<AABB> {
-  static void run(const Vec3f& pointA, const Vec3f& pointB, AABB& bv) {
-    AABB bv_aabb(pointA, pointB);
-    convertBV(bv_aabb, bv);
-    //      bv.update(pointA,pointB);
-  }
-};
-
-}  // namespace details
-
-/// @brief Data structure depicting a height field given by the base grid
-/// dimensions and the elevation along the grid. \tparam BV one of the bounding
-/// volume class in \ref Bounding_Volume.
-///
-/// An height field is defined by its base dimensions along the X and Y axes and
-/// a set ofpoints defined by their altitude, regularly dispatched on the grid.
-/// The height field is centered at the origin and the corners of the geometry
-/// correspond to the following coordinates [± x_dim/2; ± y_dim/2].
-template <typename BV>
-class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  typedef CollisionGeometry Base;
-
-  typedef HFNode<BV> Node;
-  typedef std::vector<Node, Eigen::aligned_allocator<Node>> BVS;
-
-  /// @brief Constructing an empty HeightField
-  HeightField()
-      : CollisionGeometry(),
-        min_height((std::numeric_limits<FCL_REAL>::min)()),
-        max_height((std::numeric_limits<FCL_REAL>::max)()) {}
-
-  /// @brief Constructing an HeightField from its base dimensions and the set of
-  /// heights points.
-  ///        The granularity of the height field along X and Y direction is
-  ///        extraded from the Z grid.
-  ///
-  /// \param[in] x_dim Dimension along the X axis
-  /// \param[in] y_dim Dimension along the Y axis
-  /// \param[in] heights Matrix containing the altitude of each point compositng
-  /// the height field
-  /// \param[in] min_height Minimal height of the height field
-  ///
-  HeightField(const FCL_REAL x_dim, const FCL_REAL y_dim,
-              const MatrixXf& heights, const FCL_REAL min_height = (FCL_REAL)0)
-      : CollisionGeometry() {
-    init(x_dim, y_dim, heights, min_height);
-  }
-
-  /// @brief Copy contructor from another HeightField
-  ///
-  /// \param[in] other to copy.
-  ///
-  HeightField(const HeightField& other)
-      : CollisionGeometry(other),
-        x_dim(other.x_dim),
-        y_dim(other.y_dim),
-        heights(other.heights),
-        min_height(other.min_height),
-        max_height(other.max_height),
-        x_grid(other.x_grid),
-        y_grid(other.y_grid),
-        bvs(other.bvs),
-        num_bvs(other.num_bvs) {}
-
-  /// @brief Returns a const reference of the grid along the X direction.
-  const VecXf& getXGrid() const { return x_grid; }
-  /// @brief Returns a const reference of the grid along the Y direction.
-  const VecXf& getYGrid() const { return y_grid; }
-
-  /// @brief Returns a const reference of the heights
-  const MatrixXf& getHeights() const { return heights; }
-
-  /// @brief Returns the dimension of the Height Field along the X direction.
-  FCL_REAL getXDim() const { return x_dim; }
-  /// @brief Returns the dimension of the Height Field along the Y direction.
-  FCL_REAL getYDim() const { return y_dim; }
-
-  /// @brief Returns the minimal height value of the Height Field.
-  FCL_REAL getMinHeight() const { return min_height; }
-  /// @brief Returns the maximal height value of the Height Field.
-  FCL_REAL getMaxHeight() const { return max_height; }
-
-  virtual HeightField<BV>* clone() const { return new HeightField(*this); }
-
-  const BVS& getNodes() const { return bvs; }
-
-  /// @brief deconstruction, delete mesh data related.
-  virtual ~HeightField() {}
-
-  /// @brief Compute the AABB for the HeightField, used for broad-phase
-  /// collision
-  void computeLocalAABB() {
-    const Vec3f A(x_grid[0], y_grid[0], min_height);
-    const Vec3f B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
-                  max_height);
-    const AABB aabb_(A, B);
-
-    aabb_radius = (A - B).norm() / 2.;
-    aabb_local = aabb_;
-    aabb_center = aabb_.center();
-  }
-
-  /// @brief Update Height Field height
-  void updateHeights(const MatrixXf& new_heights) {
-    if (new_heights.rows() != heights.rows() ||
-        new_heights.cols() != heights.cols())
-      HPP_FCL_THROW_PRETTY(
-          "The matrix containing the new heights values does not have the same "
-          "matrix size as the original one.\n"
-          "\tinput values - rows: "
-              << new_heights.rows() << " - cols: " << new_heights.cols() << "\n"
-              << "\texpected values - rows: " << heights.rows()
-              << " - cols: " << heights.cols() << "\n",
-          std::invalid_argument);
-
-    heights = new_heights.cwiseMax(min_height);
-    this->max_height = recursiveUpdateHeight(0);
-    assert(this->max_height == heights.maxCoeff());
-  }
-
- protected:
-  void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf& heights,
-            const FCL_REAL min_height) {
-    this->x_dim = x_dim;
-    this->y_dim = y_dim;
-    this->heights = heights.cwiseMax(min_height);
-    this->min_height = min_height;
-    this->max_height = heights.maxCoeff();
-
-    const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows();
-    assert(NX >= 2 && "The number of columns is too small.");
-    assert(NY >= 2 && "The number of rows is too small.");
-
-    x_grid = VecXf::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim);
-    y_grid = VecXf::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim);
-
-    // Allocate BVS
-    const size_t num_tot_bvs =
-        (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1));
-    bvs.resize(num_tot_bvs);
-    num_bvs = 0;
-
-    // Build tree
-    buildTree();
-  }
-
-  /// @brief Get the object type: it is a HFIELD
-  OBJECT_TYPE getObjectType() const { return OT_HFIELD; }
-
-  Vec3f computeCOM() const { return Vec3f::Zero(); }
-
-  FCL_REAL computeVolume() const { return 0; }
-
-  Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); }
-
- protected:
-  /// @brief Dimensions in meters along X and Y directions
-  FCL_REAL x_dim, y_dim;
-
-  /// @brief Elevation values in meters of the Height Field
-  MatrixXf heights;
-
-  /// @brief Minimal height of the Height Field: all values bellow min_height
-  /// will be discarded.
-  FCL_REAL min_height, max_height;
-
-  /// @brief Grids along the X and Y directions. Useful for plotting or other
-  /// related things.
-  VecXf x_grid, y_grid;
-
-  /// @brief Bounding volume hierarchy
-  BVS bvs;
-  unsigned int num_bvs;
-
-  /// @brief Build the bounding volume hierarchy
-  int buildTree() {
-    num_bvs = 1;
-    const FCL_REAL max_recursive_height =
-        recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1);
-    assert(max_recursive_height == max_height &&
-           "the maximal height is not correct");
-    HPP_FCL_UNUSED_VARIABLE(max_recursive_height);
-
-    bvs.resize(num_bvs);
-    return BVH_OK;
-  }
-
-  FCL_REAL recursiveUpdateHeight(const size_t bv_id) {
-    HFNode<BV>& bv_node = bvs[bv_id];
-
-    FCL_REAL max_height;
-    if (bv_node.isLeaf()) {
-      max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff();
-    } else {
-      FCL_REAL
-      max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
-      max_right_height = recursiveUpdateHeight(bv_node.rightChild());
-
-      max_height = (std::max)(max_left_height, max_right_height);
-    }
-
-    bv_node.max_height = max_height;
-
-    const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
-    const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size],
-                       y_grid[bv_node.y_id + bv_node.y_size], max_height);
-
-    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
-
-    return max_height;
-  }
-
-  FCL_REAL recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id,
-                              const Eigen::DenseIndex x_size,
-                              const Eigen::DenseIndex y_id,
-                              const Eigen::DenseIndex y_size) {
-    assert(x_id < heights.cols() && "x_id is out of bounds");
-    assert(y_id < heights.rows() && "y_id is out of bounds");
-    assert(x_size >= 0 && y_size >= 0 &&
-           "x_size or y_size are not of correct value");
-    assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
-
-    HFNode<BV>& bv_node = bvs[bv_id];
-    FCL_REAL max_height;
-    if (x_size == 1 &&
-        y_size == 1)  // don't build any BV for the current child node
-    {
-      max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
-    } else {
-      bv_node.first_child = num_bvs;
-      num_bvs += 2;
-
-      FCL_REAL max_left_height = min_height, max_right_height = min_height;
-      if (x_size >= y_size)  // splitting along the X axis
-      {
-        Eigen::DenseIndex x_size_half = x_size / 2;
-        if (x_size == 1) x_size_half = 1;
-        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
-                                             x_size_half, y_id, y_size);
-
-        max_right_height =
-            recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,
-                               x_size - x_size_half, y_id, y_size);
-      } else  // splitting along the Y axis
-      {
-        Eigen::DenseIndex y_size_half = y_size / 2;
-        if (y_size == 1) y_size_half = 1;
-        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
-                                             y_id, y_size_half);
-
-        max_right_height =
-            recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
-                               y_id + y_size_half, y_size - y_size_half);
-      }
-
-      max_height = (std::max)(max_left_height, max_right_height);
-    }
-
-    bv_node.max_height = max_height;
-    //    max_height = std::max(max_height,min_height);
-
-    const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height);
-    assert(x_id + x_size < x_grid.size());
-    assert(y_id + y_size < y_grid.size());
-    const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
-                       max_height);
-
-    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
-    bv_node.x_id = x_id;
-    bv_node.y_id = y_id;
-    bv_node.x_size = x_size;
-    bv_node.y_size = y_size;
-
-    if (bv_node.isLeaf()) {
-      int& contact_active_faces = bv_node.contact_active_faces;
-      contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP);
-      contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM);
-
-      if (bv_node.x_id == 0)  // first col
-        contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST);
-
-      if (bv_node.y_id == 0)  // first row (TOP)
-        contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH);
-
-      if (bv_node.x_id + 1 == heights.cols() - 1)  // last col
-        contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST);
-
-      if (bv_node.y_id + 1 == heights.rows() - 1)  // last row (BOTTOM)
-        contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH);
-    }
-
-    return max_height;
-  }
-
- public:
-  /// @brief Access the bv giving the its index
-  const HFNode<BV>& getBV(unsigned int i) const {
-    if (i >= num_bvs)
-      HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
-    return bvs[i];
-  }
-
-  /// @brief Access the bv giving the its index
-  HFNode<BV>& getBV(unsigned int i) {
-    if (i >= num_bvs)
-      HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
-    return bvs[i];
-  }
-
-  /// @brief Get the BV type: default is unknown
-  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const HeightField& other = *other_ptr;
-
-    return x_dim == other.x_dim && y_dim == other.y_dim &&
-           heights == other.heights && min_height == other.min_height &&
-           max_height == other.max_height && x_grid == other.x_grid &&
-           y_grid == other.y_grid && bvs == other.bvs &&
-           num_bvs == other.num_bvs;
-  }
-};
-
-/// @brief Specialization of getNodeType() for HeightField with different BV
-/// types
-template <>
-NODE_TYPE HeightField<AABB>::getNodeType() const;
-
-template <>
-NODE_TYPE HeightField<OBB>::getNodeType() const;
-
-template <>
-NODE_TYPE HeightField<RSS>::getNodeType() const;
-
-template <>
-NODE_TYPE HeightField<kIOS>::getNodeType() const;
-
-template <>
-NODE_TYPE HeightField<OBBRSS>::getNodeType() const;
-
-template <>
-NODE_TYPE HeightField<KDOP<16>>::getNodeType() const;
-
-template <>
-NODE_TYPE HeightField<KDOP<18>>::getNodeType() const;
-
-template <>
-NODE_TYPE HeightField<KDOP<24>>::getNodeType() const;
-
-/// @}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/hfield.h>
diff --git a/include/hpp/fcl/internal/BV_fitter.h b/include/hpp/fcl/internal/BV_fitter.h
index 514741f0bbc7655de3fb355b7eb2772dcf38b167..438bd599ed53855810cc93042cdbfc3109e8a995 100644
--- a/include/hpp/fcl/internal/BV_fitter.h
+++ b/include/hpp/fcl/internal/BV_fitter.h
@@ -1,223 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_BV_FITTER_H
-#define HPP_FCL_BV_FITTER_H
-
-#include <hpp/fcl/BVH/BVH_internal.h>
-#include <hpp/fcl/BV/kIOS.h>
-#include <hpp/fcl/BV/OBBRSS.h>
-#include <hpp/fcl/BV/AABB.h>
-#include <iostream>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Compute a bounding volume that fits a set of n points.
-template <typename BV>
-void fit(Vec3f* ps, unsigned int n, BV& bv) {
-  for (unsigned int i = 0; i < n; ++i)  // TODO(jcarpent): vectorize
-  {
-    bv += ps[i];
-  }
-}
-
-template <>
-void fit<OBB>(Vec3f* ps, unsigned int n, OBB& bv);
-
-template <>
-void fit<RSS>(Vec3f* ps, unsigned int n, RSS& bv);
-
-template <>
-void fit<kIOS>(Vec3f* ps, unsigned int n, kIOS& bv);
-
-template <>
-void fit<OBBRSS>(Vec3f* ps, unsigned int n, OBBRSS& bv);
-
-template <>
-void fit<AABB>(Vec3f* ps, unsigned int n, AABB& bv);
-
-/// @brief The class for the default algorithm fitting a bounding volume to a
-/// set of points
-template <typename BV>
-class HPP_FCL_DLLAPI BVFitterTpl {
- public:
-  /// @brief default deconstructor
-  virtual ~BVFitterTpl() {}
-
-  /// @brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) {
-    vertices = vertices_;
-    prev_vertices = NULL;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Prepare the geometry primitive data for fitting, for deformable
-  /// mesh
-  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_,
-           BVHModelType type_) {
-    vertices = vertices_;
-    prev_vertices = prev_vertices_;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Compute the fitting BV
-  virtual BV fit(unsigned int* primitive_indices,
-                 unsigned int num_primitives) = 0;
-
-  /// @brief Clear the geometry primitive data
-  void clear() {
-    vertices = NULL;
-    prev_vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
- protected:
-  Vec3f* vertices;
-  Vec3f* prev_vertices;
-  Triangle* tri_indices;
-  BVHModelType type;
-};
-
-/// @brief The class for the default algorithm fitting a bounding volume to a
-/// set of points
-template <typename BV>
-class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl<BV> {
-  typedef BVFitterTpl<BV> Base;
-
- public:
-  /// @brief Compute a bounding volume that fits a set of primitives (points or
-  /// triangles). The primitive data was set by set function and
-  /// primitive_indices is the primitive index relative to the data
-  BV fit(unsigned int* primitive_indices, unsigned int num_primitives) {
-    BV bv;
-
-    if (type == BVH_MODEL_TRIANGLES)  /// The primitive is triangle
-    {
-      for (unsigned int i = 0; i < num_primitives; ++i) {
-        Triangle t = tri_indices[primitive_indices[i]];
-        bv += vertices[t[0]];
-        bv += vertices[t[1]];
-        bv += vertices[t[2]];
-
-        if (prev_vertices)  /// can fitting both current and previous frame
-        {
-          bv += prev_vertices[t[0]];
-          bv += prev_vertices[t[1]];
-          bv += prev_vertices[t[2]];
-        }
-      }
-    } else if (type == BVH_MODEL_POINTCLOUD)  /// The primitive is point
-    {
-      for (unsigned int i = 0; i < num_primitives; ++i) {
-        bv += vertices[primitive_indices[i]];
-
-        if (prev_vertices)  /// can fitting both current and previous frame
-        {
-          bv += prev_vertices[primitive_indices[i]];
-        }
-      }
-    }
-
-    return bv;
-  }
-
- protected:
-  using Base::prev_vertices;
-  using Base::tri_indices;
-  using Base::type;
-  using Base::vertices;
-};
-
-/// @brief Specification of BVFitter for OBB bounding volume
-template <>
-class HPP_FCL_DLLAPI BVFitter<OBB> : public BVFitterTpl<OBB> {
- public:
-  /// @brief Compute a bounding volume that fits a set of primitives (points or
-  /// triangles). The primitive data was set by set function and
-  /// primitive_indices is the primitive index relative to the data.
-  OBB fit(unsigned int* primitive_indices, unsigned int num_primitives);
-};
-
-/// @brief Specification of BVFitter for RSS bounding volume
-template <>
-class HPP_FCL_DLLAPI BVFitter<RSS> : public BVFitterTpl<RSS> {
- public:
-  /// @brief Compute a bounding volume that fits a set of primitives (points or
-  /// triangles). The primitive data was set by set function and
-  /// primitive_indices is the primitive index relative to the data.
-  RSS fit(unsigned int* primitive_indices, unsigned int num_primitives);
-};
-
-/// @brief Specification of BVFitter for kIOS bounding volume
-template <>
-class HPP_FCL_DLLAPI BVFitter<kIOS> : public BVFitterTpl<kIOS> {
- public:
-  /// @brief Compute a bounding volume that fits a set of primitives (points or
-  /// triangles). The primitive data was set by set function and
-  /// primitive_indices is the primitive index relative to the data.
-  kIOS fit(unsigned int* primitive_indices, unsigned int num_primitives);
-};
-
-/// @brief Specification of BVFitter for OBBRSS bounding volume
-template <>
-class HPP_FCL_DLLAPI BVFitter<OBBRSS> : public BVFitterTpl<OBBRSS> {
- public:
-  /// @brief Compute a bounding volume that fits a set of primitives (points or
-  /// triangles). The primitive data was set by set function and
-  /// primitive_indices is the primitive index relative to the data.
-  OBBRSS fit(unsigned int* primitive_indices, unsigned int num_primitives);
-};
-
-/// @brief Specification of BVFitter for AABB bounding volume
-template <>
-class HPP_FCL_DLLAPI BVFitter<AABB> : public BVFitterTpl<AABB> {
- public:
-  /// @brief Compute a bounding volume that fits a set of primitives (points or
-  /// triangles). The primitive data was set by set function and
-  /// primitive_indices is the primitive index relative to the data.
-  AABB fit(unsigned int* primitive_indices, unsigned int num_primitives);
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/BV_fitter.h>
diff --git a/include/hpp/fcl/internal/BV_splitter.h b/include/hpp/fcl/internal/BV_splitter.h
index 24c5c6077b39ad287ee43bb1d45d97fb2b9e83da..85a586bdb76828b5d9d3cbb1ed7c50b265906a4e 100644
--- a/include/hpp/fcl/internal/BV_splitter.h
+++ b/include/hpp/fcl/internal/BV_splitter.h
@@ -1,286 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_BV_SPLITTER_H
-#define HPP_FCL_BV_SPLITTER_H
-
-#include <hpp/fcl/BVH/BVH_internal.h>
-#include <hpp/fcl/BV/kIOS.h>
-#include <hpp/fcl/BV/OBBRSS.h>
-#include <vector>
-#include <iostream>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Three types of split algorithms are provided in FCL as default
-enum SplitMethodType {
-  SPLIT_METHOD_MEAN,
-  SPLIT_METHOD_MEDIAN,
-  SPLIT_METHOD_BV_CENTER
-};
-
-/// @brief A class describing the split rule that splits each BV node
-template <typename BV>
-class BVSplitter {
- public:
-  BVSplitter(SplitMethodType method)
-      : split_vector(0, 0, 0), split_method(method) {}
-
-  /// @brief Default deconstructor
-  virtual ~BVSplitter() {}
-
-  /// @brief Set the geometry data needed by the split rule
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) {
-    vertices = vertices_;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Compute the split rule according to a subset of geometry and the
-  /// corresponding BV node
-  void computeRule(const BV& bv, unsigned int* primitive_indices,
-                   unsigned int num_primitives) {
-    switch (split_method) {
-      case SPLIT_METHOD_MEAN:
-        computeRule_mean(bv, primitive_indices, num_primitives);
-        break;
-      case SPLIT_METHOD_MEDIAN:
-        computeRule_median(bv, primitive_indices, num_primitives);
-        break;
-      case SPLIT_METHOD_BV_CENTER:
-        computeRule_bvcenter(bv, primitive_indices, num_primitives);
-        break;
-      default:
-        std::cerr << "Split method not supported" << std::endl;
-    }
-  }
-
-  /// @brief Apply the split rule on a given point
-  bool apply(const Vec3f& q) const { return q[split_axis] > split_value; }
-
-  /// @brief Clear the geometry data set before
-  void clear() {
-    vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
- protected:
-  /// @brief The axis based on which the split decision is made. For most BV,
-  /// the axis is aligned with one of the world coordinate, so only split_axis
-  /// is needed. For oriented node, we can use a vector to make a better split
-  /// decision.
-  int split_axis;
-  Vec3f split_vector;
-
-  /// @brief The split threshold, different primitives are splitted according
-  /// whether their projection on the split_axis is larger or smaller than the
-  /// threshold
-  FCL_REAL split_value;
-
-  /// @brief The mesh vertices or points handled by the splitter
-  Vec3f* vertices;
-
-  /// @brief The triangles handled by the splitter
-  Triangle* tri_indices;
-
-  /// @brief Whether the geometry is mesh or point cloud
-  BVHModelType type;
-
-  /// @brief The split algorithm used
-  SplitMethodType split_method;
-
-  /// @brief Split algorithm 1: Split the node from center
-  void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) {
-    Vec3f center = bv.center();
-    int axis = 2;
-
-    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
-      axis = 0;
-    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
-      axis = 1;
-
-    split_axis = axis;
-    split_value = center[axis];
-  }
-
-  /// @brief Split algorithm 2: Split the node according to the mean of the data
-  /// contained
-  void computeRule_mean(const BV& bv, unsigned int* primitive_indices,
-                        unsigned int num_primitives) {
-    int axis = 2;
-
-    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
-      axis = 0;
-    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
-      axis = 1;
-
-    split_axis = axis;
-    FCL_REAL sum = 0;
-
-    if (type == BVH_MODEL_TRIANGLES) {
-      for (unsigned int i = 0; i < num_primitives; ++i) {
-        const Triangle& t = tri_indices[primitive_indices[i]];
-        sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] +
-                vertices[t[2]][split_axis]);
-      }
-
-      sum /= 3;
-    } else if (type == BVH_MODEL_POINTCLOUD) {
-      for (unsigned int i = 0; i < num_primitives; ++i) {
-        sum += vertices[primitive_indices[i]][split_axis];
-      }
-    }
-
-    split_value = sum / num_primitives;
-  }
-
-  /// @brief Split algorithm 3: Split the node according to the median of the
-  /// data contained
-  void computeRule_median(const BV& bv, unsigned int* primitive_indices,
-                          unsigned int num_primitives) {
-    int axis = 2;
-
-    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
-      axis = 0;
-    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
-      axis = 1;
-
-    split_axis = axis;
-    std::vector<FCL_REAL> proj((size_t)num_primitives);
-
-    if (type == BVH_MODEL_TRIANGLES) {
-      for (unsigned int i = 0; i < num_primitives; ++i) {
-        const Triangle& t = tri_indices[primitive_indices[i]];
-        proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] +
-                   vertices[t[2]][split_axis]) /
-                  3;
-      }
-    } else if (type == BVH_MODEL_POINTCLOUD) {
-      for (unsigned int i = 0; i < num_primitives; ++i)
-        proj[i] = vertices[primitive_indices[i]][split_axis];
-    }
-
-    std::sort(proj.begin(), proj.end());
-
-    if (num_primitives % 2 == 1) {
-      split_value = proj[(num_primitives - 1) / 2];
-    } else {
-      split_value =
-          (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
-    }
-  }
-};
-
-template <>
-bool HPP_FCL_DLLAPI BVSplitter<OBB>::apply(const Vec3f& q) const;
-
-template <>
-bool HPP_FCL_DLLAPI BVSplitter<RSS>::apply(const Vec3f& q) const;
-
-template <>
-bool HPP_FCL_DLLAPI BVSplitter<kIOS>::apply(const Vec3f& q) const;
-
-template <>
-bool HPP_FCL_DLLAPI BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<OBB>::computeRule_bvcenter(
-    const OBB& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<OBB>::computeRule_mean(
-    const OBB& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<OBB>::computeRule_median(
-    const OBB& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<RSS>::computeRule_bvcenter(
-    const RSS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<RSS>::computeRule_mean(
-    const RSS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<RSS>::computeRule_median(
-    const RSS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<kIOS>::computeRule_bvcenter(
-    const kIOS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<kIOS>::computeRule_mean(
-    const kIOS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<kIOS>::computeRule_median(
-    const kIOS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<OBBRSS>::computeRule_bvcenter(
-    const OBBRSS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<OBBRSS>::computeRule_mean(
-    const OBBRSS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-template <>
-void HPP_FCL_DLLAPI BVSplitter<OBBRSS>::computeRule_median(
-    const OBBRSS& bv, unsigned int* primitive_indices,
-    unsigned int num_primitives);
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/BV_splitter.h>
diff --git a/include/hpp/fcl/internal/intersect.h b/include/hpp/fcl/internal/intersect.h
index 1166bc74fc081aea7db901766e3f55a937b79738..bc4fe38c4476ba746d543bb898e58f5a85de987a 100644
--- a/include/hpp/fcl/internal/intersect.h
+++ b/include/hpp/fcl/internal/intersect.h
@@ -1,190 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_INTERSECT_H
-#define HPP_FCL_INTERSECT_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/math/transform.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief CCD intersect kernel among primitives
-class HPP_FCL_DLLAPI Intersect {
- public:
-  static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2,
-                                 const Vec3f& v3, Vec3f* n, FCL_REAL* t);
-};  // class Intersect
-
-/// @brief Project functions
-class HPP_FCL_DLLAPI Project {
- public:
-  struct HPP_FCL_DLLAPI ProjectResult {
-    /// @brief Parameterization of the projected point (based on the simplex to
-    /// be projected, use 2 or 3 or 4 of the array)
-    FCL_REAL parameterization[4];
-
-    /// @brief square distance from the query point to the projected simplex
-    FCL_REAL sqr_distance;
-
-    /// @brief the code of the projection type
-    unsigned int encode;
-
-    ProjectResult() : sqr_distance(-1), encode(0) {}
-  };
-
-  /// @brief Project point p onto line a-b
-  static ProjectResult projectLine(const Vec3f& a, const Vec3f& b,
-                                   const Vec3f& p);
-
-  /// @brief Project point p onto triangle a-b-c
-  static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b,
-                                       const Vec3f& c, const Vec3f& p);
-
-  /// @brief Project point p onto tetrahedra a-b-c-d
-  static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b,
-                                         const Vec3f& c, const Vec3f& d,
-                                         const Vec3f& p);
-
-  /// @brief Project origin (0) onto line a-b
-  static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b);
-
-  /// @brief Project origin (0) onto triangle a-b-c
-  static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b,
-                                             const Vec3f& c);
-
-  /// @brief Project origin (0) onto tetrahedran a-b-c-d
-  static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b,
-                                               const Vec3f& c, const Vec3f& d);
-};
-
-/// @brief Triangle distance functions
-class HPP_FCL_DLLAPI TriangleDistance {
- public:
-  /// @brief Returns closest points between an segment pair.
-  /// The first segment is P + t * A
-  /// The second segment is Q + t * B
-  /// X, Y are the closest points on the two segments
-  /// VEC is the vector between X and Y
-  static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
-                        const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y);
-
-  /// Compute squared distance between triangles
-  /// @param S and T are two triangles
-  /// @retval P, Q closest points if triangles do not intersect.
-  /// @return squared distance if triangles do not intersect, 0 otherwise.
-  /// If the triangles are disjoint, P and Q give the closet points of
-  /// S and T respectively. However,
-  /// if the triangles overlap, P and Q are basically a random pair of points
-  /// from the triangles, not coincident points on the intersection of the
-  /// triangles, as might be expected.
-  static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P,
-                                 Vec3f& Q);
-
-  static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
-                                 const Vec3f& S3, const Vec3f& T1,
-                                 const Vec3f& T2, const Vec3f& T3, Vec3f& P,
-                                 Vec3f& Q);
-
-  /// Compute squared distance between triangles
-  /// @param S and T are two triangles
-  /// @param R, Tl, rotation and translation applied to T,
-  /// @retval P, Q closest points if triangles do not intersect.
-  /// @return squared distance if triangles do not intersect, 0 otherwise.
-  /// If the triangles are disjoint, P and Q give the closet points of
-  /// S and T respectively. However,
-  /// if the triangles overlap, P and Q are basically a random pair of points
-  /// from the triangles, not coincident points on the intersection of the
-  /// triangles, as might be expected.
-  static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
-                                 const Matrix3f& R, const Vec3f& Tl, Vec3f& P,
-                                 Vec3f& Q);
-
-  /// Compute squared distance between triangles
-  /// @param S and T are two triangles
-  /// @param tf, rotation and translation applied to T,
-  /// @retval P, Q closest points if triangles do not intersect.
-  /// @return squared distance if triangles do not intersect, 0 otherwise.
-  /// If the triangles are disjoint, P and Q give the closet points of
-  /// S and T respectively. However,
-  /// if the triangles overlap, P and Q are basically a random pair of points
-  /// from the triangles, not coincident points on the intersection of the
-  /// triangles, as might be expected.
-  static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
-                                 const Transform3f& tf, Vec3f& P, Vec3f& Q);
-
-  /// Compute squared distance between triangles
-  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
-  /// @param R, Tl, rotation and translation applied to T1, T2, T3,
-  /// @retval P, Q closest points if triangles do not intersect.
-  /// @return squared distance if triangles do not intersect, 0 otherwise.
-  /// If the triangles are disjoint, P and Q give the closet points of
-  /// S and T respectively. However,
-  /// if the triangles overlap, P and Q are basically a random pair of points
-  /// from the triangles, not coincident points on the intersection of the
-  /// triangles, as might be expected.
-  static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
-                                 const Vec3f& S3, const Vec3f& T1,
-                                 const Vec3f& T2, const Vec3f& T3,
-                                 const Matrix3f& R, const Vec3f& Tl, Vec3f& P,
-                                 Vec3f& Q);
-
-  /// Compute squared distance between triangles
-  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
-  /// @param tf, rotation and translation applied to T1, T2, T3,
-  /// @retval P, Q closest points if triangles do not intersect.
-  /// @return squared distance if triangles do not intersect, 0 otherwise.
-  /// If the triangles are disjoint, P and Q give the closet points of
-  /// S and T respectively. However,
-  /// if the triangles overlap, P and Q are basically a random pair of points
-  /// from the triangles, not coincident points on the intersection of the
-  /// triangles, as might be expected.
-  static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
-                                 const Vec3f& S3, const Vec3f& T1,
-                                 const Vec3f& T2, const Vec3f& T3,
-                                 const Transform3f& tf, Vec3f& P, Vec3f& Q);
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/intersect.h>
diff --git a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h
index 9e3f6ec01443b35c2bb4902d7b73401032d419cc..cc3ecf58a619873101f6eaab497b39c7993e8b64 100644
--- a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h
+++ b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h
@@ -1,266 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2024, INRIA
- *  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 Louis Montaut */
-
-#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
-#define HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
-
-#include "hpp/fcl/collision_data.h"
-#include "hpp/fcl/collision_utility.h"
-#include "hpp/fcl/narrowphase/narrowphase.h"
-#include "hpp/fcl/contact_patch/contact_patch_solver.h"
-#include "hpp/fcl/shape/geometric_shapes_traits.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Shape-shape contact patch computation.
-/// Assumes that `csolver` and the `ContactPatchResult` have already been set up
-/// by the `ContactPatchRequest`.
-template <typename ShapeType1, typename ShapeType2>
-struct ComputeShapeShapeContactPatch {
-  static void run(const CollisionGeometry* o1, const Transform3f& tf1,
-                  const CollisionGeometry* o2, const Transform3f& tf2,
-                  const CollisionResult& collision_result,
-                  const ContactPatchSolver* csolver,
-                  const ContactPatchRequest& request,
-                  ContactPatchResult& result) {
-    // Note: see specializations for Plane and Halfspace below.
-    if (!collision_result.isCollision()) {
-      return;
-    }
-    HPP_FCL_ASSERT(
-        result.check(request),
-        "The contact patch result and request are incompatible (issue of "
-        "contact patch size or maximum number of contact patches). Make sure "
-        "result is initialized with request.",
-        std::logic_error);
-
-    const ShapeType1& s1 = static_cast<const ShapeType1&>(*o1);
-    const ShapeType2& s2 = static_cast<const ShapeType2&>(*o2);
-    for (size_t i = 0; i < collision_result.numContacts(); ++i) {
-      if (i >= request.max_num_patch) {
-        break;
-      }
-      csolver->setSupportGuess(collision_result.cached_support_func_guess);
-      const Contact& contact = collision_result.getContact(i);
-      ContactPatch& contact_patch = result.getUnusedContactPatch();
-      csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch);
-    }
-  }
-};
-
-/// @brief Computes the contact patch between a Plane/Halfspace and another
-/// shape.
-/// @tparam InvertShapes set to true if the first shape of the collision pair
-/// is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling
-/// this function).
-template <bool InvertShapes, typename OtherShapeType, typename PlaneOrHalfspace>
-void computePatchPlaneOrHalfspace(const OtherShapeType& s1,
-                                  const Transform3f& tf1,
-                                  const PlaneOrHalfspace& s2,
-                                  const Transform3f& tf2,
-                                  const ContactPatchSolver* csolver,
-                                  const Contact& contact,
-                                  ContactPatch& contact_patch) {
-  HPP_FCL_UNUSED_VARIABLE(s2);
-  HPP_FCL_UNUSED_VARIABLE(tf2);
-  constructContactPatchFrameFromContact(contact, contact_patch);
-  if ((bool)(shape_traits<OtherShapeType>::IsStrictlyConvex)) {
-    // Only one point of contact; it has already been computed.
-    contact_patch.addPoint(contact.pos);
-    return;
-  }
-
-  // We only need to compute the support set in the direction of the normal.
-  // We need to temporarily express the patch in the local frame of shape1.
-  SupportSet& support_set = csolver->support_set_shape1;
-  support_set.tf.rotation().noalias() =
-      tf1.rotation().transpose() * contact_patch.tf.rotation();
-  support_set.tf.translation().noalias() =
-      tf1.rotation().transpose() *
-      (contact_patch.tf.translation() - tf1.translation());
-
-  // Note: for now, taking into account swept-sphere radius does not change
-  // anything to the support set computations. However it will be used in the
-  // future if we want to store the offsets to the support plane for each point
-  // in a support set.
-  using SupportOptions = details::SupportOptions;
-  if (InvertShapes) {
-    support_set.direction = ContactPatch::PatchDirection::INVERTED;
-    details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
-        &s1, support_set, csolver->support_guess[1], csolver->supports_data[1],
-        csolver->num_samples_curved_shapes, csolver->patch_tolerance);
-  } else {
-    support_set.direction = ContactPatch::PatchDirection::DEFAULT;
-    details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
-        &s1, support_set, csolver->support_guess[0], csolver->supports_data[0],
-        csolver->num_samples_curved_shapes, csolver->patch_tolerance);
-  }
-  csolver->getResult(contact, &(support_set.points()), contact_patch);
-}
-
-#define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace)          \
-  template <typename OtherShapeType>                                          \
-  struct ComputeShapeShapeContactPatch<OtherShapeType, PlaneOrHspace> {       \
-    static void run(const CollisionGeometry* o1, const Transform3f& tf1,      \
-                    const CollisionGeometry* o2, const Transform3f& tf2,      \
-                    const CollisionResult& collision_result,                  \
-                    const ContactPatchSolver* csolver,                        \
-                    const ContactPatchRequest& request,                       \
-                    ContactPatchResult& result) {                             \
-      if (!collision_result.isCollision()) {                                  \
-        return;                                                               \
-      }                                                                       \
-      HPP_FCL_ASSERT(                                                         \
-          result.check(request),                                              \
-          "The contact patch result and request are incompatible (issue of "  \
-          "contact patch size or maximum number of contact patches). Make "   \
-          "sure "                                                             \
-          "result is initialized with request.",                              \
-          std::logic_error);                                                  \
-                                                                              \
-      const OtherShapeType& s1 = static_cast<const OtherShapeType&>(*o1);     \
-      const PlaneOrHspace& s2 = static_cast<const PlaneOrHspace&>(*o2);       \
-      for (size_t i = 0; i < collision_result.numContacts(); ++i) {           \
-        if (i >= request.max_num_patch) {                                     \
-          break;                                                              \
-        }                                                                     \
-        csolver->setSupportGuess(collision_result.cached_support_func_guess); \
-        const Contact& contact = collision_result.getContact(i);              \
-        ContactPatch& contact_patch = result.getUnusedContactPatch();         \
-        computePatchPlaneOrHalfspace<false, OtherShapeType, PlaneOrHspace>(   \
-            s1, tf1, s2, tf2, csolver, contact, contact_patch);               \
-      }                                                                       \
-    }                                                                         \
-  };                                                                          \
-                                                                              \
-  template <typename OtherShapeType>                                          \
-  struct ComputeShapeShapeContactPatch<PlaneOrHspace, OtherShapeType> {       \
-    static void run(const CollisionGeometry* o1, const Transform3f& tf1,      \
-                    const CollisionGeometry* o2, const Transform3f& tf2,      \
-                    const CollisionResult& collision_result,                  \
-                    const ContactPatchSolver* csolver,                        \
-                    const ContactPatchRequest& request,                       \
-                    ContactPatchResult& result) {                             \
-      if (!collision_result.isCollision()) {                                  \
-        return;                                                               \
-      }                                                                       \
-      HPP_FCL_ASSERT(                                                         \
-          result.check(request),                                              \
-          "The contact patch result and request are incompatible (issue of "  \
-          "contact patch size or maximum number of contact patches). Make "   \
-          "sure "                                                             \
-          "result is initialized with request.",                              \
-          std::logic_error);                                                  \
-                                                                              \
-      const PlaneOrHspace& s1 = static_cast<const PlaneOrHspace&>(*o1);       \
-      const OtherShapeType& s2 = static_cast<const OtherShapeType&>(*o2);     \
-      for (size_t i = 0; i < collision_result.numContacts(); ++i) {           \
-        if (i >= request.max_num_patch) {                                     \
-          break;                                                              \
-        }                                                                     \
-        csolver->setSupportGuess(collision_result.cached_support_func_guess); \
-        const Contact& contact = collision_result.getContact(i);              \
-        ContactPatch& contact_patch = result.getUnusedContactPatch();         \
-        computePatchPlaneOrHalfspace<true, OtherShapeType, PlaneOrHspace>(    \
-            s2, tf2, s1, tf1, csolver, contact, contact_patch);               \
-      }                                                                       \
-    }                                                                         \
-  };
-
-PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane)
-PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace)
-
-#define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2)           \
-  template <>                                                                \
-  struct ComputeShapeShapeContactPatch<PlaneOrHspace1, PlaneOrHspace2> {     \
-    static void run(const CollisionGeometry* o1, const Transform3f& tf1,     \
-                    const CollisionGeometry* o2, const Transform3f& tf2,     \
-                    const CollisionResult& collision_result,                 \
-                    const ContactPatchSolver* csolver,                       \
-                    const ContactPatchRequest& request,                      \
-                    ContactPatchResult& result) {                            \
-      HPP_FCL_UNUSED_VARIABLE(o1);                                           \
-      HPP_FCL_UNUSED_VARIABLE(tf1);                                          \
-      HPP_FCL_UNUSED_VARIABLE(o2);                                           \
-      HPP_FCL_UNUSED_VARIABLE(tf2);                                          \
-      HPP_FCL_UNUSED_VARIABLE(csolver);                                      \
-      if (!collision_result.isCollision()) {                                 \
-        return;                                                              \
-      }                                                                      \
-      HPP_FCL_ASSERT(                                                        \
-          result.check(request),                                             \
-          "The contact patch result and request are incompatible (issue of " \
-          "contact patch size or maximum number of contact patches). Make "  \
-          "sure "                                                            \
-          "result is initialized with request.",                             \
-          std::logic_error);                                                 \
-                                                                             \
-      for (size_t i = 0; i < collision_result.numContacts(); ++i) {          \
-        if (i >= request.max_num_patch) {                                    \
-          break;                                                             \
-        }                                                                    \
-        const Contact& contact = collision_result.getContact(i);             \
-        ContactPatch& contact_patch = result.getUnusedContactPatch();        \
-        constructContactPatchFrameFromContact(contact, contact_patch);       \
-        contact_patch.addPoint(contact.pos);                                 \
-      }                                                                      \
-    }                                                                        \
-  };
-
-PLANE_HSPACE_CONTACT_PATCH(Plane, Plane)
-PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace)
-PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane)
-PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace)
-
-#undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH
-#undef PLANE_HSPACE_CONTACT_PATCH
-
-template <typename ShapeType1, typename ShapeType2>
-void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
-                            const CollisionGeometry* o2, const Transform3f& tf2,
-                            const CollisionResult& collision_result,
-                            const ContactPatchSolver* csolver,
-                            const ContactPatchRequest& request,
-                            ContactPatchResult& result) {
-  return ComputeShapeShapeContactPatch<ShapeType1, ShapeType2>::run(
-      o1, tf1, o2, tf2, collision_result, csolver, request, result);
-}
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/shape_shape_contact_patch_func.h>
diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h
index 3321b4b387bc52e49aa92e9887e752853c700ba1..3789fb52a0e8737e5036085d39ad864a2700e622 100644
--- a/include/hpp/fcl/internal/shape_shape_func.h
+++ b/include/hpp/fcl/internal/shape_shape_func.h
@@ -1,316 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2014, CNRS-LAAS
- *  Copyright (c) 2024, INRIA
- *  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 Florent Lamiraux */
-
-#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H
-#define HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/collision_utility.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shapes_traits.h>
-
-namespace hpp {
-namespace fcl {
-
-template <typename ShapeType1, typename ShapeType2>
-struct ShapeShapeDistancer {
-  static FCL_REAL run(const CollisionGeometry* o1, const Transform3f& tf1,
-                      const CollisionGeometry* o2, const Transform3f& tf2,
-                      const GJKSolver* nsolver, const DistanceRequest& request,
-                      DistanceResult& result) {
-    if (request.isSatisfied(result)) return result.min_distance;
-
-    // Witness points on shape1 and shape2, normal pointing from shape1 to
-    // shape2.
-    Vec3f p1, p2, normal;
-    const FCL_REAL distance = ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
-        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
-        normal);
-
-    result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE,
-                  p1, p2, normal);
-
-    return distance;
-  }
-
-  static FCL_REAL run(const CollisionGeometry* o1, const Transform3f& tf1,
-                      const CollisionGeometry* o2, const Transform3f& tf2,
-                      const GJKSolver* nsolver,
-                      const bool compute_signed_distance, Vec3f& p1, Vec3f& p2,
-                      Vec3f& normal) {
-    const ShapeType1* obj1 = static_cast<const ShapeType1*>(o1);
-    const ShapeType2* obj2 = static_cast<const ShapeType2*>(o2);
-    return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2,
-                                  compute_signed_distance, p1, p2, normal);
-  }
-};
-
-/// @brief Shape-shape distance computation.
-/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
-/// a `CollisionRequest`.
-///
-/// @note This function is typically used for collision pairs containing two
-/// primitive shapes.
-/// @note This function might be specialized for some pairs of shapes.
-template <typename ShapeType1, typename ShapeType2>
-FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
-                            const CollisionGeometry* o2, const Transform3f& tf2,
-                            const GJKSolver* nsolver,
-                            const DistanceRequest& request,
-                            DistanceResult& result) {
-  return ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
-      o1, tf1, o2, tf2, nsolver, request, result);
-}
-
-namespace internal {
-/// @brief Shape-shape distance computation.
-/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
-/// a `CollisionRequest`.
-///
-/// @note This function is typically used for collision pairs complex structures
-/// like BVHs, HeightFields or Octrees. These structures contain sets of
-/// primitive shapes.
-/// This function is meant to be called on the pairs of primitive shapes of
-/// these structures.
-/// @note This function might be specialized for some pairs of shapes.
-template <typename ShapeType1, typename ShapeType2>
-FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
-                            const CollisionGeometry* o2, const Transform3f& tf2,
-                            const GJKSolver* nsolver,
-                            const bool compute_signed_distance, Vec3f& p1,
-                            Vec3f& p2, Vec3f& normal) {
-  return ::hpp::fcl::ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
-      o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal);
-}
-}  // namespace internal
-
-/// @brief Shape-shape collision detection.
-/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
-/// a `CollisionRequest`.
-///
-/// @note This function is typically used for collision pairs containing two
-/// primitive shapes.
-/// Complex structures like BVHs, HeightFields or Octrees contain sets of
-/// primitive shapes should use the `ShapeShapeDistance` function to do their
-/// internal collision detection checks.
-template <typename ShapeType1, typename ShapeType2>
-struct ShapeShapeCollider {
-  static std::size_t run(const CollisionGeometry* o1, const Transform3f& tf1,
-                         const CollisionGeometry* o2, const Transform3f& tf2,
-                         const GJKSolver* nsolver,
-                         const CollisionRequest& request,
-                         CollisionResult& result) {
-    if (request.isSatisfied(result)) return result.numContacts();
-
-    const bool compute_penetration =
-        request.enable_contact || (request.security_margin < 0);
-    Vec3f p1, p2, normal;
-    FCL_REAL distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
-        o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal);
-
-    size_t num_contacts = 0;
-    const FCL_REAL distToCollision = distance - request.security_margin;
-
-    internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
-                                               p1, p2, normal);
-    if (distToCollision <= request.collision_distance_threshold &&
-        result.numContacts() < request.num_max_contacts) {
-      if (result.numContacts() < request.num_max_contacts) {
-        Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal,
-                        distance);
-        result.addContact(contact);
-      }
-      num_contacts = result.numContacts();
-    }
-
-    return num_contacts;
-  }
-};
-
-template <typename ShapeType1, typename ShapeType2>
-std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
-                              const Transform3f& tf1,
-                              const CollisionGeometry* o2,
-                              const Transform3f& tf2, const GJKSolver* nsolver,
-                              const CollisionRequest& request,
-                              CollisionResult& result) {
-  return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
-      o1, tf1, o2, tf2, nsolver, request, result);
-}
-
-// clang-format off
-// ==============================================================================================================
-// ==============================================================================================================
-// ==============================================================================================================
-// Shape distance algorithms based on:
-// - built-in function: 0
-// - GJK:               1
-//
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | box        |  1  |   0    |    1    |   1  |    1     |   0   |      0     |    1     |    1      |    1   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | sphere     |/////|   0    |    0    |   1  |    0     |   0   |      0     |    0     |    1      |    1   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | capsule    |/////|////////|    0    |   1  |    1     |   0   |      0     |    1     |    1      |    1   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | cone       |/////|////////|/////////|   1  |    1     |   0   |      0     |    1     |    1      |    1   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | cylinder   |/////|////////|/////////|//////|    1     |   0   |      0     |    1     |    1      |    1   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | plane      |/////|////////|/////////|//////|//////////|   ?   |      ?     |    0     |    0      |    0   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | half-space |/////|////////|/////////|//////|//////////|///////|      ?     |    0     |    0      |    0   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|    0     |    1      |    1   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | ellipsoid  |/////|////////|/////////|//////|//////////|///////|////////////|//////////|    1      |    1   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-// | convex     |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////|    1   |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
-//
-// Number of pairs: 55
-//   - Specialized: 26
-//   - GJK:         29
-// clang-format on
-
-#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2)                            \
-  template <>                                                                  \
-  HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance<T1, T2>(                \
-      const CollisionGeometry* o1, const Transform3f& tf1,                     \
-      const CollisionGeometry* o2, const Transform3f& tf2,                     \
-      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \
-      Vec3f& p2, Vec3f& normal);                                               \
-  template <>                                                                  \
-  HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance<T2, T1>(                \
-      const CollisionGeometry* o1, const Transform3f& tf1,                     \
-      const CollisionGeometry* o2, const Transform3f& tf2,                     \
-      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \
-      Vec3f& p2, Vec3f& normal);                                               \
-  template <>                                                                  \
-  inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T1, T2>(                   \
-      const CollisionGeometry* o1, const Transform3f& tf1,                     \
-      const CollisionGeometry* o2, const Transform3f& tf2,                     \
-      const GJKSolver* nsolver, const DistanceRequest& request,                \
-      DistanceResult& result) {                                                \
-    result.o1 = o1;                                                            \
-    result.o2 = o2;                                                            \
-    result.b1 = DistanceResult::NONE;                                          \
-    result.b2 = DistanceResult::NONE;                                          \
-    result.min_distance = internal::ShapeShapeDistance<T1, T2>(                \
-        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
-        result.nearest_points[0], result.nearest_points[1], result.normal);    \
-    return result.min_distance;                                                \
-  }                                                                            \
-  template <>                                                                  \
-  inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T2, T1>(                   \
-      const CollisionGeometry* o1, const Transform3f& tf1,                     \
-      const CollisionGeometry* o2, const Transform3f& tf2,                     \
-      const GJKSolver* nsolver, const DistanceRequest& request,                \
-      DistanceResult& result) {                                                \
-    result.o1 = o1;                                                            \
-    result.o2 = o2;                                                            \
-    result.b1 = DistanceResult::NONE;                                          \
-    result.b2 = DistanceResult::NONE;                                          \
-    result.min_distance = internal::ShapeShapeDistance<T2, T1>(                \
-        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
-        result.nearest_points[0], result.nearest_points[1], result.normal);    \
-    return result.min_distance;                                                \
-  }
-
-#define SHAPE_SELF_DISTANCE_SPECIALIZATION(T)                                  \
-  template <>                                                                  \
-  HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance<T, T>(                  \
-      const CollisionGeometry* o1, const Transform3f& tf1,                     \
-      const CollisionGeometry* o2, const Transform3f& tf2,                     \
-      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \
-      Vec3f& p2, Vec3f& normal);                                               \
-  template <>                                                                  \
-  inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T, T>(                     \
-      const CollisionGeometry* o1, const Transform3f& tf1,                     \
-      const CollisionGeometry* o2, const Transform3f& tf2,                     \
-      const GJKSolver* nsolver, const DistanceRequest& request,                \
-      DistanceResult& result) {                                                \
-    result.o1 = o1;                                                            \
-    result.o2 = o2;                                                            \
-    result.b1 = DistanceResult::NONE;                                          \
-    result.b2 = DistanceResult::NONE;                                          \
-    result.min_distance = internal::ShapeShapeDistance<T, T>(                  \
-        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
-        result.nearest_points[0], result.nearest_points[1], result.normal);    \
-    return result.min_distance;                                                \
-  }
-
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere)
-SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane)
-SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Plane)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane)
-SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere)
-SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace)
-SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane)
-SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace)
-
-#undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
-#undef SHAPE_SELF_DISTANCE_SPECIALIZATION
-
-}  // namespace fcl
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/shape_shape_func.h>
diff --git a/include/hpp/fcl/internal/tools.h b/include/hpp/fcl/internal/tools.h
index 346141a500ae534a2379059bb04ed09d9e6ac91d..aa11afec1967b185fb33b796b885555c1a3b7403 100644
--- a/include/hpp/fcl/internal/tools.h
+++ b/include/hpp/fcl/internal/tools.h
@@ -1,215 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 Joseph Mirabel */
-
-#ifndef HPP_FCL_INTERNAL_TOOLS_H
-#define HPP_FCL_INTERNAL_TOOLS_H
-
-#include <hpp/fcl/fwd.hh>
-
-#include <cmath>
-#include <iostream>
-#include <limits>
-
-#include <hpp/fcl/data_types.h>
-
-namespace hpp {
-namespace fcl {
-
-template <typename Derived>
-static inline typename Derived::Scalar triple(
-    const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y,
-    const Eigen::MatrixBase<Derived>& z) {
-  return x.derived().dot(y.derived().cross(z.derived()));
-}
-
-template <typename Derived1, typename Derived2, typename Derived3>
-void generateCoordinateSystem(const Eigen::MatrixBase<Derived1>& _w,
-                              const Eigen::MatrixBase<Derived2>& _u,
-                              const Eigen::MatrixBase<Derived3>& _v) {
-  typedef typename Derived1::Scalar T;
-
-  Eigen::MatrixBase<Derived1>& w = const_cast<Eigen::MatrixBase<Derived1>&>(_w);
-  Eigen::MatrixBase<Derived2>& u = const_cast<Eigen::MatrixBase<Derived2>&>(_u);
-  Eigen::MatrixBase<Derived3>& v = const_cast<Eigen::MatrixBase<Derived3>&>(_v);
-
-  T inv_length;
-  if (std::abs(w[0]) >= std::abs(w[1])) {
-    inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
-    u[0] = -w[2] * inv_length;
-    u[1] = (T)0;
-    u[2] = w[0] * inv_length;
-    v[0] = w[1] * u[2];
-    v[1] = w[2] * u[0] - w[0] * u[2];
-    v[2] = -w[1] * u[0];
-  } else {
-    inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
-    u[0] = (T)0;
-    u[1] = w[2] * inv_length;
-    u[2] = -w[1] * inv_length;
-    v[0] = w[1] * u[2] - w[2] * u[1];
-    v[1] = -w[0] * u[2];
-    v[2] = w[0] * u[1];
-  }
-}
-
-/* ----- Start Matrices ------ */
-template <typename Derived, typename OtherDerived>
-void relativeTransform(const Eigen::MatrixBase<Derived>& R1,
-                       const Eigen::MatrixBase<OtherDerived>& t1,
-                       const Eigen::MatrixBase<Derived>& R2,
-                       const Eigen::MatrixBase<OtherDerived>& t2,
-                       const Eigen::MatrixBase<Derived>& R,
-                       const Eigen::MatrixBase<OtherDerived>& t) {
-  const_cast<Eigen::MatrixBase<Derived>&>(R) = R1.transpose() * R2;
-  const_cast<Eigen::MatrixBase<OtherDerived>&>(t) = R1.transpose() * (t2 - t1);
-}
-
-/// @brief compute the eigen vector and eigen vector of a matrix. dout is the
-/// eigen values, vout is the eigen vectors
-template <typename Derived, typename Vector>
-void eigen(const Eigen::MatrixBase<Derived>& m,
-           typename Derived::Scalar dout[3], Vector* vout) {
-  typedef typename Derived::Scalar Scalar;
-  Derived R(m.derived());
-  int n = 3;
-  int j, iq, ip, i;
-  Scalar tresh, theta, tau, t, sm, s, h, g, c;
-
-  Scalar b[3];
-  Scalar z[3];
-  Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
-  Scalar d[3];
-
-  for (ip = 0; ip < n; ++ip) {
-    b[ip] = d[ip] = R(ip, ip);
-    z[ip] = 0;
-  }
-
-  for (i = 0; i < 50; ++i) {
-    sm = 0;
-    for (ip = 0; ip < n; ++ip)
-      for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq));
-    if (sm == 0.0) {
-      vout[0] << v[0][0], v[0][1], v[0][2];
-      vout[1] << v[1][0], v[1][1], v[1][2];
-      vout[2] << v[2][0], v[2][1], v[2][2];
-      dout[0] = d[0];
-      dout[1] = d[1];
-      dout[2] = d[2];
-      return;
-    }
-
-    if (i < 3)
-      tresh = 0.2 * sm / (n * n);
-    else
-      tresh = 0.0;
-
-    for (ip = 0; ip < n; ++ip) {
-      for (iq = ip + 1; iq < n; ++iq) {
-        g = 100.0 * std::abs(R(ip, iq));
-        if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) &&
-            std::abs(d[iq]) + g == std::abs(d[iq]))
-          R(ip, iq) = 0.0;
-        else if (std::abs(R(ip, iq)) > tresh) {
-          h = d[iq] - d[ip];
-          if (std::abs(h) + g == std::abs(h))
-            t = (R(ip, iq)) / h;
-          else {
-            theta = 0.5 * h / (R(ip, iq));
-            t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta));
-            if (theta < 0.0) t = -t;
-          }
-          c = 1.0 / std::sqrt(1 + t * t);
-          s = t * c;
-          tau = s / (1.0 + c);
-          h = t * R(ip, iq);
-          z[ip] -= h;
-          z[iq] += h;
-          d[ip] -= h;
-          d[iq] += h;
-          R(ip, iq) = 0.0;
-          for (j = 0; j < ip; ++j) {
-            g = R(j, ip);
-            h = R(j, iq);
-            R(j, ip) = g - s * (h + g * tau);
-            R(j, iq) = h + s * (g - h * tau);
-          }
-          for (j = ip + 1; j < iq; ++j) {
-            g = R(ip, j);
-            h = R(j, iq);
-            R(ip, j) = g - s * (h + g * tau);
-            R(j, iq) = h + s * (g - h * tau);
-          }
-          for (j = iq + 1; j < n; ++j) {
-            g = R(ip, j);
-            h = R(iq, j);
-            R(ip, j) = g - s * (h + g * tau);
-            R(iq, j) = h + s * (g - h * tau);
-          }
-          for (j = 0; j < n; ++j) {
-            g = v[j][ip];
-            h = v[j][iq];
-            v[j][ip] = g - s * (h + g * tau);
-            v[j][iq] = h + s * (g - h * tau);
-          }
-        }
-      }
-    }
-    for (ip = 0; ip < n; ++ip) {
-      b[ip] += z[ip];
-      d[ip] = b[ip];
-      z[ip] = 0.0;
-    }
-  }
-
-  std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
-
-  return;
-}
-
-template <typename Derived, typename OtherDerived>
-bool isEqual(const Eigen::MatrixBase<Derived>& lhs,
-             const Eigen::MatrixBase<OtherDerived>& rhs,
-             const FCL_REAL tol = std::numeric_limits<FCL_REAL>::epsilon() *
-                                  100) {
-  return ((lhs - rhs).array().abs() < tol).all();
-}
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/tools.h>
diff --git a/include/hpp/fcl/internal/traversal.h b/include/hpp/fcl/internal/traversal.h
index e999d8c43eac5cfba52cf9ad743391503e8c4e3a..f0cbf51fce490787043580f473fcbc962d53eecc 100644
--- a/include/hpp/fcl/internal/traversal.h
+++ b/include/hpp/fcl/internal/traversal.h
@@ -1,76 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2019, LAAS CNRS
- *  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 Joseph Mirabel */
-
-#ifndef HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H
-#define HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H
-
-/// @cond INTERNAL
-
-namespace hpp {
-namespace fcl {
-
-enum { RelativeTransformationIsIdentity = 1 };
-
-namespace details {
-template <bool enabled>
-struct HPP_FCL_DLLAPI RelativeTransformation {
-  RelativeTransformation() : R(Matrix3f::Identity()) {}
-
-  const Matrix3f& _R() const { return R; }
-  const Vec3f& _T() const { return T; }
-
-  Matrix3f R;
-  Vec3f T;
-};
-
-template <>
-struct HPP_FCL_DLLAPI RelativeTransformation<false> {
-  static const Matrix3f& _R() {
-    HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error);
-  }
-  static const Vec3f& _T() {
-    HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error);
-  }
-};
-}  // namespace details
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal.h>
diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h
index a3c56f8d4d5e1537c92f2f46d3413fd9b7e4ca8b..b5f5d9938f0642f33283fa31612448f82a73a172 100644
--- a/include/hpp/fcl/internal/traversal_node_base.h
+++ b/include/hpp/fcl/internal/traversal_node_base.h
@@ -1,175 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_TRAVERSAL_NODE_BASE_H
-#define HPP_FCL_TRAVERSAL_NODE_BASE_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision_data.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Node structure encoding the information required for traversal.
-
-class TraversalNodeBase {
- public:
-  TraversalNodeBase() : enable_statistics(false) {}
-
-  virtual ~TraversalNodeBase() {}
-
-  virtual void preprocess() {}
-
-  virtual void postprocess() {}
-
-  /// @brief Whether b is a leaf node in the first BVH tree
-  virtual bool isFirstNodeLeaf(unsigned int /*b*/) const { return true; }
-
-  /// @brief Whether b is a leaf node in the second BVH tree
-  virtual bool isSecondNodeLeaf(unsigned int /*b*/) const { return true; }
-
-  /// @brief Traverse the subtree of the node in the first tree first
-  virtual bool firstOverSecond(unsigned int /*b1*/, unsigned int /*b2*/) const {
-    return true;
-  }
-
-  /// @brief Get the left child of the node b in the first tree
-  virtual int getFirstLeftChild(unsigned int b) const { return (int)b; }
-
-  /// @brief Get the right child of the node b in the first tree
-  virtual int getFirstRightChild(unsigned int b) const { return (int)b; }
-
-  /// @brief Get the left child of the node b in the second tree
-  virtual int getSecondLeftChild(unsigned int b) const { return (int)b; }
-
-  /// @brief Get the right child of the node b in the second tree
-  virtual int getSecondRightChild(unsigned int b) const { return (int)b; }
-
-  /// @brief Whether store some statistics information during traversal
-  void enableStatistics(bool enable) { enable_statistics = enable; }
-
-  /// @brief configuation of first object
-  Transform3f tf1;
-
-  /// @brief configuration of second object
-  Transform3f tf2;
-
-  /// @brief Whether stores statistics
-  bool enable_statistics;
-};
-
-/// @defgroup Traversal_For_Collision
-/// regroup class about traversal for distance.
-/// @{
-
-/// @brief Node structure encoding the information required for collision
-/// traversal.
-class CollisionTraversalNodeBase : public TraversalNodeBase {
- public:
-  CollisionTraversalNodeBase(const CollisionRequest& request_)
-      : request(request_), result(NULL) {}
-
-  virtual ~CollisionTraversalNodeBase() {}
-
-  /// BV test between b1 and b2
-  /// @param b1, b2 Bounding volumes to test,
-  /// @retval sqrDistLowerBound square of a lower bound of the minimal
-  ///         distance between bounding volumes.
-  virtual bool BVDisjoints(unsigned int b1, unsigned int b2,
-                           FCL_REAL& sqrDistLowerBound) const = 0;
-
-  /// @brief Leaf test between node b1 and b2, if they are both leafs
-  virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/,
-                            FCL_REAL& /*sqrDistLowerBound*/) const = 0;
-
-  /// @brief Check whether the traversal can stop
-  bool canStop() const { return this->request.isSatisfied(*(this->result)); }
-
-  /// @brief request setting for collision
-  const CollisionRequest& request;
-
-  /// @brief collision result kept during the traversal iteration
-  CollisionResult* result;
-};
-
-/// @}
-
-/// @defgroup Traversal_For_Distance
-/// regroup class about traversal for distance.
-/// @{
-
-/// @brief Node structure encoding the information required for distance
-/// traversal.
-class DistanceTraversalNodeBase : public TraversalNodeBase {
- public:
-  DistanceTraversalNodeBase() : result(NULL) {}
-
-  virtual ~DistanceTraversalNodeBase() {}
-
-  /// @brief BV test between b1 and b2
-  /// @return a lower bound of the distance between the two BV.
-  /// @note except for OBB, this method returns the distance.
-  virtual FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/,
-                                        unsigned int /*b2*/) const {
-    return (std::numeric_limits<FCL_REAL>::max)();
-  }
-
-  /// @brief Leaf test between node b1 and b2, if they are both leafs
-  virtual void leafComputeDistance(unsigned int b1, unsigned int b2) const = 0;
-
-  /// @brief Check whether the traversal can stop
-  virtual bool canStop(FCL_REAL /*c*/) const { return false; }
-
-  /// @brief request setting for distance
-  DistanceRequest request;
-
-  /// @brief distance result kept during the traversal iteration
-  DistanceResult* result;
-};
-
-///@}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal_node_base.h>
diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h
index c24a0c00279f68d38c6b6ebe7f1d8e7f01f3cf53..c99f06b17bb084b125c120d7c0a16833fbc8601e 100644
--- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h
@@ -1,487 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H
-#define HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/internal/traversal_node_base.h>
-#include <hpp/fcl/internal/traversal.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @addtogroup Traversal_For_Collision
-/// @{
-
-/// @brief Traversal node for collision between BVH and shape
-template <typename BV, typename S>
-class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase {
- public:
-  BVHShapeCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    num_bv_tests = 0;
-    num_leaf_tests = 0;
-    query_time_seconds = 0.0;
-  }
-
-  /// @brief Whether the BV node in the first BVH tree is leaf
-  bool isFirstNodeLeaf(unsigned int b) const {
-    return model1->getBV(b).isLeaf();
-  }
-
-  /// @brief Obtain the left child of BV node in the first BVH
-  int getFirstLeftChild(unsigned int b) const {
-    return model1->getBV(b).leftChild();
-  }
-
-  /// @brief Obtain the right child of BV node in the first BVH
-  int getFirstRightChild(unsigned int b) const {
-    return model1->getBV(b).rightChild();
-  }
-
-  const BVHModel<BV>* model1;
-  const S* model2;
-  BV model2_bv;
-
-  mutable int num_bv_tests;
-  mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
-};
-
-/// @brief Traversal node for collision between mesh and shape
-template <typename BV, typename S,
-          int _Options = RelativeTransformationIsIdentity>
-class MeshShapeCollisionTraversalNode
-    : public BVHShapeCollisionTraversalNode<BV, S> {
- public:
-  enum {
-    Options = _Options,
-    RTIsIdentity = _Options & RelativeTransformationIsIdentity
-  };
-
-  MeshShapeCollisionTraversalNode(const CollisionRequest& request)
-      : BVHShapeCollisionTraversalNode<BV, S>(request) {
-    vertices = NULL;
-    tri_indices = NULL;
-
-    nsolver = NULL;
-  }
-
-  /// test between BV b1 and shape
-  /// @param b1 BV to test,
-  /// @retval sqrDistLowerBound square of a lower bound of the minimal
-  ///         distance between bounding volumes.
-  /// @brief BV culling test in one BVTT node
-  bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
-                   FCL_REAL& sqrDistLowerBound) const {
-    if (this->enable_statistics) this->num_bv_tests++;
-    bool disjoint;
-    if (RTIsIdentity)
-      disjoint = !this->model1->getBV(b1).bv.overlap(
-          this->model2_bv, this->request, sqrDistLowerBound);
-    else
-      disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
-                          this->model1->getBV(b1).bv, this->model2_bv,
-                          this->request, sqrDistLowerBound);
-    if (disjoint)
-      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
-                                               sqrDistLowerBound);
-    assert(!disjoint || sqrDistLowerBound > 0);
-    return disjoint;
-  }
-
-  /// @brief Intersection testing between leaves (one triangle and one shape)
-  void leafCollides(unsigned int b1, unsigned int /*b2*/,
-                    FCL_REAL& sqrDistLowerBound) const {
-    if (this->enable_statistics) this->num_leaf_tests++;
-    const BVNode<BV>& node = this->model1->getBV(b1);
-
-    int primitive_id = node.primitiveId();
-
-    const Triangle& tri_id = this->tri_indices[primitive_id];
-    const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
-                        this->vertices[tri_id[2]]);
-
-    // When reaching this point, `this->solver` has already been set up
-    // by the CollisionRequest `this->request`.
-    // The only thing we need to (and can) pass to `ShapeShapeDistance` is
-    // whether or not penetration information is should be computed in case of
-    // collision.
-    const bool compute_penetration =
-        this->request.enable_contact || (this->request.security_margin < 0);
-    Vec3f c1, c2, normal;
-    FCL_REAL distance;
-
-    if (RTIsIdentity) {
-      static const Transform3f Id;
-      distance = internal::ShapeShapeDistance<TriangleP, S>(
-          &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration,
-          c1, c2, normal);
-    } else {
-      distance = internal::ShapeShapeDistance<TriangleP, S>(
-          &tri, this->tf1, this->model2, this->tf2, this->nsolver,
-          compute_penetration, c1, c2, normal);
-    }
-    const FCL_REAL distToCollision = distance - this->request.security_margin;
-
-    internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result),
-                                               distToCollision, c1, c2, normal);
-
-    if (distToCollision <= this->request.collision_distance_threshold) {
-      sqrDistLowerBound = 0;
-      if (this->result->numContacts() < this->request.num_max_contacts) {
-        this->result->addContact(Contact(this->model1, this->model2,
-                                         primitive_id, Contact::NONE, c1, c2,
-                                         normal, distance));
-        assert(this->result->isCollision());
-      }
-    } else {
-      sqrDistLowerBound = distToCollision * distToCollision;
-    }
-
-    assert(this->result->isCollision() || sqrDistLowerBound > 0);
-  }  // leafCollides
-
-  Vec3f* vertices;
-  Triangle* tri_indices;
-
-  const GJKSolver* nsolver;
-};
-
-/// @}
-
-/// @addtogroup Traversal_For_Distance
-/// @{
-
-/// @brief Traversal node for distance computation between BVH and shape
-template <typename BV, typename S>
-class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
- public:
-  BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
-    model1 = NULL;
-    model2 = NULL;
-
-    num_bv_tests = 0;
-    num_leaf_tests = 0;
-    query_time_seconds = 0.0;
-  }
-
-  /// @brief Whether the BV node in the first BVH tree is leaf
-  bool isFirstNodeLeaf(unsigned int b) const {
-    return model1->getBV(b).isLeaf();
-  }
-
-  /// @brief Obtain the left child of BV node in the first BVH
-  int getFirstLeftChild(unsigned int b) const {
-    return model1->getBV(b).leftChild();
-  }
-
-  /// @brief Obtain the right child of BV node in the first BVH
-  int getFirstRightChild(unsigned int b) const {
-    return model1->getBV(b).rightChild();
-  }
-
-  /// @brief BV culling test in one BVTT node
-  FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
-    return model1->getBV(b1).bv.distance(model2_bv);
-  }
-
-  const BVHModel<BV>* model1;
-  const S* model2;
-  BV model2_bv;
-
-  mutable int num_bv_tests;
-  mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
-};
-
-/// @brief Traversal node for distance computation between shape and BVH
-template <typename S, typename BV>
-class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase {
- public:
-  ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
-    model1 = NULL;
-    model2 = NULL;
-
-    num_bv_tests = 0;
-    num_leaf_tests = 0;
-    query_time_seconds = 0.0;
-  }
-
-  /// @brief Whether the BV node in the second BVH tree is leaf
-  bool isSecondNodeLeaf(unsigned int b) const {
-    return model2->getBV(b).isLeaf();
-  }
-
-  /// @brief Obtain the left child of BV node in the second BVH
-  int getSecondLeftChild(unsigned int b) const {
-    return model2->getBV(b).leftChild();
-  }
-
-  /// @brief Obtain the right child of BV node in the second BVH
-  int getSecondRightChild(unsigned int b) const {
-    return model2->getBV(b).rightChild();
-  }
-
-  /// @brief BV culling test in one BVTT node
-  FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const {
-    return model1_bv.distance(model2->getBV(b2).bv);
-  }
-
-  const S* model1;
-  const BVHModel<BV>* model2;
-  BV model1_bv;
-
-  mutable int num_bv_tests;
-  mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
-};
-
-/// @brief Traversal node for distance between mesh and shape
-template <typename BV, typename S>
-class MeshShapeDistanceTraversalNode
-    : public BVHShapeDistanceTraversalNode<BV, S> {
- public:
-  MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>() {
-    vertices = NULL;
-    tri_indices = NULL;
-
-    rel_err = 0;
-    abs_err = 0;
-
-    nsolver = NULL;
-  }
-
-  /// @brief Distance testing between leaves (one triangle and one shape)
-  void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
-    if (this->enable_statistics) this->num_leaf_tests++;
-
-    const BVNode<BV>& node = this->model1->getBV(b1);
-
-    int primitive_id = node.primitiveId();
-
-    const Triangle& tri_id = tri_indices[primitive_id];
-    const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
-                        this->vertices[tri_id[2]]);
-
-    Vec3f p1, p2, normal;
-    const FCL_REAL distance = internal::ShapeShapeDistance<TriangleP, S>(
-        &tri, this->tf1, this->model2, this->tf2, this->nsolver,
-        this->request.enable_signed_distance, p1, p2, normal);
-
-    this->result->update(distance, this->model1, this->model2, primitive_id,
-                         DistanceResult::NONE, p1, p2, normal);
-  }
-
-  /// @brief Whether the traversal process can stop early
-  bool canStop(FCL_REAL c) const {
-    if ((c >= this->result->min_distance - abs_err) &&
-        (c * (1 + rel_err) >= this->result->min_distance))
-      return true;
-    return false;
-  }
-
-  Vec3f* vertices;
-  Triangle* tri_indices;
-
-  FCL_REAL rel_err;
-  FCL_REAL abs_err;
-
-  const GJKSolver* nsolver;
-};
-
-/// @cond IGNORE
-namespace details {
-
-template <typename BV, typename S>
-void meshShapeDistanceOrientedNodeleafComputeDistance(
-    unsigned int b1, unsigned int /* b2 */, const BVHModel<BV>* model1,
-    const S& model2, Vec3f* vertices, Triangle* tri_indices,
-    const Transform3f& tf1, const Transform3f& tf2, const GJKSolver* nsolver,
-    bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request,
-    DistanceResult& result) {
-  if (enable_statistics) num_leaf_tests++;
-
-  const BVNode<BV>& node = model1->getBV(b1);
-  int primitive_id = node.primitiveId();
-
-  const Triangle& tri_id = tri_indices[primitive_id];
-  const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
-                      vertices[tri_id[2]]);
-
-  Vec3f p1, p2, normal;
-  const FCL_REAL distance = internal::ShapeShapeDistance<TriangleP, S>(
-      &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
-      normal);
-
-  result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE,
-                p1, p2, normal);
-}
-
-template <typename BV, typename S>
-static inline void distancePreprocessOrientedNode(
-    const BVHModel<BV>* model1, Vec3f* vertices, Triangle* tri_indices,
-    int init_tri_id, const S& model2, const Transform3f& tf1,
-    const Transform3f& tf2, const GJKSolver* nsolver,
-    const DistanceRequest& request, DistanceResult& result) {
-  const Triangle& tri_id = tri_indices[init_tri_id];
-  const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
-                      vertices[tri_id[2]]);
-
-  Vec3f p1, p2, normal;
-  const FCL_REAL distance = internal::ShapeShapeDistance<TriangleP, S>(
-      &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
-      normal);
-  result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE,
-                p1, p2, normal);
-}
-
-}  // namespace details
-
-/// @endcond
-
-/// @brief Traversal node for distance between mesh and shape, when mesh BVH is
-/// one of the oriented node (RSS, kIOS, OBBRSS)
-template <typename S>
-class MeshShapeDistanceTraversalNodeRSS
-    : public MeshShapeDistanceTraversalNode<RSS, S> {
- public:
-  MeshShapeDistanceTraversalNodeRSS()
-      : MeshShapeDistanceTraversalNode<RSS, S>() {}
-
-  void preprocess() {
-    details::distancePreprocessOrientedNode(
-        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
-        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
-  }
-
-  void postprocess() {}
-
-  FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
-    if (this->enable_statistics) this->num_bv_tests++;
-    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
-                    this->model2_bv, this->model1->getBV(b1).bv);
-  }
-
-  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
-    details::meshShapeDistanceOrientedNodeleafComputeDistance(
-        b1, b2, this->model1, *(this->model2), this->vertices,
-        this->tri_indices, this->tf1, this->tf2, this->nsolver,
-        this->enable_statistics, this->num_leaf_tests, this->request,
-        *(this->result));
-  }
-};
-
-template <typename S>
-class MeshShapeDistanceTraversalNodekIOS
-    : public MeshShapeDistanceTraversalNode<kIOS, S> {
- public:
-  MeshShapeDistanceTraversalNodekIOS()
-      : MeshShapeDistanceTraversalNode<kIOS, S>() {}
-
-  void preprocess() {
-    details::distancePreprocessOrientedNode(
-        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
-        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
-  }
-
-  void postprocess() {}
-
-  FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
-    if (this->enable_statistics) this->num_bv_tests++;
-    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
-                    this->model2_bv, this->model1->getBV(b1).bv);
-  }
-
-  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
-    details::meshShapeDistanceOrientedNodeleafComputeDistance(
-        b1, b2, this->model1, *(this->model2), this->vertices,
-        this->tri_indices, this->tf1, this->tf2, this->nsolver,
-        this->enable_statistics, this->num_leaf_tests, this->request,
-        *(this->result));
-  }
-};
-
-template <typename S>
-class MeshShapeDistanceTraversalNodeOBBRSS
-    : public MeshShapeDistanceTraversalNode<OBBRSS, S> {
- public:
-  MeshShapeDistanceTraversalNodeOBBRSS()
-      : MeshShapeDistanceTraversalNode<OBBRSS, S>() {}
-
-  void preprocess() {
-    details::distancePreprocessOrientedNode(
-        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
-        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
-  }
-
-  void postprocess() {}
-
-  FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
-    if (this->enable_statistics) this->num_bv_tests++;
-    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
-                    this->model2_bv, this->model1->getBV(b1).bv);
-  }
-
-  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
-    details::meshShapeDistanceOrientedNodeleafComputeDistance(
-        b1, b2, this->model1, *(this->model2), this->vertices,
-        this->tri_indices, this->tf1, this->tf2, this->nsolver,
-        this->enable_statistics, this->num_leaf_tests, this->request,
-        *(this->result));
-  }
-};
-
-/// @}
-
-}  // namespace fcl
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal_node_bvh_shape.h>
diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h
index bd20af2ac5ebdc300be562d618fed8aac8e03b7c..835610130aff7d83c6816c088428a1879583936d 100644
--- a/include/hpp/fcl/internal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/internal/traversal_node_bvhs.h
@@ -1,557 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_TRAVERSAL_NODE_MESHES_H
-#define HPP_FCL_TRAVERSAL_NODE_MESHES_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/internal/traversal_node_base.h>
-#include <hpp/fcl/BV/BV_node.h>
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/internal/intersect.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/internal/traversal.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
-
-#include <cassert>
-
-namespace hpp {
-namespace fcl {
-
-/// @addtogroup Traversal_For_Collision
-/// @{
-
-/// @brief Traversal node for collision between BVH models
-template <typename BV>
-class BVHCollisionTraversalNode : public CollisionTraversalNodeBase {
- public:
-  BVHCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    num_bv_tests = 0;
-    num_leaf_tests = 0;
-    query_time_seconds = 0.0;
-  }
-
-  /// @brief Whether the BV node in the first BVH tree is leaf
-  bool isFirstNodeLeaf(unsigned int b) const {
-    assert(model1 != NULL && "model1 is NULL");
-    return model1->getBV(b).isLeaf();
-  }
-
-  /// @brief Whether the BV node in the second BVH tree is leaf
-  bool isSecondNodeLeaf(unsigned int b) const {
-    assert(model2 != NULL && "model2 is NULL");
-    return model2->getBV(b).isLeaf();
-  }
-
-  /// @brief Determine the traversal order, is the first BVTT subtree better
-  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
-    FCL_REAL sz1 = model1->getBV(b1).bv.size();
-    FCL_REAL sz2 = model2->getBV(b2).bv.size();
-
-    bool l1 = model1->getBV(b1).isLeaf();
-    bool l2 = model2->getBV(b2).isLeaf();
-
-    if (l2 || (!l1 && (sz1 > sz2))) return true;
-    return false;
-  }
-
-  /// @brief Obtain the left child of BV node in the first BVH
-  int getFirstLeftChild(unsigned int b) const {
-    return model1->getBV(b).leftChild();
-  }
-
-  /// @brief Obtain the right child of BV node in the first BVH
-  int getFirstRightChild(unsigned int b) const {
-    return model1->getBV(b).rightChild();
-  }
-
-  /// @brief Obtain the left child of BV node in the second BVH
-  int getSecondLeftChild(unsigned int b) const {
-    return model2->getBV(b).leftChild();
-  }
-
-  /// @brief Obtain the right child of BV node in the second BVH
-  int getSecondRightChild(unsigned int b) const {
-    return model2->getBV(b).rightChild();
-  }
-
-  /// @brief The first BVH model
-  const BVHModel<BV>* model1;
-  /// @brief The second BVH model
-  const BVHModel<BV>* model2;
-
-  /// @brief statistical information
-  mutable int num_bv_tests;
-  mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
-};
-
-/// @brief Traversal node for collision between two meshes
-template <typename BV, int _Options = RelativeTransformationIsIdentity>
-class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
- public:
-  enum {
-    Options = _Options,
-    RTIsIdentity = _Options & RelativeTransformationIsIdentity
-  };
-
-  MeshCollisionTraversalNode(const CollisionRequest& request)
-      : BVHCollisionTraversalNode<BV>(request) {
-    vertices1 = NULL;
-    vertices2 = NULL;
-    tri_indices1 = NULL;
-    tri_indices2 = NULL;
-  }
-
-  /// BV test between b1 and b2
-  /// @param b1, b2 Bounding volumes to test,
-  /// @retval sqrDistLowerBound square of a lower bound of the minimal
-  ///         distance between bounding volumes.
-  bool BVDisjoints(unsigned int b1, unsigned int b2,
-                   FCL_REAL& sqrDistLowerBound) const {
-    if (this->enable_statistics) this->num_bv_tests++;
-    bool disjoint;
-    if (RTIsIdentity)
-      disjoint = !this->model1->getBV(b1).overlap(
-          this->model2->getBV(b2), this->request, sqrDistLowerBound);
-    else {
-      disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
-                          this->model1->getBV(b1).bv, this->request,
-                          sqrDistLowerBound);
-    }
-    if (disjoint)
-      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
-                                               sqrDistLowerBound);
-    return disjoint;
-  }
-
-  /// Intersection testing between leaves (two triangles)
-  ///
-  /// @param b1, b2 id of primitive in bounding volume hierarchy
-  /// @retval sqrDistLowerBound squared lower bound of distance between
-  ///         primitives if they are not in collision.
-  ///
-  /// This method supports a security margin. If the distance between
-  /// the primitives is less than the security margin, the objects are
-  /// considered as in collision. in this case a contact point is
-  /// returned in the CollisionResult.
-  ///
-  /// @note If the distance between objects is less than the security margin,
-  ///       and the object are not colliding, the penetration depth is
-  ///       negative.
-  void leafCollides(unsigned int b1, unsigned int b2,
-                    FCL_REAL& sqrDistLowerBound) const {
-    if (this->enable_statistics) this->num_leaf_tests++;
-
-    const BVNode<BV>& node1 = this->model1->getBV(b1);
-    const BVNode<BV>& node2 = this->model2->getBV(b2);
-
-    int primitive_id1 = node1.primitiveId();
-    int primitive_id2 = node2.primitiveId();
-
-    const Triangle& tri_id1 = tri_indices1[primitive_id1];
-    const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
-    const Vec3f& P1 = vertices1[tri_id1[0]];
-    const Vec3f& P2 = vertices1[tri_id1[1]];
-    const Vec3f& P3 = vertices1[tri_id1[2]];
-    const Vec3f& Q1 = vertices2[tri_id2[0]];
-    const Vec3f& Q2 = vertices2[tri_id2[1]];
-    const Vec3f& Q3 = vertices2[tri_id2[2]];
-
-    TriangleP tri1(P1, P2, P3);
-    TriangleP tri2(Q1, Q2, Q3);
-
-    // TODO(louis): MeshCollisionTraversalNode should have its own GJKSolver.
-    GJKSolver solver(this->request);
-
-    const bool compute_penetration =
-        this->request.enable_contact || (this->request.security_margin < 0);
-    Vec3f p1, p2, normal;
-    DistanceResult distanceResult;
-    FCL_REAL distance = internal::ShapeShapeDistance<TriangleP, TriangleP>(
-        &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1,
-        p2, normal);
-
-    const FCL_REAL distToCollision = distance - this->request.security_margin;
-
-    internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result),
-                                               distToCollision, p1, p2, normal);
-
-    if (distToCollision <=
-        this->request.collision_distance_threshold) {  // collision
-      sqrDistLowerBound = 0;
-      if (this->result->numContacts() < this->request.num_max_contacts) {
-        this->result->addContact(Contact(this->model1, this->model2,
-                                         primitive_id1, primitive_id2, p1, p2,
-                                         normal, distance));
-      }
-    } else
-      sqrDistLowerBound = distToCollision * distToCollision;
-  }
-
-  Vec3f* vertices1;
-  Vec3f* vertices2;
-
-  Triangle* tri_indices1;
-  Triangle* tri_indices2;
-
-  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
-};
-
-/// @brief Traversal node for collision between two meshes if their underlying
-/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
-typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB;
-typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS;
-typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS;
-typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
-
-/// @}
-
-namespace details {
-template <typename BV>
-struct DistanceTraversalBVDistanceLowerBound_impl {
-  static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
-    return b1.distance(b2);
-  }
-  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
-                      const BVNode<BV>& b2) {
-    return distance(R, T, b1.bv, b2.bv);
-  }
-};
-
-template <>
-struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
-  static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
-    FCL_REAL sqrDistLowerBound;
-    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
-    // request.break_distance = ?
-    if (b1.overlap(b2, request, sqrDistLowerBound)) {
-      // TODO A penetration upper bound should be computed.
-      return -1;
-    }
-    return sqrt(sqrDistLowerBound);
-  }
-  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1,
-                      const BVNode<OBB>& b2) {
-    FCL_REAL sqrDistLowerBound;
-    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
-    // request.break_distance = ?
-    if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
-      // TODO A penetration upper bound should be computed.
-      return -1;
-    }
-    return sqrt(sqrDistLowerBound);
-  }
-};
-
-template <>
-struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
-  static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
-    FCL_REAL sqrDistLowerBound;
-    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
-    // request.break_distance = ?
-    if (b1.overlap(b2, request, sqrDistLowerBound)) {
-      // TODO A penetration upper bound should be computed.
-      return -1;
-    }
-    return sqrt(sqrDistLowerBound);
-  }
-  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1,
-                      const BVNode<AABB>& b2) {
-    FCL_REAL sqrDistLowerBound;
-    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
-    // request.break_distance = ?
-    if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
-      // TODO A penetration upper bound should be computed.
-      return -1;
-    }
-    return sqrt(sqrDistLowerBound);
-  }
-};
-}  // namespace details
-
-/// @addtogroup Traversal_For_Distance
-/// @{
-
-/// @brief Traversal node for distance computation between BVH models
-template <typename BV>
-class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
- public:
-  BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
-    model1 = NULL;
-    model2 = NULL;
-
-    num_bv_tests = 0;
-    num_leaf_tests = 0;
-    query_time_seconds = 0.0;
-  }
-
-  /// @brief Whether the BV node in the first BVH tree is leaf
-  bool isFirstNodeLeaf(unsigned int b) const {
-    return model1->getBV(b).isLeaf();
-  }
-
-  /// @brief Whether the BV node in the second BVH tree is leaf
-  bool isSecondNodeLeaf(unsigned int b) const {
-    return model2->getBV(b).isLeaf();
-  }
-
-  /// @brief Determine the traversal order, is the first BVTT subtree better
-  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
-    FCL_REAL sz1 = model1->getBV(b1).bv.size();
-    FCL_REAL sz2 = model2->getBV(b2).bv.size();
-
-    bool l1 = model1->getBV(b1).isLeaf();
-    bool l2 = model2->getBV(b2).isLeaf();
-
-    if (l2 || (!l1 && (sz1 > sz2))) return true;
-    return false;
-  }
-
-  /// @brief Obtain the left child of BV node in the first BVH
-  int getFirstLeftChild(unsigned int b) const {
-    return model1->getBV(b).leftChild();
-  }
-
-  /// @brief Obtain the right child of BV node in the first BVH
-  int getFirstRightChild(unsigned int b) const {
-    return model1->getBV(b).rightChild();
-  }
-
-  /// @brief Obtain the left child of BV node in the second BVH
-  int getSecondLeftChild(unsigned int b) const {
-    return model2->getBV(b).leftChild();
-  }
-
-  /// @brief Obtain the right child of BV node in the second BVH
-  int getSecondRightChild(unsigned int b) const {
-    return model2->getBV(b).rightChild();
-  }
-
-  /// @brief The first BVH model
-  const BVHModel<BV>* model1;
-  /// @brief The second BVH model
-  const BVHModel<BV>* model2;
-
-  /// @brief statistical information
-  mutable int num_bv_tests;
-  mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
-};
-
-/// @brief Traversal node for distance computation between two meshes
-template <typename BV, int _Options = RelativeTransformationIsIdentity>
-class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
- public:
-  enum {
-    Options = _Options,
-    RTIsIdentity = _Options & RelativeTransformationIsIdentity
-  };
-
-  using BVHDistanceTraversalNode<BV>::enable_statistics;
-  using BVHDistanceTraversalNode<BV>::request;
-  using BVHDistanceTraversalNode<BV>::result;
-  using BVHDistanceTraversalNode<BV>::tf1;
-  using BVHDistanceTraversalNode<BV>::model1;
-  using BVHDistanceTraversalNode<BV>::model2;
-  using BVHDistanceTraversalNode<BV>::num_bv_tests;
-  using BVHDistanceTraversalNode<BV>::num_leaf_tests;
-
-  MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
-    vertices1 = NULL;
-    vertices2 = NULL;
-    tri_indices1 = NULL;
-    tri_indices2 = NULL;
-
-    rel_err = this->request.rel_err;
-    abs_err = this->request.abs_err;
-  }
-
-  void preprocess() {
-    if (!RTIsIdentity) preprocessOrientedNode();
-  }
-
-  void postprocess() {
-    if (!RTIsIdentity) postprocessOrientedNode();
-  }
-
-  /// @brief BV culling test in one BVTT node
-  FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
-    if (enable_statistics) num_bv_tests++;
-    if (RTIsIdentity)
-      return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
-          model1->getBV(b1), model2->getBV(b2));
-    else
-      return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
-          RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
-  }
-
-  /// @brief Distance testing between leaves (two triangles)
-  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
-    if (this->enable_statistics) this->num_leaf_tests++;
-
-    const BVNode<BV>& node1 = this->model1->getBV(b1);
-    const BVNode<BV>& node2 = this->model2->getBV(b2);
-
-    int primitive_id1 = node1.primitiveId();
-    int primitive_id2 = node2.primitiveId();
-
-    const Triangle& tri_id1 = tri_indices1[primitive_id1];
-    const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
-    const Vec3f& t11 = vertices1[tri_id1[0]];
-    const Vec3f& t12 = vertices1[tri_id1[1]];
-    const Vec3f& t13 = vertices1[tri_id1[2]];
-
-    const Vec3f& t21 = vertices2[tri_id2[0]];
-    const Vec3f& t22 = vertices2[tri_id2[1]];
-    const Vec3f& t23 = vertices2[tri_id2[2]];
-
-    // nearest point pair
-    Vec3f P1, P2, normal;
-
-    FCL_REAL d2;
-    if (RTIsIdentity)
-      d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
-                                            P2);
-    else
-      d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
-                                            RT._R(), RT._T(), P1, P2);
-    FCL_REAL d = sqrt(d2);
-
-    this->result->update(d, this->model1, this->model2, primitive_id1,
-                         primitive_id2, P1, P2, normal);
-  }
-
-  /// @brief Whether the traversal process can stop early
-  bool canStop(FCL_REAL c) const {
-    if ((c >= this->result->min_distance - abs_err) &&
-        (c * (1 + rel_err) >= this->result->min_distance))
-      return true;
-    return false;
-  }
-
-  Vec3f* vertices1;
-  Vec3f* vertices2;
-
-  Triangle* tri_indices1;
-  Triangle* tri_indices2;
-
-  /// @brief relative and absolute error, default value is 0.01 for both terms
-  FCL_REAL rel_err;
-  FCL_REAL abs_err;
-
-  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
-
- private:
-  void preprocessOrientedNode() {
-    const int init_tri_id1 = 0, init_tri_id2 = 0;
-    const Triangle& init_tri1 = tri_indices1[init_tri_id1];
-    const Triangle& init_tri2 = tri_indices2[init_tri_id2];
-
-    Vec3f init_tri1_points[3];
-    Vec3f init_tri2_points[3];
-
-    init_tri1_points[0] = vertices1[init_tri1[0]];
-    init_tri1_points[1] = vertices1[init_tri1[1]];
-    init_tri1_points[2] = vertices1[init_tri1[2]];
-
-    init_tri2_points[0] = vertices2[init_tri2[0]];
-    init_tri2_points[1] = vertices2[init_tri2[1]];
-    init_tri2_points[2] = vertices2[init_tri2[2]];
-
-    Vec3f p1, p2, normal;
-    FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance(
-        init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
-        init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
-        RT._T(), p1, p2));
-
-    result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
-                   normal);
-  }
-  void postprocessOrientedNode() {
-    /// the points obtained by triDistance are not in world space: both are in
-    /// object1's local coordinate system, so we need to convert them into the
-    /// world space.
-    if (request.enable_nearest_points && (result->o1 == model1) &&
-        (result->o2 == model2)) {
-      result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
-      result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
-    }
-  }
-};
-
-/// @brief Traversal node for distance computation between two meshes if their
-/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
-typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
-typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
-typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
-
-/// @}
-
-/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to
-/// be transformed
-namespace details {
-
-template <typename BV>
-inline const Matrix3f& getBVAxes(const BV& bv) {
-  return bv.axes;
-}
-
-template <>
-inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
-  return bv.obb.axes;
-}
-
-}  // namespace details
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal_node_bvhs.h>
diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h
index 8bd5617c6cb193d07d79245c9b2f640809dff45a..8384a2b94e96d745a7b07563bbcb34fdabcc7189 100644
--- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h
+++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h
@@ -1,758 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2021-2024, INRIA.
- *  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 HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H
-#define HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
-#include <hpp/fcl/internal/traversal_node_base.h>
-#include <hpp/fcl/internal/traversal.h>
-#include <hpp/fcl/internal/intersect.h>
-#include <hpp/fcl/hfield.h>
-#include <hpp/fcl/shape/convex.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @addtogroup Traversal_For_Collision
-/// @{
-
-namespace details {
-template <typename BV>
-Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
-                                               const HeightField<BV>& model) {
-  const MatrixXf& heights = model.getHeights();
-  const VecXf& x_grid = model.getXGrid();
-  const VecXf& y_grid = model.getYGrid();
-
-  const FCL_REAL min_height = model.getMinHeight();
-
-  const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
-                 y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
-  const Eigen::Block<const MatrixXf, 2, 2> cell =
-      heights.block<2, 2>(node.y_id, node.x_id);
-
-  assert(cell.maxCoeff() > min_height &&
-         "max_height is lower than min_height");  // Check whether the geometry
-                                                  // is degenerated
-
-  std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
-      Vec3f(x0, y0, min_height),
-      Vec3f(x0, y1, min_height),
-      Vec3f(x1, y1, min_height),
-      Vec3f(x1, y0, min_height),
-      Vec3f(x0, y0, cell(0, 0)),
-      Vec3f(x0, y1, cell(1, 0)),
-      Vec3f(x1, y1, cell(1, 1)),
-      Vec3f(x1, y0, cell(0, 1)),
-  }));
-
-  std::shared_ptr<std::vector<Quadrilateral>> polygons(
-      new std::vector<Quadrilateral>(6));
-  (*polygons)[0].set(0, 3, 2, 1);  // x+ side
-  (*polygons)[1].set(0, 1, 5, 4);  // y- side
-  (*polygons)[2].set(1, 2, 6, 5);  // x- side
-  (*polygons)[3].set(2, 3, 7, 6);  // y+ side
-  (*polygons)[4].set(3, 0, 4, 7);  // z- side
-  (*polygons)[5].set(4, 5, 6, 7);  // z+ side
-
-  return Convex<Quadrilateral>(pts,  // points
-                               8,    // num points
-                               polygons,
-                               6  // number of polygons
-  );
-}
-
-enum class FaceOrientationConvexPart1 {
-  BOTTOM = 0,
-  TOP = 1,
-  WEST = 2,
-  SOUTH_EAST = 4,
-  NORTH = 8,
-};
-
-enum class FaceOrientationConvexPart2 {
-  BOTTOM = 0,
-  TOP = 1,
-  SOUTH = 2,
-  NORTH_WEST = 4,
-  EAST = 8,
-};
-
-template <typename BV>
-void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
-                          Convex<Triangle>& convex1, int& convex1_active_faces,
-                          Convex<Triangle>& convex2,
-                          int& convex2_active_faces) {
-  const MatrixXf& heights = model.getHeights();
-  const VecXf& x_grid = model.getXGrid();
-  const VecXf& y_grid = model.getYGrid();
-
-  const FCL_REAL min_height = model.getMinHeight();
-
-  const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
-                 y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
-  const FCL_REAL max_height = node.max_height;
-  const Eigen::Block<const MatrixXf, 2, 2> cell =
-      heights.block<2, 2>(node.y_id, node.x_id);
-
-  const int contact_active_faces = node.contact_active_faces;
-  convex1_active_faces = 0;
-  convex2_active_faces = 0;
-
-  typedef HFNodeBase::FaceOrientation FaceOrientation;
-
-  if (contact_active_faces & FaceOrientation::TOP) {
-    convex1_active_faces |= int(details::FaceOrientationConvexPart1::TOP);
-    convex2_active_faces |= int(details::FaceOrientationConvexPart2::TOP);
-  }
-
-  if (contact_active_faces & FaceOrientation::BOTTOM) {
-    convex1_active_faces |= int(details::FaceOrientationConvexPart1::BOTTOM);
-    convex2_active_faces |= int(details::FaceOrientationConvexPart2::BOTTOM);
-  }
-
-  // Specific orientation for Convex1
-  if (contact_active_faces & FaceOrientation::WEST) {
-    convex1_active_faces |= int(details::FaceOrientationConvexPart1::WEST);
-  }
-
-  if (contact_active_faces & FaceOrientation::NORTH) {
-    convex1_active_faces |= int(details::FaceOrientationConvexPart1::NORTH);
-  }
-
-  // Specific orientation for Convex2
-  if (contact_active_faces & FaceOrientation::EAST) {
-    convex2_active_faces |= int(details::FaceOrientationConvexPart2::EAST);
-  }
-
-  if (contact_active_faces & FaceOrientation::SOUTH) {
-    convex2_active_faces |= int(details::FaceOrientationConvexPart2::SOUTH);
-  }
-
-  assert(max_height > min_height &&
-         "max_height is lower than min_height");  // Check whether the geometry
-                                                  // is degenerated
-  HPP_FCL_UNUSED_VARIABLE(max_height);
-
-  {
-    std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
-        Vec3f(x0, y0, min_height),  // A
-        Vec3f(x0, y1, min_height),  // B
-        Vec3f(x1, y0, min_height),  // C
-        Vec3f(x0, y0, cell(0, 0)),  // D
-        Vec3f(x0, y1, cell(1, 0)),  // E
-        Vec3f(x1, y0, cell(0, 1)),  // F
-    }));
-
-    std::shared_ptr<std::vector<Triangle>> triangles(
-        new std::vector<Triangle>(8));
-    (*triangles)[0].set(0, 2, 1);  // bottom
-    (*triangles)[1].set(3, 4, 5);  // top
-    (*triangles)[2].set(0, 1, 3);  // West 1
-    (*triangles)[3].set(3, 1, 4);  // West 2
-    (*triangles)[4].set(1, 2, 5);  // South-East 1
-    (*triangles)[5].set(1, 5, 4);  // South-East 1
-    (*triangles)[6].set(0, 5, 2);  // North 1
-    (*triangles)[7].set(5, 0, 3);  // North 2
-
-    convex1.set(pts,  // points
-                6,    // num points
-                triangles,
-                8  // number of polygons
-    );
-  }
-
-  {
-    std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
-        Vec3f(x0, y1, min_height),  // A
-        Vec3f(x1, y1, min_height),  // B
-        Vec3f(x1, y0, min_height),  // C
-        Vec3f(x0, y1, cell(1, 0)),  // D
-        Vec3f(x1, y1, cell(1, 1)),  // E
-        Vec3f(x1, y0, cell(0, 1)),  // F
-    }));
-
-    std::shared_ptr<std::vector<Triangle>> triangles(
-        new std::vector<Triangle>(8));
-    (*triangles)[0].set(2, 1, 0);  // bottom
-    (*triangles)[1].set(3, 4, 5);  // top
-    (*triangles)[2].set(0, 1, 3);  // South 1
-    (*triangles)[3].set(3, 1, 4);  // South 2
-    (*triangles)[4].set(0, 5, 2);  // North West 1
-    (*triangles)[5].set(0, 3, 5);  // North West 2
-    (*triangles)[6].set(1, 2, 5);  // East 1
-    (*triangles)[7].set(4, 1, 2);  // East 2
-
-    convex2.set(pts,  // points
-                6,    // num points
-                triangles,
-                8  // number of polygons
-    );
-  }
-}
-
-inline Vec3f projectTriangle(const Vec3f& pointA, const Vec3f& pointB,
-                             const Vec3f& pointC, const Vec3f& point) {
-  const Project::ProjectResult result =
-      Project::projectTriangle(pointA, pointB, pointC, point);
-  Vec3f res = result.parameterization[0] * pointA +
-              result.parameterization[1] * pointB +
-              result.parameterization[2] * pointC;
-
-  return res;
-}
-
-inline Vec3f projectTetrahedra(const Vec3f& pointA, const Vec3f& pointB,
-                               const Vec3f& pointC, const Vec3f& pointD,
-                               const Vec3f& point) {
-  const Project::ProjectResult result =
-      Project::projectTetrahedra(pointA, pointB, pointC, pointD, point);
-  Vec3f res = result.parameterization[0] * pointA +
-              result.parameterization[1] * pointB +
-              result.parameterization[2] * pointC +
-              result.parameterization[3] * pointD;
-
-  return res;
-}
-
-inline Vec3f computeTriangleNormal(const Triangle& triangle,
-                                   const std::vector<Vec3f>& points) {
-  const Vec3f pointA = points[triangle[0]];
-  const Vec3f pointB = points[triangle[1]];
-  const Vec3f pointC = points[triangle[2]];
-
-  const Vec3f normal = (pointB - pointA).cross(pointC - pointA).normalized();
-  assert(!normal.array().isNaN().any() && "normal is ill-defined");
-
-  return normal;
-}
-
-inline Vec3f projectPointOnTriangle(const Vec3f& contact_point,
-                                    const Triangle& triangle,
-                                    const std::vector<Vec3f>& points) {
-  const Vec3f pointA = points[triangle[0]];
-  const Vec3f pointB = points[triangle[1]];
-  const Vec3f pointC = points[triangle[2]];
-
-  const Vec3f contact_point_projected =
-      projectTriangle(pointA, pointB, pointC, contact_point);
-
-  return contact_point_projected;
-}
-
-inline FCL_REAL distanceContactPointToTriangle(
-    const Vec3f& contact_point, const Triangle& triangle,
-    const std::vector<Vec3f>& points) {
-  const Vec3f contact_point_projected =
-      projectPointOnTriangle(contact_point, triangle, points);
-  return (contact_point_projected - contact_point).norm();
-}
-
-inline FCL_REAL distanceContactPointToFace(const size_t face_id,
-                                           const Vec3f& contact_point,
-                                           const Convex<Triangle>& convex,
-                                           size_t& closest_face_id) {
-  assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]");
-
-  const std::vector<Vec3f>& points = *(convex.points);
-  if (face_id <= 1) {
-    const Triangle& triangle = (*(convex.polygons))[face_id];
-    closest_face_id = face_id;
-    return distanceContactPointToTriangle(contact_point, triangle, points);
-  } else {
-    const Triangle& triangle1 = (*(convex.polygons))[face_id];
-    const FCL_REAL distance_to_triangle1 =
-        distanceContactPointToTriangle(contact_point, triangle1, points);
-
-    const Triangle& triangle2 = (*(convex.polygons))[face_id + 1];
-    const FCL_REAL distance_to_triangle2 =
-        distanceContactPointToTriangle(contact_point, triangle2, points);
-
-    if (distance_to_triangle1 > distance_to_triangle2) {
-      closest_face_id = face_id + 1;
-      return distance_to_triangle2;
-    } else {
-      closest_face_id = face_id;
-      return distance_to_triangle1;
-    }
-  }
-}
-
-template <typename Polygone, typename Shape>
-bool binCorrection(const Convex<Polygone>& convex,
-                   const int convex_active_faces, const Shape& shape,
-                   const Transform3f& shape_pose, FCL_REAL& distance,
-                   Vec3f& contact_1, Vec3f& contact_2, Vec3f& normal,
-                   Vec3f& face_normal, const bool is_collision) {
-  const FCL_REAL prec = 1e-12;
-  const std::vector<Vec3f>& points = *(convex.points);
-
-  bool hfield_witness_is_on_bin_side = true;
-
-  //  int closest_face_id_bottom_face = -1;
-  //  int closest_face_id_top_face = -1;
-
-  std::vector<size_t> active_faces;
-  active_faces.reserve(5);
-  active_faces.push_back(0);
-  active_faces.push_back(1);
-
-  if (convex_active_faces & 2) active_faces.push_back(2);
-  if (convex_active_faces & 4) active_faces.push_back(4);
-  if (convex_active_faces & 8) active_faces.push_back(6);
-
-  Triangle face_triangle;
-  FCL_REAL shortest_distance_to_face = (std::numeric_limits<FCL_REAL>::max)();
-  face_normal = normal;
-  for (const size_t active_face : active_faces) {
-    size_t closest_face_id;
-    const FCL_REAL distance_to_face = distanceContactPointToFace(
-        active_face, contact_1, convex, closest_face_id);
-
-    const bool contact_point_is_on_face = distance_to_face <= prec;
-    if (contact_point_is_on_face) {
-      hfield_witness_is_on_bin_side = false;
-      face_triangle = (*(convex.polygons))[closest_face_id];
-      shortest_distance_to_face = distance_to_face;
-      break;
-    } else if (distance_to_face < shortest_distance_to_face) {
-      face_triangle = (*(convex.polygons))[closest_face_id];
-      shortest_distance_to_face = distance_to_face;
-    }
-  }
-
-  // We correct only if there is a collision with the bin
-  if (is_collision) {
-    if (!face_triangle.isValid())
-      HPP_FCL_THROW_PRETTY("face_triangle is not initialized",
-                           std::logic_error);
-
-    const Vec3f face_pointA = points[face_triangle[0]];
-    face_normal = computeTriangleNormal(face_triangle, points);
-
-    int hint = 0;
-    // Since we compute the support manually, we need to take into account the
-    // sphere swept radius of the shape.
-    // TODO: take into account the swept-sphere radius of the bin.
-    const Vec3f _support = getSupport<details::SupportOptions::WithSweptSphere>(
-        &shape, -shape_pose.rotation().transpose() * face_normal, hint);
-    const Vec3f support =
-        shape_pose.rotation() * _support + shape_pose.translation();
-
-    // Project support into the inclined bin having triangle
-    const FCL_REAL offset_plane = face_normal.dot(face_pointA);
-    const Plane projection_plane(face_normal, offset_plane);
-    const FCL_REAL distance_support_projection_plane =
-        projection_plane.signedDistance(support);
-
-    const Vec3f projected_support =
-        support - distance_support_projection_plane * face_normal;
-
-    // We need now to project the projected in the triangle shape
-    contact_1 =
-        projectPointOnTriangle(projected_support, face_triangle, points);
-    contact_2 = contact_1 + distance_support_projection_plane * face_normal;
-    normal = face_normal;
-    distance = -std::fabs(distance_support_projection_plane);
-  }
-
-  return hfield_witness_is_on_bin_side;
-}
-
-template <typename Polygone, typename Shape, int Options>
-bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
-                   const Convex<Polygone>& convex1,
-                   const int convex1_active_faces,
-                   const Convex<Polygone>& convex2,
-                   const int convex2_active_faces, const Transform3f& tf1,
-                   const Shape& shape, const Transform3f& tf2,
-                   FCL_REAL& distance, Vec3f& c1, Vec3f& c2, Vec3f& normal,
-                   Vec3f& normal_top, bool& hfield_witness_is_on_bin_side) {
-  enum { RTIsIdentity = Options & RelativeTransformationIsIdentity };
-
-  const Transform3f Id;
-  // The solver `nsolver` has already been set up by the collision request
-  // `request`. If GJK early stopping is enabled through `request`, it will be
-  // used.
-  // The only thing we need to make sure is that in case of collision, the
-  // penetration information is computed (as we do bins comparison).
-  const bool compute_penetration = true;
-  Vec3f contact1_1, contact1_2, contact2_1, contact2_2;
-  Vec3f normal1, normal1_top, normal2, normal2_top;
-  FCL_REAL distance1, distance2;
-
-  if (RTIsIdentity) {
-    distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
-        &convex1, Id, &shape, tf2, nsolver, compute_penetration, contact1_1,
-        contact1_2, normal1);
-  } else {
-    distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
-        &convex1, tf1, &shape, tf2, nsolver, compute_penetration, contact1_1,
-        contact1_2, normal1);
-  }
-  bool collision1 = (distance1 - request.security_margin <=
-                     request.collision_distance_threshold);
-
-  bool hfield_witness_is_on_bin_side1 =
-      binCorrection(convex1, convex1_active_faces, shape, tf2, distance1,
-                    contact1_1, contact1_2, normal1, normal1_top, collision1);
-
-  if (RTIsIdentity) {
-    distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
-        &convex2, Id, &shape, tf2, nsolver, compute_penetration, contact2_1,
-        contact2_2, normal2);
-  } else {
-    distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
-        &convex2, tf1, &shape, tf2, nsolver, compute_penetration, contact2_1,
-        contact2_2, normal2);
-  }
-  bool collision2 = (distance2 - request.security_margin <=
-                     request.collision_distance_threshold);
-
-  bool hfield_witness_is_on_bin_side2 =
-      binCorrection(convex2, convex2_active_faces, shape, tf2, distance2,
-                    contact2_1, contact2_2, normal2, normal2_top, collision2);
-
-  if (collision1 && collision2) {
-    if (distance1 > distance2)  // switch values
-    {
-      distance = distance2;
-      c1 = contact2_1;
-      c2 = contact2_2;
-      normal = normal2;
-      normal_top = normal2_top;
-      hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
-    } else {
-      distance = distance1;
-      c1 = contact1_1;
-      c2 = contact1_2;
-      normal = normal1;
-      normal_top = normal1_top;
-      hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
-    }
-    return true;
-  } else if (collision1) {
-    distance = distance1;
-    c1 = contact1_1;
-    c2 = contact1_2;
-    normal = normal1;
-    normal_top = normal1_top;
-    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
-    return true;
-  } else if (collision2) {
-    distance = distance2;
-    c1 = contact2_1;
-    c2 = contact2_2;
-    normal = normal2;
-    normal_top = normal2_top;
-    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
-    return true;
-  }
-
-  if (distance1 > distance2)  // switch values
-  {
-    distance = distance2;
-    c1 = contact2_1;
-    c2 = contact2_2;
-    normal = normal2;
-    normal_top = normal2_top;
-    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
-  } else {
-    distance = distance1;
-    c1 = contact1_1;
-    c2 = contact1_2;
-    normal = normal1;
-    normal_top = normal1_top;
-    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
-  }
-  return false;
-}
-
-}  // namespace details
-
-/// @brief Traversal node for collision between height field and shape
-template <typename BV, typename S,
-          int _Options = RelativeTransformationIsIdentity>
-class HeightFieldShapeCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  typedef CollisionTraversalNodeBase Base;
-  typedef Eigen::Array<FCL_REAL, 1, 2> Array2d;
-
-  enum {
-    Options = _Options,
-    RTIsIdentity = _Options & RelativeTransformationIsIdentity
-  };
-
-  HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    num_bv_tests = 0;
-    num_leaf_tests = 0;
-    query_time_seconds = 0.0;
-
-    nsolver = NULL;
-    count = 0;
-  }
-
-  /// @brief Whether the BV node in the first BVH tree is leaf
-  bool isFirstNodeLeaf(unsigned int b) const {
-    return model1->getBV(b).isLeaf();
-  }
-
-  /// @brief Obtain the left child of BV node in the first BVH
-  int getFirstLeftChild(unsigned int b) const {
-    return static_cast<int>(model1->getBV(b).leftChild());
-  }
-
-  /// @brief Obtain the right child of BV node in the first BVH
-  int getFirstRightChild(unsigned int b) const {
-    return static_cast<int>(model1->getBV(b).rightChild());
-  }
-
-  /// test between BV b1 and shape
-  /// @param b1 BV to test,
-  /// @retval sqrDistLowerBound square of a lower bound of the minimal
-  ///         distance between bounding volumes.
-  /// @brief BV culling test in one BVTT node
-  bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
-                   FCL_REAL& sqrDistLowerBound) const {
-    if (this->enable_statistics) this->num_bv_tests++;
-
-    bool disjoint;
-    if (RTIsIdentity) {
-      assert(false && "must never happened");
-      disjoint = !this->model1->getBV(b1).bv.overlap(
-          this->model2_bv, this->request, sqrDistLowerBound);
-    } else {
-      disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
-                          this->model1->getBV(b1).bv, this->model2_bv,
-                          this->request, sqrDistLowerBound);
-    }
-
-    if (disjoint)
-      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
-                                               sqrDistLowerBound);
-
-    assert(!disjoint || sqrDistLowerBound > 0);
-    return disjoint;
-  }
-
-  /// @brief Intersection testing between leaves (one Convex and one shape)
-  void leafCollides(unsigned int b1, unsigned int /*b2*/,
-                    FCL_REAL& sqrDistLowerBound) const {
-    count++;
-    if (this->enable_statistics) this->num_leaf_tests++;
-    const HFNode<BV>& node = this->model1->getBV(b1);
-
-    // Split quadrilateral primitives into two convex shapes corresponding to
-    // polyhedron with triangular bases. This is essential to keep the convexity
-
-    //    typedef Convex<Quadrilateral> ConvexQuadrilateral;
-    //    const ConvexQuadrilateral convex =
-    //    details::buildConvexQuadrilateral(node,*this->model1);
-
-    typedef Convex<Triangle> ConvexTriangle;
-    ConvexTriangle convex1, convex2;
-    int convex1_active_faces, convex2_active_faces;
-    // TODO: inherit from hfield's inflation here
-    details::buildConvexTriangles(node, *this->model1, convex1,
-                                  convex1_active_faces, convex2,
-                                  convex2_active_faces);
-
-    // Compute aabb_local for BoundingVolumeGuess case in the GJK solver
-    if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-      convex1.computeLocalAABB();
-      convex2.computeLocalAABB();
-    }
-
-    FCL_REAL distance;
-    //    Vec3f contact_point, normal;
-    Vec3f c1, c2, normal, normal_face;
-    bool hfield_witness_is_on_bin_side;
-
-    bool collision = details::shapeDistance<Triangle, S, Options>(
-        nsolver, this->request, convex1, convex1_active_faces, convex2,
-        convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance,
-        c1, c2, normal, normal_face, hfield_witness_is_on_bin_side);
-
-    FCL_REAL distToCollision = distance - this->request.security_margin;
-    if (distToCollision <= this->request.collision_distance_threshold) {
-      sqrDistLowerBound = 0;
-      if (this->result->numContacts() < this->request.num_max_contacts) {
-        if (normal_face.isApprox(normal) &&
-            (collision || !hfield_witness_is_on_bin_side)) {
-          this->result->addContact(Contact(this->model1, this->model2, (int)b1,
-                                           (int)Contact::NONE, c1, c2, normal,
-                                           distance));
-          assert(this->result->isCollision());
-        }
-      }
-    } else
-      sqrDistLowerBound = distToCollision * distToCollision;
-
-    //    const Vec3f c1 = contact_point - distance * 0.5 * normal;
-    //    const Vec3f c2 = contact_point + distance * 0.5 * normal;
-    internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
-                                               distToCollision, c1, c2, normal);
-
-    assert(this->result->isCollision() || sqrDistLowerBound > 0);
-  }
-
-  const GJKSolver* nsolver;
-
-  const HeightField<BV>* model1;
-  const S* model2;
-  BV model2_bv;
-
-  mutable int num_bv_tests;
-  mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
-  mutable int count;
-};
-
-/// @}
-
-/// @addtogroup Traversal_For_Distance
-/// @{
-
-/// @brief Traversal node for distance between height field and shape
-template <typename BV, typename S,
-          int _Options = RelativeTransformationIsIdentity>
-class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
- public:
-  typedef DistanceTraversalNodeBase Base;
-
-  enum {
-    Options = _Options,
-    RTIsIdentity = _Options & RelativeTransformationIsIdentity
-  };
-
-  HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
-    model1 = NULL;
-    model2 = NULL;
-
-    num_leaf_tests = 0;
-    query_time_seconds = 0.0;
-
-    rel_err = 0;
-    abs_err = 0;
-    nsolver = NULL;
-  }
-
-  /// @brief Whether the BV node in the first BVH tree is leaf
-  bool isFirstNodeLeaf(unsigned int b) const {
-    return model1->getBV(b).isLeaf();
-  }
-
-  /// @brief Obtain the left child of BV node in the first BVH
-  int getFirstLeftChild(unsigned int b) const {
-    return model1->getBV(b).leftChild();
-  }
-
-  /// @brief Obtain the right child of BV node in the first BVH
-  int getFirstRightChild(unsigned int b) const {
-    return model1->getBV(b).rightChild();
-  }
-
-  /// @brief BV culling test in one BVTT node
-  FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
-    return model1->getBV(b1).bv.distance(
-        model2_bv);  // TODO(jcarpent): tf1 is not taken into account here.
-  }
-
-  /// @brief Distance testing between leaves (one bin of the height field and
-  /// one shape)
-  /// TODO(louis): deal with Hfield-Shape distance just like in Hfield-Shape
-  /// collision (bin correction etc).
-  void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
-    if (this->enable_statistics) this->num_leaf_tests++;
-
-    const BVNode<BV>& node = this->model1->getBV(b1);
-
-    typedef Convex<Quadrilateral> ConvexQuadrilateral;
-    const ConvexQuadrilateral convex =
-        details::buildConvexQuadrilateral(node, *this->model1);
-
-    Vec3f p1, p2, normal;
-    const FCL_REAL distance =
-        internal::ShapeShapeDistance<ConvexQuadrilateral, S>(
-            &convex, this->tf1, this->model2, this->tf2, this->nsolver,
-            this->request.enable_signed_distance, p1, p2, normal);
-
-    this->result->update(distance, this->model1, this->model2, b1,
-                         DistanceResult::NONE, p1, p2, normal);
-  }
-
-  /// @brief Whether the traversal process can stop early
-  bool canStop(FCL_REAL c) const {
-    if ((c >= this->result->min_distance - abs_err) &&
-        (c * (1 + rel_err) >= this->result->min_distance))
-      return true;
-    return false;
-  }
-
-  FCL_REAL rel_err;
-  FCL_REAL abs_err;
-
-  const GJKSolver* nsolver;
-
-  const HeightField<BV>* model1;
-  const S* model2;
-  BV model2_bv;
-
-  mutable int num_bv_tests;
-  mutable int num_leaf_tests;
-  mutable FCL_REAL query_time_seconds;
-};
-
-/// @}
-
-}  // namespace fcl
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal_node_hfield_shape.h>
diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h
index 9a0094e1367f4141e35e96f9cb09003b9589ed53..44f8b32347e91f0e0813d264ca79962b000d96e5 100644
--- a/include/hpp/fcl/internal/traversal_node_octree.h
+++ b/include/hpp/fcl/internal/traversal_node_octree.h
@@ -1,1363 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2022-2023, INRIA
- *  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 HPP_FCL_TRAVERSAL_NODE_OCTREE_H
-#define HPP_FCL_TRAVERSAL_NODE_OCTREE_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/internal/traversal_node_base.h>
-#include <hpp/fcl/internal/traversal_node_hfield_shape.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/hfield.h>
-#include <hpp/fcl/octree.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Algorithms for collision related with octree
-class HPP_FCL_DLLAPI OcTreeSolver {
- private:
-  const GJKSolver* solver;
-
-  mutable const CollisionRequest* crequest;
-  mutable const DistanceRequest* drequest;
-
-  mutable CollisionResult* cresult;
-  mutable DistanceResult* dresult;
-
- public:
-  OcTreeSolver(const GJKSolver* solver_)
-      : solver(solver_),
-        crequest(NULL),
-        drequest(NULL),
-        cresult(NULL),
-        dresult(NULL) {}
-
-  /// @brief collision between two octrees
-  void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
-                       const Transform3f& tf1, const Transform3f& tf2,
-                       const CollisionRequest& request_,
-                       CollisionResult& result_) const {
-    crequest = &request_;
-    cresult = &result_;
-
-    OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
-                           tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
-  }
-
-  /// @brief distance between two octrees
-  void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
-                      const Transform3f& tf1, const Transform3f& tf2,
-                      const DistanceRequest& request_,
-                      DistanceResult& result_) const {
-    drequest = &request_;
-    dresult = &result_;
-
-    OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
-                          tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
-  }
-
-  /// @brief collision between octree and mesh
-  template <typename BV>
-  void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
-                           const Transform3f& tf1, const Transform3f& tf2,
-                           const CollisionRequest& request_,
-                           CollisionResult& result_) const {
-    crequest = &request_;
-    cresult = &result_;
-
-    OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
-                               tree2, 0, tf1, tf2);
-  }
-
-  /// @brief distance between octree and mesh
-  template <typename BV>
-  void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
-                          const Transform3f& tf1, const Transform3f& tf2,
-                          const DistanceRequest& request_,
-                          DistanceResult& result_) const {
-    drequest = &request_;
-    dresult = &result_;
-
-    OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
-                              tree2, 0, tf1, tf2);
-  }
-
-  /// @brief collision between mesh and octree
-  template <typename BV>
-  void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
-                           const Transform3f& tf1, const Transform3f& tf2,
-                           const CollisionRequest& request_,
-                           CollisionResult& result_) const
-
-  {
-    crequest = &request_;
-    cresult = &result_;
-
-    OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
-                               tree1, 0, tf2, tf1);
-  }
-
-  /// @brief distance between mesh and octree
-  template <typename BV>
-  void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
-                          const Transform3f& tf1, const Transform3f& tf2,
-                          const DistanceRequest& request_,
-                          DistanceResult& result_) const {
-    drequest = &request_;
-    dresult = &result_;
-
-    OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(),
-                              tree2->getRootBV(), tf1, tf2);
-  }
-
-  template <typename BV>
-  void OcTreeHeightFieldIntersect(
-      const OcTree* tree1, const HeightField<BV>* tree2, const Transform3f& tf1,
-      const Transform3f& tf2, const CollisionRequest& request_,
-      CollisionResult& result_, FCL_REAL& sqrDistLowerBound) const {
-    crequest = &request_;
-    cresult = &result_;
-
-    OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(),
-                                      tree1->getRootBV(), tree2, 0, tf1, tf2,
-                                      sqrDistLowerBound);
-  }
-
-  template <typename BV>
-  void HeightFieldOcTreeIntersect(const HeightField<BV>* tree1,
-                                  const OcTree* tree2, const Transform3f& tf1,
-                                  const Transform3f& tf2,
-                                  const CollisionRequest& request_,
-                                  CollisionResult& result_,
-                                  FCL_REAL& sqrDistLowerBound) const {
-    crequest = &request_;
-    cresult = &result_;
-
-    HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(),
-                                      tree2->getRootBV(), tf1, tf2,
-                                      sqrDistLowerBound);
-  }
-
-  /// @brief collision between octree and shape
-  template <typename S>
-  void OcTreeShapeIntersect(const OcTree* tree, const S& s,
-                            const Transform3f& tf1, const Transform3f& tf2,
-                            const CollisionRequest& request_,
-                            CollisionResult& result_) const {
-    crequest = &request_;
-    cresult = &result_;
-
-    AABB bv2;
-    computeBV<AABB>(s, Transform3f(), bv2);
-    OBB obb2;
-    convertBV(bv2, tf2, obb2);
-    OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
-                                obb2, tf1, tf2);
-  }
-
-  /// @brief collision between shape and octree
-  template <typename S>
-  void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
-                            const Transform3f& tf1, const Transform3f& tf2,
-                            const CollisionRequest& request_,
-                            CollisionResult& result_) const {
-    crequest = &request_;
-    cresult = &result_;
-
-    AABB bv1;
-    computeBV<AABB>(s, Transform3f(), bv1);
-    OBB obb1;
-    convertBV(bv1, tf1, obb1);
-    OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
-                                obb1, tf2, tf1);
-  }
-
-  /// @brief distance between octree and shape
-  template <typename S>
-  void OcTreeShapeDistance(const OcTree* tree, const S& s,
-                           const Transform3f& tf1, const Transform3f& tf2,
-                           const DistanceRequest& request_,
-                           DistanceResult& result_) const {
-    drequest = &request_;
-    dresult = &result_;
-
-    AABB aabb2;
-    computeBV<AABB>(s, tf2, aabb2);
-    OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
-                               aabb2, tf1, tf2);
-  }
-
-  /// @brief distance between shape and octree
-  template <typename S>
-  void ShapeOcTreeDistance(const S& s, const OcTree* tree,
-                           const Transform3f& tf1, const Transform3f& tf2,
-                           const DistanceRequest& request_,
-                           DistanceResult& result_) const {
-    drequest = &request_;
-    dresult = &result_;
-
-    AABB aabb1;
-    computeBV<AABB>(s, tf1, aabb1);
-    OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
-                               aabb1, tf2, tf1);
-  }
-
- private:
-  template <typename S>
-  bool OcTreeShapeDistanceRecurse(const OcTree* tree1,
-                                  const OcTree::OcTreeNode* root1,
-                                  const AABB& bv1, const S& s,
-                                  const AABB& aabb2, const Transform3f& tf1,
-                                  const Transform3f& tf2) const {
-    if (!tree1->nodeHasChildren(root1)) {
-      if (tree1->isNodeOccupied(root1)) {
-        Box box;
-        Transform3f box_tf;
-        constructBox(bv1, tf1, box, box_tf);
-
-        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-          box.computeLocalAABB();
-        }
-
-        Vec3f p1, p2, normal;
-        const FCL_REAL distance = internal::ShapeShapeDistance<Box, S>(
-            &box, box_tf, &s, tf2, this->solver,
-            this->drequest->enable_signed_distance, p1, p2, normal);
-
-        this->dresult->update(distance, tree1, &s,
-                              (int)(root1 - tree1->getRoot()),
-                              DistanceResult::NONE, p1, p2, normal);
-
-        return drequest->isSatisfied(*dresult);
-      } else
-        return false;
-    }
-
-    if (!tree1->isNodeOccupied(root1)) return false;
-
-    for (unsigned int i = 0; i < 8; ++i) {
-      if (tree1->nodeChildExists(root1, i)) {
-        const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
-        AABB child_bv;
-        computeChildBV(bv1, i, child_bv);
-
-        AABB aabb1;
-        convertBV(child_bv, tf1, aabb1);
-        FCL_REAL d = aabb1.distance(aabb2);
-        if (d < dresult->min_distance) {
-          if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1,
-                                         tf2))
-            return true;
-        }
-      }
-    }
-
-    return false;
-  }
-
-  template <typename S>
-  bool OcTreeShapeIntersectRecurse(const OcTree* tree1,
-                                   const OcTree::OcTreeNode* root1,
-                                   const AABB& bv1, const S& s, const OBB& obb2,
-                                   const Transform3f& tf1,
-                                   const Transform3f& tf2) const {
-    // Empty OcTree is considered free.
-    if (!root1) return false;
-
-    /// stop when 1) bounding boxes of two objects not overlap; OR
-    ///           2) at least of one the nodes is free; OR
-    ///           2) (two uncertain nodes or one node occupied and one node
-    ///           uncertain) AND cost not required
-    if (tree1->isNodeFree(root1))
-      return false;
-    else if ((tree1->isNodeUncertain(root1) || s.isUncertain()))
-      return false;
-    else {
-      OBB obb1;
-      convertBV(bv1, tf1, obb1);
-      FCL_REAL sqrDistLowerBound;
-      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
-        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
-                                                 sqrDistLowerBound);
-        return false;
-      }
-    }
-
-    if (!tree1->nodeHasChildren(root1)) {
-      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
-
-      Box box;
-      Transform3f box_tf;
-      constructBox(bv1, tf1, box, box_tf);
-      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-        box.computeLocalAABB();
-      }
-
-      bool contactNotAdded =
-          (cresult->numContacts() >= crequest->num_max_contacts);
-      std::size_t ncontact = ShapeShapeCollide<Box, S>(
-          &box, box_tf, &s, tf2, solver, *crequest, *cresult);
-      assert(ncontact == 0 || ncontact == 1);
-      if (!contactNotAdded && ncontact == 1) {
-        // Update contact information.
-        const Contact& c = cresult->getContact(cresult->numContacts() - 1);
-        cresult->setContact(
-            cresult->numContacts() - 1,
-            Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()),
-                    c.b2, c.pos, c.normal, c.penetration_depth));
-      }
-
-      // no need to call `internal::updateDistanceLowerBoundFromLeaf` here
-      // as it is already done internally in `ShapeShapeCollide` above.
-      return crequest->isSatisfied(*cresult);
-    }
-
-    for (unsigned int i = 0; i < 8; ++i) {
-      if (tree1->nodeChildExists(root1, i)) {
-        const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
-        AABB child_bv;
-        computeChildBV(bv1, i, child_bv);
-
-        if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1,
-                                        tf2))
-          return true;
-      }
-    }
-
-    return false;
-  }
-
-  template <typename BV>
-  bool OcTreeMeshDistanceRecurse(const OcTree* tree1,
-                                 const OcTree::OcTreeNode* root1,
-                                 const AABB& bv1, const BVHModel<BV>* tree2,
-                                 unsigned int root2, const Transform3f& tf1,
-                                 const Transform3f& tf2) const {
-    if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) {
-      if (tree1->isNodeOccupied(root1)) {
-        Box box;
-        Transform3f box_tf;
-        constructBox(bv1, tf1, box, box_tf);
-
-        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-          box.computeLocalAABB();
-        }
-
-        size_t primitive_id =
-            static_cast<size_t>(tree2->getBV(root2).primitiveId());
-        const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
-        const TriangleP tri((*(tree2->vertices))[tri_id[0]],
-                            (*(tree2->vertices))[tri_id[1]],
-                            (*(tree2->vertices))[tri_id[2]]);
-
-        Vec3f p1, p2, normal;
-        const FCL_REAL distance = internal::ShapeShapeDistance<Box, TriangleP>(
-            &box, box_tf, &tri, tf2, this->solver,
-            this->drequest->enable_signed_distance, p1, p2, normal);
-
-        this->dresult->update(distance, tree1, tree2,
-                              (int)(root1 - tree1->getRoot()),
-                              static_cast<int>(primitive_id), p1, p2, normal);
-
-        return this->drequest->isSatisfied(*dresult);
-      } else
-        return false;
-    }
-
-    if (!tree1->isNodeOccupied(root1)) return false;
-
-    if (tree2->getBV(root2).isLeaf() ||
-        (tree1->nodeHasChildren(root1) &&
-         (bv1.size() > tree2->getBV(root2).bv.size()))) {
-      for (unsigned int i = 0; i < 8; ++i) {
-        if (tree1->nodeChildExists(root1, i)) {
-          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
-          AABB child_bv;
-          computeChildBV(bv1, i, child_bv);
-
-          FCL_REAL d;
-          AABB aabb1, aabb2;
-          convertBV(child_bv, tf1, aabb1);
-          convertBV(tree2->getBV(root2).bv, tf2, aabb2);
-          d = aabb1.distance(aabb2);
-
-          if (d < dresult->min_distance) {
-            if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
-                                          tf1, tf2))
-              return true;
-          }
-        }
-      }
-    } else {
-      FCL_REAL d;
-      AABB aabb1, aabb2;
-      convertBV(bv1, tf1, aabb1);
-      unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
-      convertBV(tree2->getBV(child).bv, tf2, aabb2);
-      d = aabb1.distance(aabb2);
-
-      if (d < dresult->min_distance) {
-        if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
-                                      tf2))
-          return true;
-      }
-
-      child = (unsigned int)tree2->getBV(root2).rightChild();
-      convertBV(tree2->getBV(child).bv, tf2, aabb2);
-      d = aabb1.distance(aabb2);
-
-      if (d < dresult->min_distance) {
-        if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
-                                      tf2))
-          return true;
-      }
-    }
-
-    return false;
-  }
-
-  /// \return True if the request is satisfied.
-  template <typename BV>
-  bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
-                                  const OcTree::OcTreeNode* root1,
-                                  const AABB& bv1, const BVHModel<BV>* tree2,
-                                  unsigned int root2, const Transform3f& tf1,
-                                  const Transform3f& tf2) const {
-    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
-    // code in this if(!root1) did not output anything so the empty OcTree is
-    // considered free. Should an empty OcTree be considered free ?
-
-    // Empty OcTree is considered free.
-    if (!root1) return false;
-    BVNode<BV> const& bvn2 = tree2->getBV(root2);
-
-    /// stop when 1) bounding boxes of two objects not overlap; OR
-    ///           2) at least one of the nodes is free; OR
-    ///           2) (two uncertain nodes OR one node occupied and one node
-    ///           uncertain) AND cost not required
-    if (tree1->isNodeFree(root1))
-      return false;
-    else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
-      return false;
-    else {
-      OBB obb1, obb2;
-      convertBV(bv1, tf1, obb1);
-      convertBV(bvn2.bv, tf2, obb2);
-      FCL_REAL sqrDistLowerBound;
-      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
-        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
-                                                 sqrDistLowerBound);
-        return false;
-      }
-    }
-
-    // Check if leaf collides.
-    if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
-      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
-      Box box;
-      Transform3f box_tf;
-      constructBox(bv1, tf1, box, box_tf);
-      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-        box.computeLocalAABB();
-      }
-
-      size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
-      const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
-      const TriangleP tri((*(tree2->vertices))[tri_id[0]],
-                          (*(tree2->vertices))[tri_id[1]],
-                          (*(tree2->vertices))[tri_id[2]]);
-
-      // When reaching this point, `this->solver` has already been set up
-      // by the CollisionRequest `this->crequest`.
-      // The only thing we need to (and can) pass to `ShapeShapeDistance` is
-      // whether or not penetration information is should be computed in case of
-      // collision.
-      const bool compute_penetration = this->crequest->enable_contact ||
-                                       (this->crequest->security_margin < 0);
-      Vec3f c1, c2, normal;
-      const FCL_REAL distance = internal::ShapeShapeDistance<Box, TriangleP>(
-          &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2,
-          normal);
-      const FCL_REAL distToCollision =
-          distance - this->crequest->security_margin;
-
-      internal::updateDistanceLowerBoundFromLeaf(
-          *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
-
-      if (cresult->numContacts() < crequest->num_max_contacts) {
-        if (distToCollision <= crequest->collision_distance_threshold) {
-          cresult->addContact(Contact(
-              tree1, tree2, (int)(root1 - tree1->getRoot()),
-              static_cast<int>(primitive_id), c1, c2, normal, distance));
-        }
-      }
-      return crequest->isSatisfied(*cresult);
-    }
-
-    // Determine which tree to traverse first.
-    if (bvn2.isLeaf() ||
-        (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
-      for (unsigned int i = 0; i < 8; ++i) {
-        if (tree1->nodeChildExists(root1, i)) {
-          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
-          AABB child_bv;
-          computeChildBV(bv1, i, child_bv);
-
-          if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2,
-                                         tf1, tf2))
-            return true;
-        }
-      }
-    } else {
-      if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
-                                     (unsigned int)bvn2.leftChild(), tf1, tf2))
-        return true;
-
-      if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
-                                     (unsigned int)bvn2.rightChild(), tf1, tf2))
-        return true;
-    }
-
-    return false;
-  }
-
-  /// \return True if the request is satisfied.
-  template <typename BV>
-  bool OcTreeHeightFieldIntersectRecurse(
-      const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
-      const HeightField<BV>* tree2, unsigned int root2, const Transform3f& tf1,
-      const Transform3f& tf2, FCL_REAL& sqrDistLowerBound) const {
-    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
-    // code in this if(!root1) did not output anything so the empty OcTree is
-    // considered free. Should an empty OcTree be considered free ?
-
-    // Empty OcTree is considered free.
-    if (!root1) return false;
-    HFNode<BV> const& bvn2 = tree2->getBV(root2);
-
-    /// stop when 1) bounding boxes of two objects not overlap; OR
-    ///           2) at least one of the nodes is free; OR
-    ///           2) (two uncertain nodes OR one node occupied and one node
-    ///           uncertain) AND cost not required
-    if (tree1->isNodeFree(root1))
-      return false;
-    else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
-      return false;
-    else {
-      OBB obb1, obb2;
-      convertBV(bv1, tf1, obb1);
-      convertBV(bvn2.bv, tf2, obb2);
-      FCL_REAL sqrDistLowerBound_;
-      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) {
-        if (sqrDistLowerBound_ < sqrDistLowerBound)
-          sqrDistLowerBound = sqrDistLowerBound_;
-        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
-                                                 sqrDistLowerBound);
-        return false;
-      }
-    }
-
-    // Check if leaf collides.
-    if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
-      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
-      Box box;
-      Transform3f box_tf;
-      constructBox(bv1, tf1, box, box_tf);
-      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-        box.computeLocalAABB();
-      }
-
-      typedef Convex<Triangle> ConvexTriangle;
-      ConvexTriangle convex1, convex2;
-      int convex1_active_faces, convex2_active_faces;
-      details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces,
-                                    convex2, convex2_active_faces);
-      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-        convex1.computeLocalAABB();
-        convex2.computeLocalAABB();
-      }
-
-      Vec3f c1, c2, normal, normal_top;
-      FCL_REAL distance;
-      bool hfield_witness_is_on_bin_side;
-
-      bool collision = details::shapeDistance<Triangle, Box, 0>(
-          solver, *crequest, convex1, convex1_active_faces, convex2,
-          convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal,
-          normal_top, hfield_witness_is_on_bin_side);
-
-      FCL_REAL distToCollision =
-          distance - crequest->security_margin * (normal_top.dot(normal));
-
-      if (distToCollision <= crequest->collision_distance_threshold) {
-        sqrDistLowerBound = 0;
-        if (crequest->num_max_contacts > cresult->numContacts()) {
-          if (normal_top.isApprox(normal) &&
-              (collision || !hfield_witness_is_on_bin_side)) {
-            cresult->addContact(
-                Contact(tree1, tree2, (int)(root1 - tree1->getRoot()),
-                        (int)Contact::NONE, c1, c2, -normal, distance));
-          }
-        }
-      } else
-        sqrDistLowerBound = distToCollision * distToCollision;
-
-      //    const Vec3f c1 = contact_point - distance * 0.5 * normal;
-      //    const Vec3f c2 = contact_point + distance * 0.5 * normal;
-      internal::updateDistanceLowerBoundFromLeaf(
-          *crequest, *cresult, distToCollision, c1, c2, -normal);
-
-      assert(cresult->isCollision() || sqrDistLowerBound > 0);
-      return crequest->isSatisfied(*cresult);
-    }
-
-    // Determine which tree to traverse first.
-    if (bvn2.isLeaf() ||
-        (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
-      for (unsigned int i = 0; i < 8; ++i) {
-        if (tree1->nodeChildExists(root1, i)) {
-          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
-          AABB child_bv;
-          computeChildBV(bv1, i, child_bv);
-
-          if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2,
-                                                root2, tf1, tf2,
-                                                sqrDistLowerBound))
-            return true;
-        }
-      }
-    } else {
-      if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
-                                            (unsigned int)bvn2.leftChild(), tf1,
-                                            tf2, sqrDistLowerBound))
-        return true;
-
-      if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
-                                            (unsigned int)bvn2.rightChild(),
-                                            tf1, tf2, sqrDistLowerBound))
-        return true;
-    }
-
-    return false;
-  }
-
-  /// \return True if the request is satisfied.
-  template <typename BV>
-  bool HeightFieldOcTreeIntersectRecurse(
-      const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2,
-      const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1,
-      const Transform3f& tf2, FCL_REAL& sqrDistLowerBound) const {
-    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
-    // code in this if(!root1) did not output anything so the empty OcTree is
-    // considered free. Should an empty OcTree be considered free ?
-
-    // Empty OcTree is considered free.
-    if (!root2) return false;
-    HFNode<BV> const& bvn1 = tree1->getBV(root1);
-
-    /// stop when 1) bounding boxes of two objects not overlap; OR
-    ///           2) at least one of the nodes is free; OR
-    ///           2) (two uncertain nodes OR one node occupied and one node
-    ///           uncertain) AND cost not required
-    if (tree2->isNodeFree(root2))
-      return false;
-    else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain()))
-      return false;
-    else {
-      OBB obb1, obb2;
-      convertBV(bvn1.bv, tf1, obb1);
-      convertBV(bv2, tf2, obb2);
-      FCL_REAL sqrDistLowerBound_;
-      if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) {
-        if (sqrDistLowerBound_ < sqrDistLowerBound)
-          sqrDistLowerBound = sqrDistLowerBound_;
-        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
-                                                 sqrDistLowerBound);
-        return false;
-      }
-    }
-
-    // Check if leaf collides.
-    if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) {
-      assert(tree2->isNodeOccupied(root2));  // it isn't free nor uncertain.
-      Box box;
-      Transform3f box_tf;
-      constructBox(bv2, tf2, box, box_tf);
-      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-        box.computeLocalAABB();
-      }
-
-      typedef Convex<Triangle> ConvexTriangle;
-      ConvexTriangle convex1, convex2;
-      int convex1_active_faces, convex2_active_faces;
-      details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces,
-                                    convex2, convex2_active_faces);
-      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-        convex1.computeLocalAABB();
-        convex2.computeLocalAABB();
-      }
-
-      Vec3f c1, c2, normal, normal_top;
-      FCL_REAL distance;
-      bool hfield_witness_is_on_bin_side;
-
-      bool collision = details::shapeDistance<Triangle, Box, 0>(
-          solver, *crequest, convex1, convex1_active_faces, convex2,
-          convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal,
-          normal_top, hfield_witness_is_on_bin_side);
-
-      FCL_REAL distToCollision =
-          distance - crequest->security_margin * (normal_top.dot(normal));
-
-      if (distToCollision <= crequest->collision_distance_threshold) {
-        sqrDistLowerBound = 0;
-        if (crequest->num_max_contacts > cresult->numContacts()) {
-          if (normal_top.isApprox(normal) &&
-              (collision || !hfield_witness_is_on_bin_side)) {
-            cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE,
-                                        (int)(root2 - tree2->getRoot()), c1, c2,
-                                        normal, distance));
-          }
-        }
-      } else
-        sqrDistLowerBound = distToCollision * distToCollision;
-
-      //    const Vec3f c1 = contact_point - distance * 0.5 * normal;
-      //    const Vec3f c2 = contact_point + distance * 0.5 * normal;
-      internal::updateDistanceLowerBoundFromLeaf(
-          *crequest, *cresult, distToCollision, c1, c2, normal);
-
-      assert(cresult->isCollision() || sqrDistLowerBound > 0);
-      return crequest->isSatisfied(*cresult);
-    }
-
-    // Determine which tree to traverse first.
-    if (bvn1.isLeaf() ||
-        (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) {
-      for (unsigned int i = 0; i < 8; ++i) {
-        if (tree2->nodeChildExists(root2, i)) {
-          const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
-          AABB child_bv;
-          computeChildBV(bv2, i, child_bv);
-
-          if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child,
-                                                child_bv, tf1, tf2,
-                                                sqrDistLowerBound))
-            return true;
-        }
-      }
-    } else {
-      if (HeightFieldOcTreeIntersectRecurse(
-              tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1,
-              tf2, sqrDistLowerBound))
-        return true;
-
-      if (HeightFieldOcTreeIntersectRecurse(
-              tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1,
-              tf2, sqrDistLowerBound))
-        return true;
-    }
-
-    return false;
-  }
-
-  bool OcTreeDistanceRecurse(const OcTree* tree1,
-                             const OcTree::OcTreeNode* root1, const AABB& bv1,
-                             const OcTree* tree2,
-                             const OcTree::OcTreeNode* root2, const AABB& bv2,
-                             const Transform3f& tf1,
-                             const Transform3f& tf2) const {
-    if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
-      if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
-        Box box1, box2;
-        Transform3f box1_tf, box2_tf;
-        constructBox(bv1, tf1, box1, box1_tf);
-        constructBox(bv2, tf2, box2, box2_tf);
-
-        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
-          box1.computeLocalAABB();
-          box2.computeLocalAABB();
-        }
-
-        Vec3f p1, p2, normal;
-        const FCL_REAL distance = internal::ShapeShapeDistance<Box, Box>(
-            &box1, box1_tf, &box2, box2_tf, this->solver,
-            this->drequest->enable_signed_distance, p1, p2, normal);
-
-        this->dresult->update(distance, tree1, tree2,
-                              (int)(root1 - tree1->getRoot()),
-                              (int)(root2 - tree2->getRoot()), p1, p2, normal);
-
-        return drequest->isSatisfied(*dresult);
-      } else
-        return false;
-    }
-
-    if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
-      return false;
-
-    if (!tree2->nodeHasChildren(root2) ||
-        (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
-      for (unsigned int i = 0; i < 8; ++i) {
-        if (tree1->nodeChildExists(root1, i)) {
-          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
-          AABB child_bv;
-          computeChildBV(bv1, i, child_bv);
-
-          FCL_REAL d;
-          AABB aabb1, aabb2;
-          convertBV(bv1, tf1, aabb1);
-          convertBV(bv2, tf2, aabb2);
-          d = aabb1.distance(aabb2);
-
-          if (d < dresult->min_distance) {
-            if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
-                                      tf1, tf2))
-              return true;
-          }
-        }
-      }
-    } else {
-      for (unsigned int i = 0; i < 8; ++i) {
-        if (tree2->nodeChildExists(root2, i)) {
-          const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
-          AABB child_bv;
-          computeChildBV(bv2, i, child_bv);
-
-          FCL_REAL d;
-          AABB aabb1, aabb2;
-          convertBV(bv1, tf1, aabb1);
-          convertBV(bv2, tf2, aabb2);
-          d = aabb1.distance(aabb2);
-
-          if (d < dresult->min_distance) {
-            if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
-                                      tf1, tf2))
-              return true;
-          }
-        }
-      }
-    }
-
-    return false;
-  }
-
-  bool OcTreeIntersectRecurse(const OcTree* tree1,
-                              const OcTree::OcTreeNode* root1, const AABB& bv1,
-                              const OcTree* tree2,
-                              const OcTree::OcTreeNode* root2, const AABB& bv2,
-                              const Transform3f& tf1,
-                              const Transform3f& tf2) const {
-    // Empty OcTree is considered free.
-    if (!root1) return false;
-    if (!root2) return false;
-
-    /// stop when 1) bounding boxes of two objects not overlap; OR
-    ///           2) at least one of the nodes is free; OR
-    ///           2) (two uncertain nodes OR one node occupied and one node
-    ///           uncertain) AND cost not required
-    if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
-      return false;
-    else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
-      return false;
-
-    bool bothAreLeaves =
-        (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
-    if (!bothAreLeaves || !crequest->enable_contact) {
-      OBB obb1, obb2;
-      convertBV(bv1, tf1, obb1);
-      convertBV(bv2, tf2, obb2);
-      FCL_REAL sqrDistLowerBound;
-      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
-        if (cresult->distance_lower_bound > 0 &&
-            sqrDistLowerBound <
-                cresult->distance_lower_bound * cresult->distance_lower_bound)
-          cresult->distance_lower_bound =
-              sqrt(sqrDistLowerBound) - crequest->security_margin;
-        return false;
-      }
-      if (crequest->enable_contact) {  // Overlap
-        if (cresult->numContacts() < crequest->num_max_contacts)
-          cresult->addContact(
-              Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
-                      static_cast<int>(root2 - tree2->getRoot())));
-        return crequest->isSatisfied(*cresult);
-      }
-    }
-
-    // Both node are leaves
-    if (bothAreLeaves) {
-      assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2));
-
-      Box box1, box2;
-      Transform3f box1_tf, box2_tf;
-      constructBox(bv1, tf1, box1, box1_tf);
-      constructBox(bv2, tf2, box2, box2_tf);
-
-      if (this->solver->gjk_initial_guess ==
-          GJKInitialGuess::BoundingVolumeGuess) {
-        box1.computeLocalAABB();
-        box2.computeLocalAABB();
-      }
-
-      // When reaching this point, `this->solver` has already been set up
-      // by the CollisionRequest `this->request`.
-      // The only thing we need to (and can) pass to `ShapeShapeDistance` is
-      // whether or not penetration information is should be computed in case of
-      // collision.
-      const bool compute_penetration = (this->crequest->enable_contact ||
-                                        (this->crequest->security_margin < 0));
-      Vec3f c1, c2, normal;
-      FCL_REAL distance = internal::ShapeShapeDistance<Box, Box>(
-          &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1,
-          c2, normal);
-
-      const FCL_REAL distToCollision =
-          distance - this->crequest->security_margin;
-
-      internal::updateDistanceLowerBoundFromLeaf(
-          *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
-
-      if (this->cresult->numContacts() < this->crequest->num_max_contacts) {
-        if (distToCollision <= this->crequest->collision_distance_threshold)
-          this->cresult->addContact(
-              Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
-                      static_cast<int>(root2 - tree2->getRoot()), c1, c2,
-                      normal, distance));
-      }
-
-      return crequest->isSatisfied(*cresult);
-    }
-
-    // Determine which tree to traverse first.
-    if (!tree2->nodeHasChildren(root2) ||
-        (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
-      for (unsigned int i = 0; i < 8; ++i) {
-        if (tree1->nodeChildExists(root1, i)) {
-          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
-          AABB child_bv;
-          computeChildBV(bv1, i, child_bv);
-
-          if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
-                                     tf1, tf2))
-            return true;
-        }
-      }
-    } else {
-      for (unsigned int i = 0; i < 8; ++i) {
-        if (tree2->nodeChildExists(root2, i)) {
-          const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
-          AABB child_bv;
-          computeChildBV(bv2, i, child_bv);
-
-          if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
-                                     tf1, tf2))
-            return true;
-        }
-      }
-    }
-
-    return false;
-  }
-};
-
-/// @addtogroup Traversal_For_Collision
-/// @{
-
-/// @brief Traversal node for octree collision
-class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  OcTreeCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; }
-
-  void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const {
-    otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
-    sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
-    sqrDistLowerBound *= sqrDistLowerBound;
-  }
-
-  const OcTree* model1;
-  const OcTree* model2;
-
-  Transform3f tf1, tf2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for shape-octree collision
-template <typename S>
-class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
-    return false;
-  }
-
-  void leafCollides(unsigned int, unsigned int,
-                    FCL_REAL& sqrDistLowerBound) const {
-    otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
-    sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
-    sqrDistLowerBound *= sqrDistLowerBound;
-  }
-
-  const S* model1;
-  const OcTree* model2;
-
-  Transform3f tf1, tf2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for octree-shape collision
-
-template <typename S>
-class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const {
-    return false;
-  }
-
-  void leafCollides(unsigned int, unsigned int,
-                    FCL_REAL& sqrDistLowerBound) const {
-    otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
-    sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
-    sqrDistLowerBound *= sqrDistLowerBound;
-  }
-
-  const OcTree* model1;
-  const S* model2;
-
-  Transform3f tf1, tf2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for mesh-octree collision
-template <typename BV>
-class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
-    return false;
-  }
-
-  void leafCollides(unsigned int, unsigned int,
-                    FCL_REAL& sqrDistLowerBound) const {
-    otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
-    sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
-    sqrDistLowerBound *= sqrDistLowerBound;
-  }
-
-  const BVHModel<BV>* model1;
-  const OcTree* model2;
-
-  Transform3f tf1, tf2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for octree-mesh collision
-template <typename BV>
-class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
-    return false;
-  }
-
-  void leafCollides(unsigned int, unsigned int,
-                    FCL_REAL& sqrDistLowerBound) const {
-    otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
-    sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
-    sqrDistLowerBound *= sqrDistLowerBound;
-  }
-
-  const OcTree* model1;
-  const BVHModel<BV>* model2;
-
-  Transform3f tf1, tf2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for octree-height-field collision
-template <typename BV>
-class HPP_FCL_DLLAPI OcTreeHeightFieldCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
-    return false;
-  }
-
-  void leafCollides(unsigned int, unsigned int,
-                    FCL_REAL& sqrDistLowerBound) const {
-    otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request,
-                                         *result, sqrDistLowerBound);
-  }
-
-  const OcTree* model1;
-  const HeightField<BV>* model2;
-
-  Transform3f tf1, tf2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for octree-height-field collision
-template <typename BV>
-class HPP_FCL_DLLAPI HeightFieldOcTreeCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
-    return false;
-  }
-
-  void leafCollides(unsigned int, unsigned int,
-                    FCL_REAL& sqrDistLowerBound) const {
-    otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request,
-                                         *result, sqrDistLowerBound);
-  }
-
-  const HeightField<BV>* model1;
-  const OcTree* model2;
-
-  Transform3f tf1, tf2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @}
-
-/// @addtogroup Traversal_For_Distance
-/// @{
-
-/// @brief Traversal node for octree distance
-class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode
-    : public DistanceTraversalNodeBase {
- public:
-  OcTreeDistanceTraversalNode() {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  FCL_REAL BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
-
-  bool BVDistanceLowerBound(unsigned, unsigned, FCL_REAL&) const {
-    return false;
-  }
-
-  void leafComputeDistance(unsigned, unsigned int) const {
-    otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
-  }
-
-  const OcTree* model1;
-  const OcTree* model2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for shape-octree distance
-template <typename Shape>
-class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode
-    : public DistanceTraversalNodeBase {
- public:
-  ShapeOcTreeDistanceTraversalNode() {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
-
-  void leafComputeDistance(unsigned int, unsigned int) const {
-    otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
-  }
-
-  const Shape* model1;
-  const OcTree* model2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for octree-shape distance
-template <typename Shape>
-class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode
-    : public DistanceTraversalNodeBase {
- public:
-  OcTreeShapeDistanceTraversalNode() {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
-
-  void leafComputeDistance(unsigned int, unsigned int) const {
-    otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
-  }
-
-  const OcTree* model1;
-  const Shape* model2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for mesh-octree distance
-template <typename BV>
-class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode
-    : public DistanceTraversalNodeBase {
- public:
-  MeshOcTreeDistanceTraversalNode() {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
-
-  void leafComputeDistance(unsigned int, unsigned int) const {
-    otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
-  }
-
-  const BVHModel<BV>* model1;
-  const OcTree* model2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @brief Traversal node for octree-mesh distance
-template <typename BV>
-class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode
-    : public DistanceTraversalNodeBase {
- public:
-  OcTreeMeshDistanceTraversalNode() {
-    model1 = NULL;
-    model2 = NULL;
-
-    otsolver = NULL;
-  }
-
-  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
-
-  void leafComputeDistance(unsigned int, unsigned int) const {
-    otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
-  }
-
-  const OcTree* model1;
-  const BVHModel<BV>* model2;
-
-  const OcTreeSolver* otsolver;
-};
-
-/// @}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal_node_octree.h>
diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h
index 20b5c71db5b590a9ac4e0b8cd060f754150ec254..b94dd7a845e9576de0aed1ded6e2f285c45ffaa4 100644
--- a/include/hpp/fcl/internal/traversal_node_setup.h
+++ b/include/hpp/fcl/internal/traversal_node_setup.h
@@ -1,817 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_TRAVERSAL_NODE_SETUP_H
-#define HPP_FCL_TRAVERSAL_NODE_SETUP_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/internal/tools.h>
-#include <hpp/fcl/internal/traversal_node_shapes.h>
-
-#include <hpp/fcl/internal/traversal_node_bvhs.h>
-#include <hpp/fcl/internal/traversal_node_bvh_shape.h>
-
-// #include <hpp/fcl/internal/traversal_node_hfields.h>
-#include <hpp/fcl/internal/traversal_node_hfield_shape.h>
-
-#ifdef HPP_FCL_HAS_OCTOMAP
-#include <hpp/fcl/internal/traversal_node_octree.h>
-#endif
-
-#include <hpp/fcl/BVH/BVH_utility.h>
-
-namespace hpp {
-namespace fcl {
-
-#ifdef HPP_FCL_HAS_OCTOMAP
-/// @brief Initialize traversal node for collision between two octrees, given
-/// current object transform
-inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1,
-                       const Transform3f& tf1, const OcTree& model2,
-                       const Transform3f& tf2, const OcTreeSolver* otsolver,
-                       CollisionResult& result) {
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for distance between two octrees, given
-/// current object transform
-inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1,
-                       const Transform3f& tf1, const OcTree& model2,
-                       const Transform3f& tf2, const OcTreeSolver* otsolver,
-                       const DistanceRequest& request, DistanceResult& result)
-
-{
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one shape and one
-/// octree, given current object transform
-template <typename S>
-bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node, const S& model1,
-                const Transform3f& tf1, const OcTree& model2,
-                const Transform3f& tf2, const OcTreeSolver* otsolver,
-                CollisionResult& result) {
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one octree and one
-/// shape, given current object transform
-template <typename S>
-bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
-                const OcTree& model1, const Transform3f& tf1, const S& model2,
-                const Transform3f& tf2, const OcTreeSolver* otsolver,
-                CollisionResult& result) {
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for distance between one shape and one
-/// octree, given current object transform
-template <typename S>
-bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node, const S& model1,
-                const Transform3f& tf1, const OcTree& model2,
-                const Transform3f& tf2, const OcTreeSolver* otsolver,
-                const DistanceRequest& request, DistanceResult& result) {
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for distance between one octree and one
-/// shape, given current object transform
-template <typename S>
-bool initialize(OcTreeShapeDistanceTraversalNode<S>& node, const OcTree& model1,
-                const Transform3f& tf1, const S& model2, const Transform3f& tf2,
-                const OcTreeSolver* otsolver, const DistanceRequest& request,
-                DistanceResult& result) {
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one mesh and one
-/// octree, given current object transform
-template <typename BV>
-bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
-                const BVHModel<BV>& model1, const Transform3f& tf1,
-                const OcTree& model2, const Transform3f& tf2,
-                const OcTreeSolver* otsolver, CollisionResult& result) {
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one octree and one
-/// mesh, given current object transform
-template <typename BV>
-bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
-                const OcTree& model1, const Transform3f& tf1,
-                const BVHModel<BV>& model2, const Transform3f& tf2,
-                const OcTreeSolver* otsolver, CollisionResult& result) {
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one octree and one
-/// height field, given current object transform
-template <typename BV>
-bool initialize(OcTreeHeightFieldCollisionTraversalNode<BV>& node,
-                const OcTree& model1, const Transform3f& tf1,
-                const HeightField<BV>& model2, const Transform3f& tf2,
-                const OcTreeSolver* otsolver, CollisionResult& result) {
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one height field and
-/// one octree, given current object transform
-template <typename BV>
-bool initialize(HeightFieldOcTreeCollisionTraversalNode<BV>& node,
-                const HeightField<BV>& model1, const Transform3f& tf1,
-                const OcTree& model2, const Transform3f& tf2,
-                const OcTreeSolver* otsolver, CollisionResult& result) {
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for distance between one mesh and one
-/// octree, given current object transform
-template <typename BV>
-bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
-                const BVHModel<BV>& model1, const Transform3f& tf1,
-                const OcTree& model2, const Transform3f& tf2,
-                const OcTreeSolver* otsolver, const DistanceRequest& request,
-                DistanceResult& result) {
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one octree and one
-/// mesh, given current object transform
-template <typename BV>
-bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node, const OcTree& model1,
-                const Transform3f& tf1, const BVHModel<BV>& model2,
-                const Transform3f& tf2, const OcTreeSolver* otsolver,
-                const DistanceRequest& request, DistanceResult& result) {
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.model2 = &model2;
-
-  node.otsolver = otsolver;
-
-  node.tf1 = tf1;
-  node.tf2 = tf2;
-
-  return true;
-}
-
-#endif
-
-/// @brief Initialize traversal node for collision between two geometric shapes,
-/// given current object transform
-template <typename S1, typename S2>
-bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1,
-                const Transform3f& tf1, const S2& shape2,
-                const Transform3f& tf2, const GJKSolver* nsolver,
-                CollisionResult& result) {
-  node.model1 = &shape1;
-  node.tf1 = tf1;
-  node.model2 = &shape2;
-  node.tf2 = tf2;
-  node.nsolver = nsolver;
-
-  node.result = &result;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one mesh and one
-/// shape, given current object transform
-template <typename BV, typename S>
-bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
-                BVHModel<BV>& model1, Transform3f& tf1, const S& model2,
-                const Transform3f& tf2, const GJKSolver* nsolver,
-                CollisionResult& result, bool use_refit = false,
-                bool refit_bottomup = false) {
-  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  if (!tf1.isIdentity() &&
-      model1.vertices.get())  // TODO(jcarpent): vectorized version
-  {
-    std::vector<Vec3f> vertices_transformed(model1.num_vertices);
-    const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
-    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
-      const Vec3f& p = model1_vertices_[i];
-      Vec3f new_v = tf1.transform(p);
-      vertices_transformed[i] = new_v;
-    }
-
-    model1.beginReplaceModel();
-    model1.replaceSubModel(vertices_transformed);
-    model1.endReplaceModel(use_refit, refit_bottomup);
-
-    tf1.setIdentity();
-  }
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-  node.nsolver = nsolver;
-
-  computeBV(model2, tf2, node.model2_bv);
-
-  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
-  node.tri_indices =
-      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
-
-  node.result = &result;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one mesh and one
-/// shape
-template <typename BV, typename S>
-bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
-                const BVHModel<BV>& model1, const Transform3f& tf1,
-                const S& model2, const Transform3f& tf2,
-                const GJKSolver* nsolver, CollisionResult& result) {
-  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-  node.nsolver = nsolver;
-
-  computeBV(model2, tf2, node.model2_bv);
-
-  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
-  node.tri_indices =
-      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
-
-  node.result = &result;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for collision between one mesh and one
-/// shape, given current object transform
-template <typename BV, typename S>
-bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
-                HeightField<BV>& model1, Transform3f& tf1, const S& model2,
-                const Transform3f& tf2, const GJKSolver* nsolver,
-                CollisionResult& result, bool use_refit = false,
-                bool refit_bottomup = false);
-
-/// @brief Initialize traversal node for collision between one mesh and one
-/// shape
-template <typename BV, typename S>
-bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
-                const HeightField<BV>& model1, const Transform3f& tf1,
-                const S& model2, const Transform3f& tf2,
-                const GJKSolver* nsolver, CollisionResult& result) {
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-  node.nsolver = nsolver;
-
-  computeBV(model2, tf2, node.model2_bv);
-
-  node.result = &result;
-
-  return true;
-}
-
-/// @cond IGNORE
-namespace details {
-template <typename S, typename BV, template <typename> class OrientedNode>
-static inline bool setupShapeMeshCollisionOrientedNode(
-    OrientedNode<S>& node, const S& model1, const Transform3f& tf1,
-    const BVHModel<BV>& model2, const Transform3f& tf2,
-    const GJKSolver* nsolver, CollisionResult& result) {
-  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-  node.nsolver = nsolver;
-
-  computeBV(model1, tf1, node.model1_bv);
-
-  node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
-  node.tri_indices =
-      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
-
-  node.result = &result;
-
-  return true;
-}
-}  // namespace details
-/// @endcond
-
-/// @brief Initialize traversal node for collision between two meshes, given the
-/// current transforms
-template <typename BV>
-bool initialize(
-    MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
-    BVHModel<BV>& model1, Transform3f& tf1, BVHModel<BV>& model2,
-    Transform3f& tf2, CollisionResult& result, bool use_refit = false,
-    bool refit_bottomup = false) {
-  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  if (!tf1.isIdentity() && model1.vertices.get()) {
-    std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
-    const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
-    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
-      const Vec3f& p = model1_vertices_[i];
-      Vec3f new_v = tf1.transform(p);
-      vertices_transformed1[i] = new_v;
-    }
-
-    model1.beginReplaceModel();
-    model1.replaceSubModel(vertices_transformed1);
-    model1.endReplaceModel(use_refit, refit_bottomup);
-
-    tf1.setIdentity();
-  }
-
-  if (!tf2.isIdentity() && model2.vertices.get()) {
-    std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
-    const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
-    for (unsigned int i = 0; i < model2.num_vertices; ++i) {
-      const Vec3f& p = model2_vertices_[i];
-      Vec3f new_v = tf2.transform(p);
-      vertices_transformed2[i] = new_v;
-    }
-
-    model2.beginReplaceModel();
-    model2.replaceSubModel(vertices_transformed2);
-    model2.endReplaceModel(use_refit, refit_bottomup);
-
-    tf2.setIdentity();
-  }
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-
-  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
-  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
-
-  node.tri_indices1 =
-      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
-  node.tri_indices2 =
-      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
-
-  node.result = &result;
-
-  return true;
-}
-
-template <typename BV>
-bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
-                const BVHModel<BV>& model1, const Transform3f& tf1,
-                const BVHModel<BV>& model2, const Transform3f& tf2,
-                CollisionResult& result) {
-  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  node.vertices1 = model1.vertices ? model1.vertices->data() : NULL;
-  node.vertices2 = model2.vertices ? model2.vertices->data() : NULL;
-
-  node.tri_indices1 =
-      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
-  node.tri_indices2 =
-      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-
-  node.result = &result;
-
-  node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
-  node.RT.T.noalias() = tf1.getRotation().transpose() *
-                        (tf2.getTranslation() - tf1.getTranslation());
-
-  return true;
-}
-
-/// @brief Initialize traversal node for distance between two geometric shapes
-template <typename S1, typename S2>
-bool initialize(ShapeDistanceTraversalNode<S1, S2>& node, const S1& shape1,
-                const Transform3f& tf1, const S2& shape2,
-                const Transform3f& tf2, const GJKSolver* nsolver,
-                const DistanceRequest& request, DistanceResult& result) {
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &shape1;
-  node.tf1 = tf1;
-  node.model2 = &shape2;
-  node.tf2 = tf2;
-  node.nsolver = nsolver;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for distance computation between two
-/// meshes, given the current transforms
-template <typename BV>
-bool initialize(
-    MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
-    BVHModel<BV>& model1, Transform3f& tf1, BVHModel<BV>& model2,
-    Transform3f& tf2, const DistanceRequest& request, DistanceResult& result,
-    bool use_refit = false, bool refit_bottomup = false) {
-  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  if (!tf1.isIdentity() && model1.vertices.get()) {
-    std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
-    const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
-    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
-      const Vec3f& p = model1_vertices_[i];
-      Vec3f new_v = tf1.transform(p);
-      vertices_transformed1[i] = new_v;
-    }
-
-    model1.beginReplaceModel();
-    model1.replaceSubModel(vertices_transformed1);
-    model1.endReplaceModel(use_refit, refit_bottomup);
-
-    tf1.setIdentity();
-  }
-
-  if (!tf2.isIdentity() && model2.vertices.get()) {
-    std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
-    const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
-    for (unsigned int i = 0; i < model2.num_vertices; ++i) {
-      const Vec3f& p = model2_vertices_[i];
-      Vec3f new_v = tf2.transform(p);
-      vertices_transformed2[i] = new_v;
-    }
-
-    model2.beginReplaceModel();
-    model2.replaceSubModel(vertices_transformed2);
-    model2.endReplaceModel(use_refit, refit_bottomup);
-
-    tf2.setIdentity();
-  }
-
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-
-  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
-  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
-
-  node.tri_indices1 =
-      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
-  node.tri_indices2 =
-      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
-
-  return true;
-}
-
-/// @brief Initialize traversal node for distance computation between two meshes
-template <typename BV>
-bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
-                const BVHModel<BV>& model1, const Transform3f& tf1,
-                const BVHModel<BV>& model2, const Transform3f& tf2,
-                const DistanceRequest& request, DistanceResult& result) {
-  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-
-  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
-  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
-
-  node.tri_indices1 =
-      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
-  node.tri_indices2 =
-      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
-
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
-
-  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
-                    tf2.getTranslation(), node.RT.R, node.RT.T);
-
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-
-  return true;
-}
-
-/// @brief Initialize traversal node for distance computation between one mesh
-/// and one shape, given the current transforms
-template <typename BV, typename S>
-bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
-                BVHModel<BV>& model1, Transform3f& tf1, const S& model2,
-                const Transform3f& tf2, const GJKSolver* nsolver,
-                const DistanceRequest& request, DistanceResult& result,
-                bool use_refit = false, bool refit_bottomup = false) {
-  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  if (!tf1.isIdentity() && model1.vertices.get()) {
-    const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
-    std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
-    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
-      const Vec3f& p = model1_vertices_[i];
-      Vec3f new_v = tf1.transform(p);
-      vertices_transformed1[i] = new_v;
-    }
-
-    model1.beginReplaceModel();
-    model1.replaceSubModel(vertices_transformed1);
-    model1.endReplaceModel(use_refit, refit_bottomup);
-
-    tf1.setIdentity();
-  }
-
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-  node.nsolver = nsolver;
-
-  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
-  node.tri_indices =
-      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
-
-  computeBV(model2, tf2, node.model2_bv);
-
-  return true;
-}
-
-/// @cond IGNORE
-namespace details {
-
-template <typename BV, typename S, template <typename> class OrientedNode>
-static inline bool setupMeshShapeDistanceOrientedNode(
-    OrientedNode<S>& node, const BVHModel<BV>& model1, const Transform3f& tf1,
-    const S& model2, const Transform3f& tf2, const GJKSolver* nsolver,
-    const DistanceRequest& request, DistanceResult& result) {
-  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
-    HPP_FCL_THROW_PRETTY(
-        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
-        std::invalid_argument)
-
-  node.request = request;
-  node.result = &result;
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-  node.nsolver = nsolver;
-
-  computeBV(model2, tf2, node.model2_bv);
-
-  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
-  node.tri_indices =
-      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
-
-  return true;
-}
-}  // namespace details
-/// @endcond
-
-/// @brief Initialize traversal node for distance computation between one mesh
-/// and one shape, specialized for RSS type
-template <typename S>
-bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& node,
-                const BVHModel<RSS>& model1, const Transform3f& tf1,
-                const S& model2, const Transform3f& tf2,
-                const GJKSolver* nsolver, const DistanceRequest& request,
-                DistanceResult& result) {
-  return details::setupMeshShapeDistanceOrientedNode(
-      node, model1, tf1, model2, tf2, nsolver, request, result);
-}
-
-/// @brief Initialize traversal node for distance computation between one mesh
-/// and one shape, specialized for kIOS type
-template <typename S>
-bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& node,
-                const BVHModel<kIOS>& model1, const Transform3f& tf1,
-                const S& model2, const Transform3f& tf2,
-                const GJKSolver* nsolver, const DistanceRequest& request,
-                DistanceResult& result) {
-  return details::setupMeshShapeDistanceOrientedNode(
-      node, model1, tf1, model2, tf2, nsolver, request, result);
-}
-
-/// @brief Initialize traversal node for distance computation between one mesh
-/// and one shape, specialized for OBBRSS type
-template <typename S>
-bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& node,
-                const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
-                const S& model2, const Transform3f& tf2,
-                const GJKSolver* nsolver, const DistanceRequest& request,
-                DistanceResult& result) {
-  return details::setupMeshShapeDistanceOrientedNode(
-      node, model1, tf1, model2, tf2, nsolver, request, result);
-}
-
-}  // namespace fcl
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal_node_setup.h>
diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/hpp/fcl/internal/traversal_node_shapes.h
index e30db3cb62adb035427c7050105178593ef695d9..996730308a9c96737c6ccaea19879046df710f5a 100644
--- a/include/hpp/fcl/internal/traversal_node_shapes.h
+++ b/include/hpp/fcl/internal/traversal_node_shapes.h
@@ -1,127 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_TRAVERSAL_NODE_SHAPES_H
-#define HPP_FCL_TRAVERSAL_NODE_SHAPES_H
-
-/// @cond INTERNAL
-
-#include "hpp/fcl/collision_data.h"
-#include "hpp/fcl/BV/BV.h"
-#include "hpp/fcl/shape/geometric_shapes_utility.h"
-#include "hpp/fcl/internal/traversal_node_base.h"
-#include "hpp/fcl/internal/shape_shape_func.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @addtogroup Traversal_For_Collision
-/// @{
-
-/// @brief Traversal node for collision between two shapes
-template <typename S1, typename S2>
-class HPP_FCL_DLLAPI ShapeCollisionTraversalNode
-    : public CollisionTraversalNodeBase {
- public:
-  ShapeCollisionTraversalNode(const CollisionRequest& request)
-      : CollisionTraversalNodeBase(request) {
-    model1 = NULL;
-    model2 = NULL;
-
-    nsolver = NULL;
-  }
-
-  /// @brief BV culling test in one BVTT node
-  bool BVDisjoints(int, int, FCL_REAL&) const {
-    HPP_FCL_THROW_PRETTY("Not implemented", std::runtime_error);
-  }
-
-  /// @brief Intersection testing between leaves (two shapes)
-  void leafCollides(int, int, FCL_REAL&) const {
-    ShapeShapeCollide<S1, S2>(this->model1, this->tf1, this->model2, this->tf2,
-                              this->nsolver, this->request, *(this->result));
-  }
-
-  const S1* model1;
-  const S2* model2;
-
-  const GJKSolver* nsolver;
-};
-
-/// @}
-
-/// @addtogroup Traversal_For_Distance
-/// @{
-
-/// @brief Traversal node for distance between two shapes
-template <typename S1, typename S2>
-class HPP_FCL_DLLAPI ShapeDistanceTraversalNode
-    : public DistanceTraversalNodeBase {
- public:
-  ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
-    model1 = NULL;
-    model2 = NULL;
-
-    nsolver = NULL;
-  }
-
-  /// @brief BV culling test in one BVTT node
-  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const {
-    return -1;  // should not be used
-  }
-
-  /// @brief Distance testing between leaves (two shapes)
-  void leafComputeDistance(unsigned int, unsigned int) const {
-    ShapeShapeDistance<S1, S2>(this->model1, this->tf1, this->model2, this->tf2,
-                               this->nsolver, this->request, *(this->result));
-  }
-
-  const S1* model1;
-  const S2* model2;
-
-  const GJKSolver* nsolver;
-};
-
-/// @}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal_node_shapes.h>
diff --git a/include/hpp/fcl/internal/traversal_recurse.h b/include/hpp/fcl/internal/traversal_recurse.h
index 1f15fe4f2e985dc98d85bfd9205101ba9956783c..6fd2bf7ae4196230c0d7ecdfa44ff2596e963517 100644
--- a/include/hpp/fcl/internal/traversal_recurse.h
+++ b/include/hpp/fcl/internal/traversal_recurse.h
@@ -1,83 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_TRAVERSAL_RECURSE_H
-#define HPP_FCL_TRAVERSAL_RECURSE_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/BVH/BVH_front.h>
-#include <queue>
-#include <hpp/fcl/internal/traversal_node_base.h>
-#include <hpp/fcl/internal/traversal_node_bvhs.h>
-
-namespace hpp {
-namespace fcl {
-
-/// Recurse function for collision
-/// @param node collision node,
-/// @param b1, b2 ids of bounding volume nodes for object 1 and object 2
-/// @retval sqrDistLowerBound squared lower bound on distance between objects.
-void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1,
-                      unsigned int b2, BVHFrontList* front_list,
-                      FCL_REAL& sqrDistLowerBound);
-
-void collisionNonRecurse(CollisionTraversalNodeBase* node,
-                         BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound);
-
-/// @brief Recurse function for distance
-void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
-                     unsigned int b2, BVHFrontList* front_list);
-
-/// @brief Recurse function for distance, using queue acceleration
-void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
-                          unsigned int b2, BVHFrontList* front_list,
-                          unsigned int qsize);
-
-/// @brief Recurse function for front list propagation
-void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
-                                           const CollisionRequest& request,
-                                           CollisionResult& result,
-                                           BVHFrontList* front_list);
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-/// @endcond
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/internal/traversal_recurse.h>
diff --git a/include/hpp/fcl/logging.h b/include/hpp/fcl/logging.h
index c65624b3dde8b8e037fc80942be73675b5a700c2..e9f591ebcd28fda9ffd8c40b48d9013010ddb598 100644
--- a/include/hpp/fcl/logging.h
+++ b/include/hpp/fcl/logging.h
@@ -1,54 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2024, INRIA
- *  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.
- */
-
-/// This file defines basic logging macros for HPP-FCL, based on Boost.Log.
-/// To enable logging, define the preprocessor macro `HPP_FCL_ENABLE_LOGGING`.
-
-#ifndef HPP_FCL_LOGGING_H
-#define HPP_FCL_LOGGING_H
-
-#ifdef HPP_FCL_ENABLE_LOGGING
-#include <boost/log/trivial.hpp>
-#define HPP_FCL_LOG_INFO(message) BOOST_LOG_TRIVIAL(info) << message
-#define HPP_FCL_LOG_DEBUG(message) BOOST_LOG_TRIVIAL(debug) << message
-#define HPP_FCL_LOG_WARNING(message) BOOST_LOG_TRIVIAL(warning) << message
-#define HPP_FCL_LOG_ERROR(message) BOOST_LOG_TRIVIAL(error) << message
-#else
-#define HPP_FCL_LOG_INFO(message)
-#define HPP_FCL_LOG_DEBUG(message)
-#define HPP_FCL_LOG_WARNING(message)
-#define HPP_FCL_LOG_ERROR(message)
-#endif
-
-#endif  // HPP_FCL_LOGGING_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/logging.h>
diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h
index 9c1bdb0b5b59265da25ded3a8aa91d70591299b0..0b4e853e93de4de1daaf17f8339ee0c1e3f38c46 100644
--- a/include/hpp/fcl/math/matrix_3f.h
+++ b/include/hpp/fcl/math/matrix_3f.h
@@ -1,46 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_MATRIX_3F_H
-#define HPP_FCL_MATRIX_3F_H
-
-#warning "This file is deprecated. Include <hpp/fcl/data_types.h> instead."
-
-// List of equivalent includes.
-#include <hpp/fcl/data_types.h>
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/math/matrix_3f.h>
diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h
index 9a1d31e5d11ecacd0c626d69b5174d672997ca59..970a2a9362939e36eddfd1157fe7f4b5f9017c80 100644
--- a/include/hpp/fcl/math/transform.h
+++ b/include/hpp/fcl/math/transform.h
@@ -1,272 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_TRANSFORM_H
-#define HPP_FCL_TRANSFORM_H
-
-#include "hpp/fcl/fwd.hh"
-#include "hpp/fcl/data_types.h"
-
-namespace hpp {
-namespace fcl {
-
-HPP_FCL_DEPRECATED typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
-typedef Eigen::Quaternion<FCL_REAL> Quatf;
-
-static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) {
-  o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
-  return o;
-}
-
-/// @brief Simple transform class used locally by InterpMotion
-class HPP_FCL_DLLAPI Transform3f {
-  /// @brief Matrix cache
-  Matrix3f R;
-
-  /// @brief Translation vector
-  Vec3f T;
-
- public:
-  /// @brief Default transform is no movement
-  Transform3f() {
-    setIdentity();  // set matrix_set true
-  }
-
-  static Transform3f Identity() { return Transform3f(); }
-
-  /// @brief Construct transform from rotation and translation
-  template <typename Matrixx3Like, typename Vector3Like>
-  Transform3f(const Eigen::MatrixBase<Matrixx3Like>& R_,
-              const Eigen::MatrixBase<Vector3Like>& T_)
-      : R(R_), T(T_) {}
-
-  /// @brief Construct transform from rotation and translation
-  template <typename Vector3Like>
-  Transform3f(const Quatf& q_, const Eigen::MatrixBase<Vector3Like>& T_)
-      : R(q_.toRotationMatrix()), T(T_) {}
-
-  /// @brief Construct transform from rotation
-  Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {}
-
-  /// @brief Construct transform from rotation
-  Transform3f(const Quatf& q_) : R(q_), T(Vec3f::Zero()) {}
-
-  /// @brief Construct transform from translation
-  Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {}
-
-  /// @brief Construct transform from other transform
-  Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {}
-
-  /// @brief operator =
-  Transform3f& operator=(const Transform3f& tf) {
-    R = tf.R;
-    T = tf.T;
-    return *this;
-  }
-
-  /// @brief get translation
-  inline const Vec3f& getTranslation() const { return T; }
-
-  /// @brief get translation
-  inline const Vec3f& translation() const { return T; }
-
-  /// @brief get translation
-  inline Vec3f& translation() { return T; }
-
-  /// @brief get rotation
-  inline const Matrix3f& getRotation() const { return R; }
-
-  /// @brief get rotation
-  inline const Matrix3f& rotation() const { return R; }
-
-  /// @brief get rotation
-  inline Matrix3f& rotation() { return R; }
-
-  /// @brief get quaternion
-  inline Quatf getQuatRotation() const { return Quatf(R); }
-
-  /// @brief set transform from rotation and translation
-  template <typename Matrix3Like, typename Vector3Like>
-  inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
-                           const Eigen::MatrixBase<Vector3Like>& T_) {
-    R.noalias() = R_;
-    T.noalias() = T_;
-  }
-
-  /// @brief set transform from rotation and translation
-  inline void setTransform(const Quatf& q_, const Vec3f& T_) {
-    R = q_.toRotationMatrix();
-    T = T_;
-  }
-
-  /// @brief set transform from rotation
-  template <typename Derived>
-  inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
-    R.noalias() = R_;
-  }
-
-  /// @brief set transform from translation
-  template <typename Derived>
-  inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
-    T.noalias() = T_;
-  }
-
-  /// @brief set transform from rotation
-  inline void setQuatRotation(const Quatf& q_) { R = q_.toRotationMatrix(); }
-
-  /// @brief transform a given vector by the transform
-  template <typename Derived>
-  inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const {
-    return R * v + T;
-  }
-
-  /// @brief transform a given vector by the inverse of the transform
-  template <typename Derived>
-  inline Vec3f inverseTransform(const Eigen::MatrixBase<Derived>& v) const {
-    return R.transpose() * (v - T);
-  }
-
-  /// @brief inverse transform
-  inline Transform3f& inverseInPlace() {
-    R.transposeInPlace();
-    T = -R * T;
-    return *this;
-  }
-
-  /// @brief inverse transform
-  inline Transform3f inverse() {
-    return Transform3f(R.transpose(), -R.transpose() * T);
-  }
-
-  /// @brief inverse the transform and multiply with another
-  inline Transform3f inverseTimes(const Transform3f& other) const {
-    return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
-  }
-
-  /// @brief multiply with another transform
-  inline const Transform3f& operator*=(const Transform3f& other) {
-    T += R * other.T;
-    R *= other.R;
-    return *this;
-  }
-
-  /// @brief multiply with another transform
-  inline Transform3f operator*(const Transform3f& other) const {
-    return Transform3f(R * other.R, R * other.T + T);
-  }
-
-  /// @brief check whether the transform is identity
-  inline bool isIdentity(
-      const FCL_REAL& prec =
-          Eigen::NumTraits<FCL_REAL>::dummy_precision()) const {
-    return R.isIdentity(prec) && T.isZero(prec);
-  }
-
-  /// @brief set the transform to be identity transform
-  inline void setIdentity() {
-    R.setIdentity();
-    T.setZero();
-  }
-
-  /// @brief return a random transform
-  static Transform3f Random() { return Transform3f().setRandom(); }
-
-  /// @brief set the transform to a random transform
-  Transform3f& setRandom();
-
-  bool operator==(const Transform3f& other) const {
-    return (R == other.getRotation()) && (T == other.getTranslation());
-  }
-
-  bool operator!=(const Transform3f& other) const { return !(*this == other); }
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-template <typename Derived>
-inline Quatf fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
-                           FCL_REAL angle) {
-  return Quatf(Eigen::AngleAxis<FCL_REAL>(angle, axis));
-}
-
-/// @brief Uniformly random quaternion sphere.
-/// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio).
-inline Quatf uniformRandomQuaternion() {
-  // Rotational part
-  const FCL_REAL u1 = (FCL_REAL)rand() / RAND_MAX;
-  const FCL_REAL u2 = (FCL_REAL)rand() / RAND_MAX;
-  const FCL_REAL u3 = (FCL_REAL)rand() / RAND_MAX;
-
-  const FCL_REAL mult1 = std::sqrt(FCL_REAL(1.0) - u1);
-  const FCL_REAL mult2 = std::sqrt(u1);
-
-  static const FCL_REAL PI_value = static_cast<FCL_REAL>(EIGEN_PI);
-  FCL_REAL s2 = std::sin(2 * PI_value * u2);
-  FCL_REAL c2 = std::cos(2 * PI_value * u2);
-  FCL_REAL s3 = std::sin(2 * PI_value * u3);
-  FCL_REAL c3 = std::cos(2 * PI_value * u3);
-
-  Quatf q;
-  q.w() = mult1 * s2;
-  q.x() = mult1 * c2;
-  q.y() = mult2 * s3;
-  q.z() = mult2 * c3;
-  return q;
-}
-
-inline Transform3f& Transform3f::setRandom() {
-  const Quatf q = uniformRandomQuaternion();
-  this->rotation() = q.matrix();
-  this->translation().setRandom();
-
-  return *this;
-}
-
-/// @brief Construct othonormal basis from vector.
-/// The z-axis is the normalized input vector.
-inline Matrix3f constructOrthonormalBasisFromVector(const Vec3f& vec) {
-  Matrix3f basis = Matrix3f::Zero();
-  basis.col(2) = vec.normalized();
-  basis.col(1) = -vec.unitOrthogonal();
-  basis.col(0) = basis.col(1).cross(vec);
-  return basis;
-}
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/math/transform.h>
diff --git a/include/hpp/fcl/math/types.h b/include/hpp/fcl/math/types.h
index ece985db8a8fd650eb50def532e25682ddc38839..932a7e59000e5070f14ae0debf520b23b7e89fca 100644
--- a/include/hpp/fcl/math/types.h
+++ b/include/hpp/fcl/math/types.h
@@ -1,46 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 Joseph Mirabel */
-
-#ifndef HPP_FCL_MATH_TYPES_H
-#define HPP_FCL_MATH_TYPES_H
-
-#warning "This file is deprecated. Include <hpp/fcl/data_types.h> instead."
-
-// List of equivalent includes.
-#include <hpp/fcl/data_types.h>
-
-#endif
\ No newline at end of file
+#include <hpp/fcl/coal.hpp>
+#include <coal/math/types.h>
diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h
index 4389bba75eacb5eb04179427729ba42081772e14..e6ac46c7acba16fdef73bcb2bf20c0a8a839436c 100644
--- a/include/hpp/fcl/math/vec_3f.h
+++ b/include/hpp/fcl/math/vec_3f.h
@@ -1,46 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_VEC_3F_H
-#define HPP_FCL_VEC_3F_H
-
-#warning "This file is deprecated. Include <hpp/fcl/data_types.h> instead."
-
-// List of equivalent includes.
-#include <hpp/fcl/data_types.h>
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/math/vec_3f.h>
diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/hpp/fcl/mesh_loader/assimp.h
index 115933740591f44c45943b88532109186b032753..139538928150598736f4a114a49ca43945ceacd3 100644
--- a/include/hpp/fcl/mesh_loader/assimp.h
+++ b/include/hpp/fcl/mesh_loader/assimp.h
@@ -1,130 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2016-2019, CNRS - LAAS
- *  Copyright (c) 2019, INRIA
- *  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.
- */
-
-#ifndef HPP_FCL_MESH_LOADER_ASSIMP_H
-#define HPP_FCL_MESH_LOADER_ASSIMP_H
-
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/config.hh>
-#include <hpp/fcl/BV/OBBRSS.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-
-struct aiScene;
-namespace Assimp {
-class Importer;
-}
-
-namespace hpp {
-namespace fcl {
-
-namespace internal {
-
-struct HPP_FCL_DLLAPI TriangleAndVertices {
-  std::vector<fcl::Vec3f> vertices_;
-  std::vector<fcl::Triangle> triangles_;
-};
-
-struct HPP_FCL_DLLAPI Loader {
-  Loader();
-  ~Loader();
-
-  void load(const std::string& resource_path);
-
-  Assimp::Importer* importer;
-  aiScene const* scene;
-};
-
-/**
- * @brief      Recursive procedure for building a mesh
- *
- * @param[in]  scale           Scale to apply when reading the ressource
- * @param[in]  scene           Pointer to the assimp scene
- * @param[in]  vertices_offset Current number of vertices in the model
- * @param      tv              Triangles and Vertices of the mesh submodels
- */
-HPP_FCL_DLLAPI void buildMesh(const fcl::Vec3f& scale, const aiScene* scene,
-                              unsigned vertices_offset,
-                              TriangleAndVertices& tv);
-
-/**
- * @brief      Convert an assimp scene to a mesh
- *
- * @param[in]  scale  Scale to apply when reading the ressource
- * @param[in]  scene  Pointer to the assimp scene
- * @param[out] mesh  The mesh that must be built
- */
-template <class BoundingVolume>
-inline void meshFromAssimpScene(
-    const fcl::Vec3f& scale, const aiScene* scene,
-    const shared_ptr<BVHModel<BoundingVolume> >& mesh) {
-  TriangleAndVertices tv;
-
-  int res = mesh->beginModel();
-
-  if (res != fcl::BVH_OK) {
-    HPP_FCL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error);
-  }
-
-  buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv);
-  mesh->addSubModel(tv.vertices_, tv.triangles_);
-
-  mesh->endModel();
-}
-
-}  // namespace internal
-
-/**
- * @brief      Read a mesh file and convert it to a polyhedral mesh
- *
- * @param[in]  resource_path  Path to the ressource mesh file to be read
- * @param[in]  scale          Scale to apply when reading the ressource
- * @param[out] polyhedron     The resulted polyhedron
- */
-template <class BoundingVolume>
-inline void loadPolyhedronFromResource(
-    const std::string& resource_path, const fcl::Vec3f& scale,
-    const shared_ptr<BVHModel<BoundingVolume> >& polyhedron) {
-  internal::Loader scene;
-  scene.load(resource_path);
-
-  internal::meshFromAssimpScene(scale, scene.scene, polyhedron);
-}
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // HPP_FCL_MESH_LOADER_ASSIMP_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/mesh_loader/assimp.h>
diff --git a/include/hpp/fcl/mesh_loader/loader.h b/include/hpp/fcl/mesh_loader/loader.h
index 2c3f36249727693ace8fd042e0d10581ca0486a2..bbb43f1d0ded45cb3bcb69be7c5d8df385edc22d 100644
--- a/include/hpp/fcl/mesh_loader/loader.h
+++ b/include/hpp/fcl/mesh_loader/loader.h
@@ -1,105 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2016, CNRS - LAAS
- *  Copyright (c) 2020, INRIA
- *  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.
- */
-
-#ifndef HPP_FCL_MESH_LOADER_LOADER_H
-#define HPP_FCL_MESH_LOADER_LOADER_H
-
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/config.hh>
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/collision_object.h>
-
-#include <map>
-#include <ctime>
-
-namespace hpp {
-namespace fcl {
-/// Base class for building polyhedron from files.
-/// This class builds a new object for each file.
-class HPP_FCL_DLLAPI MeshLoader {
- public:
-  virtual ~MeshLoader() {}
-
-  virtual BVHModelPtr_t load(const std::string& filename,
-                             const Vec3f& scale = Vec3f::Ones());
-
-  /// Create an OcTree from a file in binary octomap format.
-  /// \todo add OctreePtr_t
-  virtual CollisionGeometryPtr_t loadOctree(const std::string& filename);
-
-  MeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : bvType_(bvType) {}
-
- private:
-  const NODE_TYPE bvType_;
-};
-
-/// Class for building polyhedron from files with cache mechanism.
-/// This class builds a new object for each different file.
-/// If method CachedMeshLoader::load is called twice with the same arguments,
-/// the second call returns the result of the first call.
-class HPP_FCL_DLLAPI CachedMeshLoader : public MeshLoader {
- public:
-  virtual ~CachedMeshLoader() {}
-
-  CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {}
-
-  virtual BVHModelPtr_t load(const std::string& filename, const Vec3f& scale);
-
-  struct HPP_FCL_DLLAPI Key {
-    std::string filename;
-    Vec3f scale;
-
-    Key(const std::string& f, const Vec3f& s) : filename(f), scale(s) {}
-
-    bool operator<(const CachedMeshLoader::Key& b) const;
-  };
-  struct HPP_FCL_DLLAPI Value {
-    BVHModelPtr_t model;
-    std::time_t mtime;
-  };
-  typedef std::map<Key, Value> Cache_t;
-
-  const Cache_t& cache() const { return cache_; }
-
- private:
-  Cache_t cache_;
-};
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif  // FCL_MESH_LOADER_LOADER_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/mesh_loader/loader.h>
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 206109ef8718d3e4bc251b9a5d34b5b429dc7820..cab294b93e5060735905420debd8e58ea2ff1072 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -1,457 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2021-2024, INRIA
- *  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 HPP_FCL_GJK_H
-#define HPP_FCL_GJK_H
-
-#include <vector>
-
-#include "hpp/fcl/narrowphase/minkowski_difference.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace details {
-
-/// @brief class for GJK algorithm
-///
-/// @note The computations are performed in the frame of the first shape.
-struct HPP_FCL_DLLAPI GJK {
-  struct HPP_FCL_DLLAPI SimplexV {
-    /// @brief support vector for shape 0 and 1.
-    Vec3f w0, w1;
-    /// @brief support vector (i.e., the furthest point on the shape along the
-    /// support direction)
-    Vec3f w;
-  };
-
-  typedef unsigned char vertex_id_t;
-
-  /// @brief A simplex is a set of up to 4 vertices.
-  /// Its rank is the number of vertices it contains.
-  /// @note This data structure does **not** own the vertices it refers to.
-  /// To be efficient, the constructor of `GJK` creates storage for 4 vertices.
-  /// Since GJK does not need any more storage, it reuses these vertices
-  /// throughout the algorithm by using multiple instance of this `Simplex`
-  /// class.
-  struct HPP_FCL_DLLAPI Simplex {
-    /// @brief simplex vertex
-    SimplexV* vertex[4];
-    /// @brief size of simplex (number of vertices)
-    vertex_id_t rank;
-
-    Simplex() {}
-
-    void reset() {
-      rank = 0;
-      for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr;
-    }
-  };
-
-  /// @brief Status of the GJK algorithm:
-  /// DidNotRun: GJK has not been run.
-  /// Failed: GJK did not converge (it exceeded the maximum number of
-  /// iterations).
-  /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited
-  ///     before converting. The shapes are not in collision.
-  /// NoCollision: GJK converged and the shapes are not in collision.
-  /// Collision: GJK converged and the shapes are in collision.
-  /// Failed: GJK did not converge.
-  enum Status {
-    DidNotRun,
-    Failed,
-    NoCollisionEarlyStopped,
-    NoCollision,
-    CollisionWithPenetrationInformation,
-    Collision
-  };
-
- public:
-  FCL_REAL distance_upper_bound;
-  Status status;
-  GJKVariant gjk_variant;
-  GJKConvergenceCriterion convergence_criterion;
-  GJKConvergenceCriterionType convergence_criterion_type;
-
-  MinkowskiDiff const* shape;
-  Vec3f ray;
-  support_func_guess_t support_hint;
-  /// @brief The distance between the two shapes, computed by GJK.
-  /// If the distance is below GJK's threshold, the shapes are in collision in
-  /// the eyes of GJK. If `distance_upper_bound` is set to a value lower than
-  /// infinity, GJK will early stop as soon as it finds `distance` to be greater
-  /// than `distance_upper_bound`.
-  FCL_REAL distance;
-  Simplex* simplex;  // Pointer to the result of the last run of GJK.
-
- private:
-  // max_iteration and tolerance are made private
-  // because they are meant to be set by the `reset` function.
-  size_t max_iterations;
-  FCL_REAL tolerance;
-
-  SimplexV store_v[4];
-  SimplexV* free_v[4];
-  vertex_id_t nfree;
-  vertex_id_t current;
-  Simplex simplices[2];
-  size_t iterations;
-  size_t iterations_momentum_stop;
-
- public:
-  /// \param max_iterations_ number of iteration before GJK returns failure.
-  /// \param tolerance_ precision of the algorithm.
-  ///
-  /// The tolerance argument is useful for continuous shapes and for polyhedron
-  /// with some vertices closer than this threshold.
-  ///
-  /// Suggested values are 100 iterations and a tolerance of 1e-6.
-  GJK(size_t max_iterations_, FCL_REAL tolerance_)
-      : max_iterations(max_iterations_), tolerance(tolerance_) {
-    HPP_FCL_ASSERT(tolerance_ > 0, "Tolerance must be positive.",
-                   std::invalid_argument);
-    initialize();
-  }
-
-  /// @brief resets the GJK algorithm, preparing it for a new run.
-  /// Other than the maximum number of iterations and the tolerance,
-  /// this function does **not** modify the parameters of the GJK algorithm.
-  void reset(size_t max_iterations_, FCL_REAL tolerance_);
-
-  /// @brief GJK algorithm, given the initial value guess
-  Status evaluate(
-      const MinkowskiDiff& shape, const Vec3f& guess,
-      const support_func_guess_t& supportHint = support_func_guess_t::Zero());
-
-  /// @brief apply the support function along a direction, the result is return
-  /// in sv
-  inline void getSupport(const Vec3f& d, SimplexV& sv,
-                         support_func_guess_t& hint) const {
-    shape->support(d, sv.w0, sv.w1, hint);
-    sv.w = sv.w0 - sv.w1;
-  }
-
-  /// @brief whether the simplex enclose the origin
-  bool encloseOrigin();
-
-  /// @brief get the underlying simplex using in GJK, can be used for cache in
-  /// next iteration
-  inline Simplex* getSimplex() const { return simplex; }
-
-  /// Tells whether the closest points are available.
-  bool hasClosestPoints() const { return distance < distance_upper_bound; }
-
-  /// Get the witness points on each object, and the corresponding normal.
-  /// @param[in] shape is the Minkowski difference of the two shapes.
-  /// @param[out] w0 is the witness point on shape0.
-  /// @param[out] w1 is the witness point on shape1.
-  /// @param[out] normal is the normal of the separating plane found by
-  /// GJK. It points from shape0 to shape1.
-  void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
-                                 Vec3f& w1, Vec3f& normal) const;
-
-  /// @brief get the guess from current simplex
-  Vec3f getGuessFromSimplex() const;
-
-  /// @brief Distance threshold for early break.
-  /// GJK stops when it proved the distance is more than this threshold.
-  /// @note The closest points will be erroneous in this case.
-  ///       If you want the closest points, set this to infinity (the default).
-  void setDistanceEarlyBreak(const FCL_REAL& dup) {
-    distance_upper_bound = dup;
-  }
-
-  /// @brief Convergence check used to stop GJK when shapes are not in
-  /// collision.
-  bool checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
-                        const FCL_REAL& omega) const;
-
-  /// @brief Get the max number of iterations of GJK.
-  size_t getNumMaxIterations() const { return max_iterations; }
-
-  /// @brief Get the tolerance of GJK.
-  FCL_REAL getTolerance() const { return tolerance; }
-
-  /// @brief Get the number of iterations of the last run of GJK.
-  size_t getNumIterations() const { return iterations; }
-
-  /// @brief Get GJK number of iterations before momentum stops.
-  /// Only usefull if the Nesterov or Polyak acceleration activated.
-  size_t getNumIterationsMomentumStopped() const {
-    return iterations_momentum_stop;
-  }
-
- private:
-  /// @brief Initializes the GJK algorithm.
-  /// This function should only be called by the constructor.
-  /// Otherwise use \ref reset.
-  void initialize();
-
-  /// @brief discard one vertex from the simplex
-  inline void removeVertex(Simplex& simplex);
-
-  /// @brief append one vertex to the simplex
-  inline void appendVertex(Simplex& simplex, const Vec3f& v,
-                           support_func_guess_t& hint);
-
-  /// @brief Project origin (0) onto line a-b
-  /// For a detailed explanation of how to efficiently project onto a simplex,
-  /// check out Ericson's book, page 403:
-  /// https://realtimecollisiondetection.net/ To sum up, a simplex has a voronoi
-  /// region for each feature it has (vertex, edge, face). We find the voronoi
-  /// region in which the origin lies and stop as soon as we find it; we then
-  /// project onto it and return the result. We start by voronoi regions
-  /// generated by vertices then move on to edges then faces. Checking voronoi
-  /// regions is done using simple dot products. Moreover, edges voronoi checks
-  /// reuse computations of vertices voronoi checks. The same goes for faces
-  /// which reuse checks from edges.
-  /// Finally, in addition to the voronoi procedure, checks relying on the order
-  /// of construction
-  /// of the simplex are added. To know more about these, visit
-  /// https://caseymuratori.com/blog_0003.
-  bool projectLineOrigin(const Simplex& current, Simplex& next);
-
-  /// @brief Project origin (0) onto triangle a-b-c
-  /// See \ref projectLineOrigin for an explanation on simplex projections.
-  bool projectTriangleOrigin(const Simplex& current, Simplex& next);
-
-  /// @brief Project origin (0) onto tetrahedron a-b-c-d
-  /// See \ref projectLineOrigin for an explanation on simplex projections.
-  bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
-};
-
-/// @brief class for EPA algorithm
-struct HPP_FCL_DLLAPI EPA {
-  typedef GJK::SimplexV SimplexVertex;
-  struct HPP_FCL_DLLAPI SimplexFace {
-    Vec3f n;
-    FCL_REAL d;
-    bool ignore;          // If the origin does not project inside the face, we
-                          // ignore this face.
-    size_t vertex_id[3];  // Index of vertex in sv_store.
-    SimplexFace* adjacent_faces[3];  // A face has three adjacent faces.
-    SimplexFace* prev_face;          // The previous face in the list.
-    SimplexFace* next_face;          // The next face in the list.
-    size_t adjacent_edge[3];         // Each face has 3 edges: `0`, `1` and `2`.
-                              // The i-th adjacent face is bound (to this face)
-                              // along its `adjacent_edge[i]`-th edge
-                              // (with 0 <= i <= 2).
-    size_t pass;
-
-    SimplexFace() : n(Vec3f::Zero()), ignore(false) {};
-  };
-
-  /// @brief The simplex list of EPA is a linked list of faces.
-  /// Note: EPA's linked list does **not** own any memory.
-  /// The memory it refers to is contiguous and owned by a std::vector.
-  struct HPP_FCL_DLLAPI SimplexFaceList {
-    SimplexFace* root;
-    size_t count;
-    SimplexFaceList() : root(nullptr), count(0) {}
-
-    void reset() {
-      root = nullptr;
-      count = 0;
-    }
-
-    void append(SimplexFace* face) {
-      face->prev_face = nullptr;
-      face->next_face = root;
-      if (root != nullptr) root->prev_face = face;
-      root = face;
-      ++count;
-    }
-
-    void remove(SimplexFace* face) {
-      if (face->next_face != nullptr)
-        face->next_face->prev_face = face->prev_face;
-      if (face->prev_face != nullptr)
-        face->prev_face->next_face = face->next_face;
-      if (face == root) root = face->next_face;
-      --count;
-    }
-  };
-
-  /// @brief We bind the face `fa` along its edge `ea` to the face `fb` along
-  /// its edge `fb`.
-  static inline void bind(SimplexFace* fa, size_t ea, SimplexFace* fb,
-                          size_t eb) {
-    assert(ea == 0 || ea == 1 || ea == 2);
-    assert(eb == 0 || eb == 1 || eb == 2);
-    fa->adjacent_edge[ea] = eb;
-    fa->adjacent_faces[ea] = fb;
-    fb->adjacent_edge[eb] = ea;
-    fb->adjacent_faces[eb] = fa;
-  }
-
-  struct HPP_FCL_DLLAPI SimplexHorizon {
-    SimplexFace* current_face;  // current face in the horizon
-    SimplexFace* first_face;    // first face in the horizon
-    size_t num_faces;           // number of faces in the horizon
-    SimplexHorizon()
-        : current_face(nullptr), first_face(nullptr), num_faces(0) {}
-  };
-
-  enum Status {
-    DidNotRun = -1,
-    Failed = 0,
-    Valid = 1,
-    AccuracyReached = 1 << 1 | Valid,
-    Degenerated = 1 << 1 | Failed,
-    NonConvex = 2 << 1 | Failed,
-    InvalidHull = 3 << 1 | Failed,
-    OutOfFaces = 4 << 1 | Failed,
-    OutOfVertices = 5 << 1 | Failed,
-    FallBack = 6 << 1 | Failed
-  };
-
- public:
-  Status status;
-  GJK::Simplex result;
-  Vec3f normal;
-  support_func_guess_t support_hint;
-  FCL_REAL depth;
-  SimplexFace* closest_face;
-
- private:
-  // max_iteration and tolerance are made private
-  // because they are meant to be set by the `reset` function.
-  size_t max_iterations;
-  FCL_REAL tolerance;
-
-  std::vector<SimplexVertex> sv_store;
-  std::vector<SimplexFace> fc_store;
-  SimplexFaceList hull, stock;
-  size_t num_vertices;  // number of vertices in polytpoe constructed by EPA
-  size_t iterations;
-
- public:
-  EPA(size_t max_iterations_, FCL_REAL tolerance_)
-      : max_iterations(max_iterations_), tolerance(tolerance_) {
-    initialize();
-  }
-
-  /// @brief Copy constructor of EPA.
-  /// Mostly needed for the copy constructor of `GJKSolver`.
-  EPA(const EPA& other)
-      : max_iterations(other.max_iterations),
-        tolerance(other.tolerance),
-        sv_store(other.sv_store),
-        fc_store(other.fc_store) {
-    initialize();
-  }
-
-  /// @brief Get the max number of iterations of EPA.
-  size_t getNumMaxIterations() const { return max_iterations; }
-
-  /// @brief Get the max number of vertices of EPA.
-  size_t getNumMaxVertices() const { return sv_store.size(); }
-
-  /// @brief Get the max number of faces of EPA.
-  size_t getNumMaxFaces() const { return fc_store.size(); }
-
-  /// @brief Get the tolerance of EPA.
-  FCL_REAL getTolerance() const { return tolerance; }
-
-  /// @brief Get the number of iterations of the last run of EPA.
-  size_t getNumIterations() const { return iterations; }
-
-  /// @brief Get the number of vertices in the polytope of the last run of EPA.
-  size_t getNumVertices() const { return num_vertices; }
-
-  /// @brief Get the number of faces in the polytope of the last run of EPA.
-  size_t getNumFaces() const { return hull.count; }
-
-  /// @brief resets the EPA algorithm, preparing it for a new run.
-  /// It potentially reallocates memory for the vertices and faces
-  /// if the passed parameters are bigger than the previous ones.
-  /// This function does **not** modify the parameters of the EPA algorithm,
-  /// i.e. the maximum number of iterations and the tolerance.
-  /// @note calling this function destroys the previous state of EPA.
-  /// In the future, we may want to copy it instead, i.e. when EPA will
-  /// be (properly) warm-startable.
-  void reset(size_t max_iterations, FCL_REAL tolerance);
-
-  /// \return a Status which can be demangled using (status & Valid) or
-  ///         (status & Failed). The other values provide a more detailled
-  ///         status
-  Status evaluate(GJK& gjk, const Vec3f& guess);
-
-  /// Get the witness points on each object, and the corresponding normal.
-  /// @param[in] shape is the Minkowski difference of the two shapes.
-  /// @param[out] w0 is the witness point on shape0.
-  /// @param[out] w1 is the witness point on shape1.
-  /// @param[in] normal is the normal found by EPA. It points from shape0 to
-  /// shape1. The normal is used to correct the witness points on the shapes if
-  /// the shapes have a non-zero swept-sphere radius.
-  void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
-                                 Vec3f& w1, Vec3f& normal) const;
-
- private:
-  /// @brief Allocates memory for the EPA algorithm.
-  /// This function should only be called by the constructor.
-  /// Otherwise use \ref reset.
-  void initialize();
-
-  bool getEdgeDist(SimplexFace* face, const SimplexVertex& a,
-                   const SimplexVertex& b, FCL_REAL& dist);
-
-  /// @brief Add a new face to the polytope.
-  /// This function sets the `ignore` flag to `true` if the origin does not
-  /// project inside the face.
-  SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex,
-                       bool force = false);
-
-  /// @brief Find the best polytope face to split
-  SimplexFace* findClosestFace();
-
-  /// @brief the goal is to add a face connecting vertex w and face edge f[e]
-  bool expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
-              SimplexHorizon& horizon);
-
-  // @brief Use this function to debug expand if needed.
-  // void PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w);
-};
-
-}  // namespace details
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/narrowphase/gjk.h>
diff --git a/include/hpp/fcl/narrowphase/minkowski_difference.h b/include/hpp/fcl/narrowphase/minkowski_difference.h
index f6c0f93f4c72a4fb3deadb5392ef30d8d97f5241..5f6ca0e08d9b58065b868b6e20ad75e5011ff373 100644
--- a/include/hpp/fcl/narrowphase/minkowski_difference.h
+++ b/include/hpp/fcl/narrowphase/minkowski_difference.h
@@ -1,188 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2021-2024, INRIA
- *  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.
- */
-
-/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
-
-#ifndef HPP_FCL_MINKOWSKI_DIFFERENCE_H
-#define HPP_FCL_MINKOWSKI_DIFFERENCE_H
-
-#include "hpp/fcl/shape/geometric_shapes.h"
-#include "hpp/fcl/math/transform.h"
-#include "hpp/fcl/narrowphase/support_functions.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace details {
-
-/// @brief Minkowski difference class of two shapes
-///
-/// @note The Minkowski difference is expressed in the frame of the first shape.
-struct HPP_FCL_DLLAPI MinkowskiDiff {
-  typedef Eigen::Array<FCL_REAL, 1, 2> Array2d;
-
-  /// @brief points to two shapes
-  const ShapeBase* shapes[2];
-
-  /// @brief Store temporary data for the computation of the support point for
-  /// each shape.
-  ShapeSupportData data[2];
-
-  /// @brief rotation from shape1 to shape0
-  /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
-  Matrix3f oR1;
-
-  /// @brief translation from shape1 to shape0
-  /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
-  Vec3f ot1;
-
-  /// @brief The radii of the sphere swepted around each shape of the Minkowski
-  /// difference. The 2 values correspond to the swept-sphere radius of shape 0
-  /// and shape 1.
-  Array2d swept_sphere_radius;
-
-  /// @brief Wether or not to use the normalize heuristic in the GJK Nesterov
-  /// acceleration. This setting is only applied if the Nesterov acceleration in
-  /// the GJK class is active.
-  bool normalize_support_direction;
-
-  typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
-                                     const Vec3f& dir, Vec3f& support0,
-                                     Vec3f& support1,
-                                     support_func_guess_t& hint,
-                                     ShapeSupportData data[2]);
-  GetSupportFunction getSupportFunc;
-
-  MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {}
-
-  /// @brief Set the two shapes, assuming the relative transformation between
-  /// them is identity.
-  /// Consequently, all support points computed with the MinkowskiDiff
-  /// will be expressed in the world frame.
-  /// @param shape0 the first shape.
-  /// @param shape1 the second shape.
-  /// @tparam SupportOptions is a value of the SupportOptions enum. If set to
-  /// `WithSweptSphere`, the support computation will take into account the
-  /// swept sphere radius of the shapes. If set to `NoSweptSphere`, where
-  /// this information is simply stored in the Minkowski's difference
-  /// `swept_sphere_radius` array. This array is then used to correct the
-  /// solution found when GJK or EPA have converged.
-  ///
-  /// @note In practice, there is no need to take into account the swept-sphere
-  /// radius in the iterations of GJK/EPA. It is in fact detrimental to the
-  /// convergence of both algos. This is because it makes corners and edges of
-  /// shapes look strictly convex to the algorithms, which leads to poor
-  /// convergence. This swept sphere template parameter is only here for
-  /// debugging purposes and for specific uses cases where the swept sphere
-  /// radius is needed in the support function. The rule is simple. When
-  /// interacting with GJK/EPA, the `SupportOptions` template
-  /// parameter should **always** be set to `NoSweptSphere` (except for
-  /// debugging or testing purposes). When working directly with the shapes
-  /// involved in the collision, and not relying on GJK/EPA, the
-  /// `SupportOptions` template parameter should be set to `WithSweptSphere`.
-  /// This is for example the case for specialized collision/distance functions.
-  template <int _SupportOptions = SupportOptions::NoSweptSphere>
-  void set(const ShapeBase* shape0, const ShapeBase* shape1);
-
-  /// @brief Set the two shapes, with a relative transformation from shape0 to
-  /// shape1. Consequently, all support points computed with the MinkowskiDiff
-  /// will be expressed in the frame of shape0.
-  /// @param shape0 the first shape.
-  /// @param shape1 the second shape.
-  /// @param tf0 the transformation of the first shape.
-  /// @param tf1 the transformation of the second shape.
-  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
-  /// ShapeBase*)` for more details.
-  template <int _SupportOptions = SupportOptions::NoSweptSphere>
-  void set(const ShapeBase* shape0, const ShapeBase* shape1,
-           const Transform3f& tf0, const Transform3f& tf1);
-
-  /// @brief support function for shape0.
-  /// The output vector is expressed in the local frame of shape0.
-  /// @return a point which belongs to the set {argmax_{v in shape0}
-  /// v.dot(dir)}, i.e a support of shape0 in direction dir.
-  /// @param dir support direction.
-  /// @param hint used to initialize the search when shape is a ConvexBase
-  /// object.
-  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
-  /// ShapeBase*)` for more details.
-  template <int _SupportOptions = SupportOptions::NoSweptSphere>
-  inline Vec3f support0(const Vec3f& dir, int& hint) const {
-    return getSupport<_SupportOptions>(shapes[0], dir, hint);
-  }
-
-  /// @brief support function for shape1.
-  /// The output vector is expressed in the local frame of shape0.
-  /// This is mandatory because in the end we are interested in support points
-  /// of the Minkowski difference; hence the supports of shape0 and shape1 must
-  /// live in the same frame.
-  /// @return a point which belongs to the set {tf * argmax_{v in shape1}
-  /// v.dot(R^T * dir)}, i.e. the support of shape1 in direction dir (tf is the
-  /// tranform from shape1 to shape0).
-  /// @param dir support direction.
-  /// @param hint used to initialize the search when shape is a ConvexBase.
-  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
-  /// ShapeBase*)` for more details.
-  template <int _SupportOptions = SupportOptions::NoSweptSphere>
-  inline Vec3f support1(const Vec3f& dir, int& hint) const {
-    // clang-format off
-    return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1;
-    // clang-format on
-  }
-
-  /// @brief Support function for the pair of shapes. This method assumes `set`
-  /// has already been called.
-  /// @param[in] dir the support direction.
-  /// @param[out] supp0 support of shape0 in direction dir, expressed in the
-  /// frame of shape0.
-  /// @param[out] supp1 support of shape1 in direction -dir, expressed in the
-  /// frame of shape0.
-  /// @param[in/out] hint used to initialize the search when shape is a
-  /// ConvexBase object.
-  inline void support(const Vec3f& dir, Vec3f& supp0, Vec3f& supp1,
-                      support_func_guess_t& hint) const {
-    assert(getSupportFunc != NULL);
-    getSupportFunc(*this, dir, supp0, supp1, hint,
-                   const_cast<ShapeSupportData*>(data));
-  }
-};
-
-}  // namespace details
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // HPP_FCL_MINKOWSKI_DIFFERENCE_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/narrowphase/minkowski_difference.h>
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index ff9a5785b1f095a5143d297949db5ecae1c10fb8..0a079085d9c8884e47bac3f4c2867efc1da24af7 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -1,729 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
- *  All rights reserved.
- *  Copyright (c) 2021-2024, INRIA
- *  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, Florent Lamiraux */
-
-#ifndef HPP_FCL_NARROWPHASE_H
-#define HPP_FCL_NARROWPHASE_H
-
-#include <limits>
-
-#include <hpp/fcl/narrowphase/gjk.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/narrowphase/narrowphase_defaults.h>
-#include <hpp/fcl/logging.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief collision and distance solver based on the GJK and EPA algorithms.
-/// Originally, GJK and EPA were implemented in fcl which itself took
-/// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA
-/// have been largely modified to be faster and more robust to numerical
-/// accuracy and edge cases.
-struct HPP_FCL_DLLAPI GJKSolver {
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  /// @brief GJK algorithm
-  mutable details::GJK gjk;
-
-  /// @brief maximum number of iterations of GJK
-  size_t gjk_max_iterations;
-
-  /// @brief tolerance of GJK
-  FCL_REAL gjk_tolerance;
-
-  /// @brief which warm start to use for GJK
-  GJKInitialGuess gjk_initial_guess;
-
-  /// @brief Whether smart guess can be provided
-  /// @Deprecated Use gjk_initial_guess instead
-  HPP_FCL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
-  bool enable_cached_guess;
-
-  /// @brief smart guess
-  mutable Vec3f cached_guess;
-
-  /// @brief smart guess for the support function
-  mutable support_func_guess_t support_func_cached_guess;
-
-  /// @brief If GJK can guarantee that the distance between the shapes is
-  /// greater than this value, it will early stop.
-  FCL_REAL distance_upper_bound;
-
-  /// @brief Variant of the GJK algorithm (Default, Nesterov or Polyak).
-  GJKVariant gjk_variant;
-
-  /// @brief Convergence criterion for GJK
-  GJKConvergenceCriterion gjk_convergence_criterion;
-
-  /// @brief Absolute or relative convergence criterion for GJK
-  GJKConvergenceCriterionType gjk_convergence_criterion_type;
-
-  /// @brief EPA algorithm
-  mutable details::EPA epa;
-
-  /// @brief maximum number of iterations of EPA
-  size_t epa_max_iterations;
-
-  /// @brief tolerance of EPA
-  FCL_REAL epa_tolerance;
-
-  /// @brief Minkowski difference used by GJK and EPA algorithms
-  mutable details::MinkowskiDiff minkowski_difference;
-
- private:
-  // Used internally for assertion checks.
-  static constexpr FCL_REAL m_dummy_precision = 1e-6;
-
- public:
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  /// @brief Default constructor for GJK algorithm
-  /// By default, we don't want EPA to allocate memory because
-  /// certain functions of the `GJKSolver` class have specializations
-  /// which don't use EPA (and/or GJK).
-  /// So we give EPA's constructor a max number of iterations of zero.
-  /// Only the functions that need EPA will reset the algorithm and allocate
-  /// memory if needed.
-  GJKSolver()
-      : gjk(GJK_DEFAULT_MAX_ITERATIONS, GJK_DEFAULT_TOLERANCE),
-        gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
-        gjk_tolerance(GJK_DEFAULT_TOLERANCE),
-        gjk_initial_guess(GJKInitialGuess::DefaultGuess),
-        enable_cached_guess(false),  // Use gjk_initial_guess instead
-        cached_guess(Vec3f(1, 0, 0)),
-        support_func_cached_guess(support_func_guess_t::Zero()),
-        distance_upper_bound((std::numeric_limits<FCL_REAL>::max)()),
-        gjk_variant(GJKVariant::DefaultGJK),
-        gjk_convergence_criterion(GJKConvergenceCriterion::Default),
-        gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute),
-        epa(0, EPA_DEFAULT_TOLERANCE),
-        epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
-        epa_tolerance(EPA_DEFAULT_TOLERANCE) {}
-
-  /// @brief Constructor from a DistanceRequest
-  ///
-  /// \param[in] request DistanceRequest input
-  ///
-  /// See the default constructor; by default, we don't want
-  /// EPA to allocate memory so we call EPA's constructor with 0 max
-  /// number of iterations.
-  /// However, the `set` method stores the actual values of the request.
-  /// EPA will thus allocate memory only if needed.
-  explicit GJKSolver(const DistanceRequest& request)
-      : gjk(request.gjk_max_iterations, request.gjk_tolerance),
-        epa(0, request.epa_tolerance) {
-    this->cached_guess = Vec3f(1, 0, 0);
-    this->support_func_cached_guess = support_func_guess_t::Zero();
-
-    set(request);
-  }
-
-  /// @brief setter from a DistanceRequest
-  ///
-  /// \param[in] request DistanceRequest input
-  ///
-  void set(const DistanceRequest& request) {
-    // ---------------------
-    // GJK settings
-    this->gjk_initial_guess = request.gjk_initial_guess;
-    this->enable_cached_guess = request.enable_cached_gjk_guess;
-    if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
-        this->enable_cached_guess) {
-      this->cached_guess = request.cached_gjk_guess;
-      this->support_func_cached_guess = request.cached_support_func_guess;
-    }
-    this->gjk_max_iterations = request.gjk_max_iterations;
-    this->gjk_tolerance = request.gjk_tolerance;
-    // For distance computation, we don't want GJK to early stop
-    this->distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
-    this->gjk_variant = request.gjk_variant;
-    this->gjk_convergence_criterion = request.gjk_convergence_criterion;
-    this->gjk_convergence_criterion_type =
-        request.gjk_convergence_criterion_type;
-
-    // ---------------------
-    // EPA settings
-    this->epa_max_iterations = request.epa_max_iterations;
-    this->epa_tolerance = request.epa_tolerance;
-
-    // ---------------------
-    // Reset GJK and EPA status
-    this->epa.status = details::EPA::Status::DidNotRun;
-    this->gjk.status = details::GJK::Status::DidNotRun;
-  }
-
-  /// @brief Constructor from a CollisionRequest
-  ///
-  /// \param[in] request CollisionRequest input
-  ///
-  /// See the default constructor; by default, we don't want
-  /// EPA to allocate memory so we call EPA's constructor with 0 max
-  /// number of iterations.
-  /// However, the `set` method stores the actual values of the request.
-  /// EPA will thus allocate memory only if needed.
-  explicit GJKSolver(const CollisionRequest& request)
-      : gjk(request.gjk_max_iterations, request.gjk_tolerance),
-        epa(0, request.epa_tolerance) {
-    this->cached_guess = Vec3f(1, 0, 0);
-    this->support_func_cached_guess = support_func_guess_t::Zero();
-
-    set(request);
-  }
-
-  /// @brief setter from a CollisionRequest
-  ///
-  /// \param[in] request CollisionRequest input
-  ///
-  void set(const CollisionRequest& request) {
-    // ---------------------
-    // GJK settings
-    this->gjk_initial_guess = request.gjk_initial_guess;
-    this->enable_cached_guess = request.enable_cached_gjk_guess;
-    if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
-        this->enable_cached_guess) {
-      this->cached_guess = request.cached_gjk_guess;
-      this->support_func_cached_guess = request.cached_support_func_guess;
-    }
-    this->gjk_tolerance = request.gjk_tolerance;
-    this->gjk_max_iterations = request.gjk_max_iterations;
-    // The distance upper bound should be at least greater to the requested
-    // security margin. Otherwise, we will likely miss some collisions.
-    this->distance_upper_bound = (std::max)(
-        0., (std::max)(request.distance_upper_bound, request.security_margin));
-    this->gjk_variant = request.gjk_variant;
-    this->gjk_convergence_criterion = request.gjk_convergence_criterion;
-    this->gjk_convergence_criterion_type =
-        request.gjk_convergence_criterion_type;
-
-    // ---------------------
-    // EPA settings
-    this->epa_max_iterations = request.epa_max_iterations;
-    this->epa_tolerance = request.epa_tolerance;
-
-    // ---------------------
-    // Reset GJK and EPA status
-    this->gjk.status = details::GJK::Status::DidNotRun;
-    this->epa.status = details::EPA::Status::DidNotRun;
-  }
-
-  /// @brief Copy constructor
-  GJKSolver(const GJKSolver& other) = default;
-
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  bool operator==(const GJKSolver& other) const {
-    return this->enable_cached_guess ==
-               other.enable_cached_guess &&  // use gjk_initial_guess instead
-           this->cached_guess == other.cached_guess &&
-           this->support_func_cached_guess == other.support_func_cached_guess &&
-           this->gjk_max_iterations == other.gjk_max_iterations &&
-           this->gjk_tolerance == other.gjk_tolerance &&
-           this->distance_upper_bound == other.distance_upper_bound &&
-           this->gjk_variant == other.gjk_variant &&
-           this->gjk_convergence_criterion == other.gjk_convergence_criterion &&
-           this->gjk_convergence_criterion_type ==
-               other.gjk_convergence_criterion_type &&
-           this->gjk_initial_guess == other.gjk_initial_guess &&
-           this->epa_max_iterations == other.epa_max_iterations &&
-           this->epa_tolerance == other.epa_tolerance;
-  }
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-
-  bool operator!=(const GJKSolver& other) const { return !(*this == other); }
-
-  /// @brief Helper to return the precision of the solver on the distance
-  /// estimate, depending on whether or not `compute_penetration` is true.
-  FCL_REAL getDistancePrecision(const bool compute_penetration) const {
-    return compute_penetration
-               ? (std::max)(this->gjk_tolerance, this->epa_tolerance)
-               : this->gjk_tolerance;
-  }
-
-  /// @brief Uses GJK and EPA to compute the distance between two shapes.
-  /// @param `s1` the first shape.
-  /// @param `tf1` the transformation of the first shape.
-  /// @param `s2` the second shape.
-  /// @param `tf2` the transformation of the second shape.
-  /// @param `compute_penetration` if true and GJK finds the shape in collision,
-  /// the EPA algorithm is also ran to compute penetration information.
-  /// @param[out] `p1` the witness point on the first shape.
-  /// @param[out] `p2` the witness point on the second shape.
-  /// @param[out] `normal` the normal of the collision, pointing from the first
-  /// to the second shape.
-  /// @return the estimate of the distance between the two shapes.
-  ///
-  /// @note: if `this->distance_upper_bound` is set to a positive value, GJK
-  /// will early stop if it finds the distance to be above this value. The
-  /// distance returned by `this->shapeDistance` will be a lower bound on the
-  /// distance between the two shapes.
-  ///
-  /// @note: the variables `this->gjk.status` and `this->epa.status` can be used
-  /// to examine the status of GJK and EPA.
-  ///
-  /// @note: GJK and EPA give an estimate of the distance between the two
-  /// shapes. This estimate is precise up to the tolerance of the algorithms:
-  ///   - If `compute_penetration` is false, the distance is precise up to
-  ///     `gjk_tolerance`.
-  ///   - If `compute_penetration` is true, the distance is precise up to
-  ///     `std::max(gjk_tolerance, epa_tolerance)`
-  /// It's up to the user to decide whether the shapes are in collision or not,
-  /// based on that estimate.
-  template <typename S1, typename S2>
-  FCL_REAL shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2,
-                         const Transform3f& tf2, const bool compute_penetration,
-                         Vec3f& p1, Vec3f& p2, Vec3f& normal) const {
-    constexpr bool relative_transformation_already_computed = false;
-    FCL_REAL distance;
-    this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2,
-                       normal, relative_transformation_already_computed);
-    return distance;
-  }
-
-  /// @brief Partial specialization of `shapeDistance` for the case where the
-  /// second shape is a triangle. It is more efficient to pre-compute the
-  /// relative transformation between the two shapes before calling GJK/EPA.
-  template <typename S1>
-  FCL_REAL shapeDistance(const S1& s1, const Transform3f& tf1,
-                         const TriangleP& s2, const Transform3f& tf2,
-                         const bool compute_penetration, Vec3f& p1, Vec3f& p2,
-                         Vec3f& normal) const {
-    const Transform3f tf_1M2(tf1.inverseTimes(tf2));
-    TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b),
-                  tf_1M2.transform(s2.c));
-
-    constexpr bool relative_transformation_already_computed = true;
-    FCL_REAL distance;
-    this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1,
-                       p2, normal, relative_transformation_already_computed);
-    return distance;
-  }
-
-  /// @brief See other partial template specialization of shapeDistance above.
-  template <typename S2>
-  FCL_REAL shapeDistance(const TriangleP& s1, const Transform3f& tf1,
-                         const S2& s2, const Transform3f& tf2,
-                         const bool compute_penetration, Vec3f& p1, Vec3f& p2,
-                         Vec3f& normal) const {
-    FCL_REAL distance = this->shapeDistance<S2>(
-        s2, tf2, s1, tf1, compute_penetration, p2, p1, normal);
-    normal = -normal;
-    return distance;
-  }
-
- protected:
-  /// @brief initialize GJK.
-  /// This method assumes `minkowski_difference` has been set.
-  template <typename S1, typename S2>
-  void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3f& guess,
-                          support_func_guess_t& support_hint,
-                          const Vec3f& default_guess = Vec3f(1, 0, 0)) const {
-    // There is no reason not to warm-start the support function, so we always
-    // do it.
-    support_hint = this->support_func_cached_guess;
-    // The following switch takes care of the GJK warm-start.
-    switch (gjk_initial_guess) {
-      case GJKInitialGuess::DefaultGuess:
-        guess = default_guess;
-        break;
-      case GJKInitialGuess::CachedGuess:
-        guess = this->cached_guess;
-        break;
-      case GJKInitialGuess::BoundingVolumeGuess:
-        if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
-          HPP_FCL_THROW_PRETTY(
-              "computeLocalAABB must have been called on the shapes before "
-              "using "
-              "GJKInitialGuess::BoundingVolumeGuess.",
-              std::logic_error);
-        }
-        guess.noalias() =
-            s1.aabb_local.center() -
-            (this->minkowski_difference.oR1 * s2.aabb_local.center() +
-             this->minkowski_difference.ot1);
-        break;
-      default:
-        HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error);
-    }
-    // TODO: use gjk_initial_guess instead
-    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-    if (this->enable_cached_guess) {
-      guess = this->cached_guess;
-    }
-    HPP_FCL_COMPILER_DIAGNOSTIC_POP
-  }
-
-  /// @brief Runs the GJK algorithm.
-  /// @param `s1` the first shape.
-  /// @param `tf1` the transformation of the first shape.
-  /// @param `s2` the second shape.
-  /// @param `tf2` the transformation of the second shape.
-  /// @param `compute_penetration` if true and if the shapes are in found in
-  /// collision, the EPA algorithm is also ran to compute penetration
-  /// information.
-  /// @param[out] `distance` the distance between the two shapes.
-  /// @param[out] `p1` the witness point on the first shape.
-  /// @param[out] `p2` the witness point on the second shape.
-  /// @param[out] `normal` the normal of the collision, pointing from the first
-  /// to the second shape.
-  /// @param `relative_transformation_already_computed` whether the relative
-  /// transformation between the two shapes has already been computed.
-  /// @tparam SupportOptions, see `MinkowskiDiff::set`. Whether the support
-  /// computations should take into account the shapes' swept-sphere radii
-  /// during the iterations of GJK and EPA. Please leave this default value to
-  /// `false` unless you know what you are doing. This template parameter is
-  /// only used for debugging/testing purposes. In short, there is no need to
-  /// take into account the swept sphere radius when computing supports in the
-  /// iterations of GJK and EPA. GJK and EPA will correct the solution once they
-  /// have converged.
-  /// @return the estimate of the distance between the two shapes.
-  ///
-  /// @note: The variables `this->gjk.status` and `this->epa.status` can be used
-  /// to examine the status of GJK and EPA.
-  template <typename S1, typename S2,
-            int _SupportOptions = details::SupportOptions::NoSweptSphere>
-  void runGJKAndEPA(
-      const S1& s1, const Transform3f& tf1, const S2& s2,
-      const Transform3f& tf2, const bool compute_penetration,
-      FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal,
-      const bool relative_transformation_already_computed = false) const {
-    // Reset internal state of GJK algorithm
-    if (relative_transformation_already_computed)
-      this->minkowski_difference.set<_SupportOptions>(&s1, &s2);
-    else
-      this->minkowski_difference.set<_SupportOptions>(&s1, &s2, tf1, tf2);
-    this->gjk.reset(this->gjk_max_iterations, this->gjk_tolerance);
-    this->gjk.setDistanceEarlyBreak(this->distance_upper_bound);
-    this->gjk.gjk_variant = this->gjk_variant;
-    this->gjk.convergence_criterion = this->gjk_convergence_criterion;
-    this->gjk.convergence_criterion_type = this->gjk_convergence_criterion_type;
-    this->epa.status = details::EPA::Status::DidNotRun;
-
-    // Get initial guess for GJK: default, cached or bounding volume guess
-    Vec3f guess;
-    support_func_guess_t support_hint;
-    getGJKInitialGuess(*(this->minkowski_difference.shapes[0]),
-                       *(this->minkowski_difference.shapes[1]), guess,
-                       support_hint);
-
-    this->gjk.evaluate(this->minkowski_difference, guess, support_hint);
-
-    switch (this->gjk.status) {
-      case details::GJK::DidNotRun:
-        HPP_FCL_ASSERT(false, "GJK did not run. It should have!",
-                       std::logic_error);
-        this->cached_guess = Vec3f(1, 0, 0);
-        this->support_func_cached_guess.setZero();
-        distance = -(std::numeric_limits<FCL_REAL>::max)();
-        p1 = p2 = normal =
-            Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
-        break;
-      case details::GJK::Failed:
-        //
-        // GJK ran out of iterations.
-        HPP_FCL_LOG_WARNING("GJK ran out of iterations.");
-        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-        break;
-      case details::GJK::NoCollisionEarlyStopped:
-        //
-        // Case where GJK early stopped because the distance was found to be
-        // above the `distance_upper_bound`.
-        // The two witness points have no meaning.
-        GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
-                                                  normal);
-        HPP_FCL_ASSERT(distance >= this->gjk.distance_upper_bound -
-                                       this->m_dummy_precision,
-                       "The distance should be bigger than GJK's "
-                       "`distance_upper_bound`.",
-                       std::logic_error);
-        break;
-      case details::GJK::NoCollision:
-        //
-        // Case where GJK converged and proved that the shapes are not in
-        // collision, i.e their distance is above GJK's tolerance (default
-        // 1e-6).
-        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-        HPP_FCL_ASSERT(std::abs((p1 - p2).norm() - distance) <=
-                           this->gjk.getTolerance() + this->m_dummy_precision,
-                       "The distance found by GJK should coincide with the "
-                       "distance between the closest points.",
-                       std::logic_error);
-        break;
-      //
-      // Next are the cases where GJK found the shapes to be in collision, i.e.
-      // their distance is below GJK's tolerance (default 1e-6).
-      case details::GJK::CollisionWithPenetrationInformation:
-        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-        HPP_FCL_ASSERT(
-            distance <= this->gjk.getTolerance() + this->m_dummy_precision,
-            "The distance found by GJK should be negative or at "
-            "least below GJK's tolerance.",
-            std::logic_error);
-        break;
-      case details::GJK::Collision:
-        if (!compute_penetration) {
-          // Skip EPA and set the witness points and the normal to nans.
-          GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
-                                                    normal);
-        } else {
-          //
-          // GJK was not enough to recover the penetration information.
-          // We need to run the EPA algorithm to find the witness points,
-          // penetration depth and the normal.
-
-          // Reset EPA algorithm. Potentially allocate memory if
-          // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's
-          // current storage.
-          this->epa.reset(this->epa_max_iterations, this->epa_tolerance);
-
-          // TODO: understand why EPA's performance is so bad on cylinders and
-          // cones.
-          this->epa.evaluate(this->gjk, -guess);
-
-          switch (epa.status) {
-            //
-            // In the following switch cases, until the "Valid" case,
-            // EPA either ran out of iterations, of faces or of vertices.
-            // The depth, witness points and the normal are still valid,
-            // simply not at the precision of EPA's tolerance.
-            // The flag `HPP_FCL_ENABLE_LOGGING` enables feebdack on these
-            // cases.
-            //
-            // TODO: Remove OutOfFaces and OutOfVertices statuses and simply
-            // compute the upper bound on max faces and max vertices as a
-            // function of the number of iterations.
-            case details::EPA::OutOfFaces:
-              HPP_FCL_LOG_WARNING("EPA ran out of faces.");
-              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-              break;
-            case details::EPA::OutOfVertices:
-              HPP_FCL_LOG_WARNING("EPA ran out of vertices.");
-              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-              break;
-            case details::EPA::Failed:
-              HPP_FCL_LOG_WARNING("EPA ran out of iterations.");
-              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-              break;
-            case details::EPA::Valid:
-            case details::EPA::AccuracyReached:
-              HPP_FCL_ASSERT(
-                  -epa.depth <= epa.getTolerance() + this->m_dummy_precision,
-                  "EPA's penetration distance should be negative (or "
-                  "at least below EPA's tolerance).",
-                  std::logic_error);
-              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-              break;
-            case details::EPA::Degenerated:
-              HPP_FCL_LOG_WARNING(
-                  "EPA warning: created a polytope with a degenerated face.");
-              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-              break;
-            case details::EPA::NonConvex:
-              HPP_FCL_LOG_WARNING(
-                  "EPA warning: EPA got called onto non-convex shapes.");
-              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-              break;
-            case details::EPA::InvalidHull:
-              HPP_FCL_LOG_WARNING("EPA warning: created an invalid polytope.");
-              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
-              break;
-            case details::EPA::DidNotRun:
-              HPP_FCL_ASSERT(false, "EPA did not run. It should have!",
-                             std::logic_error);
-              HPP_FCL_LOG_ERROR("EPA error: did not run. It should have.");
-              EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
-                                                     normal);
-              break;
-            case details::EPA::FallBack:
-              HPP_FCL_ASSERT(
-                  false,
-                  "EPA went into fallback mode. It should never do that.",
-                  std::logic_error);
-              HPP_FCL_LOG_ERROR("EPA error: FallBack.");
-              EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
-                                                     normal);
-              break;
-          }
-        }
-        break;  // End of case details::GJK::Collision
-    }
-  }
-
-  void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3f& tf1,
-                                                 FCL_REAL& distance, Vec3f& p1,
-                                                 Vec3f& p2,
-                                                 Vec3f& normal) const {
-    HPP_FCL_UNUSED_VARIABLE(tf1);
-    // Cache gjk result for potential future call to this GJKSolver.
-    this->cached_guess = this->gjk.ray;
-    this->support_func_cached_guess = this->gjk.support_hint;
-
-    distance = this->gjk.distance;
-    p1 = p2 = normal =
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
-    // If we absolutely want to return some witness points, we could use
-    // the following code (or simply merge the early stopped case with the
-    // valid case below):
-    // gjk.getWitnessPointsAndNormal(minkowski_difference, p1, p2, normal);
-    // p1 = tf1.transform(p1);
-    // p2 = tf1.transform(p2);
-    // normal = tf1.getRotation() * normal;
-  }
-
-  void GJKExtractWitnessPointsAndNormal(const Transform3f& tf1,
-                                        FCL_REAL& distance, Vec3f& p1,
-                                        Vec3f& p2, Vec3f& normal) const {
-    // Apart from early stopping, there are two cases where GJK says there is no
-    // collision:
-    // 1. GJK proved the distance is above its tolerance (default 1e-6).
-    // 2. GJK ran out of iterations.
-    // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus
-    // it can safely be normalized.
-    HPP_FCL_ASSERT(
-        this->gjk.ray.norm() >
-            this->gjk.getTolerance() - this->m_dummy_precision,
-        "The norm of GJK's ray should be bigger than GJK's tolerance.",
-        std::logic_error);
-    // Cache gjk result for potential future call to this GJKSolver.
-    this->cached_guess = this->gjk.ray;
-    this->support_func_cached_guess = this->gjk.support_hint;
-
-    distance = this->gjk.distance;
-    // TODO: On degenerated case, the closest points may be non-unique.
-    // (i.e. an object face normal is colinear to `gjk.ray`)
-    gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal);
-    Vec3f p = tf1.transform(0.5 * (p1 + p2));
-    normal = tf1.getRotation() * normal;
-    p1.noalias() = p - 0.5 * distance * normal;
-    p2.noalias() = p + 0.5 * distance * normal;
-  }
-
-  void GJKCollisionExtractWitnessPointsAndNormal(const Transform3f& tf1,
-                                                 FCL_REAL& distance, Vec3f& p1,
-                                                 Vec3f& p2,
-                                                 Vec3f& normal) const {
-    HPP_FCL_UNUSED_VARIABLE(tf1);
-    HPP_FCL_ASSERT(this->gjk.distance <=
-                       this->gjk.getTolerance() + this->m_dummy_precision,
-                   "The distance should be lower than GJK's tolerance.",
-                   std::logic_error);
-    // Because GJK has returned the `Collision` status and EPA has not run,
-    // we purposefully do not cache the result of GJK, because ray is zero.
-    // However, we can cache the support function hint.
-    // this->cached_guess = this->gjk.ray;
-    this->support_func_cached_guess = this->gjk.support_hint;
-
-    distance = this->gjk.distance;
-    p1 = p2 = normal =
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
-  }
-
-  void EPAExtractWitnessPointsAndNormal(const Transform3f& tf1,
-                                        FCL_REAL& distance, Vec3f& p1,
-                                        Vec3f& p2, Vec3f& normal) const {
-    // Cache EPA result for potential future call to this GJKSolver.
-    // This caching allows to warm-start the next GJK call.
-    this->cached_guess = -(this->epa.depth * this->epa.normal);
-    this->support_func_cached_guess = this->epa.support_hint;
-    distance = (std::min)(0., -this->epa.depth);
-    this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2,
-                                        normal);
-    // The following is very important to understand why EPA can sometimes
-    // return a normal that is not colinear to the vector $p_1 - p_2$ when
-    // working with tolerances like $\epsilon = 10^{-3}$.
-    // It can be resumed with a simple idea:
-    //     EPA is an algorithm meant to find the penetration depth and the
-    //     normal. It is not meant to find the closest points.
-    // Again, the issue here is **not** the normal, it's $p_1$ and $p_2$.
-    //
-    // More details:
-    // We'll denote $S_1$ and $S_2$ the two shapes, $n$ the normal and $p_1$ and
-    // $p_2$ the witness points. In theory, when EPA converges to $\epsilon =
-    // 0$, the normal and witness points verify the following property (P):
-    //   - $p_1 \in \partial \sigma_{S_1}(n)$,
-    //   - $p_2 \in \partial \sigma_{S_2}(-n),
-    // where $\sigma_{S_1}$ and $\sigma_{S_2}$ are the support functions of
-    // $S_1$ and $S_2$. The $\partial \sigma(n)$ simply denotes the support set
-    // of the support function in the direction $n$. (Note: I am leaving out the
-    // details of frame choice for the support function, to avoid making the
-    // mathematical notation too heavy.)
-    // --> In practice, EPA converges to $\epsilon > 0$.
-    // On polytopes and the likes, this does not change much and the property
-    // given above is still valid.
-    // --> However, this is very different on curved surfaces, such as
-    // ellipsoids, cylinders, cones, capsules etc. For these shapes, converging
-    // at $\epsilon = 10^{-6}$ or to $\epsilon = 10^{-3}$ does not change the
-    // normal much, but the property (P) given above is no longer valid, which
-    // means that the points $p_1$ and $p_2$ do not necessarily belong to the
-    // support sets in the direction of $n$ and thus $n$ and $p_1 - p_2$ are not
-    // colinear.
-    //
-    // Do not panic! This is fine.
-    // Although the property above is not verified, it's almost verified,
-    // meaning that $p_1$ and $p_2$ belong to support sets in directions that
-    // are very close to $n$.
-    //
-    // Solution to compute better $p_1$ and $p_2$:
-    // We compute the middle points of the current $p_1$ and $p_2$ and we use
-    // the normal and the distance given by EPA to compute the new $p_1$ and
-    // $p_2$.
-    Vec3f p = tf1.transform(0.5 * (p1 + p2));
-    normal = tf1.getRotation() * normal;
-    p1.noalias() = p - 0.5 * distance * normal;
-    p2.noalias() = p + 0.5 * distance * normal;
-  }
-
-  void EPAFailedExtractWitnessPointsAndNormal(const Transform3f& tf1,
-                                              FCL_REAL& distance, Vec3f& p1,
-                                              Vec3f& p2, Vec3f& normal) const {
-    this->cached_guess = Vec3f(1, 0, 0);
-    this->support_func_cached_guess.setZero();
-
-    HPP_FCL_UNUSED_VARIABLE(tf1);
-    distance = -(std::numeric_limits<FCL_REAL>::max)();
-    p1 = p2 = normal =
-        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
-  }
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/narrowphase/narrowphase.h>
diff --git a/include/hpp/fcl/narrowphase/narrowphase_defaults.h b/include/hpp/fcl/narrowphase/narrowphase_defaults.h
index fcf3cd41c035d322492b8b609466c5ddae135961..c17096f9a67eb1da5bf41cc9ef82f194512cc6e3 100644
--- a/include/hpp/fcl/narrowphase/narrowphase_defaults.h
+++ b/include/hpp/fcl/narrowphase/narrowphase_defaults.h
@@ -1,67 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2024, INRIA
- *  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.
- */
-
-/// This file defines different macros used to characterize the default behavior
-/// of the narrowphase algorithms GJK and EPA.
-
-#ifndef HPP_FCL_NARROWPHASE_DEFAULTS
-#define HPP_FCL_NARROWPHASE_DEFAULTS
-
-#include <hpp/fcl/data_types.h>
-
-namespace hpp {
-namespace fcl {
-
-/// GJK
-constexpr size_t GJK_DEFAULT_MAX_ITERATIONS = 128;
-constexpr FCL_REAL GJK_DEFAULT_TOLERANCE = 1e-6;
-/// Note: if the considered shapes are on the order of the meter, and the
-/// convergence criterion of GJK is the default VDB criterion,
-/// setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to
-/// the micro-meter.
-/// The same is true for EPA.
-constexpr FCL_REAL GJK_MINIMUM_TOLERANCE = 1e-6;
-
-/// EPA
-/// EPA build a polytope which maximum size is:
-///   - `#iterations + 4` vertices
-///   - `2 x #iterations + 4` faces
-constexpr size_t EPA_DEFAULT_MAX_ITERATIONS = 64;
-constexpr FCL_REAL EPA_DEFAULT_TOLERANCE = 1e-6;
-constexpr FCL_REAL EPA_MINIMUM_TOLERANCE = 1e-6;
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // HPP_FCL_NARROWPHASE_DEFAULTS
+#include <hpp/fcl/coal.hpp>
+#include <coal/narrowphase/narrowphase_defaults.h>
diff --git a/include/hpp/fcl/narrowphase/support_functions.h b/include/hpp/fcl/narrowphase/support_functions.h
index 951240cf58d29f95ea167ea479c9ac40e84de99c..8eb7c4b4fbfa09f1c4a518cdd4263356ea82a030 100644
--- a/include/hpp/fcl/narrowphase/support_functions.h
+++ b/include/hpp/fcl/narrowphase/support_functions.h
@@ -1,310 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2021-2024, INRIA
- *  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.
- */
-
-/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
-
-#ifndef HPP_FCL_SUPPORT_FUNCTIONS_H
-#define HPP_FCL_SUPPORT_FUNCTIONS_H
-
-#include "hpp/fcl/shape/geometric_shapes.h"
-#include "hpp/fcl/math/transform.h"
-#include "hpp/fcl/collision_data.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace details {
-
-/// @brief Options for the computation of support points.
-/// `NoSweptSphere` option is used when the support function is called
-/// by GJK or EPA. In this case, the swept sphere radius is not taken into
-/// account in the support function. It is used by GJK and EPA after they have
-/// converged to correct the solution.
-/// `WithSweptSphere` option is used when the support function is called
-/// directly by the user. In this case, the swept sphere radius is taken into
-/// account in the support function.
-enum SupportOptions {
-  NoSweptSphere = 0,
-  WithSweptSphere = 0x1,
-};
-
-// ============================================================================
-// ============================ SUPPORT FUNCTIONS =============================
-// ============================================================================
-/// @brief the support function for shape.
-/// The output support point is expressed in the local frame of the shape.
-/// @return a point which belongs to the set {argmax_{v in shape} v.dot(dir)}.
-/// @param shape the shape.
-/// @param dir support direction.
-/// @param hint used to initialize the search when shape is a ConvexBase object.
-/// @tparam SupportOptions is a value of the SupportOptions enum. If set to
-/// `WithSweptSphere`, the support functions take into account the shapes' swept
-/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const
-/// ShapeBase*)` for more details.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint);
-
-/// @brief Stores temporary data for the computation of support points.
-struct HPP_FCL_DLLAPI ShapeSupportData {
-  // @brief Tracks which points have been visited in a ConvexBase.
-  std::vector<int8_t> visited;
-
-  // @brief Tracks the last support direction used on this shape; used to
-  // warm-start the ConvexBase support function.
-  Vec3f last_dir = Vec3f::Zero();
-
-  // @brief Temporary set used to compute the convex-hull of a support set.
-  // Only used for ConvexBase and Box.
-  SupportSet::Polygon polygon;
-};
-
-/// @brief Triangle support function.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
-                     Vec3f& support, int& /*unused*/,
-                     ShapeSupportData& /*unused*/);
-
-/// @brief Box support function.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support,
-                     int& /*unused*/, ShapeSupportData& /*unused*/);
-
-/// @brief Sphere support function.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& support,
-                     int& /*unused*/, ShapeSupportData& /*unused*/);
-
-/// @brief Ellipsoid support function.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir,
-                     Vec3f& support, int& /*unused*/,
-                     ShapeSupportData& /*unused*/);
-
-/// @brief Capsule support function.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support,
-                     int& /*unused*/, ShapeSupportData& /*unused*/);
-
-/// @brief Cone support function.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
-                     int& /*unused*/, ShapeSupportData& /*unused*/);
-
-/// @brief Cylinder support function.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
-                     int& /*unused*/, ShapeSupportData& /*unused*/);
-
-/// @brief ConvexBase support function.
-/// @note See @ref LargeConvex and SmallConvex to see how to optimize
-/// ConvexBase's support computation.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
-                     int& hint, ShapeSupportData& /*unused*/);
-
-/// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of
-/// `getShapeSupport`. This is **much** faster than the linear version of
-/// `getShapeSupport` when a `ConvexBase` has more than a few dozen of vertices.
-/// @note WARNING: when using a LargeConvex, the neighbors in `ConvexBase` must
-/// have been constructed! Otherwise the support function will segfault.
-struct LargeConvex : ShapeBase {};
-/// @brief See @ref LargeConvex.
-struct SmallConvex : ShapeBase {};
-
-/// @brief Support function for large ConvexBase (>32 vertices).
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const SmallConvex* convex, const Vec3f& dir,
-                     Vec3f& support, int& hint, ShapeSupportData& data);
-
-/// @brief Support function for small ConvexBase (<32 vertices).
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
-                     Vec3f& support, int& hint, ShapeSupportData& support_data);
-
-// ============================================================================
-// ========================== SUPPORT SET FUNCTIONS ===========================
-// ============================================================================
-/// @brief Computes the support set for shape.
-/// This function assumes the frame of the support set has already been
-/// computed and that this frame is expressed w.r.t the local frame of the
-/// shape (i.e. the local frame of the shape is the WORLD frame of the support
-/// set). The support direction used to compute the support set is the positive
-/// z-axis if the support set has the DEFAULT direction; negative z-axis if it
-/// has the INVERTED direction. (In short, a shape's support set is has the
-/// DEFAULT direction if the shape is the first shape in a collision pair. It
-/// has the INVERTED direction if the shape is the second one in the collision
-/// pair).
-/// @return an approximation of the set {argmax_{v in shape} v.dot(dir)}, where
-/// dir is the support set's support direction.
-/// The support set is a plane passing by the origin of the support set frame
-/// and supported by the direction dir. As a consequence, any point added to the
-/// set is automatically projected onto this plane.
-/// @param[in] shape the shape.
-/// @param[in/out] support_set of shape.
-/// @param[in/out] hint used to initialize the search when shape is a ConvexBase
-/// object.
-/// @param[in] num_sampled_supports is only used for shapes with smooth
-/// non-strictly convex bases like cones and cylinders (their bases are
-/// circles). In such a case, if the support direction points to their base, we
-/// have to choose which points we want to add to the set. This is not needed
-/// for boxes or ConvexBase for example. Indeed, because their support sets are
-/// always polygons, we can characterize the entire support set with the
-/// vertices of the polygon.
-/// @param[in] tol given a point v on the shape, if
-/// `max_{p in shape}(p.dot(dir)) - v.dot(dir) <= tol`, where dir is the set's
-/// support direction, then v is added to the support set.
-/// Otherwise said, if a point p of the shape is at a distance `tol` from the
-/// support plane, it is added to the set. Thus, `tol` can be seen as the
-/// "thickness" of the support plane.
-/// @tparam SupportOptions is a value of the SupportOptions enum. If set to
-/// `WithSweptSphere`, the support functions take into account the shapes' swept
-/// sphere radii.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint,
-                   size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3);
-
-/// @brief Same as @ref getSupportSet(const ShapeBase*, const FCL_REAL,
-/// SupportSet&, const int) but also constructs the support set frame from
-/// `dir`.
-/// @note The support direction `dir` is expressed in the local frame of the
-/// shape.
-/// @note This function automatically deals with the `direction` of the
-/// SupportSet.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getSupportSet(const ShapeBase* shape, const Vec3f& dir,
-                   SupportSet& support_set, int& hint,
-                   size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3) {
-  support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir);
-  const Vec3f& support_dir = support_set.getNormal();
-  const Vec3f support = getSupport<_SupportOptions>(shape, support_dir, hint);
-  getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports,
-                                 tol);
-}
-
-/// @brief Triangle support set function.
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set,
-                        int& /*unused*/, ShapeSupportData& /*unused*/,
-                        size_t /*unused*/ num_sampled_supports = 6,
-                        FCL_REAL tol = 1e-3);
-
-/// @brief Box support set function.
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const Box* box, SupportSet& support_set,
-                        int& /*unused*/, ShapeSupportData& support_data,
-                        size_t /*unused*/ num_sampled_supports = 6,
-                        FCL_REAL tol = 1e-3);
-
-/// @brief Sphere support set function.
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set,
-                        int& /*unused*/, ShapeSupportData& /*unused*/,
-                        size_t /*unused*/ num_sampled_supports = 6,
-                        FCL_REAL /*unused*/ tol = 1e-3);
-
-/// @brief Ellipsoid support set function.
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set,
-                        int& /*unused*/, ShapeSupportData& /*unused*/,
-                        size_t /*unused*/ num_sampled_supports = 6,
-                        FCL_REAL /*unused*/ tol = 1e-3);
-
-/// @brief Capsule support set function.
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
-                        int& /*unused*/, ShapeSupportData& /*unused*/,
-                        size_t /*unused*/ num_sampled_supports = 6,
-                        FCL_REAL tol = 1e-3);
-
-/// @brief Cone support set function.
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
-                        int& /*unused*/, ShapeSupportData& /*unused*/,
-                        size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3);
-
-/// @brief Cylinder support set function.
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
-                        int& /*unused*/, ShapeSupportData& /*unused*/,
-                        size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3);
-
-/// @brief ConvexBase support set function.
-/// Assumes the support set frame has already been computed.
-/// @note See @ref LargeConvex and SmallConvex to see how to optimize
-/// ConvexBase's support computation.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set,
-                        int& hint, ShapeSupportData& support_data,
-                        size_t /*unused*/ num_sampled_supports = 6,
-                        FCL_REAL tol = 1e-3);
-
-/// @brief Support set function for large ConvexBase (>32 vertices).
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set,
-                        int& /*unused*/, ShapeSupportData& /*unused*/,
-                        size_t /*unused*/ num_sampled_supports = 6,
-                        FCL_REAL tol = 1e-3);
-
-/// @brief Support set function for small ConvexBase (<32 vertices).
-/// Assumes the support set frame has already been computed.
-template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set,
-                        int& hint, ShapeSupportData& support_data,
-                        size_t /*unused*/ num_sampled_supports = 6,
-                        FCL_REAL tol = 1e-3);
-
-/// @brief Computes the convex-hull of support_set. For now, this function is
-/// only needed for Box and ConvexBase.
-/// @param[in] cloud data which contains the 2d points of the support set which
-/// convex-hull we want to compute.
-/// @param[out] 2d points of the the support set's convex-hull.
-HPP_FCL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
-                                                SupportSet::Polygon& cvx_hull);
-
-}  // namespace details
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // HPP_FCL_SUPPORT_FUNCTIONS_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/narrowphase/support_functions.h>
diff --git a/include/hpp/fcl/octree.h b/include/hpp/fcl/octree.h
index feec9a7eb21430bf7608cef6f4f466df575c4287..b7f745e796bc2289b27201bc076e25a2c84a2c66 100644
--- a/include/hpp/fcl/octree.h
+++ b/include/hpp/fcl/octree.h
@@ -1,342 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2022-2024, Inria
- *  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 HPP_FCL_OCTREE_H
-#define HPP_FCL_OCTREE_H
-
-#include <algorithm>
-
-#include <octomap/octomap.h>
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/BV/AABB.h>
-#include <hpp/fcl/collision_object.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Octree is one type of collision geometry which can encode uncertainty
-/// information in the sensor data.
-class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
- protected:
-  shared_ptr<const octomap::OcTree> tree;
-
-  FCL_REAL default_occupancy;
-
-  FCL_REAL occupancy_threshold;
-  FCL_REAL free_threshold;
-
- public:
-  typedef octomap::OcTreeNode OcTreeNode;
-
-  /// @brief construct octree with a given resolution
-  explicit OcTree(FCL_REAL resolution)
-      : tree(shared_ptr<const octomap::OcTree>(
-            new octomap::OcTree(resolution))) {
-    default_occupancy = tree->getOccupancyThres();
-
-    // default occupancy/free threshold is consistent with default setting from
-    // octomap
-    occupancy_threshold = tree->getOccupancyThres();
-    free_threshold = 0;
-  }
-
-  /// @brief construct octree from octomap
-  explicit OcTree(const shared_ptr<const octomap::OcTree>& tree_)
-      : tree(tree_) {
-    default_occupancy = tree->getOccupancyThres();
-
-    // default occupancy/free threshold is consistent with default setting from
-    // octomap
-    occupancy_threshold = tree->getOccupancyThres();
-    free_threshold = 0;
-  }
-
-  ///  \brief Copy constructor
-  OcTree(const OcTree& other)
-      : CollisionGeometry(other),
-        tree(other.tree),
-        default_occupancy(other.default_occupancy),
-        occupancy_threshold(other.occupancy_threshold),
-        free_threshold(other.free_threshold) {}
-
-  /// \brief Clone *this into a new Octree
-  OcTree* clone() const { return new OcTree(*this); }
-
-  /// \brief Returns the tree associated to the underlying octomap OcTree.
-  shared_ptr<const octomap::OcTree> getTree() const { return tree; }
-
-  void exportAsObjFile(const std::string& filename) const;
-
-  /// @brief compute the AABB for the octree in its local coordinate system
-  void computeLocalAABB() {
-    typedef Eigen::Matrix<float, 3, 1> Vec3float;
-    Vec3float max_extent, min_extent;
-
-    octomap::OcTree::iterator it =
-        tree->begin((unsigned char)tree->getTreeDepth());
-    octomap::OcTree::iterator end = tree->end();
-
-    if (it == end) return;
-
-    {
-      const octomap::point3d& coord =
-          it.getCoordinate();  // getCoordinate returns a copy
-      max_extent = min_extent = Eigen::Map<const Vec3float>(&coord.x());
-      for (++it; it != end; ++it) {
-        const octomap::point3d& coord = it.getCoordinate();
-        const Vec3float pos = Eigen::Map<const Vec3float>(&coord.x());
-        max_extent = max_extent.array().max(pos.array());
-        min_extent = min_extent.array().min(pos.array());
-      }
-    }
-
-    // Account for the size of the boxes.
-    const FCL_REAL resolution = tree->getResolution();
-    max_extent.array() += float(resolution / 2.);
-    min_extent.array() -= float(resolution / 2.);
-
-    aabb_local = AABB(min_extent.cast<FCL_REAL>(), max_extent.cast<FCL_REAL>());
-    aabb_center = aabb_local.center();
-    aabb_radius = (aabb_local.min_ - aabb_center).norm();
-  }
-
-  /// @brief get the bounding volume for the root
-  AABB getRootBV() const {
-    FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
-
-    // std::cout << "octree size " << delta << std::endl;
-    return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
-  }
-
-  /// @brief Returns the depth of the octree
-  unsigned int getTreeDepth() const { return tree->getTreeDepth(); }
-
-  /// @brief Returns the size of the octree
-  unsigned long size() const { return tree->size(); }
-
-  /// @brief Returns the resolution of the octree
-  FCL_REAL getResolution() const { return tree->getResolution(); }
-
-  /// @brief get the root node of the octree
-  OcTreeNode* getRoot() const { return tree->getRoot(); }
-
-  /// @brief whether one node is completely occupied
-  bool isNodeOccupied(const OcTreeNode* node) const {
-    // return tree->isNodeOccupied(node);
-    return node->getOccupancy() >= occupancy_threshold;
-  }
-
-  /// @brief whether one node is completely free
-  bool isNodeFree(const OcTreeNode* node) const {
-    // return false; // default no definitely free node
-    return node->getOccupancy() <= free_threshold;
-  }
-
-  /// @brief whether one node is uncertain
-  bool isNodeUncertain(const OcTreeNode* node) const {
-    return (!isNodeOccupied(node)) && (!isNodeFree(node));
-  }
-
-  /// @brief transform the octree into a bunch of boxes; uncertainty information
-  /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the
-  /// boxes whose occupied probability is higher enough).
-  std::vector<Vec6f> toBoxes() const {
-    std::vector<Vec6f> boxes;
-    boxes.reserve(tree->size() / 2);
-    for (octomap::OcTree::iterator
-             it = tree->begin((unsigned char)tree->getTreeDepth()),
-             end = tree->end();
-         it != end; ++it) {
-      // if(tree->isNodeOccupied(*it))
-      if (isNodeOccupied(&*it)) {
-        FCL_REAL x = it.getX();
-        FCL_REAL y = it.getY();
-        FCL_REAL z = it.getZ();
-        FCL_REAL size = it.getSize();
-        FCL_REAL c = (*it).getOccupancy();
-        FCL_REAL t = tree->getOccupancyThres();
-
-        Vec6f box;
-        box << x, y, z, size, c, t;
-        boxes.push_back(box);
-      }
-    }
-    return boxes;
-  }
-
-  /// \brief Returns a byte description of *this
-  std::vector<uint8_t> tobytes() const {
-    typedef Eigen::Matrix<float, 3, 1> Vec3float;
-    const size_t total_size = (tree->size() * sizeof(FCL_REAL) * 3) / 2;
-    std::vector<uint8_t> bytes;
-    bytes.reserve(total_size);
-
-    for (octomap::OcTree::iterator
-             it = tree->begin((unsigned char)tree->getTreeDepth()),
-             end = tree->end();
-         it != end; ++it) {
-      const Vec3f box_pos =
-          Eigen::Map<Vec3float>(&it.getCoordinate().x()).cast<FCL_REAL>();
-      if (isNodeOccupied(&*it))
-        std::copy(box_pos.data(), box_pos.data() + sizeof(FCL_REAL) * 3,
-                  std::back_inserter(bytes));
-    }
-
-    return bytes;
-  }
-
-  /// @brief the threshold used to decide whether one node is occupied, this is
-  /// NOT the octree occupied_thresold
-  FCL_REAL getOccupancyThres() const { return occupancy_threshold; }
-
-  /// @brief the threshold used to decide whether one node is free, this is NOT
-  /// the octree free_threshold
-  FCL_REAL getFreeThres() const { return free_threshold; }
-
-  FCL_REAL getDefaultOccupancy() const { return default_occupancy; }
-
-  void setCellDefaultOccupancy(FCL_REAL d) { default_occupancy = d; }
-
-  void setOccupancyThres(FCL_REAL d) { occupancy_threshold = d; }
-
-  void setFreeThres(FCL_REAL d) { free_threshold = d; }
-
-  /// @return ptr to child number childIdx of node
-  OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) {
-#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
-    return tree->getNodeChild(node, childIdx);
-#else
-    return node->getChild(childIdx);
-#endif
-  }
-
-  /// @return const ptr to child number childIdx of node
-  const OcTreeNode* getNodeChild(const OcTreeNode* node,
-                                 unsigned int childIdx) const {
-#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
-    return tree->getNodeChild(node, childIdx);
-#else
-    return node->getChild(childIdx);
-#endif
-  }
-
-  /// @brief return true if the child at childIdx exists
-  bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const {
-#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
-    return tree->nodeChildExists(node, childIdx);
-#else
-    return node->childExists(childIdx);
-#endif
-  }
-
-  /// @brief return true if node has at least one child
-  bool nodeHasChildren(const OcTreeNode* node) const {
-#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
-    return tree->nodeHasChildren(node);
-#else
-    return node->hasChildren();
-#endif
-  }
-
-  /// @brief return object type, it is an octree
-  OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
-
-  /// @brief return node type, it is an octree
-  NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const OcTree* other_ptr = dynamic_cast<const OcTree*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const OcTree& other = *other_ptr;
-
-    return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) &&
-           default_occupancy == other.default_occupancy &&
-           occupancy_threshold == other.occupancy_threshold &&
-           free_threshold == other.free_threshold;
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief compute the bounding volume of an octree node's i-th child
-static inline void computeChildBV(const AABB& root_bv, unsigned int i,
-                                  AABB& child_bv) {
-  if (i & 1) {
-    child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
-    child_bv.max_[0] = root_bv.max_[0];
-  } else {
-    child_bv.min_[0] = root_bv.min_[0];
-    child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
-  }
-
-  if (i & 2) {
-    child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
-    child_bv.max_[1] = root_bv.max_[1];
-  } else {
-    child_bv.min_[1] = root_bv.min_[1];
-    child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
-  }
-
-  if (i & 4) {
-    child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
-    child_bv.max_[2] = root_bv.max_[2];
-  } else {
-    child_bv.min_[2] = root_bv.min_[2];
-    child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
-  }
-}
-
-///
-/// \brief Build an OcTree from a point cloud and a given resolution
-///
-/// \param[in] point_cloud The input points to insert in the OcTree
-/// \param[in] resolution of the octree.
-///
-/// \returns An OcTree that can be used for collision checking and more.
-///
-HPP_FCL_DLLAPI OcTreePtr_t
-makeOctree(const Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3>& point_cloud,
-           const FCL_REAL resolution);
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/octree.h>
diff --git a/include/hpp/fcl/serialization/AABB.h b/include/hpp/fcl/serialization/AABB.h
index 85ff6491543cb8f533bb8928b4a38eb504f319a9..b001c4ec31bc9717c3621ecb486fc1178f090edd 100644
--- a/include/hpp/fcl/serialization/AABB.h
+++ b/include/hpp/fcl/serialization/AABB.h
@@ -1,24 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_AABB_H
-#define HPP_FCL_SERIALIZATION_AABB_H
-
-#include "hpp/fcl/BV/AABB.h"
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::AABB& aabb,
-               const unsigned int /*version*/) {
-  ar& make_nvp("min_", aabb.min_);
-  ar& make_nvp("max_", aabb.max_);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_AABB_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/AABB.h>
diff --git a/include/hpp/fcl/serialization/BVH_model.h b/include/hpp/fcl/serialization/BVH_model.h
index 47e4fb787a2ff31a19d75423fe25859a6ae07874..c4f65ab501b34bd72126ae8e78b31bc29137b9f6 100644
--- a/include/hpp/fcl/serialization/BVH_model.h
+++ b/include/hpp/fcl/serialization/BVH_model.h
@@ -1,252 +1,2 @@
-//
-// Copyright (c) 2021-2022 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H
-#define HPP_FCL_SERIALIZATION_BVH_MODEL_H
-
-#include "hpp/fcl/BVH/BVH_model.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-#include "hpp/fcl/serialization/BV_node.h"
-#include "hpp/fcl/serialization/BV_splitter.h"
-#include "hpp/fcl/serialization/collision_object.h"
-#include "hpp/fcl/serialization/memory.h"
-#include "hpp/fcl/serialization/triangle.h"
-
-namespace boost {
-namespace serialization {
-
-namespace internal {
-struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase {
-  typedef hpp::fcl::BVHModelBase Base;
-  using Base::num_tris_allocated;
-  using Base::num_vertices_allocated;
-};
-}  // namespace internal
-
-template <class Archive>
-void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model,
-          const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-  if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED ||
-        bvh_model.build_state == BVH_BUILD_STATE_UPDATED) &&
-      (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) {
-    HPP_FCL_THROW_PRETTY(
-        "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or "
-        "BVH_BUILD_STATE_UPDATED state.\n"
-        "The BVHModel could not be serialized.",
-        std::invalid_argument);
-  }
-
-  ar &make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::CollisionGeometry>(
-                   bvh_model));
-
-  ar &make_nvp("num_vertices", bvh_model.num_vertices);
-  ar &make_nvp("vertices", bvh_model.vertices);
-
-  ar &make_nvp("num_tris", bvh_model.num_tris);
-  ar &make_nvp("tri_indices", bvh_model.tri_indices);
-  ar &make_nvp("build_state", bvh_model.build_state);
-
-  ar &make_nvp("prev_vertices", bvh_model.prev_vertices);
-
-  //      if(bvh_model.convex)
-  //      {
-  //        const bool has_convex = true;
-  //        ar << make_nvp("has_convex",has_convex);
-  //      }
-  //      else
-  //      {
-  //        const bool has_convex = false;
-  //        ar << make_nvp("has_convex",has_convex);
-  //      }
-}
-
-template <class Archive>
-void load(Archive &ar, hpp::fcl::BVHModelBase &bvh_model,
-          const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-
-  ar >> make_nvp("base",
-                 boost::serialization::base_object<hpp::fcl::CollisionGeometry>(
-                     bvh_model));
-
-  ar >> make_nvp("num_vertices", bvh_model.num_vertices);
-  ar >> make_nvp("vertices", bvh_model.vertices);
-
-  ar >> make_nvp("num_tris", bvh_model.num_tris);
-  ar >> make_nvp("tri_indices", bvh_model.tri_indices);
-  ar >> make_nvp("build_state", bvh_model.build_state);
-
-  ar >> make_nvp("prev_vertices", bvh_model.prev_vertices);
-
-  //      bool has_convex = true;
-  //      ar >> make_nvp("has_convex",has_convex);
-}
-
-HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::BVHModelBase)
-
-namespace internal {
-template <typename BV>
-struct BVHModelAccessor : hpp::fcl::BVHModel<BV> {
-  typedef hpp::fcl::BVHModel<BV> Base;
-  using Base::bvs;
-  using Base::num_bvs;
-  using Base::num_bvs_allocated;
-  using Base::primitive_indices;
-};
-}  // namespace internal
-
-template <class Archive, typename BV>
-void serialize(Archive &ar, hpp::fcl::BVHModel<BV> &bvh_model,
-               const unsigned int version) {
-  split_free(ar, bvh_model, version);
-}
-
-template <class Archive, typename BV>
-void save(Archive &ar, const hpp::fcl::BVHModel<BV> &bvh_model_,
-          const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-  typedef internal::BVHModelAccessor<BV> Accessor;
-  typedef BVNode<BV> Node;
-
-  const Accessor &bvh_model = reinterpret_cast<const Accessor &>(bvh_model_);
-  ar &make_nvp("base",
-               boost::serialization::base_object<BVHModelBase>(bvh_model));
-
-  //      if(bvh_model.primitive_indices)
-  //      {
-  //        const bool with_primitive_indices = true;
-  //        ar & make_nvp("with_primitive_indices",with_primitive_indices);
-  //
-  //        int num_primitives = 0;
-  //        switch(bvh_model.getModelType())
-  //        {
-  //          case BVH_MODEL_TRIANGLES:
-  //            num_primitives = bvh_model.num_tris;
-  //            break;
-  //          case BVH_MODEL_POINTCLOUD:
-  //            num_primitives = bvh_model.num_vertices;
-  //            break;
-  //          default:
-  //            ;
-  //        }
-  //
-  //        ar & make_nvp("num_primitives",num_primitives);
-  //        if(num_primitives > 0)
-  //        {
-  //          typedef Eigen::Matrix<unsigned int,1,Eigen::Dynamic>
-  //          AsPrimitiveIndexVector; const Eigen::Map<const
-  //          AsPrimitiveIndexVector>
-  //          primitive_indices_map(reinterpret_cast<const unsigned int
-  //          *>(bvh_model.primitive_indices),1,num_primitives); ar &
-  //          make_nvp("primitive_indices",primitive_indices_map);
-  ////          ar &
-  /// make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
-  //        }
-  //      }
-  //      else
-  //      {
-  //        const bool with_primitive_indices = false;
-  //        ar & make_nvp("with_primitive_indices",with_primitive_indices);
-  //      }
-  //
-
-  if (bvh_model.bvs.get()) {
-    const bool with_bvs = true;
-    ar &make_nvp("with_bvs", with_bvs);
-    ar &make_nvp("num_bvs", bvh_model.num_bvs);
-    ar &make_nvp(
-        "bvs",
-        make_array(
-            reinterpret_cast<const char *>(bvh_model.bvs->data()),
-            sizeof(Node) *
-                (std::size_t)bvh_model.num_bvs));  // Assuming BVs are POD.
-  } else {
-    const bool with_bvs = false;
-    ar &make_nvp("with_bvs", with_bvs);
-  }
-}
-
-template <class Archive, typename BV>
-void load(Archive &ar, hpp::fcl::BVHModel<BV> &bvh_model_,
-          const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-  typedef internal::BVHModelAccessor<BV> Accessor;
-  typedef BVNode<BV> Node;
-
-  Accessor &bvh_model = reinterpret_cast<Accessor &>(bvh_model_);
-
-  ar >> make_nvp("base",
-                 boost::serialization::base_object<BVHModelBase>(bvh_model));
-
-  //      bool with_primitive_indices;
-  //      ar >> make_nvp("with_primitive_indices",with_primitive_indices);
-  //      if(with_primitive_indices)
-  //      {
-  //        int num_primitives;
-  //        ar >> make_nvp("num_primitives",num_primitives);
-  //
-  //        delete[] bvh_model.primitive_indices;
-  //        if(num_primitives > 0)
-  //        {
-  //          bvh_model.primitive_indices = new unsigned int[num_primitives];
-  //          ar &
-  //          make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
-  //        }
-  //        else
-  //          bvh_model.primitive_indices = NULL;
-  //      }
-
-  bool with_bvs;
-  ar >> make_nvp("with_bvs", with_bvs);
-  if (with_bvs) {
-    unsigned int num_bvs;
-    ar >> make_nvp("num_bvs", num_bvs);
-
-    if (num_bvs != bvh_model.num_bvs) {
-      bvh_model.bvs.reset();
-      bvh_model.num_bvs = num_bvs;
-      if (num_bvs > 0)
-        bvh_model.bvs.reset(new
-                            typename BVHModel<BV>::bv_node_vector_t(num_bvs));
-    }
-    if (num_bvs > 0) {
-      ar >> make_nvp("bvs",
-                     make_array(reinterpret_cast<char *>(bvh_model.bvs->data()),
-                                sizeof(Node) * (std::size_t)num_bvs));
-    } else
-      bvh_model.bvs.reset();
-  }
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-namespace hpp {
-namespace fcl {
-
-namespace internal {
-template <typename BV>
-struct memory_footprint_evaluator<::hpp::fcl::BVHModel<BV>> {
-  static size_t run(const ::hpp::fcl::BVHModel<BV> &bvh_model) {
-    return static_cast<size_t>(bvh_model.memUsage(false));
-  }
-};
-}  // namespace internal
-
-}  // namespace fcl
-}  // namespace hpp
-
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::AABB>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::OBB>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::RSS>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::OBBRSS>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::kIOS>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<16>>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<18>>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<24>>)
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/BVH_model.h>
diff --git a/include/hpp/fcl/serialization/BV_node.h b/include/hpp/fcl/serialization/BV_node.h
index 78193cda5554a9a550a617039732b3060ba8cf93..a8407a076f223b2f9130862ef32a1c0446429515 100644
--- a/include/hpp/fcl/serialization/BV_node.h
+++ b/include/hpp/fcl/serialization/BV_node.h
@@ -1,35 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_BV_NODE_H
-#define HPP_FCL_SERIALIZATION_BV_NODE_H
-
-#include "hpp/fcl/BV/BV_node.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-#include "hpp/fcl/serialization/OBBRSS.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::BVNodeBase& node,
-               const unsigned int /*version*/) {
-  ar& make_nvp("first_child", node.first_child);
-  ar& make_nvp("first_primitive", node.first_primitive);
-  ar& make_nvp("num_primitives", node.num_primitives);
-}
-
-template <class Archive, typename BV>
-void serialize(Archive& ar, hpp::fcl::BVNode<BV>& node,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::BVNodeBase>(node));
-  ar& make_nvp("bv", node.bv);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_BV_NODE_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/BV_node.h>
diff --git a/include/hpp/fcl/serialization/BV_splitter.h b/include/hpp/fcl/serialization/BV_splitter.h
index 24eac7649b64dee4083c1d7ae534645758810ed7..7d13844e31ca6b068d2328b4670fb91034858a68 100644
--- a/include/hpp/fcl/serialization/BV_splitter.h
+++ b/include/hpp/fcl/serialization/BV_splitter.h
@@ -1,62 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H
-#define HPP_FCL_SERIALIZATION_BV_SPLITTER_H
-
-#include "hpp/fcl/internal/BV_splitter.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-namespace internal {
-template <typename BV>
-struct BVSplitterAccessor : hpp::fcl::BVSplitter<BV> {
-  typedef hpp::fcl::BVSplitter<BV> Base;
-  using Base::split_axis;
-  using Base::split_method;
-  using Base::split_value;
-  using Base::split_vector;
-  using Base::tri_indices;
-  using Base::type;
-  using Base::vertices;
-};
-}  // namespace internal
-
-template <class Archive, typename BV>
-void save(Archive &ar, const hpp::fcl::BVSplitter<BV> &splitter_,
-          const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-  typedef internal::BVSplitterAccessor<BV> Accessor;
-  const Accessor &splitter = reinterpret_cast<const Accessor &>(splitter_);
-
-  ar &make_nvp("split_axis", splitter.split_axis);
-  ar &make_nvp("split_vector", splitter.split_vector);
-  ar &make_nvp("split_value", splitter.split_value);
-  ar &make_nvp("type", splitter.type);
-  ar &make_nvp("split_method", splitter.split_method);
-}
-
-template <class Archive, typename BV>
-void load(Archive &ar, hpp::fcl::BVSplitter<BV> &splitter_,
-          const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-  typedef internal::BVSplitterAccessor<BV> Accessor;
-  Accessor &splitter = reinterpret_cast<Accessor &>(splitter_);
-
-  ar >> make_nvp("split_axis", splitter.split_axis);
-  ar >> make_nvp("split_vector", splitter.split_vector);
-  ar >> make_nvp("split_value", splitter.split_value);
-  ar >> make_nvp("type", splitter.type);
-  ar >> make_nvp("split_method", splitter.split_method);
-
-  splitter.vertices = NULL;
-  splitter.tri_indices = NULL;
-}
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/BV_splitter.h>
diff --git a/include/hpp/fcl/serialization/OBB.h b/include/hpp/fcl/serialization/OBB.h
index 2c07a44e7feca5478d4856d3749db690c4c63762..8eebfec177a02aaaafea8bc72e5164e59bd0c9da 100644
--- a/include/hpp/fcl/serialization/OBB.h
+++ b/include/hpp/fcl/serialization/OBB.h
@@ -1,25 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_OBB_H
-#define HPP_FCL_SERIALIZATION_OBB_H
-
-#include "hpp/fcl/BV/OBB.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::OBB& bv, const unsigned int /*version*/) {
-  ar& make_nvp("axes", bv.axes);
-  ar& make_nvp("To", bv.To);
-  ar& make_nvp("extent", bv.extent);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_OBB_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/OBB.h>
diff --git a/include/hpp/fcl/serialization/OBBRSS.h b/include/hpp/fcl/serialization/OBBRSS.h
index b19ce3a3632f8ad903f213715af1329284c3b88c..c9673a7f4686a43b5051643913e9e7321c614a02 100644
--- a/include/hpp/fcl/serialization/OBBRSS.h
+++ b/include/hpp/fcl/serialization/OBBRSS.h
@@ -1,27 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_OBBRSS_H
-#define HPP_FCL_SERIALIZATION_OBBRSS_H
-
-#include "hpp/fcl/BV/OBBRSS.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-#include "hpp/fcl/serialization/OBB.h"
-#include "hpp/fcl/serialization/RSS.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::OBBRSS& bv,
-               const unsigned int /*version*/) {
-  ar& make_nvp("obb", bv.obb);
-  ar& make_nvp("rss", bv.rss);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_OBBRSS_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/OBBRSS.h>
diff --git a/include/hpp/fcl/serialization/RSS.h b/include/hpp/fcl/serialization/RSS.h
index 4f1f7ee7590bbbfff25657204ef61a0a4d3bb683..b7639aa8627490d88706002ea68378c2dc2203c9 100644
--- a/include/hpp/fcl/serialization/RSS.h
+++ b/include/hpp/fcl/serialization/RSS.h
@@ -1,26 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_RSS_H
-#define HPP_FCL_SERIALIZATION_RSS_H
-
-#include "hpp/fcl/BV/RSS.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::RSS& bv, const unsigned int /*version*/) {
-  ar& make_nvp("axes", bv.axes);
-  ar& make_nvp("Tr", bv.Tr);
-  ar& make_nvp("length", make_array(bv.length, 2));
-  ar& make_nvp("radius", bv.radius);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_RSS_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/RSS.h>
diff --git a/include/hpp/fcl/serialization/archive.h b/include/hpp/fcl/serialization/archive.h
index 2da3a5db156b9ab4c7727c7b73bab1bc66cb5a7d..0a3f5c1887c420b82e9051ad58de1dbac2a370ed 100644
--- a/include/hpp/fcl/serialization/archive.h
+++ b/include/hpp/fcl/serialization/archive.h
@@ -1,296 +1,2 @@
-//
-// Copyright (c) 2017-2024 CNRS INRIA
-// This file was borrowed from the Pinocchio library:
-// https://github.com/stack-of-tasks/pinocchio
-//
-
-#ifndef HPP_FCL_SERIALIZATION_ARCHIVE_H
-#define HPP_FCL_SERIALIZATION_ARCHIVE_H
-
-#include "hpp/fcl/fwd.hh"
-
-#include <boost/serialization/nvp.hpp>
-#include <boost/archive/text_iarchive.hpp>
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/archive/xml_iarchive.hpp>
-#include <boost/archive/xml_oarchive.hpp>
-#include <boost/archive/binary_iarchive.hpp>
-#include <boost/archive/binary_oarchive.hpp>
-
-#include <fstream>
-#include <string>
-#include <sstream>
-#include <stdexcept>
-
-#if BOOST_VERSION / 100 % 1000 == 78 && __APPLE__
-// See https://github.com/qcscine/utilities/issues/5#issuecomment-1246897049 for
-// further details
-
-#ifndef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
-#define DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
-#define BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
-#endif
-
-#include <boost/asio/streambuf.hpp>
-
-#ifdef DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
-#undef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
-#endif
-
-#else
-#include <boost/asio/streambuf.hpp>
-#endif
-
-#include <boost/iostreams/device/array.hpp>
-#include <boost/iostreams/stream.hpp>
-#include <boost/iostreams/stream_buffer.hpp>
-
-// Handle NAN inside TXT or XML archives
-#include <boost/math/special_functions/nonfinite_num_facets.hpp>
-
-namespace hpp {
-namespace fcl {
-namespace serialization {
-
-///
-/// \brief Loads an object from a TXT file.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[out] object Object in which the loaded data are copied.
-/// \param[in]  filename Name of the file containing the serialized data.
-///
-template <typename T>
-inline void loadFromText(T& object, const std::string& filename) {
-  std::ifstream ifs(filename.c_str());
-  if (ifs) {
-    std::locale const new_loc(ifs.getloc(),
-                              new boost::math::nonfinite_num_get<char>);
-    ifs.imbue(new_loc);
-    boost::archive::text_iarchive ia(ifs, boost::archive::no_codecvt);
-    ia >> object;
-  } else {
-    const std::string exception_message(filename +
-                                        " does not seem to be a valid file.");
-    throw std::invalid_argument(exception_message);
-  }
-}
-
-///
-/// \brief Saves an object inside a TXT file.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[in]  object Object in which the loaded data are copied.
-/// \param[in]  filename Name of the file containing the serialized data.
-///
-template <typename T>
-inline void saveToText(const T& object, const std::string& filename) {
-  std::ofstream ofs(filename.c_str());
-  if (ofs) {
-    boost::archive::text_oarchive oa(ofs);
-    oa & object;
-  } else {
-    const std::string exception_message(filename +
-                                        " does not seem to be a valid file.");
-    throw std::invalid_argument(exception_message);
-  }
-}
-
-///
-/// \brief Loads an object from a std::stringstream.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[out] object Object in which the loaded data are copied.
-/// \param[in]  is  string stream constaining the serialized content of the
-/// object.
-///
-template <typename T>
-inline void loadFromStringStream(T& object, std::istringstream& is) {
-  std::locale const new_loc(is.getloc(),
-                            new boost::math::nonfinite_num_get<char>);
-  is.imbue(new_loc);
-  boost::archive::text_iarchive ia(is, boost::archive::no_codecvt);
-  ia >> object;
-}
-
-///
-/// \brief Saves an object inside a std::stringstream.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[in]   object Object in which the loaded data are copied.
-/// \param[out]  ss String stream constaining the serialized content of the
-/// object.
-///
-template <typename T>
-inline void saveToStringStream(const T& object, std::stringstream& ss) {
-  boost::archive::text_oarchive oa(ss);
-  oa & object;
-}
-
-///
-/// \brief Loads an object from a std::string
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[out] object Object in which the loaded data are copied.
-/// \param[in]  str  string constaining the serialized content of the object.
-///
-template <typename T>
-inline void loadFromString(T& object, const std::string& str) {
-  std::istringstream is(str);
-  loadFromStringStream(object, is);
-}
-
-///
-/// \brief Saves an object inside a std::string
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[in] object Object in which the loaded data are copied.
-///
-/// \returns a string  constaining the serialized content of the object.
-///
-template <typename T>
-inline std::string saveToString(const T& object) {
-  std::stringstream ss;
-  saveToStringStream(object, ss);
-  return ss.str();
-}
-
-///
-/// \brief Loads an object from a XML file.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[out] object Object in which the loaded data are copied.
-/// \param[in] filename Name of the file containing the serialized data.
-/// \param[in] tag_name XML Tag for the given object.
-///
-template <typename T>
-inline void loadFromXML(T& object, const std::string& filename,
-                        const std::string& tag_name) {
-  if (filename.empty()) {
-    HPP_FCL_THROW_PRETTY("Tag name should not be empty.",
-                         std::invalid_argument);
-  }
-
-  std::ifstream ifs(filename.c_str());
-  if (ifs) {
-    std::locale const new_loc(ifs.getloc(),
-                              new boost::math::nonfinite_num_get<char>);
-    ifs.imbue(new_loc);
-    boost::archive::xml_iarchive ia(ifs, boost::archive::no_codecvt);
-    ia >> boost::serialization::make_nvp(tag_name.c_str(), object);
-  } else {
-    const std::string exception_message(filename +
-                                        " does not seem to be a valid file.");
-    throw std::invalid_argument(exception_message);
-  }
-}
-
-///
-/// \brief Saves an object inside a XML file.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[in] object Object in which the loaded data are copied.
-/// \param[in] filename Name of the file containing the serialized data.
-/// \param[in] tag_name XML Tag for the given object.
-///
-template <typename T>
-inline void saveToXML(const T& object, const std::string& filename,
-                      const std::string& tag_name) {
-  if (filename.empty()) {
-    HPP_FCL_THROW_PRETTY("Tag name should not be empty.",
-                         std::invalid_argument);
-  }
-
-  std::ofstream ofs(filename.c_str());
-  if (ofs) {
-    boost::archive::xml_oarchive oa(ofs);
-    oa& boost::serialization::make_nvp(tag_name.c_str(), object);
-  } else {
-    const std::string exception_message(filename +
-                                        " does not seem to be a valid file.");
-    throw std::invalid_argument(exception_message);
-  }
-}
-
-///
-/// \brief Loads an object from a binary file.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[out] object Object in which the loaded data are copied.
-/// \param[in] filename Name of the file containing the serialized data.
-///
-template <typename T>
-inline void loadFromBinary(T& object, const std::string& filename) {
-  std::ifstream ifs(filename.c_str(), std::ios::binary);
-  if (ifs) {
-    boost::archive::binary_iarchive ia(ifs);
-    ia >> object;
-  } else {
-    const std::string exception_message(filename +
-                                        " does not seem to be a valid file.");
-    throw std::invalid_argument(exception_message);
-  }
-}
-
-///
-/// \brief Saves an object inside a binary file.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[in] object Object in which the loaded data are copied.
-/// \param[in] filename Name of the file containing the serialized data.
-///
-template <typename T>
-void saveToBinary(const T& object, const std::string& filename) {
-  std::ofstream ofs(filename.c_str(), std::ios::binary);
-  if (ofs) {
-    boost::archive::binary_oarchive oa(ofs);
-    oa & object;
-  } else {
-    const std::string exception_message(filename +
-                                        " does not seem to be a valid file.");
-    throw std::invalid_argument(exception_message);
-  }
-}
-
-///
-/// \brief Loads an object from a binary buffer.
-///
-/// \tparam T Type of the object to deserialize.
-///
-/// \param[out] object Object in which the loaded data are copied.
-/// \param[in] buffer Input buffer containing the serialized data.
-///
-template <typename T>
-inline void loadFromBuffer(T& object, boost::asio::streambuf& buffer) {
-  boost::archive::binary_iarchive ia(buffer);
-  ia >> object;
-}
-
-///
-/// \brief Saves an object to a binary buffer.
-///
-/// \tparam T Type of the object to serialize.
-///
-/// \param[in]  object Object in which the loaded data are copied.
-/// \param[out] buffer Output buffer containing the serialized data.
-///
-template <typename T>
-void saveToBuffer(const T& object, boost::asio::streambuf& buffer) {
-  boost::archive::binary_oarchive oa(buffer);
-  oa & object;
-}
-
-}  // namespace serialization
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_ARCHIVE_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/archive.h>
diff --git a/include/hpp/fcl/serialization/collision_data.h b/include/hpp/fcl/serialization/collision_data.h
index e32a54330ec405bf35605952a828d483fc5f1134..9ac4f985d1b19864f2643e645fed19380a1b16f2 100644
--- a/include/hpp/fcl/serialization/collision_data.h
+++ b/include/hpp/fcl/serialization/collision_data.h
@@ -1,184 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_COLLISION_DATA_H
-#define HPP_FCL_SERIALIZATION_COLLISION_DATA_H
-
-#include "hpp/fcl/collision_data.h"
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void save(Archive& ar, const hpp::fcl::Contact& contact,
-          const unsigned int /*version*/) {
-  ar& make_nvp("b1", contact.b1);
-  ar& make_nvp("b2", contact.b2);
-  ar& make_nvp("normal", contact.normal);
-  ar& make_nvp("nearest_points", contact.nearest_points);
-  ar& make_nvp("pos", contact.pos);
-  ar& make_nvp("penetration_depth", contact.penetration_depth);
-}
-
-template <class Archive>
-void load(Archive& ar, hpp::fcl::Contact& contact,
-          const unsigned int /*version*/) {
-  ar >> make_nvp("b1", contact.b1);
-  ar >> make_nvp("b2", contact.b2);
-  ar >> make_nvp("normal", contact.normal);
-  std::array<hpp::fcl::Vec3f, 2> nearest_points;
-  ar >> make_nvp("nearest_points", nearest_points);
-  contact.nearest_points[0] = nearest_points[0];
-  contact.nearest_points[1] = nearest_points[1];
-  ar >> make_nvp("pos", contact.pos);
-  ar >> make_nvp("penetration_depth", contact.penetration_depth);
-  contact.o1 = NULL;
-  contact.o2 = NULL;
-}
-
-HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::Contact)
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::QueryRequest& query_request,
-               const unsigned int /*version*/) {
-  ar& make_nvp("gjk_initial_guess", query_request.gjk_initial_guess);
-  // TODO: use gjk_initial_guess instead
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  ar& make_nvp("enable_cached_gjk_guess",
-               query_request.enable_cached_gjk_guess);
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-  ar& make_nvp("cached_gjk_guess", query_request.cached_gjk_guess);
-  ar& make_nvp("cached_support_func_guess",
-               query_request.cached_support_func_guess);
-  ar& make_nvp("gjk_max_iterations", query_request.gjk_max_iterations);
-  ar& make_nvp("gjk_tolerance", query_request.gjk_tolerance);
-  ar& make_nvp("gjk_variant", query_request.gjk_variant);
-  ar& make_nvp("gjk_convergence_criterion",
-               query_request.gjk_convergence_criterion);
-  ar& make_nvp("gjk_convergence_criterion_type",
-               query_request.gjk_convergence_criterion_type);
-  ar& make_nvp("epa_max_iterations", query_request.epa_max_iterations);
-  ar& make_nvp("epa_tolerance", query_request.epa_tolerance);
-  ar& make_nvp("collision_distance_threshold",
-               query_request.collision_distance_threshold);
-  ar& make_nvp("enable_timings", query_request.enable_timings);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::QueryResult& query_result,
-               const unsigned int /*version*/) {
-  ar& make_nvp("cached_gjk_guess", query_result.cached_gjk_guess);
-  ar& make_nvp("cached_support_func_guess",
-               query_result.cached_support_func_guess);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::CollisionRequest& collision_request,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::QueryRequest>(
-                   collision_request));
-  ar& make_nvp("num_max_contacts", collision_request.num_max_contacts);
-  ar& make_nvp("enable_contact", collision_request.enable_contact);
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  ar& make_nvp("enable_distance_lower_bound",
-               collision_request.enable_distance_lower_bound);
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-  ar& make_nvp("security_margin", collision_request.security_margin);
-  ar& make_nvp("break_distance", collision_request.break_distance);
-  ar& make_nvp("distance_upper_bound", collision_request.distance_upper_bound);
-}
-
-template <class Archive>
-void save(Archive& ar, const hpp::fcl::CollisionResult& collision_result,
-          const unsigned int /*version*/) {
-  ar& make_nvp("base", boost::serialization::base_object<hpp::fcl::QueryResult>(
-                           collision_result));
-  ar& make_nvp("contacts", collision_result.getContacts());
-  ar& make_nvp("distance_lower_bound", collision_result.distance_lower_bound);
-  ar& make_nvp("nearest_points", collision_result.nearest_points);
-  ar& make_nvp("normal", collision_result.normal);
-}
-
-template <class Archive>
-void load(Archive& ar, hpp::fcl::CollisionResult& collision_result,
-          const unsigned int /*version*/) {
-  ar >>
-      make_nvp("base", boost::serialization::base_object<hpp::fcl::QueryResult>(
-                           collision_result));
-  std::vector<hpp::fcl::Contact> contacts;
-  ar >> make_nvp("contacts", contacts);
-  collision_result.clear();
-  for (size_t k = 0; k < contacts.size(); ++k)
-    collision_result.addContact(contacts[k]);
-  ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound);
-  std::array<hpp::fcl::Vec3f, 2> nearest_points;
-  ar >> make_nvp("nearest_points", nearest_points);
-  collision_result.nearest_points[0] = nearest_points[0];
-  collision_result.nearest_points[1] = nearest_points[1];
-  ar >> make_nvp("normal", collision_result.normal);
-}
-
-HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionResult)
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::DistanceRequest& distance_request,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::QueryRequest>(
-                   distance_request));
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-  ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points);
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
-  ar& make_nvp("enable_signed_distance",
-               distance_request.enable_signed_distance);
-  ar& make_nvp("rel_err", distance_request.rel_err);
-  ar& make_nvp("abs_err", distance_request.abs_err);
-}
-
-template <class Archive>
-void save(Archive& ar, const hpp::fcl::DistanceResult& distance_result,
-          const unsigned int /*version*/) {
-  ar& make_nvp("base", boost::serialization::base_object<hpp::fcl::QueryResult>(
-                           distance_result));
-  ar& make_nvp("min_distance", distance_result.min_distance);
-  ar& make_nvp("nearest_points", distance_result.nearest_points);
-  ar& make_nvp("normal", distance_result.normal);
-  ar& make_nvp("b1", distance_result.b1);
-  ar& make_nvp("b2", distance_result.b2);
-}
-
-template <class Archive>
-void load(Archive& ar, hpp::fcl::DistanceResult& distance_result,
-          const unsigned int /*version*/) {
-  ar >>
-      make_nvp("base", boost::serialization::base_object<hpp::fcl::QueryResult>(
-                           distance_result));
-  ar >> make_nvp("min_distance", distance_result.min_distance);
-  std::array<hpp::fcl::Vec3f, 2> nearest_points;
-  ar >> make_nvp("nearest_points", nearest_points);
-  distance_result.nearest_points[0] = nearest_points[0];
-  distance_result.nearest_points[1] = nearest_points[1];
-  ar >> make_nvp("normal", distance_result.normal);
-  ar >> make_nvp("b1", distance_result.b1);
-  ar >> make_nvp("b2", distance_result.b2);
-  distance_result.o1 = NULL;
-  distance_result.o2 = NULL;
-}
-
-HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::DistanceResult)
-
-}  // namespace serialization
-}  // namespace boost
-
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionRequest)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionResult)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::DistanceRequest)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::DistanceResult)
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_COLLISION_DATA_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/collision_data.h>
diff --git a/include/hpp/fcl/serialization/collision_object.h b/include/hpp/fcl/serialization/collision_object.h
index 018728cf57af1e0e67f97c1d5689ab77fead4674..782976870f8c72713e887a91ea97c54d9931a366 100644
--- a/include/hpp/fcl/serialization/collision_object.h
+++ b/include/hpp/fcl/serialization/collision_object.h
@@ -1,95 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H
-#define HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H
-
-#include "hpp/fcl/collision_object.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-#include "hpp/fcl/serialization/AABB.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void save(Archive& ar, const hpp::fcl::CollisionGeometry& collision_geometry,
-          const unsigned int /*version*/) {
-  ar& make_nvp("aabb_center", collision_geometry.aabb_center);
-  ar& make_nvp("aabb_radius", collision_geometry.aabb_radius);
-  ar& make_nvp("aabb_local", collision_geometry.aabb_local);
-  ar& make_nvp("cost_density", collision_geometry.cost_density);
-  ar& make_nvp("threshold_occupied", collision_geometry.threshold_occupied);
-  ar& make_nvp("threshold_free", collision_geometry.threshold_free);
-}
-
-template <class Archive>
-void load(Archive& ar, hpp::fcl::CollisionGeometry& collision_geometry,
-          const unsigned int /*version*/) {
-  ar >> make_nvp("aabb_center", collision_geometry.aabb_center);
-  ar >> make_nvp("aabb_radius", collision_geometry.aabb_radius);
-  ar >> make_nvp("aabb_local", collision_geometry.aabb_local);
-  ar >> make_nvp("cost_density", collision_geometry.cost_density);
-  ar >> make_nvp("threshold_occupied", collision_geometry.threshold_occupied);
-  ar >> make_nvp("threshold_free", collision_geometry.threshold_free);
-  collision_geometry.user_data = NULL;  // no way to recover this
-}
-
-HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionGeometry)
-
-}  // namespace serialization
-}  // namespace boost
-
-namespace hpp {
-namespace fcl {
-
-// fwd declaration
-template <typename BV>
-class HeightField;
-
-template <typename PolygonT>
-class Convex;
-
-struct OBB;
-struct OBBRSS;
-class AABB;
-
-class OcTree;
-class Box;
-class Sphere;
-class Ellipsoid;
-class Capsule;
-class Cone;
-class TriangleP;
-class Cylinder;
-class Halfspace;
-class Plane;
-
-namespace serialization {
-template <>
-struct register_type<CollisionGeometry> {
-  template <class Archive>
-  static void on(Archive& ar) {
-    ar.template register_type<Box>();
-    ar.template register_type<Sphere>();
-    ar.template register_type<Ellipsoid>();
-    ar.template register_type<TriangleP>();
-    ar.template register_type<Capsule>();
-    ar.template register_type<Cone>();
-    ar.template register_type<Cylinder>();
-    ar.template register_type<Halfspace>();
-    ar.template register_type<Plane>();
-    ar.template register_type<OcTree>();
-    ar.template register_type<HeightField<OBB>>();
-    ar.template register_type<HeightField<OBBRSS>>();
-    ar.template register_type<HeightField<AABB>>();
-    ar.template register_type<Convex<Triangle>>();
-    ;
-  }
-};
-}  // namespace serialization
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/collision_object.h>
diff --git a/include/hpp/fcl/serialization/contact_patch.h b/include/hpp/fcl/serialization/contact_patch.h
index 71d984ca9cc77cd445aebab70758c655b9cbdd07..dca0506ff751f1c235bdc43715be52b91d0b2f7b 100644
--- a/include/hpp/fcl/serialization/contact_patch.h
+++ b/include/hpp/fcl/serialization/contact_patch.h
@@ -1,87 +1,2 @@
-//
-// Copyright (c) 2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_CONTACT_PATCH_H
-#define HPP_FCL_SERIALIZATION_CONTACT_PATCH_H
-
-#include "hpp/fcl/collision_data.h"
-#include "hpp/fcl/serialization/fwd.h"
-#include "hpp/fcl/serialization/transform.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::ContactPatch& contact_patch,
-               const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-  typedef Eigen::Matrix<FCL_REAL, 2, Eigen::Dynamic> PolygonPoints;
-
-  size_t patch_size = contact_patch.size();
-  ar& make_nvp("patch_size", patch_size);
-  if (patch_size > 0) {
-    if (Archive::is_loading::value) {
-      contact_patch.points().resize(patch_size);
-    }
-    Eigen::Map<PolygonPoints> points_map(
-        reinterpret_cast<FCL_REAL*>(contact_patch.points().data()), 2,
-        static_cast<Eigen::Index>(patch_size));
-    ar& make_nvp("points", points_map);
-  }
-
-  ar& make_nvp("penetration_depth", contact_patch.penetration_depth);
-  ar& make_nvp("direction", contact_patch.direction);
-  ar& make_nvp("tf", contact_patch.tf);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::ContactPatchRequest& request,
-               const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-
-  ar& make_nvp("max_num_patch", request.max_num_patch);
-
-  size_t num_samples_curved_shapes = request.getNumSamplesCurvedShapes();
-  FCL_REAL patch_tolerance = request.getPatchTolerance();
-  ar& make_nvp("num_samples_curved_shapes", num_samples_curved_shapes);
-  ar& make_nvp("patch_tolerance", num_samples_curved_shapes);
-
-  if (Archive::is_loading::value) {
-    request.setNumSamplesCurvedShapes(num_samples_curved_shapes);
-    request.setPatchTolerance(patch_tolerance);
-  }
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::ContactPatchResult& result,
-               const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-
-  size_t num_patches = result.numContactPatches();
-  ar& make_nvp("num_patches", num_patches);
-
-  std::vector<ContactPatch> patches;
-  patches.resize(num_patches);
-  if (Archive::is_loading::value) {
-    ar& make_nvp("patches", patches);
-
-    const ContactPatchRequest request(num_patches);
-    result.set(request);
-    for (size_t i = 0; i < num_patches; ++i) {
-      ContactPatch& patch = result.getUnusedContactPatch();
-      patch = patches[i];
-    }
-  } else {
-    patches.clear();
-    for (size_t i = 0; i < num_patches; ++i) {
-      patches.emplace_back(result.getContactPatch(i));
-    }
-    ar& make_nvp("patches", patches);
-  }
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // HPP_FCL_SERIALIZATION_CONTACT_PATCH_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/contact_patch.h>
diff --git a/include/hpp/fcl/serialization/convex.h b/include/hpp/fcl/serialization/convex.h
index 7bbf37b5d6709f7b56048dbcdc496d73ac29233e..7000a3e18175bbb37e1b657e675243baea701367 100644
--- a/include/hpp/fcl/serialization/convex.h
+++ b/include/hpp/fcl/serialization/convex.h
@@ -1,169 +1,2 @@
-//
-// Copyright (c) 2022-2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_CONVEX_H
-#define HPP_FCL_SERIALIZATION_CONVEX_H
-
-#include "hpp/fcl/shape/convex.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-#include "hpp/fcl/serialization/geometric_shapes.h"
-#include "hpp/fcl/serialization/memory.h"
-#include "hpp/fcl/serialization/triangle.h"
-#include "hpp/fcl/serialization/quadrilateral.h"
-
-namespace boost {
-namespace serialization {
-
-namespace internal {
-struct ConvexBaseAccessor : hpp::fcl::ConvexBase {
-  typedef hpp::fcl::ConvexBase Base;
-};
-
-}  // namespace internal
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::ConvexBase& convex_base,
-               const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-
-  ar& make_nvp("base", boost::serialization::base_object<hpp::fcl::ShapeBase>(
-                           convex_base));
-
-  const unsigned int num_points_previous = convex_base.num_points;
-  ar& make_nvp("num_points", convex_base.num_points);
-
-  const unsigned int num_normals_and_offsets_previous =
-      convex_base.num_normals_and_offsets;
-  ar& make_nvp("num_normals_and_offsets", convex_base.num_normals_and_offsets);
-
-  const int num_warm_start_supports_previous =
-      static_cast<int>(convex_base.support_warm_starts.points.size());
-  assert(num_warm_start_supports_previous ==
-         static_cast<int>(convex_base.support_warm_starts.indices.size()));
-  int num_warm_start_supports = num_warm_start_supports_previous;
-  ar& make_nvp("num_warm_start_supports", num_warm_start_supports);
-
-  if (Archive::is_loading::value) {
-    if (num_points_previous != convex_base.num_points) {
-      convex_base.points.reset();
-      if (convex_base.num_points > 0)
-        convex_base.points.reset(
-            new std::vector<Vec3f>(convex_base.num_points));
-    }
-
-    if (num_normals_and_offsets_previous !=
-        convex_base.num_normals_and_offsets) {
-      convex_base.normals.reset();
-      convex_base.offsets.reset();
-      if (convex_base.num_normals_and_offsets > 0) {
-        convex_base.normals.reset(
-            new std::vector<Vec3f>(convex_base.num_normals_and_offsets));
-        convex_base.offsets.reset(
-            new std::vector<FCL_REAL>(convex_base.num_normals_and_offsets));
-      }
-    }
-
-    if (num_warm_start_supports_previous != num_warm_start_supports) {
-      convex_base.support_warm_starts.points.resize(
-          static_cast<size_t>(num_warm_start_supports));
-      convex_base.support_warm_starts.indices.resize(
-          static_cast<size_t>(num_warm_start_supports));
-    }
-  }
-
-  typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> MatrixPoints;
-  if (convex_base.num_points > 0) {
-    Eigen::Map<MatrixPoints> points_map(
-        reinterpret_cast<FCL_REAL*>(convex_base.points->data()), 3,
-        convex_base.num_points);
-    ar& make_nvp("points", points_map);
-  }
-
-  typedef Eigen::Matrix<FCL_REAL, 1, Eigen::Dynamic> VecOfReals;
-  if (convex_base.num_normals_and_offsets > 0) {
-    Eigen::Map<MatrixPoints> normals_map(
-        reinterpret_cast<FCL_REAL*>(convex_base.normals->data()), 3,
-        convex_base.num_normals_and_offsets);
-    ar& make_nvp("normals", normals_map);
-
-    Eigen::Map<VecOfReals> offsets_map(
-        reinterpret_cast<FCL_REAL*>(convex_base.offsets->data()), 1,
-        convex_base.num_normals_and_offsets);
-    ar& make_nvp("offsets", offsets_map);
-  }
-
-  typedef Eigen::Matrix<int, 1, Eigen::Dynamic> VecOfInts;
-  if (num_warm_start_supports > 0) {
-    Eigen::Map<MatrixPoints> warm_start_support_points_map(
-        reinterpret_cast<FCL_REAL*>(
-            convex_base.support_warm_starts.points.data()),
-        3, num_warm_start_supports);
-    ar& make_nvp("warm_start_support_points", warm_start_support_points_map);
-
-    Eigen::Map<VecOfInts> warm_start_support_indices_map(
-        reinterpret_cast<int*>(convex_base.support_warm_starts.indices.data()),
-        1, num_warm_start_supports);
-    ar& make_nvp("warm_start_support_indices", warm_start_support_indices_map);
-  }
-
-  ar& make_nvp("center", convex_base.center);
-  // We don't save neighbors as they will be computed directly by calling
-  // fillNeighbors.
-}
-
-namespace internal {
-template <typename PolygonT>
-struct ConvexAccessor : hpp::fcl::Convex<PolygonT> {
-  typedef hpp::fcl::Convex<PolygonT> Base;
-  using Base::fillNeighbors;
-};
-
-}  // namespace internal
-
-template <class Archive, typename PolygonT>
-void serialize(Archive& ar, hpp::fcl::Convex<PolygonT>& convex_,
-               const unsigned int /*version*/) {
-  using namespace hpp::fcl;
-  typedef internal::ConvexAccessor<PolygonT> Accessor;
-
-  Accessor& convex = reinterpret_cast<Accessor&>(convex_);
-  ar& make_nvp("base", boost::serialization::base_object<ConvexBase>(convex_));
-
-  const unsigned int num_polygons_previous = convex.num_polygons;
-  ar& make_nvp("num_polygons", convex.num_polygons);
-
-  if (Archive::is_loading::value) {
-    if (num_polygons_previous != convex.num_polygons) {
-      convex.polygons.reset(new std::vector<PolygonT>(convex.num_polygons));
-    }
-  }
-
-  ar& make_array<PolygonT>(convex.polygons->data(), convex.num_polygons);
-
-  if (Archive::is_loading::value) convex.fillNeighbors();
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(hpp::fcl::Convex<hpp::fcl::Triangle>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(hpp::fcl::Convex<hpp::fcl::Quadrilateral>)
-
-namespace hpp {
-namespace fcl {
-
-// namespace internal {
-// template <typename BV>
-// struct memory_footprint_evaluator< ::hpp::fcl::BVHModel<BV> > {
-//   static size_t run(const ::hpp::fcl::BVHModel<BV> &bvh_model) {
-//     return static_cast<size_t>(bvh_model.memUsage(false));
-//   }
-// };
-// }  // namespace internal
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_CONVEX_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/convex.h>
diff --git a/include/hpp/fcl/serialization/eigen.h b/include/hpp/fcl/serialization/eigen.h
index fa7c49dc9d2a239adb0966ab3403b442421abcb2..e5c9faaa76fb8069f6fdccb124e52f8472a270be 100644
--- a/include/hpp/fcl/serialization/eigen.h
+++ b/include/hpp/fcl/serialization/eigen.h
@@ -1,116 +1,2 @@
-//
-// Copyright (c) 2017-2021 CNRS INRIA
-//
-
-/*
- Code adapted from Pinocchio and
- https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp
- Copyright (c) 2015 Michael Tao
-*/
-
-#ifndef HPP_FCL_SERIALIZATION_EIGEN_H
-#define HPP_FCL_SERIALIZATION_EIGEN_H
-
-#ifndef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION
-
-#include <Eigen/Dense>
-
-#include <boost/serialization/split_free.hpp>
-#include <boost/serialization/vector.hpp>
-#include <boost/serialization/array.hpp>
-
-// Workaround a bug in GCC >= 7 and C++17
-// ref. https://gitlab.com/libeigen/eigen/-/issues/1676
-#ifdef __GNUC__
-#if __GNUC__ >= 7 && __cplusplus >= 201703L
-namespace boost {
-namespace serialization {
-struct U;
-}
-}  // namespace boost
-namespace Eigen {
-namespace internal {
-template <>
-struct traits<boost::serialization::U> {
-  enum { Flags = 0 };
-};
-}  // namespace internal
-}  // namespace Eigen
-#endif
-#endif
-
-namespace boost {
-namespace serialization {
-
-template <class Archive, typename Scalar, int Rows, int Cols, int Options,
-          int MaxRows, int MaxCols>
-void save(Archive& ar,
-          const Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>& m,
-          const unsigned int /*version*/) {
-  Eigen::DenseIndex rows(m.rows()), cols(m.cols());
-  if (Rows == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows);
-  if (Cols == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(cols);
-  ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
-}
-
-template <class Archive, typename Scalar, int Rows, int Cols, int Options,
-          int MaxRows, int MaxCols>
-void load(Archive& ar,
-          Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>& m,
-          const unsigned int /*version*/) {
-  Eigen::DenseIndex rows = Rows, cols = Cols;
-  if (Rows == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows);
-  if (Cols == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(cols);
-  m.resize(rows, cols);
-  ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
-}
-
-template <class Archive, typename Scalar, int Rows, int Cols, int Options,
-          int MaxRows, int MaxCols>
-void serialize(Archive& ar,
-               Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>& m,
-               const unsigned int version) {
-  split_free(ar, m, version);
-}
-
-template <class Archive, typename PlainObjectBase, int MapOptions,
-          typename StrideType>
-void save(Archive& ar,
-          const Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
-          const unsigned int /*version*/) {
-  Eigen::DenseIndex rows(m.rows()), cols(m.cols());
-  if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic)
-    ar& BOOST_SERIALIZATION_NVP(rows);
-  if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic)
-    ar& BOOST_SERIALIZATION_NVP(cols);
-  ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
-}
-
-template <class Archive, typename PlainObjectBase, int MapOptions,
-          typename StrideType>
-void load(Archive& ar, Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
-          const unsigned int /*version*/) {
-  Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime,
-                    cols = PlainObjectBase::ColsAtCompileTime;
-  if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic)
-    ar >> BOOST_SERIALIZATION_NVP(rows);
-  if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic)
-    ar >> BOOST_SERIALIZATION_NVP(cols);
-  m.resize(rows, cols);
-  ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
-}
-
-template <class Archive, typename PlainObjectBase, int MapOptions,
-          typename StrideType>
-void serialize(Archive& ar,
-               Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
-               const unsigned int version) {
-  split_free(ar, m, version);
-}
-
-}  // namespace serialization
-}  // namespace boost
-//
-#endif  // ifned HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_EIGEN_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/eigen.h>
diff --git a/include/hpp/fcl/serialization/fwd.h b/include/hpp/fcl/serialization/fwd.h
index abb1943fcb368c1a1db573153c9107b973512ac8..69e442f46510d1ed606f7c50ba4fbbd3582c15d0 100644
--- a/include/hpp/fcl/serialization/fwd.h
+++ b/include/hpp/fcl/serialization/fwd.h
@@ -1,117 +1,2 @@
-//
-// Copyright (c) 2021-2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_FWD_H
-#define HPP_FCL_SERIALIZATION_FWD_H
-
-#include <type_traits>
-
-#include <boost/archive/text_iarchive.hpp>
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/archive/xml_iarchive.hpp>
-#include <boost/archive/xml_oarchive.hpp>
-#include <boost/archive/binary_iarchive.hpp>
-#include <boost/archive/binary_oarchive.hpp>
-
-#include <boost/serialization/split_free.hpp>
-#include <boost/serialization/shared_ptr.hpp>
-#include <boost/serialization/export.hpp>
-
-#include "hpp/fcl/fwd.hh"
-#include "hpp/fcl/serialization/eigen.h"
-
-#define HPP_FCL_SERIALIZATION_SPLIT(Type)                                \
-  template <class Archive>                                               \
-  void serialize(Archive& ar, Type& value, const unsigned int version) { \
-    split_free(ar, value, version);                                      \
-  }
-
-#define HPP_FCL_SERIALIZATION_DECLARE_EXPORT(T) \
-  BOOST_CLASS_EXPORT_KEY(T)                     \
-  namespace boost {                             \
-  namespace archive {                           \
-  namespace detail {                            \
-  namespace extra_detail {                      \
-  template <>                                   \
-  struct init_guid<T> {                         \
-    static guid_initializer<T> const& g;        \
-  };                                            \
-  }                                             \
-  }                                             \
-  }                                             \
-  }                                             \
-  /**/
-
-#define HPP_FCL_SERIALIZATION_DEFINE_EXPORT(T)          \
-  namespace boost {                                     \
-  namespace archive {                                   \
-  namespace detail {                                    \
-  namespace extra_detail {                              \
-  guid_initializer<T> const& init_guid<T>::g =          \
-      ::boost::serialization::singleton<                \
-          guid_initializer<T> >::get_mutable_instance() \
-          .export_guid();                               \
-  }                                                     \
-  }                                                     \
-  }                                                     \
-  }                                                     \
-  /**/
-
-namespace hpp {
-namespace fcl {
-namespace serialization {
-namespace detail {
-
-template <class Derived, class Base>
-struct init_cast_register {};
-
-template <class Derived, class Base>
-struct cast_register_initializer {
-  void init(std::true_type) const {
-    boost::serialization::void_cast_register<Derived, Base>();
-  }
-
-  void init(std::false_type) const {}
-
-  cast_register_initializer const& init() const {
-    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-    _Pragma("GCC diagnostic ignored \"-Wconversion\"")
-        BOOST_STATIC_WARNING((std::is_base_of<Base, Derived>::value));
-    HPP_FCL_COMPILER_DIAGNOSTIC_POP
-    init(std::is_base_of<Base, Derived>());
-    return *this;
-  }
-};
-
-}  // namespace detail
-
-template <typename T>
-struct register_type {
-  template <class Archive>
-  static void on(Archive& /*ar*/) {}
-};
-}  // namespace serialization
-}  // namespace fcl
-}  // namespace hpp
-
-#define HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base)                   \
-  namespace hpp {                                                            \
-  namespace fcl {                                                            \
-  namespace serialization {                                                  \
-  namespace detail {                                                         \
-  template <>                                                                \
-  struct init_cast_register<Derived, Base> {                                 \
-    static cast_register_initializer<Derived, Base> const& g;                \
-  };                                                                         \
-  cast_register_initializer<Derived, Base> const& init_cast_register<        \
-      Derived, Base>::g =                                                    \
-      ::boost::serialization::singleton<                                     \
-          cast_register_initializer<Derived, Base> >::get_mutable_instance() \
-          .init();                                                           \
-  }                                                                          \
-  }                                                                          \
-  }                                                                          \
-  }
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_FWD_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/fwd.h>
diff --git a/include/hpp/fcl/serialization/geometric_shapes.h b/include/hpp/fcl/serialization/geometric_shapes.h
index 9122a2011a121ee01575decaf94724b13498dff9..d7679f265e7739b92221694514824c0334c6bc37 100644
--- a/include/hpp/fcl/serialization/geometric_shapes.h
+++ b/include/hpp/fcl/serialization/geometric_shapes.h
@@ -1,123 +1,2 @@
-//
-// Copyright (c) 2021-2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H
-#define HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H
-
-#include "hpp/fcl/shape/geometric_shapes.h"
-#include "hpp/fcl/serialization/fwd.h"
-#include "hpp/fcl/serialization/collision_object.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::ShapeBase& shape_base,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::CollisionGeometry>(
-                   shape_base));
-  ::hpp::fcl::FCL_REAL radius = shape_base.getSweptSphereRadius();
-  ar& make_nvp("swept_sphere_radius", radius);
-
-  if (Archive::is_loading::value) {
-    shape_base.setSweptSphereRadius(radius);
-  }
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::TriangleP& triangle,
-               const unsigned int /*version*/) {
-  ar& make_nvp(
-      "base", boost::serialization::base_object<hpp::fcl::ShapeBase>(triangle));
-  ar& make_nvp("a", triangle.a);
-  ar& make_nvp("b", triangle.b);
-  ar& make_nvp("c", triangle.c);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Box& box,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::ShapeBase>(box));
-  ar& make_nvp("halfSide", box.halfSide);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Sphere& sphere,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::ShapeBase>(sphere));
-  ar& make_nvp("radius", sphere.radius);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Ellipsoid& ellipsoid,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base", boost::serialization::base_object<hpp::fcl::ShapeBase>(
-                           ellipsoid));
-  ar& make_nvp("radii", ellipsoid.radii);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Capsule& capsule,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::ShapeBase>(capsule));
-  ar& make_nvp("radius", capsule.radius);
-  ar& make_nvp("halfLength", capsule.halfLength);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Cone& cone,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::ShapeBase>(cone));
-  ar& make_nvp("radius", cone.radius);
-  ar& make_nvp("halfLength", cone.halfLength);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Cylinder& cylinder,
-               const unsigned int /*version*/) {
-  ar& make_nvp(
-      "base", boost::serialization::base_object<hpp::fcl::ShapeBase>(cylinder));
-  ar& make_nvp("radius", cylinder.radius);
-  ar& make_nvp("halfLength", cylinder.halfLength);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Halfspace& half_space,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base", boost::serialization::base_object<hpp::fcl::ShapeBase>(
-                           half_space));
-  ar& make_nvp("n", half_space.n);
-  ar& make_nvp("d", half_space.d);
-}
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Plane& plane,
-               const unsigned int /*version*/) {
-  ar& make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::ShapeBase>(plane));
-  ar& make_nvp("n", plane.n);
-  ar& make_nvp("d", plane.d);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::ShapeBase)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionGeometry)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::TriangleP)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Box)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Sphere)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Ellipsoid)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Capsule)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Cone)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Cylinder)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Halfspace)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Plane)
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/geometric_shapes.h>
diff --git a/include/hpp/fcl/serialization/hfield.h b/include/hpp/fcl/serialization/hfield.h
index 4e456a78a5f23390e46b8570a7d181c5a3bc1a5c..e0e862a73325f5e722224b26de6862921ca57f7a 100644
--- a/include/hpp/fcl/serialization/hfield.h
+++ b/include/hpp/fcl/serialization/hfield.h
@@ -1,81 +1,2 @@
-//
-// Copyright (c) 2021-2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_HFIELD_H
-#define HPP_FCL_SERIALIZATION_HFIELD_H
-
-#include "hpp/fcl/hfield.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-#include "hpp/fcl/serialization/OBBRSS.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive &ar, hpp::fcl::HFNodeBase &node,
-               const unsigned int /*version*/) {
-  ar &make_nvp("first_child", node.first_child);
-  ar &make_nvp("x_id", node.x_id);
-  ar &make_nvp("x_size", node.x_size);
-  ar &make_nvp("y_id", node.y_id);
-  ar &make_nvp("y_size", node.y_size);
-  ar &make_nvp("max_height", node.max_height);
-  ar &make_nvp("contact_active_faces", node.contact_active_faces);
-}
-
-template <class Archive, typename BV>
-void serialize(Archive &ar, hpp::fcl::HFNode<BV> &node,
-               const unsigned int /*version*/) {
-  ar &make_nvp("base",
-               boost::serialization::base_object<hpp::fcl::HFNodeBase>(node));
-  ar &make_nvp("bv", node.bv);
-}
-
-namespace internal {
-template <typename BV>
-struct HeightFieldAccessor : hpp::fcl::HeightField<BV> {
-  typedef hpp::fcl::HeightField<BV> Base;
-  using Base::bvs;
-  using Base::heights;
-  using Base::max_height;
-  using Base::min_height;
-  using Base::num_bvs;
-  using Base::x_dim;
-  using Base::x_grid;
-  using Base::y_dim;
-  using Base::y_grid;
-};
-}  // namespace internal
-
-template <class Archive, typename BV>
-void serialize(Archive &ar, hpp::fcl::HeightField<BV> &hf_model,
-               const unsigned int /*version*/) {
-  ar &make_nvp(
-      "base",
-      boost::serialization::base_object<hpp::fcl::CollisionGeometry>(hf_model));
-
-  typedef internal::HeightFieldAccessor<BV> Accessor;
-  Accessor &access = reinterpret_cast<Accessor &>(hf_model);
-
-  ar &make_nvp("x_dim", access.x_dim);
-  ar &make_nvp("y_dim", access.y_dim);
-  ar &make_nvp("heights", access.heights);
-  ar &make_nvp("min_height", access.min_height);
-  ar &make_nvp("max_height", access.max_height);
-  ar &make_nvp("x_grid", access.x_grid);
-  ar &make_nvp("y_grid", access.y_grid);
-
-  ar &make_nvp("bvs", access.bvs);
-  ar &make_nvp("num_bvs", access.num_bvs);
-}
-}  // namespace serialization
-}  // namespace boost
-
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::HeightField<::hpp::fcl::AABB>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::HeightField<::hpp::fcl::OBB>)
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(
-    ::hpp::fcl::HeightField<::hpp::fcl::OBBRSS>)
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_HFIELD_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/hfield.h>
diff --git a/include/hpp/fcl/serialization/kDOP.h b/include/hpp/fcl/serialization/kDOP.h
index 36edda0d3916ba7e22df9a7ea38b72a4bb7e6147..fe4acc157529cd82a09186abd629c741b966aa30 100644
--- a/include/hpp/fcl/serialization/kDOP.h
+++ b/include/hpp/fcl/serialization/kDOP.h
@@ -1,34 +1,2 @@
-//
-// Copyright (c) 2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_kDOP_H
-#define HPP_FCL_SERIALIZATION_kDOP_H
-
-#include "hpp/fcl/BV/kDOP.h"
-
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-namespace internal {
-template <short N>
-struct KDOPAccessor : hpp::fcl::KDOP<N> {
-  typedef hpp::fcl::KDOP<N> Base;
-  using Base::dist_;
-};
-}  // namespace internal
-
-template <class Archive, short N>
-void serialize(Archive& ar, hpp::fcl::KDOP<N>& bv_,
-               const unsigned int /*version*/) {
-  typedef internal::KDOPAccessor<N> Accessor;
-  Accessor& access = reinterpret_cast<Accessor&>(bv_);
-  ar& make_nvp("distances", make_array(access.dist_.data(), N));
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // HPP_FCL_SERIALIZATION_kDOP_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/kDOP.h>
diff --git a/include/hpp/fcl/serialization/kIOS.h b/include/hpp/fcl/serialization/kIOS.h
index 8ed8d87251486f2d3ad88d916ca93c8d82d39c27..41fe0d7f68523a35497c451432b21a1cd5cb2417 100644
--- a/include/hpp/fcl/serialization/kIOS.h
+++ b/include/hpp/fcl/serialization/kIOS.h
@@ -1,58 +1,2 @@
-//
-// Copyright (c) 2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_kIOS_H
-#define HPP_FCL_SERIALIZATION_kIOS_H
-
-#include "hpp/fcl/BV/kIOS.h"
-
-#include "hpp/fcl/serialization/OBB.h"
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::kIOS& bv, const unsigned int version) {
-  split_free(ar, bv, version);
-}
-
-template <class Archive>
-void save(Archive& ar, const hpp::fcl::kIOS& bv,
-          const unsigned int /*version*/) {
-  // Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres
-  ar& make_nvp("num_spheres", bv.num_spheres);
-
-  std::array<hpp::fcl::Vec3f, hpp::fcl::kIOS::max_num_spheres> centers{};
-  std::array<hpp::fcl::FCL_REAL, hpp::fcl::kIOS::max_num_spheres> radii;
-  for (std::size_t i = 0; i < hpp::fcl::kIOS::max_num_spheres; ++i) {
-    centers[i] = bv.spheres[i].o;
-    radii[i] = bv.spheres[i].r;
-  }
-  ar& make_nvp("centers", make_array(centers.data(), centers.size()));
-  ar& make_nvp("radii", make_array(radii.data(), radii.size()));
-
-  ar& make_nvp("obb", bv.obb);
-}
-
-template <class Archive>
-void load(Archive& ar, hpp::fcl::kIOS& bv, const unsigned int /*version*/) {
-  ar >> make_nvp("num_spheres", bv.num_spheres);
-
-  std::array<hpp::fcl::Vec3f, hpp::fcl::kIOS::max_num_spheres> centers;
-  std::array<hpp::fcl::FCL_REAL, hpp::fcl::kIOS::max_num_spheres> radii;
-  ar >> make_nvp("centers", make_array(centers.data(), centers.size()));
-  ar >> make_nvp("radii", make_array(radii.data(), radii.size()));
-  for (std::size_t i = 0; i < hpp::fcl::kIOS::max_num_spheres; ++i) {
-    bv.spheres[i].o = centers[i];
-    bv.spheres[i].r = radii[i];
-  }
-
-  ar >> make_nvp("obb", bv.obb);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // HPP_FCL_SERIALIZATION_kIOS_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/kIOS.h>
diff --git a/include/hpp/fcl/serialization/memory.h b/include/hpp/fcl/serialization/memory.h
index e1e3d320fb89f65bd8ef9dcf9696a38edf1791c9..b03c4026a5e21351c4983b068dd24a5e5ecc1d64 100644
--- a/include/hpp/fcl/serialization/memory.h
+++ b/include/hpp/fcl/serialization/memory.h
@@ -1,32 +1,2 @@
-//
-// Copyright (c) 2021 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_MEMORY_H
-#define HPP_FCL_SERIALIZATION_MEMORY_H
-
-namespace hpp {
-namespace fcl {
-
-namespace internal {
-template <typename T>
-struct memory_footprint_evaluator {
-  static size_t run(const T &) { return sizeof(T); }
-};
-}  // namespace internal
-
-/// \brief Returns the memory footpring of the input object.
-/// For POD objects, this function returns the result of sizeof(T)
-///
-/// \param[in] object whose memory footprint needs to be evaluated.
-///
-/// \return the memory footprint of the input object.
-template <typename T>
-size_t computeMemoryFootprint(const T &object) {
-  return internal::memory_footprint_evaluator<T>::run(object);
-}
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_MEMORY_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/memory.h>
diff --git a/include/hpp/fcl/serialization/octree.h b/include/hpp/fcl/serialization/octree.h
index 8ea430022c6a5a0d8134a7c77ef948959d72b65a..6d2694aa4bc96dfb55fcaae8a09d5528cd1c165d 100644
--- a/include/hpp/fcl/serialization/octree.h
+++ b/include/hpp/fcl/serialization/octree.h
@@ -1,103 +1,2 @@
-//
-// Copyright (c) 2023-2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_OCTREE_H
-#define HPP_FCL_SERIALIZATION_OCTREE_H
-
-#include <sstream>
-#include <iostream>
-
-#include <boost/serialization/string.hpp>
-
-#include "hpp/fcl/octree.h"
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-namespace internal {
-struct OcTreeAccessor : hpp::fcl::OcTree {
-  typedef hpp::fcl::OcTree Base;
-  using Base::default_occupancy;
-  using Base::free_threshold;
-  using Base::occupancy_threshold;
-  using Base::tree;
-};
-}  // namespace internal
-
-template <class Archive>
-void save_construct_data(Archive &ar, const hpp::fcl::OcTree *octree_ptr,
-                         const unsigned int /*version*/) {
-  const double resolution = octree_ptr->getResolution();
-  ar << make_nvp("resolution", resolution);
-}
-
-template <class Archive>
-void save(Archive &ar, const hpp::fcl::OcTree &octree,
-          const unsigned int /*version*/) {
-  typedef internal::OcTreeAccessor Accessor;
-  const Accessor &access = reinterpret_cast<const Accessor &>(octree);
-
-  std::ostringstream stream;
-  access.tree->write(stream);
-  const std::string stream_str = stream.str();
-  auto size = stream_str.size();
-  // We can't directly serialize stream_str because it contains binary data.
-  // This create a bug on Windows with text_archive
-  ar << make_nvp("tree_data_size", size);
-  ar << make_nvp("tree_data",
-                 make_array(stream_str.c_str(), stream_str.size()));
-
-  ar << make_nvp("base", base_object<hpp::fcl::CollisionGeometry>(octree));
-  ar << make_nvp("default_occupancy", access.default_occupancy);
-  ar << make_nvp("occupancy_threshold", access.occupancy_threshold);
-  ar << make_nvp("free_threshold", access.free_threshold);
-}
-
-template <class Archive>
-void load_construct_data(Archive &ar, hpp::fcl::OcTree *octree_ptr,
-                         const unsigned int /*version*/) {
-  double resolution;
-  ar >> make_nvp("resolution", resolution);
-  new (octree_ptr) hpp::fcl::OcTree(resolution);
-}
-
-template <class Archive>
-void load(Archive &ar, hpp::fcl::OcTree &octree,
-          const unsigned int /*version*/) {
-  typedef internal::OcTreeAccessor Accessor;
-  Accessor &access = reinterpret_cast<Accessor &>(octree);
-
-  std::size_t tree_data_size;
-  ar >> make_nvp("tree_data_size", tree_data_size);
-
-  std::string stream_str;
-  stream_str.resize(tree_data_size);
-  /// TODO use stream_str.data in C++17
-  assert(tree_data_size > 0 && "tree_data_size should be greater than 0");
-  ar >> make_nvp("tree_data", make_array(&stream_str[0], tree_data_size));
-  std::istringstream stream(stream_str);
-
-  octomap::AbstractOcTree *new_tree = octomap::AbstractOcTree::read(stream);
-  access.tree = std::shared_ptr<const octomap::OcTree>(
-      dynamic_cast<octomap::OcTree *>(new_tree));
-
-  ar >> make_nvp("base", base_object<hpp::fcl::CollisionGeometry>(octree));
-  ar >> make_nvp("default_occupancy", access.default_occupancy);
-  ar >> make_nvp("occupancy_threshold", access.occupancy_threshold);
-  ar >> make_nvp("free_threshold", access.free_threshold);
-}
-
-template <class Archive>
-void serialize(Archive &ar, hpp::fcl::OcTree &octree,
-               const unsigned int version) {
-  split_free(ar, octree, version);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::OcTree)
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_OCTREE_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/octree.h>
diff --git a/include/hpp/fcl/serialization/quadrilateral.h b/include/hpp/fcl/serialization/quadrilateral.h
index 3694607eae7ab5ca8cf225ea0d8f20e76868d809..641f8161d297af3528839194faa8165d6d8bf56a 100644
--- a/include/hpp/fcl/serialization/quadrilateral.h
+++ b/include/hpp/fcl/serialization/quadrilateral.h
@@ -1,26 +1,2 @@
-//
-// Copyright (c) 2022 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H
-#define HPP_FCL_SERIALIZATION_QUADRILATERAL_H
-
-#include "hpp/fcl/data_types.h"
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive &ar, hpp::fcl::Quadrilateral &quadrilateral,
-               const unsigned int /*version*/) {
-  ar &make_nvp("p0", quadrilateral[0]);
-  ar &make_nvp("p1", quadrilateral[1]);
-  ar &make_nvp("p2", quadrilateral[2]);
-  ar &make_nvp("p3", quadrilateral[3]);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/quadrilateral.h>
diff --git a/include/hpp/fcl/serialization/serializer.h b/include/hpp/fcl/serialization/serializer.h
index d0d33cd83e841df93a657a5cfb5643cee4515305..6b194a18edb390f17f1116800cc605da1c9783b3 100644
--- a/include/hpp/fcl/serialization/serializer.h
+++ b/include/hpp/fcl/serialization/serializer.h
@@ -1,94 +1,2 @@
-//
-// Copyright (c) 2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_SERIALIZER_H
-#define HPP_FCL_SERIALIZATION_SERIALIZER_H
-
-#include "hpp/fcl/serialization/archive.h"
-
-namespace hpp {
-namespace fcl {
-namespace serialization {
-
-struct Serializer {
-  /// \brief Loads an object from a text file.
-  template <typename T>
-  static void loadFromText(T& object, const std::string& filename) {
-    ::hpp::fcl::serialization::loadFromText(object, filename);
-  }
-
-  /// \brief Saves an object as a text file.
-  template <typename T>
-  static void saveToText(const T& object, const std::string& filename) {
-    ::hpp::fcl::serialization::saveToText(object, filename);
-  }
-
-  /// \brief Loads an object from a stream string.
-  template <typename T>
-  static void loadFromStringStream(T& object, std::istringstream& is) {
-    ::hpp::fcl::serialization::loadFromStringStream(object, is);
-  }
-
-  /// \brief Saves an object to a string stream.
-  template <typename T>
-  static void saveToStringStream(const T& object, std::stringstream& ss) {
-    ::hpp::fcl::serialization::saveToStringStream(object, ss);
-  }
-
-  /// \brief Loads an object from a string.
-  template <typename T>
-  static void loadFromString(T& object, const std::string& str) {
-    ::hpp::fcl::serialization::loadFromString(object, str);
-  }
-
-  /// \brief Saves a Derived object to a string.
-  template <typename T>
-  static std::string saveToString(const T& object) {
-    return ::hpp::fcl::serialization::saveToString(object);
-  }
-
-  /// \brief Loads an object from an XML file.
-  template <typename T>
-  static void loadFromXML(T& object, const std::string& filename,
-                          const std::string& tag_name) {
-    ::hpp::fcl::serialization::loadFromXML(object, filename, tag_name);
-  }
-
-  /// \brief Saves an object as an XML file.
-  template <typename T>
-  static void saveToXML(const T& object, const std::string& filename,
-                        const std::string& tag_name) {
-    ::hpp::fcl::serialization::saveToXML(object, filename, tag_name);
-  }
-
-  /// \brief Loads a Derived object from an binary file.
-  template <typename T>
-  static void loadFromBinary(T& object, const std::string& filename) {
-    ::hpp::fcl::serialization::loadFromBinary(object, filename);
-  }
-
-  /// \brief Saves a Derived object as an binary file.
-  template <typename T>
-  static void saveToBinary(const T& object, const std::string& filename) {
-    ::hpp::fcl::serialization::saveToBinary(object, filename);
-  }
-
-  /// \brief Loads an object from a binary buffer.
-  template <typename T>
-  static void loadFromBuffer(T& object, boost::asio::streambuf& container) {
-    ::hpp::fcl::serialization::loadFromBuffer(object, container);
-  }
-
-  /// \brief Saves an object as a binary buffer.
-  template <typename T>
-  static void saveToBuffer(const T& object, boost::asio::streambuf& container) {
-    ::hpp::fcl::serialization::saveToBuffer(object, container);
-  }
-};
-
-}  // namespace serialization
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_SERIALIZER_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/serializer.h>
diff --git a/include/hpp/fcl/serialization/transform.h b/include/hpp/fcl/serialization/transform.h
index 4c251c4472c2e6ceea8ed2417df2c81a00c9e7f0..a72222f790f8d00a091e1d1566d867bbaa889736 100644
--- a/include/hpp/fcl/serialization/transform.h
+++ b/include/hpp/fcl/serialization/transform.h
@@ -1,24 +1,2 @@
-//
-// Copyright (c) 2024 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_TRANSFORM_H
-#define HPP_FCL_SERIALIZATION_TRANSFORM_H
-
-#include "hpp/fcl/math/transform.h"
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive& ar, hpp::fcl::Transform3f& tf,
-               const unsigned int /*version*/) {
-  ar& make_nvp("R", tf.rotation());
-  ar& make_nvp("T", tf.translation());
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // HPP_FCL_SERIALIZATION_TRANSFORM_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/transform.h>
diff --git a/include/hpp/fcl/serialization/triangle.h b/include/hpp/fcl/serialization/triangle.h
index 44c532e72f0c1811ffaba4ee8f927bf34a14eb5f..5c2b23f138a17cccc47838409d5cc4e70cf71546 100644
--- a/include/hpp/fcl/serialization/triangle.h
+++ b/include/hpp/fcl/serialization/triangle.h
@@ -1,25 +1,2 @@
-//
-// Copyright (c) 2021-2022 INRIA
-//
-
-#ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H
-#define HPP_FCL_SERIALIZATION_TRIANGLE_H
-
-#include "hpp/fcl/data_types.h"
-#include "hpp/fcl/serialization/fwd.h"
-
-namespace boost {
-namespace serialization {
-
-template <class Archive>
-void serialize(Archive &ar, hpp::fcl::Triangle &triangle,
-               const unsigned int /*version*/) {
-  ar &make_nvp("p0", triangle[0]);
-  ar &make_nvp("p1", triangle[1]);
-  ar &make_nvp("p2", triangle[2]);
-}
-
-}  // namespace serialization
-}  // namespace boost
-
-#endif  // ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/serialization/triangle.h>
diff --git a/include/hpp/fcl/shape/convex.h b/include/hpp/fcl/shape/convex.h
index a69d5c65744fa9edb3ae4d613461d5777b609720..0d5dd44ab5605862199e5414569cc3b4f1943e6f 100644
--- a/include/hpp/fcl/shape/convex.h
+++ b/include/hpp/fcl/shape/convex.h
@@ -1,115 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_SHAPE_CONVEX_H
-#define HPP_FCL_SHAPE_CONVEX_H
-
-#include "hpp/fcl/shape/geometric_shapes.h"
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Convex polytope
-/// @tparam PolygonT the polygon class. It must have method \c size() and
-///         \c operator[](int i)
-template <typename PolygonT>
-class Convex : public ConvexBase {
- public:
-  /// @brief Construct an uninitialized convex object
-  Convex() : ConvexBase(), num_polygons(0) {}
-
-  /// @brief Constructing a convex, providing normal and offset of each polytype
-  /// surface, and the points and shape topology information \param ownStorage
-  /// whether this class owns the pointers of points and
-  ///                    polygons. If owned, they are deleted upon destruction.
-  /// \param points_ list of 3D points
-  /// \param num_points_ number of 3D points
-  /// \param polygons_ \copydoc Convex::polygons
-  /// \param num_polygons_ the number of polygons.
-  /// \note num_polygons_ is not the allocated size of polygons_.
-  Convex(std::shared_ptr<std::vector<Vec3f>> points_, unsigned int num_points_,
-         std::shared_ptr<std::vector<PolygonT>> polygons_,
-         unsigned int num_polygons_);
-
-  /// @brief Copy constructor
-  /// Only the list of neighbors is copied.
-  Convex(const Convex& other);
-
-  ~Convex();
-
-  /// based on http://number-none.com/blow/inertia/bb_inertia.doc
-  Matrix3f computeMomentofInertia() const;
-
-  Vec3f computeCOM() const;
-
-  FCL_REAL computeVolume() const;
-
-  ///
-  /// @brief Set the current Convex from a list of points and polygons.
-  ///
-  /// \param ownStorage whether this class owns the pointers of points and
-  ///                    polygons. If owned, they are deleted upon destruction.
-  /// \param points list of 3D points
-  /// \param num_points number of 3D points
-  /// \param polygons \copydoc Convex::polygons
-  /// \param num_polygons the number of polygons.
-  /// \note num_polygons is not the allocated size of polygons.
-  ///
-  void set(std::shared_ptr<std::vector<Vec3f>> points, unsigned int num_points,
-           std::shared_ptr<std::vector<PolygonT>> polygons,
-           unsigned int num_polygons);
-
-  /// @brief Clone (deep copy)
-  virtual Convex<PolygonT>* clone() const;
-
-  /// @brief An array of PolygonT object.
-  /// PolygonT should contains a list of vertices for each polygon,
-  /// in counter clockwise order.
-  std::shared_ptr<std::vector<PolygonT>> polygons;
-  unsigned int num_polygons;
-
- protected:
-  void fillNeighbors();
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#include "hpp/fcl/shape/details/convex.hxx"
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/shape/convex.h>
diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx
index 373921e6f359632ec04c5d133970582954f1f858..d7b78f4d7f2443c18cabb8fd3f8b405625d857cf 100644
--- a/include/hpp/fcl/shape/details/convex.hxx
+++ b/include/hpp/fcl/shape/details/convex.hxx
@@ -1,286 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2019, CNRS - LAAS
- *  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 Joseph Mirabel */
-
-#ifndef HPP_FCL_SHAPE_CONVEX_HXX
-#define HPP_FCL_SHAPE_CONVEX_HXX
-
-#include <set>
-#include <vector>
-#include <iostream>
-
-namespace hpp {
-namespace fcl {
-
-template <typename PolygonT>
-Convex<PolygonT>::Convex(std::shared_ptr<std::vector<Vec3f>> points_,
-                         unsigned int num_points_,
-                         std::shared_ptr<std::vector<PolygonT>> polygons_,
-                         unsigned int num_polygons_)
-    : ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) {
-  this->initialize(points_, num_points_);
-  this->fillNeighbors();
-  this->buildSupportWarmStart();
-}
-
-template <typename PolygonT>
-Convex<PolygonT>::Convex(const Convex<PolygonT>& other)
-    : ConvexBase(other), num_polygons(other.num_polygons) {
-  if (other.polygons.get()) {
-    polygons.reset(new std::vector<PolygonT>(*(other.polygons)));
-  } else
-    polygons.reset();
-}
-
-template <typename PolygonT>
-Convex<PolygonT>::~Convex() {}
-
-template <typename PolygonT>
-void Convex<PolygonT>::set(std::shared_ptr<std::vector<Vec3f>> points_,
-                           unsigned int num_points_,
-                           std::shared_ptr<std::vector<PolygonT>> polygons_,
-                           unsigned int num_polygons_) {
-  ConvexBase::set(points_, num_points_);
-
-  this->num_polygons = num_polygons_;
-  this->polygons = polygons_;
-
-  this->fillNeighbors();
-  this->buildSupportWarmStart();
-}
-
-template <typename PolygonT>
-Convex<PolygonT>* Convex<PolygonT>::clone() const {
-  return new Convex(*this);
-}
-
-template <typename PolygonT>
-Matrix3f Convex<PolygonT>::computeMomentofInertia() const {
-  typedef typename PolygonT::size_type size_type;
-  typedef typename PolygonT::index_type index_type;
-
-  Matrix3f C = Matrix3f::Zero();
-
-  Matrix3f C_canonical;
-  C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0,
-      1 / 120.0, 1 / 120.0, 1 / 60.0;
-
-  if (!(points.get())) {
-    std::cerr
-        << "Error in `Convex::computeMomentofInertia`! Convex has no vertices."
-        << std::endl;
-    return C;
-  }
-  const std::vector<Vec3f>& points_ = *points;
-  if (!(polygons.get())) {
-    std::cerr
-        << "Error in `Convex::computeMomentofInertia`! Convex has no polygons."
-        << std::endl;
-    return C;
-  }
-  const std::vector<PolygonT>& polygons_ = *polygons;
-  for (unsigned int i = 0; i < num_polygons; ++i) {
-    const PolygonT& polygon = polygons_[i];
-
-    // compute the center of the polygon
-    Vec3f plane_center(0, 0, 0);
-    for (size_type j = 0; j < polygon.size(); ++j)
-      plane_center += points_[polygon[(index_type)j]];
-    plane_center /= polygon.size();
-
-    // compute the volume of tetrahedron making by neighboring two points, the
-    // plane center and the reference point (zero) of the convex shape
-    const Vec3f& v3 = plane_center;
-    for (size_type j = 0; j < polygon.size(); ++j) {
-      index_type e_first = polygon[static_cast<index_type>(j)];
-      index_type e_second =
-          polygon[static_cast<index_type>((j + 1) % polygon.size())];
-      const Vec3f& v1 = points_[e_first];
-      const Vec3f& v2 = points_[e_second];
-      Matrix3f A;
-      A << v1.transpose(), v2.transpose(),
-          v3.transpose();  // this is A' in the original document
-      C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
-    }
-  }
-
-  return C.trace() * Matrix3f::Identity() - C;
-}
-
-template <typename PolygonT>
-Vec3f Convex<PolygonT>::computeCOM() const {
-  typedef typename PolygonT::size_type size_type;
-  typedef typename PolygonT::index_type index_type;
-
-  Vec3f com(0, 0, 0);
-  FCL_REAL vol = 0;
-  if (!(points.get())) {
-    std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices."
-              << std::endl;
-    return com;
-  }
-  const std::vector<Vec3f>& points_ = *points;
-  if (!(polygons.get())) {
-    std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons."
-              << std::endl;
-    return com;
-  }
-  const std::vector<PolygonT>& polygons_ = *polygons;
-  for (unsigned int i = 0; i < num_polygons; ++i) {
-    const PolygonT& polygon = polygons_[i];
-    // compute the center of the polygon
-    Vec3f plane_center(0, 0, 0);
-    for (size_type j = 0; j < polygon.size(); ++j)
-      plane_center += points_[polygon[(index_type)j]];
-    plane_center /= polygon.size();
-
-    // compute the volume of tetrahedron making by neighboring two points, the
-    // plane center and the reference point (zero) of the convex shape
-    const Vec3f& v3 = plane_center;
-    for (size_type j = 0; j < polygon.size(); ++j) {
-      index_type e_first = polygon[static_cast<index_type>(j)];
-      index_type e_second =
-          polygon[static_cast<index_type>((j + 1) % polygon.size())];
-      const Vec3f& v1 = points_[e_first];
-      const Vec3f& v2 = points_[e_second];
-      FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
-      vol += d_six_vol;
-      com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol;
-    }
-  }
-
-  return com / (vol * 4);  // here we choose zero as the reference
-}
-
-template <typename PolygonT>
-FCL_REAL Convex<PolygonT>::computeVolume() const {
-  typedef typename PolygonT::size_type size_type;
-  typedef typename PolygonT::index_type index_type;
-
-  FCL_REAL vol = 0;
-  if (!(points.get())) {
-    std::cerr << "Error in `Convex::computeVolume`! Convex has no vertices."
-              << std::endl;
-    return vol;
-  }
-  const std::vector<Vec3f>& points_ = *points;
-  if (!(polygons.get())) {
-    std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons."
-              << std::endl;
-    return vol;
-  }
-  const std::vector<PolygonT>& polygons_ = *polygons;
-  for (unsigned int i = 0; i < num_polygons; ++i) {
-    const PolygonT& polygon = polygons_[i];
-
-    // compute the center of the polygon
-    Vec3f plane_center(0, 0, 0);
-    for (size_type j = 0; j < polygon.size(); ++j)
-      plane_center += points_[polygon[(index_type)j]];
-    plane_center /= polygon.size();
-
-    // compute the volume of tetrahedron making by neighboring two points, the
-    // plane center and the reference point (zero point) of the convex shape
-    const Vec3f& v3 = plane_center;
-    for (size_type j = 0; j < polygon.size(); ++j) {
-      index_type e_first = polygon[static_cast<index_type>(j)];
-      index_type e_second =
-          polygon[static_cast<index_type>((j + 1) % polygon.size())];
-      const Vec3f& v1 = points_[e_first];
-      const Vec3f& v2 = points_[e_second];
-      FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
-      vol += d_six_vol;
-    }
-  }
-
-  return vol / 6;
-}
-
-template <typename PolygonT>
-void Convex<PolygonT>::fillNeighbors() {
-  neighbors.reset(new std::vector<Neighbors>(num_points));
-
-  typedef typename PolygonT::size_type size_type;
-  typedef typename PolygonT::index_type index_type;
-  std::vector<std::set<index_type>> nneighbors(num_points);
-  unsigned int c_nneighbors = 0;
-
-  if (!(polygons.get())) {
-    std::cerr << "Error in `Convex::fillNeighbors`! Convex has no polygons."
-              << std::endl;
-  }
-  const std::vector<PolygonT>& polygons_ = *polygons;
-  for (unsigned int l = 0; l < num_polygons; ++l) {
-    const PolygonT& polygon = polygons_[l];
-    const size_type n = polygon.size();
-
-    for (size_type j = 0; j < polygon.size(); ++j) {
-      size_type i = (j == 0) ? n - 1 : j - 1;
-      size_type k = (j == n - 1) ? 0 : j + 1;
-      index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j],
-                 pk = polygon[(index_type)k];
-      // Update neighbors of pj;
-      if (nneighbors[pj].count(pi) == 0) {
-        c_nneighbors++;
-        nneighbors[pj].insert(pi);
-      }
-      if (nneighbors[pj].count(pk) == 0) {
-        c_nneighbors++;
-        nneighbors[pj].insert(pk);
-      }
-    }
-  }
-
-  nneighbors_.reset(new std::vector<unsigned int>(c_nneighbors));
-
-  unsigned int* p_nneighbors = nneighbors_->data();
-  std::vector<Neighbors>& neighbors_ = *neighbors;
-  for (unsigned int i = 0; i < num_points; ++i) {
-    Neighbors& n = neighbors_[i];
-    if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
-      HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error);
-    n.count_ = (unsigned char)nneighbors[i].size();
-    n.n_ = p_nneighbors;
-    p_nneighbors =
-        std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
-  }
-  assert(p_nneighbors == nneighbors_->data() + c_nneighbors);
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/shape/details/convex.hxx>
diff --git a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
index dec758e546e18467716642ce74cd225607bfd909..66bb3b7e6fe4d29b0f471f5dfaafe5af28f59db5 100644
--- a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
+++ b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
@@ -1,357 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 GEOMETRIC_SHAPE_TO_BVH_MODEL_H
-#define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
-
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <boost/math/constants/constants.hpp>
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Generate BVH model from box
-template <typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box& shape,
-                      const Transform3f& pose) {
-  FCL_REAL a = shape.halfSide[0];
-  FCL_REAL b = shape.halfSide[1];
-  FCL_REAL c = shape.halfSide[2];
-  std::vector<Vec3f> points(8);
-  std::vector<Triangle> tri_indices(12);
-  points[0] = Vec3f(a, -b, c);
-  points[1] = Vec3f(a, b, c);
-  points[2] = Vec3f(-a, b, c);
-  points[3] = Vec3f(-a, -b, c);
-  points[4] = Vec3f(a, -b, -c);
-  points[5] = Vec3f(a, b, -c);
-  points[6] = Vec3f(-a, b, -c);
-  points[7] = Vec3f(-a, -b, -c);
-
-  tri_indices[0].set(0, 4, 1);
-  tri_indices[1].set(1, 4, 5);
-  tri_indices[2].set(2, 6, 3);
-  tri_indices[3].set(3, 6, 7);
-  tri_indices[4].set(3, 0, 2);
-  tri_indices[5].set(2, 0, 1);
-  tri_indices[6].set(6, 5, 7);
-  tri_indices[7].set(7, 5, 4);
-  tri_indices[8].set(1, 5, 2);
-  tri_indices[9].set(2, 5, 6);
-  tri_indices[10].set(3, 7, 0);
-  tri_indices[11].set(0, 7, 4);
-
-  for (unsigned int i = 0; i < points.size(); ++i) {
-    points[i] = pose.transform(points[i]).eval();
-  }
-
-  model.beginModel();
-  model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
-}
-
-/// @brief Generate BVH model from sphere, given the number of segments along
-/// longitude and number of rings along latitude.
-template <typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
-                      const Transform3f& pose, unsigned int seg,
-                      unsigned int ring) {
-  std::vector<Vec3f> points;
-  std::vector<Triangle> tri_indices;
-
-  FCL_REAL r = shape.radius;
-  FCL_REAL phi, phid;
-  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
-  phid = pi * 2 / seg;
-  phi = 0;
-
-  FCL_REAL theta, thetad;
-  thetad = pi / (ring + 1);
-  theta = 0;
-
-  for (unsigned int i = 0; i < ring; ++i) {
-    FCL_REAL theta_ = theta + thetad * (i + 1);
-    for (unsigned int j = 0; j < seg; ++j) {
-      points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid),
-                             r * sin(theta_) * sin(phi + j * phid),
-                             r * cos(theta_)));
-    }
-  }
-  points.push_back(Vec3f(0, 0, r));
-  points.push_back(Vec3f(0, 0, -r));
-
-  for (unsigned int i = 0; i < ring - 1; ++i) {
-    for (unsigned int j = 0; j < seg; ++j) {
-      unsigned int a, b, c, d;
-      a = i * seg + j;
-      b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
-      c = (i + 1) * seg + j;
-      d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
-      tri_indices.push_back(Triangle(a, c, b));
-      tri_indices.push_back(Triangle(b, c, d));
-    }
-  }
-
-  for (unsigned int j = 0; j < seg; ++j) {
-    unsigned int a, b;
-    a = j;
-    b = (j == seg - 1) ? 0 : (j + 1);
-    tri_indices.push_back(Triangle(ring * seg, a, b));
-
-    a = (ring - 1) * seg + j;
-    b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
-    tri_indices.push_back(Triangle(a, ring * seg + 1, b));
-  }
-
-  for (unsigned int i = 0; i < points.size(); ++i) {
-    points[i] = pose.transform(points[i]);
-  }
-
-  model.beginModel();
-  model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
-}
-
-/// @brief Generate BVH model from sphere
-/// The difference between generateBVHModel is that it gives the number of
-/// triangles faces N for a sphere with unit radius. For sphere of radius r,
-/// then the number of triangles is r * r * N so that the area represented by a
-/// single triangle is approximately the same.s
-template <typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
-                      const Transform3f& pose,
-                      unsigned int n_faces_for_unit_sphere) {
-  FCL_REAL r = shape.radius;
-  FCL_REAL n_low_bound =
-      std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r;
-  unsigned int ring = (unsigned int)ceil(n_low_bound);
-  unsigned int seg = (unsigned int)ceil(n_low_bound);
-
-  generateBVHModel(model, shape, pose, seg, ring);
-}
-
-/// @brief Generate BVH model from cylinder, given the number of segments along
-/// circle and the number of segments along axis.
-template <typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
-                      const Transform3f& pose, unsigned int tot,
-                      unsigned int h_num) {
-  std::vector<Vec3f> points;
-  std::vector<Triangle> tri_indices;
-
-  FCL_REAL r = shape.radius;
-  FCL_REAL h = shape.halfLength;
-  FCL_REAL phi, phid;
-  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
-  phid = pi * 2 / tot;
-  phi = 0;
-
-  FCL_REAL hd = 2 * h / h_num;
-
-  for (unsigned int i = 0; i < tot; ++i)
-    points.push_back(
-        Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
-
-  for (unsigned int i = 0; i < h_num - 1; ++i) {
-    for (unsigned int j = 0; j < tot; ++j) {
-      points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j),
-                             h - (i + 1) * hd));
-    }
-  }
-
-  for (unsigned int i = 0; i < tot; ++i)
-    points.push_back(
-        Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
-
-  points.push_back(Vec3f(0, 0, h));
-  points.push_back(Vec3f(0, 0, -h));
-
-  for (unsigned int i = 0; i < tot; ++i) {
-    Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
-    tri_indices.push_back(tmp);
-  }
-
-  for (unsigned int i = 0; i < tot; ++i) {
-    Triangle tmp((h_num + 1) * tot + 1,
-                 h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
-    tri_indices.push_back(tmp);
-  }
-
-  for (unsigned int i = 0; i < h_num; ++i) {
-    for (unsigned int j = 0; j < tot; ++j) {
-      unsigned int a, b, c, d;
-      a = j;
-      b = (j == tot - 1) ? 0 : (j + 1);
-      c = j + tot;
-      d = (j == tot - 1) ? tot : (j + 1 + tot);
-
-      unsigned int start = i * tot;
-      tri_indices.push_back(Triangle(start + b, start + a, start + c));
-      tri_indices.push_back(Triangle(start + b, start + c, start + d));
-    }
-  }
-
-  for (unsigned int i = 0; i < points.size(); ++i) {
-    points[i] = pose.transform(points[i]);
-  }
-
-  model.beginModel();
-  model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
-}
-
-/// @brief Generate BVH model from cylinder
-/// Difference from generateBVHModel: is that it gives the circle split number
-/// tot for a cylinder with unit radius. For cylinder with larger radius, the
-/// number of circle split number is r * tot.
-template <typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
-                      const Transform3f& pose,
-                      unsigned int tot_for_unit_cylinder) {
-  FCL_REAL r = shape.radius;
-  FCL_REAL h = 2 * shape.halfLength;
-
-  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
-  unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
-  FCL_REAL phid = pi * 2 / tot;
-
-  FCL_REAL circle_edge = phid * r;
-  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
-
-  generateBVHModel(model, shape, pose, tot, h_num);
-}
-
-/// @brief Generate BVH model from cone, given the number of segments along
-/// circle and the number of segments along axis.
-template <typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
-                      const Transform3f& pose, unsigned int tot,
-                      unsigned int h_num) {
-  std::vector<Vec3f> points;
-  std::vector<Triangle> tri_indices;
-
-  FCL_REAL r = shape.radius;
-  FCL_REAL h = shape.halfLength;
-
-  FCL_REAL phi, phid;
-  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
-  phid = pi * 2 / tot;
-  phi = 0;
-
-  FCL_REAL hd = 2 * h / h_num;
-
-  for (unsigned int i = 0; i < h_num - 1; ++i) {
-    FCL_REAL h_i = h - (i + 1) * hd;
-    FCL_REAL rh = r * (0.5 - h_i / h / 2);
-    for (unsigned int j = 0; j < tot; ++j) {
-      points.push_back(
-          Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
-    }
-  }
-
-  for (unsigned int i = 0; i < tot; ++i)
-    points.push_back(
-        Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
-
-  points.push_back(Vec3f(0, 0, h));
-  points.push_back(Vec3f(0, 0, -h));
-
-  for (unsigned int i = 0; i < tot; ++i) {
-    Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
-    tri_indices.push_back(tmp);
-  }
-
-  for (unsigned int i = 0; i < tot; ++i) {
-    Triangle tmp(h_num * tot + 1,
-                 (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
-                 (h_num - 1) * tot + i);
-    tri_indices.push_back(tmp);
-  }
-
-  for (unsigned int i = 0; i < h_num - 1; ++i) {
-    for (unsigned int j = 0; j < tot; ++j) {
-      unsigned int a, b, c, d;
-      a = j;
-      b = (j == tot - 1) ? 0 : (j + 1);
-      c = j + tot;
-      d = (j == tot - 1) ? tot : (j + 1 + tot);
-
-      unsigned int start = i * tot;
-      tri_indices.push_back(Triangle(start + b, start + a, start + c));
-      tri_indices.push_back(Triangle(start + b, start + c, start + d));
-    }
-  }
-
-  for (unsigned int i = 0; i < points.size(); ++i) {
-    points[i] = pose.transform(points[i]);
-  }
-
-  model.beginModel();
-  model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
-}
-
-/// @brief Generate BVH model from cone
-/// Difference from generateBVHModel: is that it gives the circle split number
-/// tot for a cylinder with unit radius. For cone with larger radius, the number
-/// of circle split number is r * tot.
-template <typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
-                      const Transform3f& pose, unsigned int tot_for_unit_cone) {
-  FCL_REAL r = shape.radius;
-  FCL_REAL h = 2 * shape.halfLength;
-
-  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
-  unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
-  FCL_REAL phid = pi * 2 / tot;
-
-  FCL_REAL circle_edge = phid * r;
-  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
-
-  generateBVHModel(model, shape, pose, tot, h_num);
-}
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/shape/geometric_shape_to_BVH_model.h>
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index fada3e07abfd79c5c6a2485e3644af079010a3fb..dd8f59a9516e7a1db50a3520a4bc78f121f31480 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -1,1054 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_GEOMETRIC_SHAPES_H
-#define HPP_FCL_GEOMETRIC_SHAPES_H
-
-#include <vector>
-#include <memory>
-
-#include <boost/math/constants/constants.hpp>
-
-#include "hpp/fcl/collision_object.h"
-#include "hpp/fcl/data_types.h"
-
-#ifdef HPP_FCL_HAS_QHULL
-namespace orgQhull {
-class Qhull;
-}
-#endif
-
-namespace hpp {
-namespace fcl {
-
-/// @brief Base class for all basic geometric shapes
-class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry {
- public:
-  ShapeBase() {}
-
-  ///  \brief Copy constructor
-  ShapeBase(const ShapeBase& other)
-      : CollisionGeometry(other),
-        m_swept_sphere_radius(other.m_swept_sphere_radius) {}
-
-  ShapeBase& operator=(const ShapeBase& other) = default;
-
-  virtual ~ShapeBase() {};
-
-  /// @brief Get object type: a geometric shape
-  OBJECT_TYPE getObjectType() const { return OT_GEOM; }
-
-  /// @brief Set radius of sphere swept around the shape.
-  /// Must be >= 0.
-  void setSweptSphereRadius(FCL_REAL radius) {
-    if (radius < 0) {
-      HPP_FCL_THROW_PRETTY("Swept-sphere radius must be positive.",
-                           std::invalid_argument);
-    }
-    this->m_swept_sphere_radius = radius;
-  }
-
-  /// @brief Get radius of sphere swept around the shape.
-  /// This radius is always >= 0.
-  FCL_REAL getSweptSphereRadius() const { return this->m_swept_sphere_radius; }
-
- protected:
-  /// \brief Radius of the sphere swept around the shape.
-  /// Default value is 0.
-  /// Note: this property differs from `inflated` method of certain
-  /// derived classes (e.g. Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder)
-  /// in the sense that inflated returns a new shape which can be inflated but
-  /// also deflated.
-  /// Also, an inflated shape is not rounded. It simply has a different size.
-  /// Sweeping a shape with a sphere is a different operation (a Minkowski sum),
-  /// which rounds the sharp corners of a shape.
-  /// The swept sphere radius is a property of the shape itself and can be
-  /// manually updated between collision checks.
-  FCL_REAL m_swept_sphere_radius{0};
-};
-
-/// @defgroup Geometric_Shapes Geometric shapes
-/// Classes of different types of geometric shapes.
-/// @{
-
-/// @brief Triangle stores the points instead of only indices of points
-class HPP_FCL_DLLAPI TriangleP : public ShapeBase {
- public:
-  TriangleP() {};
-
-  TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_)
-      : ShapeBase(), a(a_), b(b_), c(c_) {}
-
-  TriangleP(const TriangleP& other)
-      : ShapeBase(other), a(other.a), b(other.b), c(other.c) {}
-
-  /// @brief Clone *this into a new TriangleP
-  virtual TriangleP* clone() const { return new TriangleP(*this); };
-
-  /// @brief virtual function of compute AABB in local coordinate
-  void computeLocalAABB();
-
-  NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
-
-  //  std::pair<ShapeBase*, Transform3f> inflated(const FCL_REAL value) const {
-  //    if (value == 0) return std::make_pair(new TriangleP(*this),
-  //    Transform3f()); Vec3f AB(b - a), BC(c - b), CA(a - c); AB.normalize();
-  //    BC.normalize();
-  //    CA.normalize();
-  //
-  //    Vec3f new_a(a + value * Vec3f(-AB + CA).normalized());
-  //    Vec3f new_b(b + value * Vec3f(-BC + AB).normalized());
-  //    Vec3f new_c(c + value * Vec3f(-CA + BC).normalized());
-  //
-  //    return std::make_pair(new TriangleP(new_a, new_b, new_c),
-  //    Transform3f());
-  //  }
-  //
-  //  FCL_REAL minInflationValue() const
-  //  {
-  //    return (std::numeric_limits<FCL_REAL>::max)(); // TODO(jcarpent):
-  //    implement
-  //  }
-
-  Vec3f a, b, c;
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const TriangleP* other_ptr = dynamic_cast<const TriangleP*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const TriangleP& other = *other_ptr;
-
-    return a == other.a && b == other.b && c == other.c &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief Center at zero point, axis aligned box
-class HPP_FCL_DLLAPI Box : public ShapeBase {
- public:
-  Box(FCL_REAL x, FCL_REAL y, FCL_REAL z)
-      : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {}
-
-  Box(const Vec3f& side_) : ShapeBase(), halfSide(side_ / 2) {}
-
-  Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {}
-
-  Box& operator=(const Box& other) {
-    if (this == &other) return *this;
-
-    this->halfSide = other.halfSide;
-    return *this;
-  }
-
-  /// @brief Clone *this into a new Box
-  virtual Box* clone() const { return new Box(*this); };
-
-  /// @brief Default constructor
-  Box() {}
-
-  /// @brief box side half-length
-  Vec3f halfSide;
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: a box
-  NODE_TYPE getNodeType() const { return GEOM_BOX; }
-
-  FCL_REAL computeVolume() const { return 8 * halfSide.prod(); }
-
-  Matrix3f computeMomentofInertia() const {
-    FCL_REAL V = computeVolume();
-    Vec3f s(halfSide.cwiseAbs2() * V);
-    return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
-  }
-
-  FCL_REAL minInflationValue() const { return -halfSide.minCoeff(); }
-
-  /// \brief Inflate the box by an amount given by `value`.
-  /// This value can be positive or negative but must always >=
-  /// `minInflationValue()`.
-  ///
-  /// \param[in] value of the shape inflation.
-  ///
-  /// \returns a new inflated box and the related transform to account for the
-  /// change of shape frame
-  std::pair<Box, Transform3f> inflated(const FCL_REAL value) const {
-    if (value <= minInflationValue())
-      HPP_FCL_THROW_PRETTY("value (" << value << ") "
-                                     << "is two small. It should be at least: "
-                                     << minInflationValue(),
-                           std::invalid_argument);
-    return std::make_pair(Box(2 * (halfSide + Vec3f::Constant(value))),
-                          Transform3f());
-  }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const Box* other_ptr = dynamic_cast<const Box*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const Box& other = *other_ptr;
-
-    return halfSide == other.halfSide &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief Center at zero point sphere
-class HPP_FCL_DLLAPI Sphere : public ShapeBase {
- public:
-  /// @brief Default constructor
-  Sphere() {}
-
-  explicit Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {}
-
-  Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {}
-
-  /// @brief Clone *this into a new Sphere
-  virtual Sphere* clone() const { return new Sphere(*this); };
-
-  /// @brief Radius of the sphere
-  FCL_REAL radius;
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: a sphere
-  NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
-
-  Matrix3f computeMomentofInertia() const {
-    FCL_REAL I = 0.4 * radius * radius * computeVolume();
-    return I * Matrix3f::Identity();
-  }
-
-  FCL_REAL computeVolume() const {
-    return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius *
-           radius / 3;
-  }
-
-  FCL_REAL minInflationValue() const { return -radius; }
-
-  /// \brief Inflate the sphere by an amount given by `value`.
-  /// This value can be positive or negative but must always >=
-  /// `minInflationValue()`.
-  ///
-  /// \param[in] value of the shape inflation.
-  ///
-  /// \returns a new inflated sphere and the related transform to account for
-  /// the change of shape frame
-  std::pair<Sphere, Transform3f> inflated(const FCL_REAL value) const {
-    if (value <= minInflationValue())
-      HPP_FCL_THROW_PRETTY(
-          "value (" << value << ") is two small. It should be at least: "
-                    << minInflationValue(),
-          std::invalid_argument);
-    return std::make_pair(Sphere(radius + value), Transform3f());
-  }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const Sphere* other_ptr = dynamic_cast<const Sphere*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const Sphere& other = *other_ptr;
-
-    return radius == other.radius &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief Ellipsoid centered at point zero
-class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase {
- public:
-  /// @brief Default constructor
-  Ellipsoid() {}
-
-  Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz)
-      : ShapeBase(), radii(rx, ry, rz) {}
-
-  explicit Ellipsoid(const Vec3f& radii) : radii(radii) {}
-
-  Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {}
-
-  /// @brief Clone *this into a new Ellipsoid
-  virtual Ellipsoid* clone() const { return new Ellipsoid(*this); };
-
-  /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2
-  /// + z^2/rz^2 = 1)
-  Vec3f radii;
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: an ellipsoid
-  NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; }
-
-  Matrix3f computeMomentofInertia() const {
-    FCL_REAL V = computeVolume();
-    FCL_REAL a2 = V * radii[0] * radii[0];
-    FCL_REAL b2 = V * radii[1] * radii[1];
-    FCL_REAL c2 = V * radii[2] * radii[2];
-    return (Matrix3f() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0,
-            0.2 * (a2 + b2))
-        .finished();
-  }
-
-  FCL_REAL computeVolume() const {
-    return 4 * boost::math::constants::pi<FCL_REAL>() * radii[0] * radii[1] *
-           radii[2] / 3;
-  }
-
-  FCL_REAL minInflationValue() const { return -radii.minCoeff(); }
-
-  /// \brief Inflate the ellipsoid by an amount given by `value`.
-  /// This value can be positive or negative but must always >=
-  /// `minInflationValue()`.
-  ///
-  /// \param[in] value of the shape inflation.
-  ///
-  /// \returns a new inflated ellipsoid and the related transform to account for
-  /// the change of shape frame
-  std::pair<Ellipsoid, Transform3f> inflated(const FCL_REAL value) const {
-    if (value <= minInflationValue())
-      HPP_FCL_THROW_PRETTY(
-          "value (" << value << ") is two small. It should be at least: "
-                    << minInflationValue(),
-          std::invalid_argument);
-    return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)),
-                          Transform3f());
-  }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const Ellipsoid* other_ptr = dynamic_cast<const Ellipsoid*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const Ellipsoid& other = *other_ptr;
-
-    return radii == other.radii &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief Capsule
-/// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$
-/// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule
-/// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$.
-class HPP_FCL_DLLAPI Capsule : public ShapeBase {
- public:
-  /// @brief Default constructor
-  Capsule() {}
-
-  Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
-    halfLength = lz_ / 2;
-  }
-
-  Capsule(const Capsule& other)
-      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
-
-  /// @brief Clone *this into a new Capsule
-  virtual Capsule* clone() const { return new Capsule(*this); };
-
-  /// @brief Radius of capsule
-  FCL_REAL radius;
-
-  /// @brief Half Length along z axis
-  FCL_REAL halfLength;
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: a capsule
-  NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
-
-  FCL_REAL computeVolume() const {
-    return boost::math::constants::pi<FCL_REAL>() * radius * radius *
-           ((halfLength * 2) + radius * 4 / 3.0);
-  }
-
-  Matrix3f computeMomentofInertia() const {
-    FCL_REAL v_cyl = radius * radius * (halfLength * 2) *
-                     boost::math::constants::pi<FCL_REAL>();
-    FCL_REAL v_sph = radius * radius * radius *
-                     boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
-
-    FCL_REAL h2 = halfLength * halfLength;
-    FCL_REAL r2 = radius * radius;
-    FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) +
-                  v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
-    FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
-
-    return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
-  }
-
-  FCL_REAL minInflationValue() const { return -radius; }
-
-  /// \brief Inflate the capsule by an amount given by `value`.
-  /// This value can be positive or negative but must always >=
-  /// `minInflationValue()`.
-  ///
-  /// \param[in] value of the shape inflation.
-  ///
-  /// \returns a new inflated capsule and the related transform to account for
-  /// the change of shape frame
-  std::pair<Capsule, Transform3f> inflated(const FCL_REAL value) const {
-    if (value <= minInflationValue())
-      HPP_FCL_THROW_PRETTY(
-          "value (" << value << ") is two small. It should be at least: "
-                    << minInflationValue(),
-          std::invalid_argument);
-    return std::make_pair(Capsule(radius + value, 2 * halfLength),
-                          Transform3f());
-  }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const Capsule* other_ptr = dynamic_cast<const Capsule*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const Capsule& other = *other_ptr;
-
-    return radius == other.radius && halfLength == other.halfLength &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief Cone
-/// The base of the cone is at \f$ z = - halfLength \f$ and the top is at
-/// \f$ z = halfLength \f$.
-class HPP_FCL_DLLAPI Cone : public ShapeBase {
- public:
-  /// @brief Default constructor
-  Cone() {}
-
-  Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
-    halfLength = lz_ / 2;
-  }
-
-  Cone(const Cone& other)
-      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
-
-  /// @brief Clone *this into a new Cone
-  virtual Cone* clone() const { return new Cone(*this); };
-
-  /// @brief Radius of the cone
-  FCL_REAL radius;
-
-  /// @brief Half Length along z axis
-  FCL_REAL halfLength;
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: a cone
-  NODE_TYPE getNodeType() const { return GEOM_CONE; }
-
-  FCL_REAL computeVolume() const {
-    return boost::math::constants::pi<FCL_REAL>() * radius * radius *
-           (halfLength * 2) / 3;
-  }
-
-  Matrix3f computeMomentofInertia() const {
-    FCL_REAL V = computeVolume();
-    FCL_REAL ix =
-        V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
-    FCL_REAL iz = 0.3 * V * radius * radius;
-
-    return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
-  }
-
-  Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); }
-
-  FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); }
-
-  /// \brief Inflate the cone by an amount given by `value`.
-  /// This value can be positive or negative but must always >=
-  /// `minInflationValue()`.
-  ///
-  /// \param[in] value of the shape inflation.
-  ///
-  /// \returns a new inflated cone and the related transform to account for the
-  /// change of shape frame
-  std::pair<Cone, Transform3f> inflated(const FCL_REAL value) const {
-    if (value <= minInflationValue())
-      HPP_FCL_THROW_PRETTY(
-          "value (" << value << ") is two small. It should be at least: "
-                    << minInflationValue(),
-          std::invalid_argument);
-
-    // tan(alpha) = 2*halfLength/radius;
-    const FCL_REAL tan_alpha = 2 * halfLength / radius;
-    const FCL_REAL sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha);
-    const FCL_REAL top_inflation = value / sin_alpha;
-    const FCL_REAL bottom_inflation = value;
-
-    const FCL_REAL new_lz = 2 * halfLength + top_inflation + bottom_inflation;
-    const FCL_REAL new_cz = (top_inflation + bottom_inflation) / 2.;
-    const FCL_REAL new_radius = new_lz / tan_alpha;
-
-    return std::make_pair(Cone(new_radius, new_lz),
-                          Transform3f(Vec3f(0., 0., new_cz)));
-  }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const Cone* other_ptr = dynamic_cast<const Cone*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const Cone& other = *other_ptr;
-
-    return radius == other.radius && halfLength == other.halfLength &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief Cylinder along Z axis.
-/// The cylinder is defined at its centroid.
-class HPP_FCL_DLLAPI Cylinder : public ShapeBase {
- public:
-  /// @brief Default constructor
-  Cylinder() {}
-
-  Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
-    halfLength = lz_ / 2;
-  }
-
-  Cylinder(const Cylinder& other)
-      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
-
-  Cylinder& operator=(const Cylinder& other) {
-    if (this == &other) return *this;
-
-    this->radius = other.radius;
-    this->halfLength = other.halfLength;
-    return *this;
-  }
-
-  /// @brief Clone *this into a new Cylinder
-  virtual Cylinder* clone() const { return new Cylinder(*this); };
-
-  /// @brief Radius of the cylinder
-  FCL_REAL radius;
-
-  /// @brief Half Length along z axis
-  FCL_REAL halfLength;
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: a cylinder
-  NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
-
-  FCL_REAL computeVolume() const {
-    return boost::math::constants::pi<FCL_REAL>() * radius * radius *
-           (halfLength * 2);
-  }
-
-  Matrix3f computeMomentofInertia() const {
-    FCL_REAL V = computeVolume();
-    FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
-    FCL_REAL iz = V * radius * radius / 2;
-    return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
-  }
-
-  FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); }
-
-  /// \brief Inflate the cylinder by an amount given by `value`.
-  /// This value can be positive or negative but must always >=
-  /// `minInflationValue()`.
-  ///
-  /// \param[in] value of the shape inflation.
-  ///
-  /// \returns a new inflated cylinder and the related transform to account for
-  /// the change of shape frame
-  std::pair<Cylinder, Transform3f> inflated(const FCL_REAL value) const {
-    if (value <= minInflationValue())
-      HPP_FCL_THROW_PRETTY(
-          "value (" << value << ") is two small. It should be at least: "
-                    << minInflationValue(),
-          std::invalid_argument);
-    return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)),
-                          Transform3f());
-  }
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const Cylinder* other_ptr = dynamic_cast<const Cylinder*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const Cylinder& other = *other_ptr;
-
-    return radius == other.radius && halfLength == other.halfLength &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief Base for convex polytope.
-/// @note Inherited classes are responsible for filling ConvexBase::neighbors;
-class HPP_FCL_DLLAPI ConvexBase : public ShapeBase {
- public:
-  /// @brief Build a convex hull based on Qhull library
-  /// and store the vertices and optionally the triangles
-  /// \param points, num_points the points whose convex hull should be computed.
-  /// \param keepTriangles if \c true, returns a Convex<Triangle> object which
-  ///        contains the triangle of the shape.
-  /// \param qhullCommand the command sent to qhull.
-  ///        - if \c keepTriangles is \c true, this parameter should include
-  ///          "Qt". If \c NULL, "Qt" is passed to Qhull.
-  ///        - if \c keepTriangles is \c false, an empty string is passed to
-  ///          Qhull.
-  /// \note hpp-fcl must have been compiled with option \c HPP_FCL_HAS_QHULL set
-  ///       to \c ON.
-  static ConvexBase* convexHull(std::shared_ptr<std::vector<Vec3f>>& points,
-                                unsigned int num_points, bool keepTriangles,
-                                const char* qhullCommand = NULL);
-
-  // TODO(louis): put this method in private sometime in the future.
-  HPP_FCL_DEPRECATED static ConvexBase* convexHull(
-      const Vec3f* points, unsigned int num_points, bool keepTriangles,
-      const char* qhullCommand = NULL);
-
-  virtual ~ConvexBase();
-
-  /// @brief Clone (deep copy).
-  /// This method is consistent with BVHModel `clone` method.
-  /// The copy constructor is called, which duplicates the data.
-  virtual ConvexBase* clone() const { return new ConvexBase(*this); }
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: a convex polytope
-  NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
-
-#ifdef HPP_FCL_HAS_QHULL
-  /// @brief Builds the double description of the convex polytope, i.e. the set
-  /// of hyperplanes which intersection form the polytope.
-  void buildDoubleDescription();
-#endif
-
-  struct HPP_FCL_DLLAPI Neighbors {
-    unsigned char count_;
-    unsigned int* n_;
-
-    unsigned char const& count() const { return count_; }
-    unsigned int& operator[](int i) {
-      assert(i < count_);
-      return n_[i];
-    }
-    unsigned int const& operator[](int i) const {
-      assert(i < count_);
-      return n_[i];
-    }
-
-    bool operator==(const Neighbors& other) const {
-      if (count_ != other.count_) return false;
-
-      for (int i = 0; i < count_; ++i) {
-        if (n_[i] != other.n_[i]) return false;
-      }
-
-      return true;
-    }
-
-    bool operator!=(const Neighbors& other) const { return !(*this == other); }
-  };
-
-  /// @brief Above this threshold, the convex polytope is considered large.
-  /// This influcences the way the support function is computed.
-  static constexpr size_t num_vertices_large_convex_threshold = 32;
-
-  /// @brief An array of the points of the polygon.
-  std::shared_ptr<std::vector<Vec3f>> points;
-  unsigned int num_points;
-
-  /// @brief An array of the normals of the polygon.
-  std::shared_ptr<std::vector<Vec3f>> normals;
-  /// @brief An array of the offsets to the normals of the polygon.
-  /// Note: there are as many offsets as normals.
-  std::shared_ptr<std::vector<double>> offsets;
-  unsigned int num_normals_and_offsets;
-
-  /// @brief Neighbors of each vertex.
-  /// It is an array of size num_points. For each vertex, it contains the number
-  /// of neighbors and a list of indices pointing to them.
-  std::shared_ptr<std::vector<Neighbors>> neighbors;
-
-  /// @brief center of the convex polytope, this is used for collision: center
-  /// is guaranteed in the internal of the polytope (as it is convex)
-  Vec3f center;
-
-  /// @brief The support warm start polytope contains certain points of `this`
-  /// which are support points in specific directions of space.
-  /// This struct is used to warm start the support function computation for
-  /// large meshes (`num_points` > 32).
-  struct SupportWarmStartPolytope {
-    /// @brief Array of support points to warm start the support function
-    /// computation.
-    std::vector<Vec3f> points;
-
-    /// @brief Indices of the support points warm starts.
-    /// These are the indices of the real convex, not the indices of points in
-    /// the warm start polytope.
-    std::vector<int> indices;
-  };
-
-  /// @brief Number of support warm starts.
-  static constexpr size_t num_support_warm_starts = 14;
-
-  /// @brief Support warm start polytopes.
-  SupportWarmStartPolytope support_warm_starts;
-
- protected:
-  /// @brief Construct an uninitialized convex object
-  /// Initialization is done with ConvexBase::initialize.
-  ConvexBase()
-      : ShapeBase(),
-        num_points(0),
-        num_normals_and_offsets(0),
-        center(Vec3f::Zero()) {}
-
-  /// @brief Initialize the points of the convex shape
-  /// This also initializes the ConvexBase::center.
-  ///
-  /// \param ownStorage weither the ConvexBase owns the data.
-  /// \param points_ list of 3D points  ///
-  /// \param num_points_ number of 3D points
-  void initialize(std::shared_ptr<std::vector<Vec3f>> points_,
-                  unsigned int num_points_);
-
-  /// @brief Set the points of the convex shape.
-  ///
-  /// \param ownStorage weither the ConvexBase owns the data.
-  /// \param points_ list of 3D points  ///
-  /// \param num_points_ number of 3D points
-  void set(std::shared_ptr<std::vector<Vec3f>> points_,
-           unsigned int num_points_);
-
-  /// @brief Copy constructor
-  /// Only the list of neighbors is copied.
-  ConvexBase(const ConvexBase& other);
-
-#ifdef HPP_FCL_HAS_QHULL
-  void buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh);
-#endif
-
-  /// @brief Build the support points warm starts.
-  void buildSupportWarmStart();
-
-  /// @brief Array of indices of the neighbors of each vertex.
-  /// Since we don't know a priori the number of neighbors of each vertex, we
-  /// store the indices of the neighbors in a single array.
-  /// The `neighbors` attribute, an array of `Neighbors`, is used to point each
-  /// vertex to the right indices in the `nneighbors_` array.
-  std::shared_ptr<std::vector<unsigned int>> nneighbors_;
-
- private:
-  void computeCenter();
-
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const ConvexBase* other_ptr = dynamic_cast<const ConvexBase*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const ConvexBase& other = *other_ptr;
-
-    if (num_points != other.num_points) return false;
-
-    if ((!(points.get()) && other.points.get()) ||
-        (points.get() && !(other.points.get())))
-      return false;
-    if (points.get() && other.points.get()) {
-      const std::vector<Vec3f>& points_ = *points;
-      const std::vector<Vec3f>& other_points_ = *(other.points);
-      for (unsigned int i = 0; i < num_points; ++i) {
-        if (points_[i] != (other_points_)[i]) return false;
-      }
-    }
-
-    if ((!(neighbors.get()) && other.neighbors.get()) ||
-        (neighbors.get() && !(other.neighbors.get())))
-      return false;
-    if (neighbors.get() && other.neighbors.get()) {
-      const std::vector<Neighbors>& neighbors_ = *neighbors;
-      const std::vector<Neighbors>& other_neighbors_ = *(other.neighbors);
-      for (unsigned int i = 0; i < num_points; ++i) {
-        if (neighbors_[i] != other_neighbors_[i]) return false;
-      }
-    }
-
-    if ((!(normals.get()) && other.normals.get()) ||
-        (normals.get() && !(other.normals.get())))
-      return false;
-    if (normals.get() && other.normals.get()) {
-      const std::vector<Vec3f>& normals_ = *normals;
-      const std::vector<Vec3f>& other_normals_ = *(other.normals);
-      for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
-        if (normals_[i] != other_normals_[i]) return false;
-      }
-    }
-
-    if ((!(offsets.get()) && other.offsets.get()) ||
-        (offsets.get() && !(other.offsets.get())))
-      return false;
-    if (offsets.get() && other.offsets.get()) {
-      const std::vector<double>& offsets_ = *offsets;
-      const std::vector<double>& other_offsets_ = *(other.offsets);
-      for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
-        if (offsets_[i] != other_offsets_[i]) return false;
-      }
-    }
-
-    if (this->support_warm_starts.points.size() !=
-            other.support_warm_starts.points.size() ||
-        this->support_warm_starts.indices.size() !=
-            other.support_warm_starts.indices.size()) {
-      return false;
-    }
-
-    for (size_t i = 0; i < this->support_warm_starts.points.size(); ++i) {
-      if (this->support_warm_starts.points[i] !=
-              other.support_warm_starts.points[i] ||
-          this->support_warm_starts.indices[i] !=
-              other.support_warm_starts.indices[i]) {
-        return false;
-      }
-    }
-
-    return center == other.center &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-template <typename PolygonT>
-class Convex;
-
-/// @brief Half Space: this is equivalent to the Plane in ODE.
-/// A Half space has a priviledged direction: the direction of the normal.
-/// The separation plane is defined as n * x = d; Points in the negative side of
-/// the separation plane (i.e. {x | n * x < d}) are inside the half space and
-/// points in the positive side of the separation plane (i.e. {x | n * x > d})
-/// are outside the half space.
-/// Note: prefer using a Halfspace instead of a Plane if possible, it has better
-/// behavior w.r.t. collision detection algorithms.
-class HPP_FCL_DLLAPI Halfspace : public ShapeBase {
- public:
-  /// @brief Construct a half space with normal direction and offset
-  Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) {
-    unitNormalTest();
-  }
-
-  /// @brief Construct a plane with normal direction and offset
-  Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_)
-      : ShapeBase(), n(a, b, c), d(d_) {
-    unitNormalTest();
-  }
-
-  Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {}
-
-  Halfspace(const Halfspace& other)
-      : ShapeBase(other), n(other.n), d(other.d) {}
-
-  /// @brief operator =
-  Halfspace& operator=(const Halfspace& other) {
-    n = other.n;
-    d = other.d;
-    return *this;
-  }
-
-  /// @brief Clone *this into a new Halfspace
-  virtual Halfspace* clone() const { return new Halfspace(*this); };
-
-  FCL_REAL signedDistance(const Vec3f& p) const {
-    return n.dot(p) - (d + this->getSweptSphereRadius());
-  }
-
-  FCL_REAL distance(const Vec3f& p) const {
-    return std::abs(this->signedDistance(p));
-  }
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: a half space
-  NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
-
-  FCL_REAL minInflationValue() const {
-    return std::numeric_limits<FCL_REAL>::lowest();
-  }
-
-  /// \brief Inflate the halfspace by an amount given by `value`.
-  /// This value can be positive or negative but must always >=
-  /// `minInflationValue()`.
-  ///
-  /// \param[in] value of the shape inflation.
-  ///
-  /// \returns a new inflated halfspace and the related transform to account for
-  /// the change of shape frame
-  std::pair<Halfspace, Transform3f> inflated(const FCL_REAL value) const {
-    if (value <= minInflationValue())
-      HPP_FCL_THROW_PRETTY(
-          "value (" << value << ") is two small. It should be at least: "
-                    << minInflationValue(),
-          std::invalid_argument);
-    return std::make_pair(Halfspace(n, d + value), Transform3f());
-  }
-
-  /// @brief Plane normal
-  Vec3f n;
-
-  /// @brief Plane offset
-  FCL_REAL d;
-
- protected:
-  /// @brief Turn non-unit normal into unit
-  void unitNormalTest();
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const Halfspace* other_ptr = dynamic_cast<const Halfspace*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const Halfspace& other = *other_ptr;
-
-    return n == other.n && d == other.d &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/// @brief Infinite plane.
-/// A plane can be viewed as two half spaces; it has no priviledged direction.
-/// Note: prefer using a Halfspace instead of a Plane if possible, it has better
-/// behavior w.r.t. collision detection algorithms.
-class HPP_FCL_DLLAPI Plane : public ShapeBase {
- public:
-  /// @brief Construct a plane with normal direction and offset
-  Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) {
-    unitNormalTest();
-  }
-
-  /// @brief Construct a plane with normal direction and offset
-  Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_)
-      : ShapeBase(), n(a, b, c), d(d_) {
-    unitNormalTest();
-  }
-
-  Plane() : ShapeBase(), n(1, 0, 0), d(0) {}
-
-  Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {}
-
-  /// @brief operator =
-  Plane& operator=(const Plane& other) {
-    n = other.n;
-    d = other.d;
-    return *this;
-  }
-
-  /// @brief Clone *this into a new Plane
-  virtual Plane* clone() const { return new Plane(*this); };
-
-  FCL_REAL signedDistance(const Vec3f& p) const {
-    const FCL_REAL dist = n.dot(p) - d;
-    FCL_REAL signed_dist =
-        std::abs(n.dot(p) - d) - this->getSweptSphereRadius();
-    if (dist >= 0) {
-      return signed_dist;
-    }
-    if (signed_dist >= 0) {
-      return -signed_dist;
-    }
-    return signed_dist;
-  }
-
-  FCL_REAL distance(const Vec3f& p) const {
-    return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius());
-  }
-
-  /// @brief Compute AABB
-  void computeLocalAABB();
-
-  /// @brief Get node type: a plane
-  NODE_TYPE getNodeType() const { return GEOM_PLANE; }
-
-  /// @brief Plane normal
-  Vec3f n;
-
-  /// @brief Plane offset
-  FCL_REAL d;
-
- protected:
-  /// @brief Turn non-unit normal into unit
-  void unitNormalTest();
-
- private:
-  virtual bool isEqual(const CollisionGeometry& _other) const {
-    const Plane* other_ptr = dynamic_cast<const Plane*>(&_other);
-    if (other_ptr == nullptr) return false;
-    const Plane& other = *other_ptr;
-
-    return n == other.n && d == other.d &&
-           getSweptSphereRadius() == other.getSweptSphereRadius();
-  }
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/shape/geometric_shapes.h>
diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/hpp/fcl/shape/geometric_shapes_traits.h
index e4997d09ab1f3cd546049002f1bd55e506d6e080..3afa55f580155f8ec00201a3dcc59078dabb15b0 100644
--- a/include/hpp/fcl/shape/geometric_shapes_traits.h
+++ b/include/hpp/fcl/shape/geometric_shapes_traits.h
@@ -1,160 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2015-2022, CNRS, Inria
- *  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.
- */
-
-#ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H
-#define HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H
-
-#include <hpp/fcl/shape/geometric_shapes.h>
-
-namespace hpp {
-namespace fcl {
-
-struct shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = false,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = false
-  };
-};
-
-template <typename Shape>
-struct shape_traits : shape_traits_base {};
-
-template <>
-struct shape_traits<TriangleP> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = true,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = false
-  };
-};
-
-template <>
-struct shape_traits<Box> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = true,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = false
-  };
-};
-
-template <>
-struct shape_traits<Sphere> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = true,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = true
-  };
-};
-
-template <>
-struct shape_traits<Ellipsoid> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = true,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = true
-  };
-};
-
-template <>
-struct shape_traits<Capsule> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = true,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = false
-  };
-};
-
-template <>
-struct shape_traits<Cone> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = true,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = false
-  };
-};
-
-template <>
-struct shape_traits<Cylinder> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = true,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = false
-  };
-};
-
-template <>
-struct shape_traits<ConvexBase> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = true,
-    IsInflatable = true,
-    HasInflatedSupportFunction = true,
-    IsStrictlyConvex = false
-  };
-};
-
-template <>
-struct shape_traits<Halfspace> : shape_traits_base {
-  enum {
-    NeedNormalizedDir = false,
-    NeedNesterovNormalizeHeuristic = false,
-    IsInflatable = true,
-    HasInflatedSupportFunction = false,
-    IsStrictlyConvex = false
-  };
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/shape/geometric_shapes_traits.h>
diff --git a/include/hpp/fcl/shape/geometric_shapes_utility.h b/include/hpp/fcl/shape/geometric_shapes_utility.h
index f46ce042e9f3cc5aebaa09a43c3826d8744d55cc..65e33d5340b5b11b97da71608a5d3023330a60aa 100644
--- a/include/hpp/fcl/shape/geometric_shapes_utility.h
+++ b/include/hpp/fcl/shape/geometric_shapes_utility.h
@@ -1,265 +1,2 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, 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 HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H
-#define HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H
-
-#include <vector>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/internal/BV_fitter.h>
-
-namespace hpp {
-namespace fcl {
-
-/// @cond IGNORE
-namespace details {
-/// @brief get the vertices of some convex shape which can bound the given shape
-/// in a specific configuration
-HPP_FCL_DLLAPI std::vector<Vec3f> getBoundVertices(const Box& box,
-                                                   const Transform3f& tf);
-HPP_FCL_DLLAPI std::vector<Vec3f> getBoundVertices(const Sphere& sphere,
-                                                   const Transform3f& tf);
-HPP_FCL_DLLAPI std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
-                                                   const Transform3f& tf);
-HPP_FCL_DLLAPI std::vector<Vec3f> getBoundVertices(const Capsule& capsule,
-                                                   const Transform3f& tf);
-HPP_FCL_DLLAPI std::vector<Vec3f> getBoundVertices(const Cone& cone,
-                                                   const Transform3f& tf);
-HPP_FCL_DLLAPI std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder,
-                                                   const Transform3f& tf);
-HPP_FCL_DLLAPI std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
-                                                   const Transform3f& tf);
-HPP_FCL_DLLAPI std::vector<Vec3f> getBoundVertices(const TriangleP& triangle,
-                                                   const Transform3f& tf);
-}  // namespace details
-/// @endcond
-
-/// @brief calculate a bounding volume for a shape in a specific configuration
-template <typename BV, typename S>
-inline void computeBV(const S& s, const Transform3f& tf, BV& bv) {
-  if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
-  }
-  std::vector<Vec3f> convex_bound_vertices = details::getBoundVertices(s, tf);
-  fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(),
-      bv);
-}
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, Box>(const Box& s, const Transform3f& tf,
-                                         AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, Sphere>(const Sphere& s,
-                                            const Transform3f& tf, AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, Ellipsoid>(const Ellipsoid& e,
-                                               const Transform3f& tf, AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, Capsule>(const Capsule& s,
-                                             const Transform3f& tf, AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf,
-                                          AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, Cylinder>(const Cylinder& s,
-                                              const Transform3f& tf, AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, ConvexBase>(const ConvexBase& s,
-                                                const Transform3f& tf,
-                                                AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, TriangleP>(const TriangleP& s,
-                                               const Transform3f& tf, AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, Halfspace>(const Halfspace& s,
-                                               const Transform3f& tf, AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<AABB, Plane>(const Plane& s,
-                                           const Transform3f& tf, AABB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBB, Box>(const Box& s, const Transform3f& tf,
-                                        OBB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBB, Sphere>(const Sphere& s,
-                                           const Transform3f& tf, OBB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBB, Capsule>(const Capsule& s,
-                                            const Transform3f& tf, OBB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf,
-                                         OBB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBB, Cylinder>(const Cylinder& s,
-                                             const Transform3f& tf, OBB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBB, ConvexBase>(const ConvexBase& s,
-                                               const Transform3f& tf, OBB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBB, Halfspace>(const Halfspace& s,
-                                              const Transform3f& tf, OBB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<RSS, Halfspace>(const Halfspace& s,
-                                              const Transform3f& tf, RSS& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBBRSS, Halfspace>(const Halfspace& s,
-                                                 const Transform3f& tf,
-                                                 OBBRSS& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<kIOS, Halfspace>(const Halfspace& s,
-                                               const Transform3f& tf, kIOS& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<KDOP<16>, Halfspace>(const Halfspace& s,
-                                                   const Transform3f& tf,
-                                                   KDOP<16>& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<KDOP<18>, Halfspace>(const Halfspace& s,
-                                                   const Transform3f& tf,
-                                                   KDOP<18>& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<KDOP<24>, Halfspace>(const Halfspace& s,
-                                                   const Transform3f& tf,
-                                                   KDOP<24>& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf,
-                                          OBB& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf,
-                                          RSS& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<OBBRSS, Plane>(const Plane& s,
-                                             const Transform3f& tf, OBBRSS& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<kIOS, Plane>(const Plane& s,
-                                           const Transform3f& tf, kIOS& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<KDOP<16>, Plane>(const Plane& s,
-                                               const Transform3f& tf,
-                                               KDOP<16>& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<KDOP<18>, Plane>(const Plane& s,
-                                               const Transform3f& tf,
-                                               KDOP<18>& bv);
-
-template <>
-HPP_FCL_DLLAPI void computeBV<KDOP<24>, Plane>(const Plane& s,
-                                               const Transform3f& tf,
-                                               KDOP<24>& bv);
-
-/// @brief construct a box shape (with a configuration) from a given bounding
-/// volume
-HPP_FCL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const AABB& bv, const Transform3f& tf_bv,
-                                 Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const OBB& bv, const Transform3f& tf_bv,
-                                 Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3f& tf_bv,
-                                 Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const kIOS& bv, const Transform3f& tf_bv,
-                                 Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const RSS& bv, const Transform3f& tf_bv,
-                                 Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv,
-                                 Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv,
-                                 Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv,
-                                 Box& box, Transform3f& tf);
-
-HPP_FCL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3f& tf);
-
-HPP_FCL_DLLAPI Plane transform(const Plane& a, const Transform3f& tf);
-
-HPP_FCL_DLLAPI std::array<Halfspace, 2> transformToHalfspaces(
-    const Plane& a, const Transform3f& tf);
-
-}  // namespace fcl
-
-}  // namespace hpp
-
-#endif
+#include <hpp/fcl/coal.hpp>
+#include <coal/shape/geometric_shapes_utility.h>
diff --git a/include/hpp/fcl/timings.h b/include/hpp/fcl/timings.h
index 961a26752ac08cfc2826b97fb6486a05dd433867..4500e9f19676a8aa394279368130bb42ba2dcc9a 100644
--- a/include/hpp/fcl/timings.h
+++ b/include/hpp/fcl/timings.h
@@ -1,114 +1,2 @@
-//
-// Copyright (c) 2021-2023 INRIA
-//
-
-#ifndef HPP_FCL_TIMINGS_FWD_H
-#define HPP_FCL_TIMINGS_FWD_H
-
-#include "hpp/fcl/fwd.hh"
-
-#ifdef HPP_FCL_WITH_CXX11_SUPPORT
-#include <chrono>
-#endif
-
-namespace hpp {
-namespace fcl {
-
-struct CPUTimes {
-  double wall;
-  double user;
-  double system;
-
-  CPUTimes() : wall(0), user(0), system(0) {}
-
-  void clear() { wall = user = system = 0; }
-};
-
-///
-/// @brief This class mimics the way "boost/timer/timer.hpp" operates while
-/// using the modern std::chrono library.
-///        Importantly, this class will only have an effect for C++11 and more.
-///
-struct HPP_FCL_DLLAPI Timer {
-#ifdef HPP_FCL_WITH_CXX11_SUPPORT
-  typedef std::chrono::steady_clock clock_type;
-  typedef clock_type::duration duration_type;
-#endif
-
-  /// \brief Default constructor for the timer
-  ///
-  /// \param[in] start_on_construction if true, the timer will be run just after
-  /// the object is created
-  Timer(const bool start_on_construction = true) : m_is_stopped(true) {
-    if (start_on_construction) Timer::start();
-  }
-
-  CPUTimes elapsed() const {
-    if (m_is_stopped) return m_times;
-
-    CPUTimes current(m_times);
-#ifdef HPP_FCL_WITH_CXX11_SUPPORT
-    std::chrono::time_point<std::chrono::steady_clock> current_clock =
-        std::chrono::steady_clock::now();
-    current.user += static_cast<double>(
-                        std::chrono::duration_cast<std::chrono::nanoseconds>(
-                            current_clock - m_start)
-                            .count()) *
-                    1e-3;
-#endif
-    return current;
-  }
-
-#ifdef HPP_FCL_WITH_CXX11_SUPPORT
-  duration_type duration() const { return (m_end - m_start); }
-#endif
-
-  void start() {
-    if (m_is_stopped) {
-      m_is_stopped = false;
-      m_times.clear();
-
-#ifdef HPP_FCL_WITH_CXX11_SUPPORT
-      m_start = std::chrono::steady_clock::now();
-#endif
-    }
-  }
-
-  void stop() {
-    if (m_is_stopped) return;
-    m_is_stopped = true;
-
-#ifdef HPP_FCL_WITH_CXX11_SUPPORT
-    m_end = std::chrono::steady_clock::now();
-    m_times.user += static_cast<double>(
-                        std::chrono::duration_cast<std::chrono::nanoseconds>(
-                            m_end - m_start)
-                            .count()) *
-                    1e-3;
-#endif
-  }
-
-  void resume() {
-#ifdef HPP_FCL_WITH_CXX11_SUPPORT
-    if (m_is_stopped) {
-      m_start = std::chrono::steady_clock::now();
-      m_is_stopped = false;
-    }
-#endif
-  }
-
-  bool is_stopped() const { return m_is_stopped; }
-
- protected:
-  CPUTimes m_times;
-  bool m_is_stopped;
-
-#ifdef HPP_FCL_WITH_CXX11_SUPPORT
-  std::chrono::time_point<std::chrono::steady_clock> m_start, m_end;
-#endif
-};
-
-}  // namespace fcl
-}  // namespace hpp
-
-#endif  // ifndef HPP_FCL_TIMINGS_FWD_H
+#include <hpp/fcl/coal.hpp>
+#include <coal/timings.h>
diff --git a/include/hpp/fcl/warning.hh b/include/hpp/fcl/warning.hh
new file mode 100644
index 0000000000000000000000000000000000000000..c49b82715f40bedb4b968c00e6b9fe863a2dc1e9
--- /dev/null
+++ b/include/hpp/fcl/warning.hh
@@ -0,0 +1,2 @@
+#include <hpp/fcl/coal.hpp>
+#include <coal/warning.hh>
diff --git a/package.xml b/package.xml
index 216b4a19241562fb497f64596c553668b12072c8..0428aa2f2139c8bcead49c2ed0a1049c2f94a0c7 100644
--- a/package.xml
+++ b/package.xml
@@ -7,6 +7,7 @@
   Please check the repository URL for full list of authors and maintainers. -->
   <maintainer email="jmirabel@laas.fr">Joseph Mirabel</maintainer>
   <maintainer email="justin.carpentier@inria.fr">Justin Carpentier</maintainer>
+  <maintainer email="louis.montaut@inria.fr">Louis Montaut</maintainer>
   <maintainer email="opensource@wolfgangmerkt.com">Wolfgang Merkt</maintainer>
   <license>BSD</license>
 
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index a505d0dfaac3092e887e511d985bf1bb855baa98..38b98031b6f6c42bc73554d88f4edc510fd37950 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -35,15 +35,15 @@
 include(${JRL_CMAKE_MODULES}/python-helpers.cmake)
 include(${JRL_CMAKE_MODULES}/stubs.cmake)
 
-ADD_CUSTOM_TARGET(python)
-SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
+ADD_CUSTOM_TARGET(${PROJECT_NAME}_python)
+SET_TARGET_PROPERTIES(${PROJECT_NAME}_python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
 
 # Name of the Python library
-SET(LIBRARY_NAME hppfcl)
+SET(PYTHON_LIB_NAME ${PROJECT_NAME}_pywrap)
 
-SET(${LIBRARY_NAME}_HEADERS
+SET(${PYTHON_LIB_NAME}_HEADERS
   fwd.hh
-  fcl.hh
+  coal.hh
   deprecation.hh
   broadphase/fwd.hh
   broadphase/broadphase_collision_manager.hh
@@ -61,6 +61,14 @@ IF(    NOT ENABLE_PYTHON_DOXYGEN_AUTODOC
 ELSE()
   SET(ENABLE_DOXYGEN_AUTODOC TRUE)
 
+  IF(DOXYGEN_VERSION VERSION_GREATER_EQUAL 1.9.8 AND DOXYGEN_VERSION VERSION_LESS 1.11.0)
+    # deactivate python doxygen automoc for doxygen 1.9.8 and 1.10.0,
+    # as it incorrectly parse "const" keyword,
+    # generating wrong links in C++ doc, and fail generating python doc
+    # ref. https://github.com/doxygen/doxygen/issues/10797
+    SET(ENABLE_DOXYGEN_AUTODOC FALSE)
+    MESSAGE(AUTHOR_WARNING "automoc deactivated because of doxygen 1.10. Please use <1.10 or >=1.11.")
+  ENDIF()
   EXECUTE_PROCESS(COMMAND ${PYTHON_EXECUTABLE} -c "import lxml"
     RESULT_VARIABLE _pypkg_found
     OUTPUT_QUIET
@@ -86,8 +94,8 @@ ELSE()
 ENDIF()
 IF(ENABLE_DOXYGEN_AUTODOC)
   ADD_CUSTOM_TARGET(generate_doxygen_cpp_doc
-    COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/python/doxygen_xml_parser.py
-    ${CMAKE_BINARY_DIR}/doc/doxygen-xml/index.xml
+    COMMAND ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/doc/python/doxygen_xml_parser.py
+    ${PROJECT_BINARY_DIR}/doc/doxygen-xml/index.xml
     ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc > ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc.log
     BYPRODUCTS
     ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh
@@ -96,52 +104,54 @@ IF(ENABLE_DOXYGEN_AUTODOC)
     )
   ADD_DEPENDENCIES(generate_doxygen_cpp_doc doc)
 
-  LIST(APPEND ${LIBRARY_NAME}_HEADERS
+  LIST(APPEND ${PYTHON_LIB_NAME}_HEADERS
     ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh
     )
 ENDIF()
 
-SET(${LIBRARY_NAME}_SOURCES
+SET(${PYTHON_LIB_NAME}_SOURCES
   version.cc
   math.cc
   collision-geometries.cc
   collision.cc
   contact_patch.cc
   distance.cc
-  fcl.cc
+  coal.cc
   gjk.cc
   broadphase/broadphase.cc
   )
 
-IF(HPP_FCL_HAS_OCTOMAP)
-  LIST(APPEND ${LIBRARY_NAME}_SOURCES octree.cc)
-ENDIF(HPP_FCL_HAS_OCTOMAP)
+IF(COAL_HAS_OCTOMAP)
+  LIST(APPEND ${PYTHON_LIB_NAME}_SOURCES octree.cc)
+ENDIF(COAL_HAS_OCTOMAP)
 
-ADD_LIBRARY(${LIBRARY_NAME} MODULE ${${LIBRARY_NAME}_SOURCES} ${${LIBRARY_NAME}_HEADERS})
-TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS})
-TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} PRIVATE "${CMAKE_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}")
+ADD_LIBRARY(${PYTHON_LIB_NAME} MODULE ${${PYTHON_LIB_NAME}_SOURCES} ${${PYTHON_LIB_NAME}_HEADERS})
+TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS})
+TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} PRIVATE "${PROJECT_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}")
 IF(WIN32)
-  TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC ${PYTHON_LIBRARY})
+  TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY})
 ENDIF(WIN32)
 
-ADD_DEPENDENCIES(python ${LIBRARY_NAME})
-ADD_HEADER_GROUP(${LIBRARY_NAME}_HEADERS)
-ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES)
+ADD_DEPENDENCIES(${PROJECT_NAME}_python ${PYTHON_LIB_NAME})
+ADD_HEADER_GROUP(${PYTHON_LIB_NAME}_HEADERS)
+ADD_SOURCE_GROUP(${PYTHON_LIB_NAME}_SOURCES)
 IF(ENABLE_DOXYGEN_AUTODOC)
-  ADD_DEPENDENCIES(${LIBRARY_NAME} generate_doxygen_cpp_doc)
-  TARGET_COMPILE_DEFINITIONS(${LIBRARY_NAME} PRIVATE -DHPP_FCL_HAS_DOXYGEN_AUTODOC)
+  ADD_DEPENDENCIES(${PYTHON_LIB_NAME} generate_doxygen_cpp_doc)
+  TARGET_COMPILE_DEFINITIONS(${PYTHON_LIB_NAME} PRIVATE -DCOAL_HAS_DOXYGEN_AUTODOC)
 ENDIF()
 
-TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC
+TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC
   ${PROJECT_NAME}
   eigenpy::eigenpy
   Boost::system)
 
-SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES
+SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES
   PREFIX ""
   SUFFIX "${PYTHON_EXT_SUFFIX}"
-  LIBRARY_OUTPUT_NAME ${LIBRARY_NAME}
-  LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${LIBRARY_NAME}"
+  OUTPUT_NAME "${PYTHON_LIB_NAME}"
+  LIBRARY_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${PROJECT_NAME}"
+  # On Windows, shared library are treat as binary
+  RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${PROJECT_NAME}"
   )
 
 IF(IS_ABSOLUTE ${PYTHON_SITELIB})
@@ -149,30 +159,38 @@ IF(IS_ABSOLUTE ${PYTHON_SITELIB})
 ELSE()
   SET(ABSOLUTE_PYTHON_SITELIB ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB})
 ENDIF()
-SET(${LIBRARY_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${LIBRARY_NAME})
+SET(${PYTHON_LIB_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${PROJECT_NAME})
 IF(UNIX)
-  GET_RELATIVE_RPATH(${${LIBRARY_NAME}_INSTALL_DIR} ${LIBRARY_NAME}_INSTALL_RPATH)
-  SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES INSTALL_RPATH "${${LIBRARY_NAME}_INSTALL_RPATH}")
+  GET_RELATIVE_RPATH(${${PYTHON_LIB_NAME}_INSTALL_DIR} ${PYTHON_LIB_NAME}_INSTALL_RPATH)
+  SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES INSTALL_RPATH "${${PYTHON_LIB_NAME}_INSTALL_RPATH}")
 ENDIF()
 
-INSTALL(TARGETS ${LIBRARY_NAME}
-  DESTINATION ${${LIBRARY_NAME}_INSTALL_DIR})
+INSTALL(TARGETS ${PYTHON_LIB_NAME}
+  EXPORT ${TARGETS_EXPORT_NAME}
+  DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR})
 
 # --- GENERATE STUBS
 IF(GENERATE_PYTHON_STUBS)
   LOAD_STUBGEN()
-  GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${LIBRARY_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${LIBRARY_NAME} ${PROJECT_NAME})
+  GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${PROJECT_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${PYTHON_LIB_NAME} ${PROJECT_NAME})
 ENDIF(GENERATE_PYTHON_STUBS)
 
 # --- INSTALL SCRIPTS
 SET(PYTHON_FILES
   __init__.py
   viewer.py
+  windows_dll_manager.py
   )
 
 FOREACH(python ${PYTHON_FILES})
-  PYTHON_BUILD(${LIBRARY_NAME} ${python})
+  PYTHON_BUILD(${PROJECT_NAME} ${python})
   INSTALL(FILES
-    "${CMAKE_CURRENT_SOURCE_DIR}/${LIBRARY_NAME}/${python}"
-    DESTINATION ${${LIBRARY_NAME}_INSTALL_DIR})
+    "${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}/${python}"
+    DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR})
 ENDFOREACH(python)
+
+
+if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL)
+  python_install_on_site(hppfcl __init__.py COMPONENT hpp-fcl-compatibility)
+  python_install_on_site(hppfcl viewer.py COMPONENT hpp-fcl-compatibility)
+endif()
diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc
index f6f9592aed8fe518dfc3e0b0d85f50c48f6ddc3d..28e427a278b59c7964daac122851ed3624493b4b 100644
--- a/python/broadphase/broadphase.cc
+++ b/python/broadphase/broadphase.cc
@@ -32,41 +32,41 @@
 //  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 //  POSSIBILITY OF SUCH DAMAGE.
 
-#include <hpp/fcl/fwd.hh>
-#include "../fcl.hh"
+#include "coal/fwd.hh"
+#include "../coal.hh"
 #include "../utils/std-pair.hh"
 
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
-#include "hpp/fcl/broadphase/broadphase_bruteforce.h"
-#include "hpp/fcl/broadphase/broadphase_SaP.h"
-#include "hpp/fcl/broadphase/broadphase_SSaP.h"
-#include "hpp/fcl/broadphase/broadphase_interval_tree.h"
-#include "hpp/fcl/broadphase/broadphase_spatialhash.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
+#include "coal/broadphase/broadphase_bruteforce.h"
+#include "coal/broadphase/broadphase_SaP.h"
+#include "coal/broadphase/broadphase_SSaP.h"
+#include "coal/broadphase/broadphase_interval_tree.h"
+#include "coal/broadphase/broadphase_spatialhash.h"
 
-HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+COAL_COMPILER_DIAGNOSTIC_PUSH
+COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 #include "doxygen_autodoc/functions.h"
-HPP_FCL_COMPILER_DIAGNOSTIC_POP
-#include "doxygen_autodoc/hpp/fcl/broadphase/default_broadphase_callbacks.h"
-// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
+COAL_COMPILER_DIAGNOSTIC_POP
+#include "doxygen_autodoc/coal/broadphase/default_broadphase_callbacks.h"
+// #include "doxygen_autodoc/coal/broadphase/broadphase_dynamic_AABB_tree.h"
 // #include
-//"doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
-// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_bruteforce.h"
-// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_SaP.h"
-// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_SSaP.h"
-// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_interval_tree.h"
-// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_spatialhash.h"
+//"doxygen_autodoc/coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
+// #include "doxygen_autodoc/coal/broadphase/broadphase_bruteforce.h"
+// #include "doxygen_autodoc/coal/broadphase/broadphase_SaP.h"
+// #include "doxygen_autodoc/coal/broadphase/broadphase_SSaP.h"
+// #include "doxygen_autodoc/coal/broadphase/broadphase_interval_tree.h"
+// #include "doxygen_autodoc/coal/broadphase/broadphase_spatialhash.h"
 #endif
 
 #include "broadphase_callbacks.hh"
 #include "broadphase_collision_manager.hh"
 
-using namespace hpp::fcl;
+using namespace coal;
 
-HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+COAL_COMPILER_DIAGNOSTIC_PUSH
+COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 void exposeBroadPhase() {
   CollisionCallBackBaseWrapper::expose();
   DistanceCallBackBaseWrapper::expose();
@@ -135,8 +135,8 @@ void exposeBroadPhase() {
     typedef SpatialHashingCollisionManager<HashTable> Derived;
     bp::class_<Derived, bp::bases<BroadPhaseCollisionManager>>(
         "SpatialHashingCollisionManager", bp::no_init)
-        .def(dv::init<Derived, FCL_REAL, const Vec3f &, const Vec3f &,
+        .def(dv::init<Derived, CoalScalar, const Vec3s &, const Vec3s &,
                       bp::optional<unsigned int>>());
   }
 }
-HPP_FCL_COMPILER_DIAGNOSTIC_POP
+COAL_COMPILER_DIAGNOSTIC_POP
diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh
index b6ed07d7beb891b2294018bd28a7179bd9f88b6f..b0ae02d300e479cd32313318d27372d5febf7471 100644
--- a/python/broadphase/broadphase_callbacks.hh
+++ b/python/broadphase/broadphase_callbacks.hh
@@ -32,23 +32,22 @@
 //  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 //  POSSIBILITY OF SUCH DAMAGE.
 
-#ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH
-#define HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH
+#ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH
+#define COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH
 
 #include <eigenpy/eigenpy.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/broadphase/broadphase_callbacks.h>
+#include "coal/fwd.hh"
+#include "coal/broadphase/broadphase_callbacks.h"
 
-#include "../fcl.hh"
+#include "../coal.hh"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 #include "doxygen_autodoc/functions.h"
-#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_callbacks.h"
+#include "doxygen_autodoc/coal/broadphase/broadphase_callbacks.h"
 #endif
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 struct CollisionCallBackBaseWrapper : CollisionCallBackBase,
                                       bp::wrapper<CollisionCallBackBase> {
@@ -85,7 +84,7 @@ struct DistanceCallBackBaseWrapper : DistanceCallBackBase,
     return distance(o1, o2, dist.coeffRef(0, 0));
   }
 
-  bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist) {
+  bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist) {
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wconversion"
     return this->get_override("distance")(o1, o2, dist);
@@ -108,7 +107,6 @@ struct DistanceCallBackBaseWrapper : DistanceCallBackBase,
   }
 };  // DistanceCallBackBaseWrapper
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
-#endif  // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH
+#endif  // ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH
diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh
index 796dbd475fa0dbecc0d139d830abd1fc293a420c..2624c0dcb681ef04d434259f976836ebd15809e6 100644
--- a/python/broadphase/broadphase_collision_manager.hh
+++ b/python/broadphase/broadphase_collision_manager.hh
@@ -32,27 +32,26 @@
 //  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 //  POSSIBILITY OF SUCH DAMAGE.
 
-#ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH
-#define HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH
+#ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH
+#define COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH
 
 #include <eigenpy/eigenpy.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/broadphase/broadphase_collision_manager.h>
-#include <hpp/fcl/broadphase/default_broadphase_callbacks.h>
+#include "coal/fwd.hh"
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/default_broadphase_callbacks.h"
 
-#include "../fcl.hh"
+#include "../coal.hh"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 #include "doxygen_autodoc/functions.h"
-#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_collision_manager.h"
+#include "doxygen_autodoc/coal/broadphase/broadphase_collision_manager.h"
 #endif
 
 #include <boost/algorithm/string/replace.hpp>
 #include <boost/type_index.hpp>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 struct BroadPhaseCollisionManagerWrapper
     : BroadPhaseCollisionManager,
@@ -216,7 +215,7 @@ struct BroadPhaseCollisionManagerWrapper
   template <typename Derived>
   static void exposeDerived() {
     std::string class_name = boost::typeindex::type_id<Derived>().pretty_name();
-    boost::algorithm::replace_all(class_name, "hpp::fcl::", "");
+    boost::algorithm::replace_all(class_name, "coal::", "");
 #if defined(WIN32)
     boost::algorithm::replace_all(class_name, "class ", "");
 #endif
@@ -228,7 +227,6 @@ struct BroadPhaseCollisionManagerWrapper
 
 };  // BroadPhaseCollisionManagerWrapper
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
-#endif  // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH
+#endif  // ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH
diff --git a/python/broadphase/fwd.hh b/python/broadphase/fwd.hh
index 2f8e6bc571c3506f175628f4be8f7bccb60de0a1..1367cc065ffbb5f6f61299d01edf1ae4bd257f05 100644
--- a/python/broadphase/fwd.hh
+++ b/python/broadphase/fwd.hh
@@ -2,19 +2,17 @@
 // Copyright (c) 2022 INRIA
 //
 
-#ifndef HPP_FCL_PYTHON_BROADPHASE_FWD_HH
-#define HPP_FCL_PYTHON_BROADPHASE_FWD_HH
+#ifndef COAL_PYTHON_BROADPHASE_FWD_HH
+#define COAL_PYTHON_BROADPHASE_FWD_HH
 
-#include "hppfcl/fwd.hh"
+#include "coal/fwd.hh"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace python {
 
 void exposeBroadPhase();
 
 }
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
-#endif  // ifndef HPP_FCL_PYTHON_BROADPHASE_FWD_HH
+#endif  // ifndef COAL_PYTHON_BROADPHASE_FWD_HH
diff --git a/python/fcl.cc b/python/coal.cc
similarity index 90%
rename from python/fcl.cc
rename to python/coal.cc
index 956ac8715137017306d48256241c95b5c7b2ee7c..dee8825d1bf5bb8cb820e412dd3f3e7fc570c260 100644
--- a/python/fcl.cc
+++ b/python/coal.cc
@@ -34,24 +34,24 @@
 
 #include <eigenpy/eigenpy.hpp>
 
-#include "fcl.hh"
+#include "coal.hh"
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/BVH/BVH_model.h>
+#include "coal/fwd.hh"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/BVH/BVH_model.h"
 
-#include <hpp/fcl/mesh_loader/loader.h>
+#include "coal/mesh_loader/loader.h"
 
-#include <hpp/fcl/collision.h>
+#include "coal/collision.h"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
-#include "doxygen_autodoc/hpp/fcl/mesh_loader/loader.h"
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
+#include "doxygen_autodoc/coal/mesh_loader/loader.h"
 #endif
 
 #include "../doc/python/doxygen.hh"
 #include "../doc/python/doxygen-boost.hh"
 
-using namespace hpp::fcl;
+using namespace coal;
 namespace dv = doxygen::visitor;
 
 #pragma GCC diagnostic push
@@ -83,7 +83,7 @@ void exposeMeshLoader() {
   }
 }
 
-BOOST_PYTHON_MODULE(hppfcl) {
+BOOST_PYTHON_MODULE(coal_pywrap) {
   namespace bp = boost::python;
 
   PyImport_ImportModule("warnings");
@@ -98,7 +98,7 @@ BOOST_PYTHON_MODULE(hppfcl) {
   exposeContactPatchAPI();
   exposeDistanceAPI();
   exposeGJK();
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
   exposeOctree();
 #endif
   exposeBroadPhase();
diff --git a/python/fcl.hh b/python/coal.hh
similarity index 92%
rename from python/fcl.hh
rename to python/coal.hh
index 6b06095f9cc4f40f6cb3dd850924a5ed83f47947..0dda8024412aee9612c774c84c74725dcb133096 100644
--- a/python/fcl.hh
+++ b/python/coal.hh
@@ -18,7 +18,7 @@ void exposeDistanceAPI();
 
 void exposeGJK();
 
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
 void exposeOctree();
 #endif
 
diff --git a/python/coal/__init__.py b/python/coal/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..966642738d14e62ee574d3780c44998769067264
--- /dev/null
+++ b/python/coal/__init__.py
@@ -0,0 +1,61 @@
+# Software License Agreement (BSD License)
+#
+#  Copyright (c) 2019 INRIA
+#  Author: Justin Carpentier
+#  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 CNRS-LAAS. 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.
+
+# ruff: noqa: F401, F403
+
+# On Windows, if coal.dll is not in the same directory than
+# the .pyd, it will not be loaded.
+# We first try to load coal, then, if it fail and we are on Windows:
+#  1. We add all paths inside COAL_WINDOWS_DLL_PATH to DllDirectory
+#  2. If COAL_WINDOWS_DLL_PATH we add the relative path from the
+#     package directory to the bin directory to DllDirectory
+# This solution is inspired from:
+#  - https://github.com/PixarAnimationStudios/OpenUSD/pull/1511/files
+#  - https://stackoverflow.com/questions/65334494/python-c-extension-packaging-dll-along-with-pyd
+# More resources on https://github.com/diffpy/pyobjcryst/issues/33
+try:
+    from .coal_pywrap import *  # noqa
+    from .coal_pywrap import __raw_version__, __version__
+except ImportError:
+    import platform
+
+    if platform.system() == "Windows":
+        from .windows_dll_manager import build_directory_manager, get_dll_paths
+
+        with build_directory_manager() as dll_dir_manager:
+            for p in get_dll_paths():
+                dll_dir_manager.add_dll_directory(p)
+            from .coal_pywrap import *  # noqa
+            from .coal_pywrap import __raw_version__, __version__  # noqa
+    else:
+        raise
diff --git a/python/coal/viewer.py b/python/coal/viewer.py
new file mode 100644
index 0000000000000000000000000000000000000000..bef54e2c0f3bf3a23077810bd902438ccc06de09
--- /dev/null
+++ b/python/coal/viewer.py
@@ -0,0 +1,110 @@
+# Software License Agreement (BSD License)
+#
+#  Copyright (c) 2019 CNRS
+#  Author: Joseph Mirabel
+
+import warnings
+
+import numpy as np
+from gepetto import Color
+
+import coal
+
+
+def applyConfiguration(gui, name, tf):
+    gui.applyConfiguration(
+        name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist()
+    )
+
+
+def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)):
+    if isinstance(geom, coal.Capsule):
+        return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color)
+    elif isinstance(geom, coal.Cylinder):
+        return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color)
+    elif isinstance(geom, coal.Box):
+        w, h, d = (2.0 * geom.halfSide).tolist()
+        return gui.addBox(name, w, h, d, color)
+    elif isinstance(geom, coal.Sphere):
+        return gui.addSphere(name, geom.radius, color)
+    elif isinstance(geom, coal.Cone):
+        return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color)
+    elif isinstance(geom, coal.Convex):
+        pts = [
+            geom.points(geom.polygons(f)[i]).tolist()
+            for f in range(geom.num_polygons)
+            for i in range(3)
+        ]
+        gui.addCurve(name, pts, color)
+        gui.setCurveMode(name, "TRIANGLES")
+        gui.setLightingMode(name, "ON")
+        gui.setBoolProperty(name, "BackfaceDrawing", True)
+        return True
+    elif isinstance(geom, coal.ConvexBase):
+        pts = [geom.points(i).tolist() for i in range(geom.num_points)]
+        gui.addCurve(name, pts, color)
+        gui.setCurveMode(name, "POINTS")
+        gui.setLightingMode(name, "OFF")
+        return True
+    else:
+        msg = "Unsupported geometry type for %s (%s)" % (name, type(geom))
+        warnings.warn(msg, category=UserWarning, stacklevel=2)
+        return False
+
+
+def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True):
+    gui.createGroup(group_name)
+    r = 0.01
+    if closest_points:
+        p = [group_name + "/p1", group_name + "/p2"]
+        gui.addSphere(p[0], r, Color.red)
+        gui.addSphere(p[1], r, Color.blue)
+        qid = [0, 0, 0, 1]
+        gui.applyConfigurations(
+            p,
+            [
+                res.getNearestPoint1().tolist() + qid,
+                res.getNearestPoint2().tolist() + qid,
+            ],
+        )
+    if normal:
+        n = group_name + "/normal"
+        gui.addArrow(n, r, 0.1, Color.green)
+        gui.applyConfiguration(
+            n,
+            res.getNearestPoint1().tolist()
+            + coal.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal)
+            .coeffs()
+            .tolist(),
+        )
+    gui.refresh()
+
+
+def displayCollisionResult(gui, group_name, res, color=Color.green):
+    if res.isCollision():
+        if gui.nodeExists(group_name):
+            gui.setVisibility(group_name, "ON")
+        else:
+            gui.createGroup(group_name)
+        for i in range(res.numContacts()):
+            contact = res.getContact(i)
+            n = group_name + "/contact" + str(i)
+            depth = contact.penetration_depth
+            if gui.nodeExists(n):
+                gui.setFloatProperty(n, "Size", depth)
+                gui.setFloatProperty(n, "Radius", 0.1 * depth)
+                gui.setColor(n, color)
+            else:
+                gui.addArrow(n, depth * 0.1, depth, color)
+            N = contact.normal
+            P = contact.pos
+            gui.applyConfiguration(
+                n,
+                (P - depth * N / 2).tolist()
+                + coal.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N)
+                .coeffs()
+                .tolist(),
+            )
+        gui.refresh()
+    elif gui.nodeExists(group_name):
+        gui.setVisibility(group_name, "OFF")
diff --git a/python/coal/windows_dll_manager.py b/python/coal/windows_dll_manager.py
new file mode 100644
index 0000000000000000000000000000000000000000..92516fee10921ed8952d530fce4f3c817786b44e
--- /dev/null
+++ b/python/coal/windows_dll_manager.py
@@ -0,0 +1,65 @@
+import contextlib
+import os
+import sys
+
+
+def get_dll_paths():
+    coal_paths = os.getenv("COAL_WINDOWS_DLL_PATH")
+    if coal_paths is None:
+        # From https://peps.python.org/pep-0250/#implementation
+        # lib/python-version/site-packages/package
+        RELATIVE_DLL_PATH1 = "..\\..\\..\\..\\bin"
+        # lib/site-packages/package
+        RELATIVE_DLL_PATH2 = "..\\..\\..\\bin"
+        # For unit test
+        RELATIVE_DLL_PATH3 = "..\\..\\bin"
+        return [
+            os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH1),
+            os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH2),
+            os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH3),
+        ]
+    else:
+        return coal_paths.split(os.pathsep)
+
+
+class PathManager(contextlib.AbstractContextManager):
+    """Restore PATH state after importing Python module"""
+
+    def add_dll_directory(self, dll_dir: str):
+        os.environ["PATH"] += os.pathsep + dll_dir
+
+    def __enter__(self):
+        self.old_path = os.environ["PATH"]
+        return self
+
+    def __exit__(self, *exc_details):
+        os.environ["PATH"] = self.old_path
+
+
+class DllDirectoryManager(contextlib.AbstractContextManager):
+    """Restore DllDirectory state after importing Python module"""
+
+    def add_dll_directory(self, dll_dir: str):
+        # add_dll_directory can fail on relative path and non
+        # existing path.
+        # Since we don't know all the fail criterion we just ignore
+        # thrown exception
+        try:
+            self.dll_dirs.append(os.add_dll_directory(dll_dir))
+        except OSError:
+            pass
+
+    def __enter__(self):
+        self.dll_dirs = []
+        return self
+
+    def __exit__(self, *exc_details):
+        for d in self.dll_dirs:
+            d.close()
+
+
+def build_directory_manager():
+    if sys.version_info >= (3, 8):
+        return DllDirectoryManager()
+    else:
+        return PathManager()
diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc
index 3ee0a0b508b708efe52f66eb8ec2055ca3a0ed3d..b3afc55942ec2cfef2d3688afae70370415a3a9b 100644
--- a/python/collision-geometries.cc
+++ b/python/collision-geometries.cc
@@ -1,7 +1,7 @@
 //
 // Software License Agreement (BSD License)
 //
-//  Copyright (c) 2019-2022 CNRS-LAAS INRIA
+//  Copyright (c) 2019-2024 CNRS-LAAS INRIA
 //  Author: Joseph Mirabel
 //  All rights reserved.
 //
@@ -34,50 +34,52 @@
 
 #include <eigenpy/eigenpy.hpp>
 #include <eigenpy/eigen-to-python.hpp>
+
 #if EIGENPY_VERSION_AT_LEAST(3, 8, 0)
 #include <eigenpy/id.hpp>
 #endif
-#include "fcl.hh"
+#include "coal.hh"
+
 #include "deprecation.hh"
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/shape/convex.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/hfield.h>
+#include "coal/fwd.hh"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/shape/convex.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/hfield.h"
 
-#include <hpp/fcl/serialization/memory.h>
-#include <hpp/fcl/serialization/AABB.h>
-#include <hpp/fcl/serialization/BVH_model.h>
-#include <hpp/fcl/serialization/hfield.h>
-#include <hpp/fcl/serialization/geometric_shapes.h>
-#include <hpp/fcl/serialization/convex.h>
+#include "coal/serialization/memory.h"
+#include "coal/serialization/AABB.h"
+#include "coal/serialization/BVH_model.h"
+#include "coal/serialization/hfield.h"
+#include "coal/serialization/geometric_shapes.h"
+#include "coal/serialization/convex.h"
 
 #include "pickle.hh"
 #include "serializable.hh"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 // FIXME for a reason I do not understand, doxygen fails to understand that
-// BV_splitter is not defined in hpp/fcl/BVH/BVH_model.h
-#include <hpp/fcl/internal/BV_splitter.h>
-#include <hpp/fcl/broadphase/detail/hierarchy_tree.h>
-
-#include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h"
-#include "doxygen_autodoc/hpp/fcl/BV/AABB.h"
-#include "doxygen_autodoc/hpp/fcl/hfield.h"
-#include "doxygen_autodoc/hpp/fcl/shape/geometric_shapes.h"
+// BV_splitter is not defined in coal/BVH/BVH_model.h
+#include "coal/internal/BV_splitter.h"
+#include "coal/broadphase/detail/hierarchy_tree.h"
+
+#include "doxygen_autodoc/coal/BVH/BVH_model.h"
+#include "doxygen_autodoc/coal/BV/AABB.h"
+#include "doxygen_autodoc/coal/hfield.h"
+#include "doxygen_autodoc/coal/shape/geometric_shapes.h"
 #include "doxygen_autodoc/functions.h"
 #endif
 
 using namespace boost::python;
-using namespace hpp::fcl;
-using namespace hpp::fcl::python;
+using namespace coal;
+using namespace coal::python;
 namespace dv = doxygen::visitor;
 namespace bp = boost::python;
 
 using boost::noncopyable;
 
-typedef std::vector<Vec3f> Vec3fs;
+typedef std::vector<Vec3s> Vec3ss;
 typedef std::vector<Triangle> Triangles;
 
 struct BVHModelBaseWrapper {
@@ -85,7 +87,7 @@ struct BVHModelBaseWrapper {
   typedef Eigen::Map<RowMatrixX3> MapRowMatrixX3;
   typedef Eigen::Ref<RowMatrixX3> RefRowMatrixX3;
 
-  static Vec3f& vertex(BVHModelBase& bvh, unsigned int i) {
+  static Vec3s& vertex(BVHModelBase& bvh, unsigned int i) {
     if (i >= bvh.num_vertices) throw std::out_of_range("index is out of range");
     return (*(bvh.vertices))[i];
   }
@@ -136,8 +138,8 @@ void exposeHeightField(const std::string& bvname) {
       type_name.c_str(), doxygen::class_doc<Geometry>(), no_init)
       .def(dv::init<HeightField<BV>>())
       .def(dv::init<HeightField<BV>, const HeightField<BV>&>())
-      .def(dv::init<HeightField<BV>, FCL_REAL, FCL_REAL, const MatrixXf&,
-                    bp::optional<FCL_REAL>>())
+      .def(dv::init<HeightField<BV>, CoalScalar, CoalScalar, const MatrixXs&,
+                    bp::optional<CoalScalar>>())
 
       .DEF_CLASS_FUNC(Geometry, getXDim)
       .DEF_CLASS_FUNC(Geometry, getYDim)
@@ -178,7 +180,7 @@ struct ConvexBaseWrapper {
   typedef Eigen::Map<VecOfDoubles> MapVecOfDoubles;
   typedef Eigen::Ref<VecOfDoubles> RefVecOfDoubles;
 
-  static Vec3f& point(const ConvexBase& convex, unsigned int i) {
+  static Vec3s& point(const ConvexBase& convex, unsigned int i) {
     if (i >= convex.num_points)
       throw std::out_of_range("index is out of range");
     return (*(convex.points))[i];
@@ -188,7 +190,7 @@ struct ConvexBaseWrapper {
     return MapRowMatrixX3((*(convex.points))[0].data(), convex.num_points, 3);
   }
 
-  static Vec3f& normal(const ConvexBase& convex, unsigned int i) {
+  static Vec3s& normal(const ConvexBase& convex, unsigned int i) {
     if (i >= convex.num_normals_and_offsets)
       throw std::out_of_range("index is out of range");
     return (*(convex.normals))[i];
@@ -220,7 +222,7 @@ struct ConvexBaseWrapper {
     return n;
   }
 
-  static ConvexBase* convexHull(const Vec3fs& points, bool keepTri,
+  static ConvexBase* convexHull(const Vec3ss& points, bool keepTri,
                                 const char* qhullCommand) {
     return ConvexBase::convexHull(points.data(), (unsigned int)points.size(),
                                   keepTri, qhullCommand);
@@ -237,11 +239,11 @@ struct ConvexWrapper {
     return (*convex.polygons)[i];
   }
 
-  static shared_ptr<Convex_t> constructor(const Vec3fs& _points,
+  static shared_ptr<Convex_t> constructor(const Vec3ss& _points,
                                           const Triangles& _tris) {
-    std::shared_ptr<std::vector<Vec3f>> points(
-        new std::vector<Vec3f>(_points.size()));
-    std::vector<Vec3f>& points_ = *points;
+    std::shared_ptr<std::vector<Vec3s>> points(
+        new std::vector<Vec3s>(_points.size()));
+    std::vector<Vec3s>& points_ = *points;
     for (std::size_t i = 0; i < _points.size(); ++i) points_[i] = _points[i];
 
     std::shared_ptr<std::vector<Triangle>> tris(
@@ -288,8 +290,8 @@ void exposeShapes() {
       "Box", doxygen::class_doc<ShapeBase>(), no_init)
       .def(dv::init<Box>())
       .def(dv::init<Box, const Box&>())
-      .def(dv::init<Box, FCL_REAL, FCL_REAL, FCL_REAL>())
-      .def(dv::init<Box, const Vec3f&>())
+      .def(dv::init<Box, CoalScalar, CoalScalar, CoalScalar>())
+      .def(dv::init<Box, const Vec3s&>())
       .DEF_RW_CLASS_ATTRIB(Box, halfSide)
       .def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone),
            return_value_policy<manage_new_object>())
@@ -303,7 +305,7 @@ void exposeShapes() {
   class_<Capsule, bases<ShapeBase>, shared_ptr<Capsule>>(
       "Capsule", doxygen::class_doc<Capsule>(), no_init)
       .def(dv::init<Capsule>())
-      .def(dv::init<Capsule, FCL_REAL, FCL_REAL>())
+      .def(dv::init<Capsule, CoalScalar, CoalScalar>())
       .def(dv::init<Capsule, const Capsule&>())
       .DEF_RW_CLASS_ATTRIB(Capsule, radius)
       .DEF_RW_CLASS_ATTRIB(Capsule, halfLength)
@@ -319,7 +321,7 @@ void exposeShapes() {
   class_<Cone, bases<ShapeBase>, shared_ptr<Cone>>(
       "Cone", doxygen::class_doc<Cone>(), no_init)
       .def(dv::init<Cone>())
-      .def(dv::init<Cone, FCL_REAL, FCL_REAL>())
+      .def(dv::init<Cone, CoalScalar, CoalScalar>())
       .def(dv::init<Cone, const Cone&>())
       .DEF_RW_CLASS_ATTRIB(Cone, radius)
       .DEF_RW_CLASS_ATTRIB(Cone, halfLength)
@@ -342,8 +344,7 @@ void exposeShapes() {
            bp::return_internal_reference<>())
       .def("points", &ConvexBaseWrapper::point, bp::args("self", "index"),
            "Retrieve the point given by its index.",
-           ::hpp::fcl::python::deprecated_member<
-               bp::return_internal_reference<>>())
+           ::coal::python::deprecated_member<bp::return_internal_reference<>>())
       .def("points", &ConvexBaseWrapper::points, bp::args("self"),
            "Retrieve all the points.",
            bp::with_custodian_and_ward_postcall<0, 1>())
@@ -387,7 +388,7 @@ void exposeShapes() {
   class_<Cylinder, bases<ShapeBase>, shared_ptr<Cylinder>>(
       "Cylinder", doxygen::class_doc<Cylinder>(), no_init)
       .def(dv::init<Cylinder>())
-      .def(dv::init<Cylinder, FCL_REAL, FCL_REAL>())
+      .def(dv::init<Cylinder, CoalScalar, CoalScalar>())
       .def(dv::init<Cylinder, const Cylinder&>())
       .DEF_RW_CLASS_ATTRIB(Cylinder, radius)
       .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength)
@@ -403,9 +404,10 @@ void exposeShapes() {
 
   class_<Halfspace, bases<ShapeBase>, shared_ptr<Halfspace>>(
       "Halfspace", doxygen::class_doc<Halfspace>(), no_init)
-      .def(dv::init<Halfspace, const Vec3f&, FCL_REAL>())
+      .def(dv::init<Halfspace, const Vec3s&, CoalScalar>())
       .def(dv::init<Halfspace, const Halfspace&>())
-      .def(dv::init<Halfspace, FCL_REAL, FCL_REAL, FCL_REAL, FCL_REAL>())
+      .def(
+          dv::init<Halfspace, CoalScalar, CoalScalar, CoalScalar, CoalScalar>())
       .def(dv::init<Halfspace>())
       .DEF_RW_CLASS_ATTRIB(Halfspace, n)
       .DEF_RW_CLASS_ATTRIB(Halfspace, d)
@@ -421,9 +423,9 @@ void exposeShapes() {
 
   class_<Plane, bases<ShapeBase>, shared_ptr<Plane>>(
       "Plane", doxygen::class_doc<Plane>(), no_init)
-      .def(dv::init<Plane, const Vec3f&, FCL_REAL>())
+      .def(dv::init<Plane, const Vec3s&, CoalScalar>())
       .def(dv::init<Plane, const Plane&>())
-      .def(dv::init<Plane, FCL_REAL, FCL_REAL, FCL_REAL, FCL_REAL>())
+      .def(dv::init<Plane, CoalScalar, CoalScalar, CoalScalar, CoalScalar>())
       .def(dv::init<Plane>())
       .DEF_RW_CLASS_ATTRIB(Plane, n)
       .DEF_RW_CLASS_ATTRIB(Plane, d)
@@ -440,7 +442,7 @@ void exposeShapes() {
       "Sphere", doxygen::class_doc<Sphere>(), no_init)
       .def(dv::init<Sphere>())
       .def(dv::init<Sphere, const Sphere&>())
-      .def(dv::init<Sphere, FCL_REAL>())
+      .def(dv::init<Sphere, CoalScalar>())
       .DEF_RW_CLASS_ATTRIB(Sphere, radius)
       .def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone),
            return_value_policy<manage_new_object>())
@@ -454,8 +456,8 @@ void exposeShapes() {
   class_<Ellipsoid, bases<ShapeBase>, shared_ptr<Ellipsoid>>(
       "Ellipsoid", doxygen::class_doc<Ellipsoid>(), no_init)
       .def(dv::init<Ellipsoid>())
-      .def(dv::init<Ellipsoid, FCL_REAL, FCL_REAL, FCL_REAL>())
-      .def(dv::init<Ellipsoid, Vec3f>())
+      .def(dv::init<Ellipsoid, CoalScalar, CoalScalar, CoalScalar>())
+      .def(dv::init<Ellipsoid, Vec3s>())
       .def(dv::init<Ellipsoid, const Ellipsoid&>())
       .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii)
       .def("clone", &Ellipsoid::clone,
@@ -471,7 +473,7 @@ void exposeShapes() {
   class_<TriangleP, bases<ShapeBase>, shared_ptr<TriangleP>>(
       "TriangleP", doxygen::class_doc<TriangleP>(), no_init)
       .def(dv::init<TriangleP>())
-      .def(dv::init<TriangleP, const Vec3f&, const Vec3f&, const Vec3f&>())
+      .def(dv::init<TriangleP, const Vec3s&, const Vec3s&, const Vec3s&>())
       .def(dv::init<TriangleP, const TriangleP&>())
       .DEF_RW_CLASS_ATTRIB(TriangleP, a)
       .DEF_RW_CLASS_ATTRIB(TriangleP, b)
@@ -488,8 +490,8 @@ void exposeShapes() {
 }
 
 boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) {
-  Vec3f P, Q;
-  FCL_REAL distance = self.distance(other, &P, &Q);
+  Vec3s P, Q;
+  CoalScalar distance = self.distance(other, &P, &Q);
   return boost::python::make_tuple(distance, P, Q);
 }
 
@@ -552,17 +554,17 @@ void exposeCollisionGeometries() {
                no_init)
       .def(init<>(bp::arg("self"), "Default constructor"))
       .def(init<AABB>(bp::args("self", "other"), "Copy constructor"))
-      .def(init<Vec3f>(bp::args("self", "v"),
+      .def(init<Vec3s>(bp::args("self", "v"),
                        "Creating an AABB at position v with zero size."))
-      .def(init<Vec3f, Vec3f>(bp::args("self", "a", "b"),
+      .def(init<Vec3s, Vec3s>(bp::args("self", "a", "b"),
                               "Creating an AABB with two endpoints a and b."))
-      .def(init<AABB, Vec3f>(
+      .def(init<AABB, Vec3s>(
           bp::args("self", "core", "delta"),
           "Creating an AABB centered as core and is of half-dimension delta."))
-      .def(init<Vec3f, Vec3f, Vec3f>(bp::args("self", "a", "b", "c"),
+      .def(init<Vec3s, Vec3s, Vec3s>(bp::args("self", "a", "b", "c"),
                                      "Creating an AABB contains three points."))
 
-      .def("contain", (bool(AABB::*)(const Vec3f&) const) & AABB::contain,
+      .def("contain", (bool(AABB::*)(const Vec3s&) const) & AABB::contain,
            bp::args("self", "p"), "Check whether the AABB contains a point p.")
       .def("contain", (bool(AABB::*)(const AABB&) const) & AABB::contain,
            bp::args("self", "other"),
@@ -575,7 +577,7 @@ void exposeCollisionGeometries() {
            "Check whether two AABB are overlaping and return the overloaping "
            "part if true.")
 
-      .def("distance", (FCL_REAL(AABB::*)(const AABB&) const)&AABB::distance,
+      .def("distance", (CoalScalar(AABB::*)(const AABB&) const)&AABB::distance,
            bp::args("self", "other"), "Distance between two AABBs.")
       //    .def("distance",
       //         AABB_distance_proxy,
@@ -585,18 +587,18 @@ void exposeCollisionGeometries() {
       .add_property(
           "min_",
           bp::make_function(
-              +[](AABB& self) -> Vec3f& { return self.min_; },
+              +[](AABB& self) -> Vec3s& { return self.min_; },
               bp::return_internal_reference<>()),
           bp::make_function(
-              +[](AABB& self, const Vec3f& min_) -> void { self.min_ = min_; }),
+              +[](AABB& self, const Vec3s& min_) -> void { self.min_ = min_; }),
           "The min point in the AABB.")
       .add_property(
           "max_",
           bp::make_function(
-              +[](AABB& self) -> Vec3f& { return self.max_; },
+              +[](AABB& self) -> Vec3s& { return self.max_; },
               bp::return_internal_reference<>()),
           bp::make_function(
-              +[](AABB& self, const Vec3f& max_) -> void { self.max_ = max_; }),
+              +[](AABB& self, const Vec3s& max_) -> void { self.max_ = max_; }),
           "The max point in the AABB.")
 
       .def(bp::self == bp::self)
@@ -604,7 +606,7 @@ void exposeCollisionGeometries() {
 
       .def(bp::self + bp::self)
       .def(bp::self += bp::self)
-      .def(bp::self += Vec3f())
+      .def(bp::self += Vec3s())
 
       .def("size", &AABB::volume, bp::arg("self"), "Size of the AABB.")
       .def("center", &AABB::center, bp::arg("self"), "Center of the AABB.")
@@ -614,24 +616,24 @@ void exposeCollisionGeometries() {
       .def("volume", &AABB::volume, bp::arg("self"), "Volume of the AABB.")
 
       .def("expand",
-           static_cast<AABB& (AABB::*)(const AABB&, FCL_REAL)>(&AABB::expand),
+           static_cast<AABB& (AABB::*)(const AABB&, CoalScalar)>(&AABB::expand),
            //         doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
-           //         AABB &, FCL_REAL)>(&AABB::expand)),
+           //         AABB &, CoalScalar)>(&AABB::expand)),
            //         doxygen::member_func_args(static_cast<AABB&
-           //         (AABB::*)(const AABB &, FCL_REAL)>(&AABB::expand)),
+           //         (AABB::*)(const AABB &, CoalScalar)>(&AABB::expand)),
            bp::return_internal_reference<>())
       .def("expand",
-           static_cast<AABB& (AABB::*)(const FCL_REAL)>(&AABB::expand),
+           static_cast<AABB& (AABB::*)(const CoalScalar)>(&AABB::expand),
            //         doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
-           //         FCL_REAL)>(&AABB::expand)),
+           //         CoalScalar)>(&AABB::expand)),
            //         doxygen::member_func_args(static_cast<AABB&
-           //         (AABB::*)(const FCL_REAL)>(&AABB::expand)),
+           //         (AABB::*)(const CoalScalar)>(&AABB::expand)),
            bp::return_internal_reference<>())
-      .def("expand", static_cast<AABB& (AABB::*)(const Vec3f&)>(&AABB::expand),
+      .def("expand", static_cast<AABB& (AABB::*)(const Vec3s&)>(&AABB::expand),
            //         doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
-           //         Vec3f &)>(&AABB::expand)),
+           //         Vec3s &)>(&AABB::expand)),
            //         doxygen::member_func_args(static_cast<AABB&
-           //         (AABB::*)(const Vec3f &)>(&AABB::expand)),
+           //         (AABB::*)(const Vec3s &)>(&AABB::expand)),
            bp::return_internal_reference<>())
       .def_pickle(PickleObject<AABB>())
       .def(SerializableVisitor<AABB>())
@@ -640,10 +642,10 @@ void exposeCollisionGeometries() {
 #endif
       ;
 
-  def("translate", (AABB(*)(const AABB&, const Vec3f&))&translate,
+  def("translate", (AABB(*)(const AABB&, const Vec3s&))&translate,
       bp::args("aabb", "t"), "Translate the center of AABB by t");
 
-  def("rotate", (AABB(*)(const AABB&, const Matrix3f&))&rotate,
+  def("rotate", (AABB(*)(const AABB&, const Matrix3s&))&rotate,
       bp::args("aabb", "R"), "Rotate the AABB object by R");
 
   if (!eigenpy::register_symbolic_link_to_registered_type<
@@ -698,8 +700,7 @@ void exposeCollisionGeometries() {
            bp::return_internal_reference<>())
       .def("vertices", &BVHModelBaseWrapper::vertex, bp::args("self", "index"),
            "Retrieve the vertex given by its index.",
-           ::hpp::fcl::python::deprecated_member<
-               bp::return_internal_reference<>>())
+           ::coal::python::deprecated_member<bp::return_internal_reference<>>())
       .def("vertices", &BVHModelBaseWrapper::vertices, bp::args("self"),
            "Retrieve all the vertices.",
            bp::with_custodian_and_ward_postcall<0, 1>())
@@ -725,9 +726,9 @@ void exposeCollisionGeometries() {
       .def(dv::member_func("addTriangle", &BVHModelBase::addTriangle))
       .def(dv::member_func("addTriangles", &BVHModelBase::addTriangles))
       .def(dv::member_func<int (BVHModelBase::*)(
-               const Vec3fs&, const Triangles&)>("addSubModel",
+               const Vec3ss&, const Triangles&)>("addSubModel",
                                                  &BVHModelBase::addSubModel))
-      .def(dv::member_func<int (BVHModelBase::*)(const Vec3fs&)>(
+      .def(dv::member_func<int (BVHModelBase::*)(const Vec3ss&)>(
           "addSubModel", &BVHModelBase::addSubModel))
       .def(dv::member_func("endModel", &BVHModelBase::endModel))
       // Expose function to replace a BVH
@@ -753,9 +754,9 @@ void exposeCollisionObject() {
         .def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
                       bp::optional<bool>>())
         .def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
-                      const Transform3f&, bp::optional<bool>>())
+                      const Transform3s&, bp::optional<bool>>())
         .def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
-                      const Matrix3f&, const Vec3f&, bp::optional<bool>>())
+                      const Matrix3s&, const Vec3s&, bp::optional<bool>>())
 
         .DEF_CLASS_FUNC(CollisionObject, getObjectType)
         .DEF_CLASS_FUNC(CollisionObject, getNodeType)
@@ -775,7 +776,7 @@ void exposeCollisionObject() {
                          bp::return_value_policy<bp::copy_const_reference>())
         .def(dv::member_func(
             "setTransform",
-            static_cast<void (CollisionObject::*)(const Transform3f&)>(
+            static_cast<void (CollisionObject::*)(const Transform3s&)>(
                 &CollisionObject::setTransform)))
 
         .DEF_CLASS_FUNC(CollisionObject, isIdentityTransform)
diff --git a/python/collision.cc b/python/collision.cc
index a759b193a9f72922f0f8bae8f89f34a7ebb247b7..b50a8819001aa5758118590d0198cb8b0f390a38 100644
--- a/python/collision.cc
+++ b/python/collision.cc
@@ -34,28 +34,28 @@
 
 #include <eigenpy/eigenpy.hpp>
 
-#include <hpp/fcl/fwd.hh>
-HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/serialization/collision_data.h>
-HPP_FCL_COMPILER_DIAGNOSTIC_POP
+#include "coal/fwd.hh"
+COAL_COMPILER_DIAGNOSTIC_PUSH
+COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+#include "coal/collision.h"
+#include "coal/serialization/collision_data.h"
+COAL_COMPILER_DIAGNOSTIC_POP
 
-#include "fcl.hh"
+#include "coal.hh"
 #include "deprecation.hh"
 #include "serializable.hh"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 #include "doxygen_autodoc/functions.h"
-#include "doxygen_autodoc/hpp/fcl/collision_data.h"
+#include "doxygen_autodoc/coal/collision_data.h"
 #endif
 
 #include "../doc/python/doxygen.hh"
 #include "../doc/python/doxygen-boost.hh"
 
 using namespace boost::python;
-using namespace hpp::fcl;
-using namespace hpp::fcl::python;
+using namespace coal;
+using namespace coal::python;
 
 namespace dv = doxygen::visitor;
 
@@ -65,10 +65,10 @@ const CollisionGeometry* geto(const Contact& c) {
 }
 
 struct ContactWrapper {
-  static Vec3f getNearestPoint1(const Contact& contact) {
+  static Vec3s getNearestPoint1(const Contact& contact) {
     return contact.nearest_points[0];
   }
-  static Vec3f getNearestPoint2(const Contact& contact) {
+  static Vec3s getNearestPoint2(const Contact& contact) {
     return contact.nearest_points[1];
   }
 };
@@ -94,8 +94,8 @@ void exposeCollisionAPI() {
         .def("clear", &CPUTimes::clear, arg("self"), "Reset the time values.");
   }
 
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   if (!eigenpy::register_symbolic_link_to_registered_type<QueryRequest>()) {
     class_<QueryRequest>("QueryRequest", doxygen::class_doc<QueryRequest>(),
                          no_init)
@@ -132,10 +132,10 @@ void exposeCollisionAPI() {
         .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_timings)
         .DEF_CLASS_FUNC(QueryRequest, updateGuess);
   }
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
+  COAL_COMPILER_DIAGNOSTIC_POP
 
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   if (!eigenpy::register_symbolic_link_to_registered_type<CollisionRequest>()) {
     class_<CollisionRequest, bases<QueryRequest> >(
         "CollisionRequest", doxygen::class_doc<CollisionRequest>(), no_init)
@@ -174,7 +174,7 @@ void exposeCollisionAPI() {
     class_<std::vector<CollisionRequest> >("StdVec_CollisionRequest")
         .def(vector_indexing_suite<std::vector<CollisionRequest> >());
   }
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
+  COAL_COMPILER_DIAGNOSTIC_POP
 
   if (!eigenpy::register_symbolic_link_to_registered_type<Contact>()) {
     class_<Contact>("Contact", doxygen::class_doc<Contact>(),
@@ -182,8 +182,8 @@ void exposeCollisionAPI() {
         .def(dv::init<Contact, const CollisionGeometry*,
                       const CollisionGeometry*, int, int>())
         .def(dv::init<Contact, const CollisionGeometry*,
-                      const CollisionGeometry*, int, int, const Vec3f&,
-                      const Vec3f&, FCL_REAL>())
+                      const CollisionGeometry*, int, int, const Vec3s&,
+                      const Vec3s&, CoalScalar>())
         .add_property(
             "o1",
             make_function(&geto<1>,
@@ -260,8 +260,8 @@ void exposeCollisionAPI() {
                    const CollisionRequest&, CollisionResult&)>(&collide));
   doxygen::def(
       "collide",
-      static_cast<std::size_t (*)(const CollisionGeometry*, const Transform3f&,
-                                  const CollisionGeometry*, const Transform3f&,
+      static_cast<std::size_t (*)(const CollisionGeometry*, const Transform3s&,
+                                  const CollisionGeometry*, const Transform3s&,
                                   const CollisionRequest&, CollisionResult&)>(
           &collide));
 
@@ -271,6 +271,6 @@ void exposeCollisionAPI() {
                     const CollisionGeometry*>())
       .def("__call__",
            static_cast<std::size_t (ComputeCollision::*)(
-               const Transform3f&, const Transform3f&, const CollisionRequest&,
+               const Transform3s&, const Transform3s&, const CollisionRequest&,
                CollisionResult&) const>(&ComputeCollision::operator()));
 }
diff --git a/python/contact_patch.cc b/python/contact_patch.cc
index 66099b3b303d2d1a22a50559f066c6e0cdb46aa4..831137b74dabd36077a33583176d7206508b44a4 100644
--- a/python/contact_patch.cc
+++ b/python/contact_patch.cc
@@ -34,25 +34,25 @@
 
 #include <eigenpy/eigenpy.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/contact_patch.h>
-#include <hpp/fcl/serialization/collision_data.h>
+#include "coal/fwd.hh"
+#include "coal/contact_patch.h"
+#include "coal/serialization/collision_data.h"
 
-#include "fcl.hh"
+#include "coal.hh"
 #include "deprecation.hh"
 #include "serializable.hh"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 #include "doxygen_autodoc/functions.h"
-#include "doxygen_autodoc/hpp/fcl/collision_data.h"
+#include "doxygen_autodoc/coal/collision_data.h"
 #endif
 
 #include "../doc/python/doxygen.hh"
 #include "../doc/python/doxygen-boost.hh"
 
 using namespace boost::python;
-using namespace hpp::fcl;
-using namespace hpp::fcl::python;
+using namespace coal;
+using namespace coal::python;
 
 namespace dv = doxygen::visitor;
 
@@ -93,12 +93,12 @@ void exposeContactPatchAPI() {
           ContactPatchRequest>()) {
     class_<ContactPatchRequest>(
         "ContactPatchRequest", doxygen::class_doc<ContactPatchRequest>(),
-        init<optional<size_t, size_t, FCL_REAL>>(
+        init<optional<size_t, size_t, CoalScalar>>(
             (arg("self"), arg("max_num_patch"),
              arg("num_samples_curved_shapes"), arg("patch_tolerance")),
             "ContactPatchRequest constructor."))
         .def(dv::init<ContactPatchRequest, const CollisionRequest&,
-                      bp::optional<size_t, FCL_REAL>>())
+                      bp::optional<size_t, CoalScalar>>())
         .DEF_RW_CLASS_ATTRIB(ContactPatchRequest, max_num_patch)
         .DEF_CLASS_FUNC(ContactPatchRequest, getNumSamplesCurvedShapes)
         .DEF_CLASS_FUNC(ContactPatchRequest, setNumSamplesCurvedShapes)
@@ -140,8 +140,8 @@ void exposeContactPatchAPI() {
                            ContactPatchResult&)>(&computeContactPatch));
   doxygen::def(
       "computeContactPatch",
-      static_cast<void (*)(const CollisionGeometry*, const Transform3f&,
-                           const CollisionGeometry*, const Transform3f&,
+      static_cast<void (*)(const CollisionGeometry*, const Transform3s&,
+                           const CollisionGeometry*, const Transform3s&,
                            const CollisionResult&, const ContactPatchRequest&,
                            ContactPatchResult&)>(&computeContactPatch));
 
@@ -154,7 +154,7 @@ void exposeContactPatchAPI() {
                       const CollisionGeometry*>())
         .def("__call__",
              static_cast<void (ComputeContactPatch::*)(
-                 const Transform3f&, const Transform3f&, const CollisionResult&,
+                 const Transform3s&, const Transform3s&, const CollisionResult&,
                  const ContactPatchRequest&, ContactPatchResult&) const>(
                  &ComputeContactPatch::operator()));
   }
diff --git a/python/deprecation.hh b/python/deprecation.hh
index 0c5e5adeb14bcc55852cd2ec1a9022e785664b19..2bc9a20ff25e569fa57c531f78157cc2c5ceea9b 100644
--- a/python/deprecation.hh
+++ b/python/deprecation.hh
@@ -2,15 +2,14 @@
 // Copyright (c) 2020-2021 INRIA
 //
 
-#ifndef HPP_FCL_PYTHON_UTILS_DEPRECATION_H
-#define HPP_FCL_PYTHON_UTILS_DEPRECATION_H
+#ifndef COAL_PYTHON_UTILS_DEPRECATION_H
+#define COAL_PYTHON_UTILS_DEPRECATION_H
 
 #include <Python.h>
 #include <boost/python.hpp>
 #include <string>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace python {
 
 template <class Policy = boost::python::default_call_policies>
@@ -48,7 +47,6 @@ struct deprecated_function : deprecated_warning_policy<Policy> {
 };
 
 }  // namespace python
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
-#endif  // ifndef HPP_FCL_PYTHON_UTILS_DEPRECATION_H
+#endif  // ifndef COAL_PYTHON_UTILS_DEPRECATION_H
diff --git a/python/distance.cc b/python/distance.cc
index 0231814da49b9b496bf0de72308cd7b6fb5fb65b..f4c1a26a615bb31c3e11d1c6e588ac770fa2c6ef 100644
--- a/python/distance.cc
+++ b/python/distance.cc
@@ -34,45 +34,46 @@
 
 #include <eigenpy/eigenpy.hpp>
 
-#include "fcl.hh"
+#include "coal.hh"
+
+COAL_COMPILER_DIAGNOSTIC_PUSH
+COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+#include "coal/fwd.hh"
+#include "coal/distance.h"
+#include "coal/serialization/collision_data.h"
 
-HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/serialization/collision_data.h>
 #include "deprecation.hh"
-HPP_FCL_COMPILER_DIAGNOSTIC_POP
+COAL_COMPILER_DIAGNOSTIC_POP
 
 #include "serializable.hh"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 #include "doxygen_autodoc/functions.h"
-#include "doxygen_autodoc/hpp/fcl/collision_data.h"
+#include "doxygen_autodoc/coal/collision_data.h"
 #endif
 
 using namespace boost::python;
-using namespace hpp::fcl;
-using namespace hpp::fcl::python;
+using namespace coal;
+using namespace coal::python;
 
 namespace dv = doxygen::visitor;
 
 struct DistanceResultWrapper {
-  static Vec3f getNearestPoint1(const DistanceResult& res) {
+  static Vec3s getNearestPoint1(const DistanceResult& res) {
     return res.nearest_points[0];
   }
-  static Vec3f getNearestPoint2(const DistanceResult& res) {
+  static Vec3s getNearestPoint2(const DistanceResult& res) {
     return res.nearest_points[1];
   }
 };
 
 void exposeDistanceAPI() {
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   if (!eigenpy::register_symbolic_link_to_registered_type<DistanceRequest>()) {
     class_<DistanceRequest, bases<QueryRequest> >(
         "DistanceRequest", doxygen::class_doc<DistanceRequest>(),
-        init<optional<bool, FCL_REAL, FCL_REAL> >(
+        init<optional<bool, CoalScalar, CoalScalar> >(
             (arg("self"), arg("enable_nearest_points"), arg("rel_err"),
              arg("abs_err")),
             "Constructor"))
@@ -110,7 +111,7 @@ void exposeDistanceAPI() {
         .DEF_RW_CLASS_ATTRIB(DistanceRequest, abs_err)
         .def(SerializableVisitor<DistanceRequest>());
   }
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
+  COAL_COMPILER_DIAGNOSTIC_POP
 
   if (!eigenpy::register_symbolic_link_to_registered_type<
           std::vector<DistanceRequest> >()) {
@@ -148,14 +149,14 @@ void exposeDistanceAPI() {
 
   doxygen::def(
       "distance",
-      static_cast<FCL_REAL (*)(const CollisionObject*, const CollisionObject*,
-                               const DistanceRequest&, DistanceResult&)>(
+      static_cast<CoalScalar (*)(const CollisionObject*, const CollisionObject*,
+                                 const DistanceRequest&, DistanceResult&)>(
           &distance));
   doxygen::def(
       "distance",
-      static_cast<FCL_REAL (*)(const CollisionGeometry*, const Transform3f&,
-                               const CollisionGeometry*, const Transform3f&,
-                               const DistanceRequest&, DistanceResult&)>(
+      static_cast<CoalScalar (*)(const CollisionGeometry*, const Transform3s&,
+                                 const CollisionGeometry*, const Transform3s&,
+                                 const DistanceRequest&, DistanceResult&)>(
           &distance));
 
   class_<ComputeDistance>("ComputeDistance",
@@ -163,7 +164,7 @@ void exposeDistanceAPI() {
       .def(dv::init<ComputeDistance, const CollisionGeometry*,
                     const CollisionGeometry*>())
       .def("__call__",
-           static_cast<FCL_REAL (ComputeDistance::*)(
-               const Transform3f&, const Transform3f&, const DistanceRequest&,
+           static_cast<CoalScalar (ComputeDistance::*)(
+               const Transform3s&, const Transform3s&, const DistanceRequest&,
                DistanceResult&) const>(&ComputeDistance::operator()));
 }
diff --git a/python/fwd.hh b/python/fwd.hh
index 45e24bcbf71f1d46274fce187685c2be815f7165..62a197e191aa1accbe12a8bbff245091fbf40157 100644
--- a/python/fwd.hh
+++ b/python/fwd.hh
@@ -2,13 +2,13 @@
 // Copyright (c) 2022 CNRS INRIA
 //
 
-#ifndef HPP_FCL_PYTHON_FWD_HH
-#define HPP_FCL_PYTHON_FWD_HH
+#ifndef COAL_PYTHON_FWD_HH
+#define COAL_PYTHON_FWD_HH
 
-#include <hpp/fcl/fwd.hh>
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#include "coal/fwd.hh"
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 namespace doxygen {
-using hpp::fcl::shared_ptr;
+using coal::shared_ptr;
 }
 #endif
 
@@ -37,4 +37,4 @@ namespace dv = doxygen::visitor;
 #define DEF_CLASS_FUNC2(CLASS, ATTRIB, policy) \
   def(#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB), policy)
 
-#endif  // ifndef HPP_FCL_PYTHON_FWD_HH
+#endif  // ifndef COAL_PYTHON_FWD_HH
diff --git a/python/gjk.cc b/python/gjk.cc
index c88e9b826f8ecb74979d3b1621c6a752d678dd40..cab5ebde1e736f301bb279cff6ce6eb1f216f2cb 100644
--- a/python/gjk.cc
+++ b/python/gjk.cc
@@ -34,25 +34,25 @@
 
 #include <eigenpy/eigenpy.hpp>
 
-#include "fcl.hh"
+#include "coal.hh"
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/narrowphase/gjk.h>
+#include "coal/fwd.hh"
+#include "coal/narrowphase/gjk.h"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 #include "doxygen_autodoc/functions.h"
-#include "doxygen_autodoc/hpp/fcl/narrowphase/gjk.h"
+#include "doxygen_autodoc/coal/narrowphase/gjk.h"
 #endif
 
 using namespace boost::python;
-using namespace hpp::fcl;
-using hpp::fcl::details::EPA;
-using hpp::fcl::details::GJK;
-using hpp::fcl::details::MinkowskiDiff;
-using hpp::fcl::details::SupportOptions;
+using namespace coal;
+using coal::details::EPA;
+using coal::details::GJK;
+using coal::details::MinkowskiDiff;
+using coal::details::SupportOptions;
 
 struct MinkowskiDiffWrapper {
-  static void support0(MinkowskiDiff& self, const Vec3f& dir, int& hint,
+  static void support0(MinkowskiDiff& self, const Vec3s& dir, int& hint,
                        bool compute_swept_sphere_support = false) {
     if (compute_swept_sphere_support) {
       self.support0<SupportOptions::WithSweptSphere>(dir, hint);
@@ -61,7 +61,7 @@ struct MinkowskiDiffWrapper {
     }
   }
 
-  static void support1(MinkowskiDiff& self, const Vec3f& dir, int& hint,
+  static void support1(MinkowskiDiff& self, const Vec3s& dir, int& hint,
                        bool compute_swept_sphere_support = false) {
     if (compute_swept_sphere_support) {
       self.support1<SupportOptions::WithSweptSphere>(dir, hint);
@@ -81,8 +81,8 @@ struct MinkowskiDiffWrapper {
   }
 
   static void set(MinkowskiDiff& self, const ShapeBase* shape0,
-                  const ShapeBase* shape1, const Transform3f& tf0,
-                  const Transform3f& tf1,
+                  const ShapeBase* shape1, const Transform3s& tf0,
+                  const Transform3s& tf1,
                   bool compute_swept_sphere_supports = false) {
     if (compute_swept_sphere_supports) {
       self.set<SupportOptions::WithSweptSphere>(shape0, shape1, tf0, tf1);
@@ -119,13 +119,13 @@ void exposeGJK() {
 
         .def("set",
              static_cast<void (*)(MinkowskiDiff&, const ShapeBase*,
-                                  const ShapeBase*, const Transform3f&,
-                                  const Transform3f&, bool)>(
+                                  const ShapeBase*, const Transform3s&,
+                                  const Transform3s&, bool)>(
                  &MinkowskiDiffWrapper::set),
              doxygen::member_func_doc(
                  static_cast<void (MinkowskiDiff::*)(
-                     const ShapeBase*, const ShapeBase*, const Transform3f&,
-                     const Transform3f&)>(
+                     const ShapeBase*, const ShapeBase*, const Transform3s&,
+                     const Transform3s&)>(
                      &MinkowskiDiff::set<SupportOptions::NoSweptSphere>)))
 
         .def("support0", &MinkowskiDiffWrapper::support0,
@@ -178,7 +178,7 @@ void exposeGJK() {
 
   if (!eigenpy::register_symbolic_link_to_registered_type<GJK>()) {
     class_<GJK>("GJK", doxygen::class_doc<GJK>(), no_init)
-        .def(doxygen::visitor::init<GJK, unsigned int, FCL_REAL>())
+        .def(doxygen::visitor::init<GJK, unsigned int, CoalScalar>())
         .DEF_RW_CLASS_ATTRIB(GJK, distance)
         .DEF_RW_CLASS_ATTRIB(GJK, ray)
         .DEF_RW_CLASS_ATTRIB(GJK, support_hint)
diff --git a/python/hppfcl/__init__.py b/python/hppfcl/__init__.py
index b93784303693b41c29b0e3f89f9f37e4a3691a40..1948a587886523ae735484e0cfbe6ae60e2fc0e1 100644
--- a/python/hppfcl/__init__.py
+++ b/python/hppfcl/__init__.py
@@ -1,36 +1,9 @@
-# Software License Agreement (BSD License)
-#
-#  Copyright (c) 2019 INRIA
-#  Author: Justin Carpentier
-#  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 CNRS-LAAS. 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.
+import warnings
 
-# ruff: noqa: F401, F403
-from .hppfcl import *
-from .hppfcl import __raw_version__, __version__
+warnings.warn(
+    "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning
+)
+
+from coal import Transform3s as Transform3f  # noqa
+from coal import *  # noqa
+from coal import __raw_version__, __version__  # noqa
diff --git a/python/hppfcl/viewer.py b/python/hppfcl/viewer.py
index 15bf53c2d05410b4b6b1fdebfb2b0a92eb5bf7cb..13353dce9ae3c634745d55118fd7a665750dd4d3 100644
--- a/python/hppfcl/viewer.py
+++ b/python/hppfcl/viewer.py
@@ -1,110 +1,7 @@
-# Software License Agreement (BSD License)
-#
-#  Copyright (c) 2019 CNRS
-#  Author: Joseph Mirabel
-
 import warnings
 
-import numpy as np
-from gepetto import Color
-
-import hppfcl
-
-
-def applyConfiguration(gui, name, tf):
-    gui.applyConfiguration(
-        name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist()
-    )
-
-
-def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)):
-    if isinstance(geom, hppfcl.Capsule):
-        return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color)
-    elif isinstance(geom, hppfcl.Cylinder):
-        return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color)
-    elif isinstance(geom, hppfcl.Box):
-        w, h, d = (2.0 * geom.halfSide).tolist()
-        return gui.addBox(name, w, h, d, color)
-    elif isinstance(geom, hppfcl.Sphere):
-        return gui.addSphere(name, geom.radius, color)
-    elif isinstance(geom, hppfcl.Cone):
-        return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color)
-    elif isinstance(geom, hppfcl.Convex):
-        pts = [
-            geom.points(geom.polygons(f)[i]).tolist()
-            for f in range(geom.num_polygons)
-            for i in range(3)
-        ]
-        gui.addCurve(name, pts, color)
-        gui.setCurveMode(name, "TRIANGLES")
-        gui.setLightingMode(name, "ON")
-        gui.setBoolProperty(name, "BackfaceDrawing", True)
-        return True
-    elif isinstance(geom, hppfcl.ConvexBase):
-        pts = [geom.points(i).tolist() for i in range(geom.num_points)]
-        gui.addCurve(name, pts, color)
-        gui.setCurveMode(name, "POINTS")
-        gui.setLightingMode(name, "OFF")
-        return True
-    else:
-        msg = "Unsupported geometry type for %s (%s)" % (name, type(geom))
-        warnings.warn(msg, category=UserWarning, stacklevel=2)
-        return False
-
-
-def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True):
-    gui.createGroup(group_name)
-    r = 0.01
-    if closest_points:
-        p = [group_name + "/p1", group_name + "/p2"]
-        gui.addSphere(p[0], r, Color.red)
-        gui.addSphere(p[1], r, Color.blue)
-        qid = [0, 0, 0, 1]
-        gui.applyConfigurations(
-            p,
-            [
-                res.getNearestPoint1().tolist() + qid,
-                res.getNearestPoint2().tolist() + qid,
-            ],
-        )
-    if normal:
-        n = group_name + "/normal"
-        gui.addArrow(n, r, 0.1, Color.green)
-        gui.applyConfiguration(
-            n,
-            res.getNearestPoint1().tolist()
-            + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal)
-            .coeffs()
-            .tolist(),
-        )
-    gui.refresh()
-
+warnings.warn(
+    "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning
+)
 
-def displayCollisionResult(gui, group_name, res, color=Color.green):
-    if res.isCollision():
-        if gui.nodeExists(group_name):
-            gui.setVisibility(group_name, "ON")
-        else:
-            gui.createGroup(group_name)
-        for i in range(res.numContacts()):
-            contact = res.getContact(i)
-            n = group_name + "/contact" + str(i)
-            depth = contact.penetration_depth
-            if gui.nodeExists(n):
-                gui.setFloatProperty(n, "Size", depth)
-                gui.setFloatProperty(n, "Radius", 0.1 * depth)
-                gui.setColor(n, color)
-            else:
-                gui.addArrow(n, depth * 0.1, depth, color)
-            N = contact.normal
-            P = contact.pos
-            gui.applyConfiguration(
-                n,
-                (P - depth * N / 2).tolist()
-                + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N)
-                .coeffs()
-                .tolist(),
-            )
-        gui.refresh()
-    elif gui.nodeExists(group_name):
-        gui.setVisibility(group_name, "OFF")
+from coal.viewer import *  # noqa
diff --git a/python/math.cc b/python/math.cc
index 30c20d4cedc859c92815726eb2bf56c1d6b9ce0f..0261ae5f15f3b6df555416fa158bb3a087b6b5da 100644
--- a/python/math.cc
+++ b/python/math.cc
@@ -35,21 +35,21 @@
 #include <eigenpy/eigenpy.hpp>
 #include <eigenpy/geometry.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/serialization/transform.h>
+#include "coal/fwd.hh"
+#include "coal/math/transform.h"
+#include "coal/serialization/transform.h"
 
-#include "fcl.hh"
+#include "coal.hh"
 #include "pickle.hh"
 #include "serializable.hh"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
-#include "doxygen_autodoc/hpp/fcl/math/transform.h"
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
+#include "doxygen_autodoc/coal/math/transform.h"
 #endif
 
 using namespace boost::python;
-using namespace hpp::fcl;
-using namespace hpp::fcl::python;
+using namespace coal;
+using namespace coal::python;
 
 namespace dv = doxygen::visitor;
 
@@ -57,12 +57,12 @@ struct TriangleWrapper {
   static Triangle::index_type getitem(const Triangle& t, int i) {
     if (i >= 3 || i <= -3)
       PyErr_SetString(PyExc_IndexError, "Index out of range");
-    return t[static_cast<hpp::fcl::Triangle::index_type>(i % 3)];
+    return t[static_cast<coal::Triangle::index_type>(i % 3)];
   }
   static void setitem(Triangle& t, int i, Triangle::index_type v) {
     if (i >= 3 || i <= -3)
       PyErr_SetString(PyExc_IndexError, "Index out of range");
-    t[static_cast<hpp::fcl::Triangle::index_type>(i % 3)] = v;
+    t[static_cast<coal::Triangle::index_type>(i % 3)] = v;
   }
 };
 
@@ -74,56 +74,60 @@ void exposeMaths() {
   if (!eigenpy::register_symbolic_link_to_registered_type<Eigen::AngleAxisd>())
     eigenpy::exposeAngleAxis();
 
-  eigenpy::enableEigenPySpecific<Matrix3f>();
-  eigenpy::enableEigenPySpecific<Vec3f>();
-
-  class_<Transform3f>("Transform3f", doxygen::class_doc<Transform3f>(), no_init)
-      .def(dv::init<Transform3f>())
-      .def(dv::init<Transform3f, const Matrix3f::MatrixBase&,
-                    const Vec3f::MatrixBase&>())
-      .def(dv::init<Transform3f, const Quatf&, const Vec3f::MatrixBase&>())
-      .def(dv::init<Transform3f, const Matrix3f&>())
-      .def(dv::init<Transform3f, const Quatf&>())
-      .def(dv::init<Transform3f, const Vec3f&>())
-      .def(dv::init<Transform3f, const Transform3f&>())
-
-      .def(dv::member_func("getQuatRotation", &Transform3f::getQuatRotation))
-      .def("getTranslation", &Transform3f::getTranslation,
-           doxygen::member_func_doc(&Transform3f::getTranslation),
+  eigenpy::enableEigenPySpecific<Matrix3s>();
+  eigenpy::enableEigenPySpecific<Vec3s>();
+
+  class_<Transform3s>("Transform3s", doxygen::class_doc<Transform3s>(), no_init)
+      .def(dv::init<Transform3s>())
+      .def(dv::init<Transform3s, const Matrix3s::MatrixBase&,
+                    const Vec3s::MatrixBase&>())
+      .def(dv::init<Transform3s, const Quatf&, const Vec3s::MatrixBase&>())
+      .def(dv::init<Transform3s, const Matrix3s&>())
+      .def(dv::init<Transform3s, const Quatf&>())
+      .def(dv::init<Transform3s, const Vec3s&>())
+      .def(dv::init<Transform3s, const Transform3s&>())
+
+      .def(dv::member_func("getQuatRotation", &Transform3s::getQuatRotation))
+      .def("getTranslation", &Transform3s::getTranslation,
+           doxygen::member_func_doc(&Transform3s::getTranslation),
            return_value_policy<copy_const_reference>())
-      .def("getRotation", &Transform3f::getRotation,
+      .def("getRotation", &Transform3s::getRotation,
            return_value_policy<copy_const_reference>())
-      .def("isIdentity", &Transform3f::isIdentity,
+      .def("isIdentity", &Transform3s::isIdentity,
            (bp::arg("self"),
-            bp::arg("prec") = Eigen::NumTraits<FCL_REAL>::dummy_precision()),
-           doxygen::member_func_doc(&Transform3f::getTranslation))
+            bp::arg("prec") = Eigen::NumTraits<CoalScalar>::dummy_precision()),
+           doxygen::member_func_doc(&Transform3s::getTranslation))
 
-      .def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation))
-      .def("setTranslation", &Transform3f::setTranslation<Vec3f>)
-      .def("setRotation", &Transform3f::setRotation<Matrix3f>)
+      .def(dv::member_func("setQuatRotation", &Transform3s::setQuatRotation))
+      .def("setTranslation", &Transform3s::setTranslation<Vec3s>)
+      .def("setRotation", &Transform3s::setRotation<Matrix3s>)
       .def(dv::member_func("setTransform",
-                           &Transform3f::setTransform<Matrix3f, Vec3f>))
+                           &Transform3s::setTransform<Matrix3s, Vec3s>))
       .def(dv::member_func(
           "setTransform",
-          static_cast<void (Transform3f::*)(const Quatf&, const Vec3f&)>(
-              &Transform3f::setTransform)))
-      .def(dv::member_func("setIdentity", &Transform3f::setIdentity))
-      .def(dv::member_func("Identity", &Transform3f::Identity))
+          static_cast<void (Transform3s::*)(const Quatf&, const Vec3s&)>(
+              &Transform3s::setTransform)))
+      .def(dv::member_func("setIdentity", &Transform3s::setIdentity))
+      .def(dv::member_func("Identity", &Transform3s::Identity))
       .staticmethod("Identity")
 
-      .def(dv::member_func("transform", &Transform3f::transform<Vec3f>))
-      .def("inverseInPlace", &Transform3f::inverseInPlace,
+      .def(dv::member_func("setRandom", &Transform3s::setRandom))
+      .def(dv::member_func("Random", &Transform3s::Random))
+      .staticmethod("Random")
+
+      .def(dv::member_func("transform", &Transform3s::transform<Vec3s>))
+      .def("inverseInPlace", &Transform3s::inverseInPlace,
            return_internal_reference<>(),
-           doxygen::member_func_doc(&Transform3f::inverseInPlace))
-      .def(dv::member_func("inverse", &Transform3f::inverse))
-      .def(dv::member_func("inverseTimes", &Transform3f::inverseTimes))
+           doxygen::member_func_doc(&Transform3s::inverseInPlace))
+      .def(dv::member_func("inverse", &Transform3s::inverse))
+      .def(dv::member_func("inverseTimes", &Transform3s::inverseTimes))
 
       .def(self * self)
       .def(self *= self)
       .def(self == self)
       .def(self != self)
-      .def_pickle(PickleObject<Transform3f>())
-      .def(SerializableVisitor<Transform3f>());
+      .def_pickle(PickleObject<Transform3s>())
+      .def(SerializableVisitor<Transform3s>());
 
   class_<Triangle>("Triangle", no_init)
       .def(dv::init<Triangle>())
@@ -137,9 +141,9 @@ void exposeMaths() {
       .def(self == self);
 
   if (!eigenpy::register_symbolic_link_to_registered_type<
-          std::vector<Vec3f> >()) {
-    class_<std::vector<Vec3f> >("StdVec_Vec3f")
-        .def(vector_indexing_suite<std::vector<Vec3f> >());
+          std::vector<Vec3s> >()) {
+    class_<std::vector<Vec3s> >("StdVec_Vec3s")
+        .def(vector_indexing_suite<std::vector<Vec3s> >());
   }
   if (!eigenpy::register_symbolic_link_to_registered_type<
           std::vector<Triangle> >()) {
diff --git a/python/octree.cc b/python/octree.cc
index 76ebc8b7f27ba8850ce16150e88803d336498469..a33ac166554d015a22a9a67799b9b68a377dc71a 100644
--- a/python/octree.cc
+++ b/python/octree.cc
@@ -1,14 +1,14 @@
 
-#include "fcl.hh"
+#include "coal.hh"
 
 #include <eigenpy/std-vector.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/octree.h>
+#include "coal/fwd.hh"
+#include "coal/octree.h"
 
-#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
 #include "doxygen_autodoc/functions.h"
-#include "doxygen_autodoc/hpp/fcl/octree.h"
+#include "doxygen_autodoc/coal/octree.h"
 #endif
 
 bp::object toPyBytes(std::vector<uint8_t>& bytes) {
@@ -23,19 +23,19 @@ bp::object toPyBytes(std::vector<uint8_t>& bytes) {
 #endif
 }
 
-bp::object tobytes(const hpp::fcl::OcTree& self) {
+bp::object tobytes(const coal::OcTree& self) {
   std::vector<uint8_t> bytes = self.tobytes();
   return toPyBytes(bytes);
 }
 
 void exposeOctree() {
-  using namespace hpp::fcl;
+  using namespace coal;
   namespace bp = boost::python;
   namespace dv = doxygen::visitor;
 
   bp::class_<OcTree, bp::bases<CollisionGeometry>, shared_ptr<OcTree> >(
       "OcTree", doxygen::class_doc<OcTree>(), bp::no_init)
-      .def(dv::init<OcTree, FCL_REAL>())
+      .def(dv::init<OcTree, CoalScalar>())
       .def("clone", &OcTree::clone, doxygen::member_func_doc(&OcTree::clone),
            bp::return_value_policy<bp::manage_new_object>())
       .def(dv::member_func("getTreeDepth", &OcTree::getTreeDepth))
@@ -53,7 +53,7 @@ void exposeOctree() {
       .def("tobytes", tobytes, doxygen::member_func_doc(&OcTree::tobytes));
 
   doxygen::def("makeOctree", &makeOctree);
-  eigenpy::enableEigenPySpecific<Vec6f>();
-  eigenpy::StdVectorPythonVisitor<std::vector<Vec6f>, true>::expose(
+  eigenpy::enableEigenPySpecific<Vec6s>();
+  eigenpy::StdVectorPythonVisitor<std::vector<Vec6s>, true>::expose(
       "StdVec_Vec6");
 }
diff --git a/python/pickle.hh b/python/pickle.hh
index 3e640cf0de7bbf28cca7373334dc9dc9bfb07414..7e2358582b6e3c89a0a04f3d89933e6437da7f72 100644
--- a/python/pickle.hh
+++ b/python/pickle.hh
@@ -2,8 +2,8 @@
 // Copyright (c) 2022 INRIA
 //
 
-#ifndef HPP_FCL_PYTHON_PICKLE_H
-#define HPP_FCL_PYTHON_PICKLE_H
+#ifndef COAL_PYTHON_PICKLE_H
+#define COAL_PYTHON_PICKLE_H
 
 #include <boost/python.hpp>
 #include <eigenpy/eigenpy.hpp>
@@ -13,7 +13,7 @@
 #include <sstream>
 
 using namespace boost::python;
-using namespace hpp::fcl;
+using namespace coal;
 //
 template <typename T>
 struct PickleObject : boost::python::pickle_suite {
@@ -54,4 +54,4 @@ struct PickleObject : boost::python::pickle_suite {
   static bool getstate_manages_dict() { return false; }
 };
 
-#endif  // ifndef HPP_FCL_PYTHON_PICKLE_H
+#endif  // ifndef COAL_PYTHON_PICKLE_H
diff --git a/python/serializable.hh b/python/serializable.hh
index 36950f9f6b31a87ecf1e9788d0ee60049adc652d..2ab337fb5987e0195901b04ad9e01f02799d72a8 100644
--- a/python/serializable.hh
+++ b/python/serializable.hh
@@ -4,19 +4,18 @@
 // https://github.com/stack-of-tasks/pinocchio
 //
 
-#ifndef HPP_FCL_PYTHON_SERIALIZABLE_H
-#define HPP_FCL_PYTHON_SERIALIZABLE_H
+#ifndef COAL_PYTHON_SERIALIZABLE_H
+#define COAL_PYTHON_SERIALIZABLE_H
 
 #include <boost/python.hpp>
 
-#include "hpp/fcl/serialization/archive.h"
-#include "hpp/fcl/serialization/serializer.h"
+#include "coal/serialization/archive.h"
+#include "coal/serialization/serializer.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace python {
 
-using Serializer = ::hpp::fcl::serialization::Serializer;
+using Serializer = ::coal::serialization::Serializer;
 
 namespace bp = boost::python;
 
@@ -54,7 +53,6 @@ struct SerializableVisitor
   }
 };
 }  // namespace python
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
-#endif  // ifndef HPP_FCL_PYTHON_SERIALIZABLE_H
+#endif  // ifndef COAL_PYTHON_SERIALIZABLE_H
diff --git a/python/utils/std-pair.hh b/python/utils/std-pair.hh
index 80946720122c5903d8462c8fc9cb5df269e992d4..8af3520a0152e739ca80d974b6a64098ff39f1b1 100644
--- a/python/utils/std-pair.hh
+++ b/python/utils/std-pair.hh
@@ -2,8 +2,8 @@
 // Copyright (c) 2023 INRIA
 //
 
-#ifndef HPP_FCL_PYTHON_UTILS_STD_PAIR_H
-#define HPP_FCL_PYTHON_UTILS_STD_PAIR_H
+#ifndef COAL_PYTHON_UTILS_STD_PAIR_H
+#define COAL_PYTHON_UTILS_STD_PAIR_H
 
 #include <boost/python.hpp>
 #include <utility>
@@ -62,4 +62,4 @@ struct StdPairConverter {
   }
 };
 
-#endif  // ifndef HPP_FCL_PYTHON_UTILS_STD_PAIR_H
+#endif  // ifndef COAL_PYTHON_UTILS_STD_PAIR_H
diff --git a/python/version.cc b/python/version.cc
index 4bc0eea9337a880f1653e78372991ee355de950e..658a94297b21e1e78c845c94f550ec9f82f371f3 100644
--- a/python/version.cc
+++ b/python/version.cc
@@ -2,36 +2,36 @@
 // Copyright (c) 2019-2023 CNRS INRIA
 //
 
-#include "hpp/fcl/config.hh"
-#include "fcl.hh"
+#include "coal/config.hh"
+#include "coal.hh"
 #include <boost/preprocessor/stringize.hpp>
 
 namespace bp = boost::python;
 
 inline bool checkVersionAtLeast(int major, int minor, int patch) {
-  return HPP_FCL_VERSION_AT_LEAST(major, minor, patch);
+  return COAL_VERSION_AT_LEAST(major, minor, patch);
 }
 
 inline bool checkVersionAtMost(int major, int minor, int patch) {
-  return HPP_FCL_VERSION_AT_MOST(major, minor, patch);
+  return COAL_VERSION_AT_MOST(major, minor, patch);
 }
 
 void exposeVersion() {
-  // Define release numbers of the current hpp-fcl version.
+  // Define release numbers of the current Coal version.
   bp::scope().attr("__version__") =
-      BOOST_PP_STRINGIZE(HPP_FCL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(HPP_FCL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(HPP_FCL_PATCH_VERSION);
-  bp::scope().attr("__raw_version__") = HPP_FCL_VERSION;
-  bp::scope().attr("HPP_FCL_MAJOR_VERSION") = HPP_FCL_MAJOR_VERSION;
-  bp::scope().attr("HPP_FCL_MINOR_VERSION") = HPP_FCL_MINOR_VERSION;
-  bp::scope().attr("HPP_FCL_PATCH_VERSION") = HPP_FCL_PATCH_VERSION;
+      BOOST_PP_STRINGIZE(COAL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_PATCH_VERSION);
+  bp::scope().attr("__raw_version__") = COAL_VERSION;
+  bp::scope().attr("COAL_MAJOR_VERSION") = COAL_MAJOR_VERSION;
+  bp::scope().attr("COAL_MINOR_VERSION") = COAL_MINOR_VERSION;
+  bp::scope().attr("COAL_PATCH_VERSION") = COAL_PATCH_VERSION;
 
-#if HPP_FCL_HAS_QHULL
+#if COAL_HAS_QHULL
   bp::scope().attr("WITH_QHULL") = true;
 #else
   bp::scope().attr("WITH_QHULL") = false;
 #endif
 
-#if HPP_FCL_HAS_OCTOMAP
+#if COAL_HAS_OCTOMAP
   bp::scope().attr("WITH_OCTOMAP") = true;
 #else
   bp::scope().attr("WITH_OCTOMAP") = false;
@@ -39,11 +39,11 @@ void exposeVersion() {
 
   bp::def("checkVersionAtLeast", &checkVersionAtLeast,
           bp::args("major", "minor", "patch"),
-          "Checks if the current version of hpp-fcl is at least"
+          "Checks if the current version of coal is at least"
           " the version provided by the input arguments.");
 
   bp::def("checkVersionAtMost", &checkVersionAtMost,
           bp::args("major", "minor", "patch"),
-          "Checks if the current version of hpp-fcl is at most"
+          "Checks if the current version of coal is at most"
           " the version provided by the input arguments.");
 }
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index 97f1b0723596bd4fb0dda3a744acad0b68064661..4e28a254812640a2fc9d9135fb51135e39e93676 100644
--- a/src/BV/AABB.cpp
+++ b/src/BV/AABB.cpp
@@ -35,36 +35,35 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/BV/AABB.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/BV/AABB.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/collision_data.h"
 
 #include <limits>
-#include <hpp/fcl/collision_data.h>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 AABB::AABB()
-    : min_(Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)())),
-      max_(Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)())) {}
+    : min_(Vec3s::Constant((std::numeric_limits<CoalScalar>::max)())),
+      max_(Vec3s::Constant(-(std::numeric_limits<CoalScalar>::max)())) {}
 
 bool AABB::overlap(const AABB& other, const CollisionRequest& request,
-                   FCL_REAL& sqrDistLowerBound) const {
-  const FCL_REAL break_distance_squared =
+                   CoalScalar& sqrDistLowerBound) const {
+  const CoalScalar break_distance_squared =
       request.break_distance * request.break_distance;
 
   sqrDistLowerBound =
-      (min_ - other.max_ - Vec3f::Constant(request.security_margin))
+      (min_ - other.max_ - Vec3s::Constant(request.security_margin))
           .array()
-          .max(FCL_REAL(0))
+          .max(CoalScalar(0))
           .matrix()
           .squaredNorm();
   if (sqrDistLowerBound > break_distance_squared) return false;
 
   sqrDistLowerBound =
-      (other.min_ - max_ - Vec3f::Constant(request.security_margin))
+      (other.min_ - max_ - Vec3s::Constant(request.security_margin))
           .array()
-          .max(FCL_REAL(0))
+          .max(CoalScalar(0))
           .matrix()
           .squaredNorm();
   if (sqrDistLowerBound > break_distance_squared) return false;
@@ -72,23 +71,23 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request,
   return true;
 }
 
-FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const {
-  FCL_REAL result = 0;
+CoalScalar AABB::distance(const AABB& other, Vec3s* P, Vec3s* Q) const {
+  CoalScalar result = 0;
   for (Eigen::DenseIndex i = 0; i < 3; ++i) {
-    const FCL_REAL& amin = min_[i];
-    const FCL_REAL& amax = max_[i];
-    const FCL_REAL& bmin = other.min_[i];
-    const FCL_REAL& bmax = other.max_[i];
+    const CoalScalar& amin = min_[i];
+    const CoalScalar& amax = max_[i];
+    const CoalScalar& bmin = other.min_[i];
+    const CoalScalar& bmax = other.max_[i];
 
     if (amin > bmax) {
-      FCL_REAL delta = bmax - amin;
+      CoalScalar delta = bmax - amin;
       result += delta * delta;
       if (P && Q) {
         (*P)[i] = amin;
         (*Q)[i] = bmax;
       }
     } else if (bmin > amax) {
-      FCL_REAL delta = amax - bmin;
+      CoalScalar delta = amax - bmin;
       result += delta * delta;
       if (P && Q) {
         (*P)[i] = amax;
@@ -97,11 +96,11 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const {
     } else {
       if (P && Q) {
         if (bmin >= amin) {
-          FCL_REAL t = 0.5 * (amax + bmin);
+          CoalScalar t = 0.5 * (amax + bmin);
           (*P)[i] = t;
           (*Q)[i] = t;
         } else {
-          FCL_REAL t = 0.5 * (amin + bmax);
+          CoalScalar t = 0.5 * (amin + bmax);
           (*P)[i] = t;
           (*Q)[i] = t;
         }
@@ -112,19 +111,19 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const {
   return std::sqrt(result);
 }
 
-FCL_REAL AABB::distance(const AABB& other) const {
-  FCL_REAL result = 0;
+CoalScalar AABB::distance(const AABB& other) const {
+  CoalScalar result = 0;
   for (Eigen::DenseIndex i = 0; i < 3; ++i) {
-    const FCL_REAL& amin = min_[i];
-    const FCL_REAL& amax = max_[i];
-    const FCL_REAL& bmin = other.min_[i];
-    const FCL_REAL& bmax = other.max_[i];
+    const CoalScalar& amin = min_[i];
+    const CoalScalar& amax = max_[i];
+    const CoalScalar& bmin = other.min_[i];
+    const CoalScalar& bmax = other.max_[i];
 
     if (amin > bmax) {
-      FCL_REAL delta = bmax - amin;
+      CoalScalar delta = bmax - amin;
       result += delta * delta;
     } else if (bmin > amax) {
-      FCL_REAL delta = amax - bmin;
+      CoalScalar delta = amax - bmin;
       result += delta * delta;
     }
   }
@@ -132,15 +131,15 @@ FCL_REAL AABB::distance(const AABB& other) const {
   return std::sqrt(result);
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
              const AABB& b2) {
   AABB bb1(translate(rotate(b1, R0), T0));
   return bb1.overlap(b2);
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
              const AABB& b2, const CollisionRequest& request,
-             FCL_REAL& sqrDistLowerBound) {
+             CoalScalar& sqrDistLowerBound) {
   AABB bb1(translate(rotate(b1, R0), T0));
   return bb1.overlap(b2, request, sqrDistLowerBound);
 }
@@ -150,15 +149,15 @@ bool AABB::overlap(const Plane& p) const {
   // points in the directions normal and -normal.
   // If both points lie on different sides of the plane, there is an overlap
   // between the AABB and the plane. Otherwise, there is no overlap.
-  const Vec3f halfside = (this->max_ - this->min_) / 2;
-  const Vec3f center = (this->max_ + this->min_) / 2;
+  const Vec3s halfside = (this->max_ - this->min_) / 2;
+  const Vec3s center = (this->max_ + this->min_) / 2;
 
-  const Vec3f support1 = (p.n.array() > 0).select(halfside, -halfside) + center;
-  const Vec3f support2 =
+  const Vec3s support1 = (p.n.array() > 0).select(halfside, -halfside) + center;
+  const Vec3s support2 =
       ((-p.n).array() > 0).select(halfside, -halfside) + center;
 
-  const FCL_REAL dist1 = p.n.dot(support1) - p.d;
-  const FCL_REAL dist2 = p.n.dot(support2) - p.d;
+  const CoalScalar dist1 = p.n.dot(support1) - p.d;
+  const CoalScalar dist2 = p.n.dot(support2) - p.d;
   const int sign1 = (dist1 > 0) ? 1 : -1;
   const int sign2 = (dist2 > 0) ? 1 : -1;
 
@@ -170,8 +169,8 @@ bool AABB::overlap(const Plane& p) const {
     // Both supports are on the same side of the plane.
     // We now need to check if they are on the same side of the plane inflated
     // by the swept-sphere radius.
-    const FCL_REAL ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius();
-    const FCL_REAL ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius();
+    const CoalScalar ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius();
+    const CoalScalar ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius();
     const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1;
     const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1;
     return ssr_sign1 != ssr_sign2;
@@ -186,12 +185,10 @@ bool AABB::overlap(const Halfspace& hs) const {
   // If the support is below the plane defined by the halfspace, there is an
   // overlap between the AABB and the halfspace. Otherwise, there is no
   // overlap.
-  Vec3f halfside = (this->max_ - this->min_) / 2;
-  Vec3f center = (this->max_ + this->min_) / 2;
-  Vec3f support = ((-hs.n).array() > 0).select(halfside, -halfside) + center;
+  Vec3s halfside = (this->max_ - this->min_) / 2;
+  Vec3s center = (this->max_ + this->min_) / 2;
+  Vec3s support = ((-hs.n).array() > 0).select(halfside, -halfside) + center;
   return (hs.signedDistance(support) < 0);
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 892d25bd873b6c9e43aaffc4dd54f471e9051442..b956fe144cccb8cc5b32d1924a1014e3cca503fe 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -35,45 +35,44 @@
 
 /** \author Jia Pan, Florent Lamiraux */
 
-#include <hpp/fcl/BV/OBB.h>
-#include <hpp/fcl/BVH/BVH_utility.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/internal/tools.h>
+#include "coal/BV/OBB.h"
+#include "coal/BVH/BVH_utility.h"
+#include "coal/math/transform.h"
+#include "coal/collision_data.h"
+#include "coal/internal/tools.h"
 
 #include <iostream>
 #include <limits>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 /// @brief Compute the 8 vertices of a OBB
-inline void computeVertices(const OBB& b, Vec3f vertices[8]) {
-  Matrix3f extAxes(b.axes * b.extent.asDiagonal());
-  vertices[0].noalias() = b.To + extAxes * Vec3f(-1, -1, -1);
-  vertices[1].noalias() = b.To + extAxes * Vec3f(1, -1, -1);
-  vertices[2].noalias() = b.To + extAxes * Vec3f(1, 1, -1);
-  vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1, -1);
-  vertices[4].noalias() = b.To + extAxes * Vec3f(-1, -1, 1);
-  vertices[5].noalias() = b.To + extAxes * Vec3f(1, -1, 1);
-  vertices[6].noalias() = b.To + extAxes * Vec3f(1, 1, 1);
-  vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1);
+inline void computeVertices(const OBB& b, Vec3s vertices[8]) {
+  Matrix3s extAxes(b.axes * b.extent.asDiagonal());
+  vertices[0].noalias() = b.To + extAxes * Vec3s(-1, -1, -1);
+  vertices[1].noalias() = b.To + extAxes * Vec3s(1, -1, -1);
+  vertices[2].noalias() = b.To + extAxes * Vec3s(1, 1, -1);
+  vertices[3].noalias() = b.To + extAxes * Vec3s(-1, 1, -1);
+  vertices[4].noalias() = b.To + extAxes * Vec3s(-1, -1, 1);
+  vertices[5].noalias() = b.To + extAxes * Vec3s(1, -1, 1);
+  vertices[6].noalias() = b.To + extAxes * Vec3s(1, 1, 1);
+  vertices[7].noalias() = b.To + extAxes * Vec3s(-1, 1, 1);
 }
 
 /// @brief OBB merge method when the centers of two smaller OBB are far away
 inline OBB merge_largedist(const OBB& b1, const OBB& b2) {
   OBB b;
-  Vec3f vertex[16];
+  Vec3s vertex[16];
   computeVertices(b1, vertex);
   computeVertices(b2, vertex + 8);
-  Matrix3f M;
-  Vec3f E[3];
-  Matrix3f::Scalar s[3] = {0, 0, 0};
+  Matrix3s M;
+  Vec3s E[3];
+  CoalScalar s[3] = {0, 0, 0};
 
   // obb axes
   b.axes.col(0).noalias() = (b1.To - b2.To).normalized();
 
-  Vec3f vertex_proj[16];
+  Vec3s vertex_proj[16];
   for (int i = 0; i < 16; ++i)
     vertex_proj[i].noalias() =
         vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0));
@@ -103,7 +102,7 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) {
   b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid];
 
   // set obb centers and extensions
-  Vec3f center, extent;
+  Vec3s center, extent;
   getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent);
 
   b.To.noalias() = center;
@@ -122,16 +121,16 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
   Quatf q((q0.coeffs() + q1.coeffs()).normalized());
   b.axes = q.toRotationMatrix();
 
-  Vec3f vertex[8], diff;
-  FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
-  Vec3f pmin(real_max, real_max, real_max);
-  Vec3f pmax(-real_max, -real_max, -real_max);
+  Vec3s vertex[8], diff;
+  CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
+  Vec3s pmin(real_max, real_max, real_max);
+  Vec3s pmax(-real_max, -real_max, -real_max);
 
   computeVertices(b1, vertex);
   for (int i = 0; i < 8; ++i) {
     diff = vertex[i] - b.To;
     for (int j = 0; j < 3; ++j) {
-      FCL_REAL dot = diff.dot(b.axes.col(j));
+      CoalScalar dot = diff.dot(b.axes.col(j));
       if (dot > pmax[j])
         pmax[j] = dot;
       else if (dot < pmin[j])
@@ -143,7 +142,7 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
   for (int i = 0; i < 8; ++i) {
     diff = vertex[i] - b.To;
     for (int j = 0; j < 3; ++j) {
-      FCL_REAL dot = diff.dot(b.axes.col(j));
+      CoalScalar dot = diff.dot(b.axes.col(j));
       if (dot > pmax[j])
         pmax[j] = dot;
       else if (dot < pmin[j])
@@ -159,12 +158,12 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
   return b;
 }
 
-bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-                 const Vec3f& b) {
-  FCL_REAL t, s;
-  const FCL_REAL reps = 1e-6;
+bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                 const Vec3s& b) {
+  CoalScalar t, s;
+  const CoalScalar reps = 1e-6;
 
-  Matrix3f Bf(B.array().abs() + reps);
+  Matrix3s Bf(B.array().abs() + reps);
   // Bf += reps;
 
   // if any of these tests are one-sided, then the polyhedra are disjoint
@@ -287,20 +286,20 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
 }
 
 namespace internal {
-inline FCL_REAL obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a,
-                                         const Vec3f& b, const Matrix3f& Bf) {
+inline CoalScalar obbDisjoint_check_A_axis(const Vec3s& T, const Vec3s& a,
+                                           const Vec3s& b, const Matrix3s& Bf) {
   // |T| - |B| * b - a
-  Vec3f AABB_corner(T.cwiseAbs() - a);
+  Vec3s AABB_corner(T.cwiseAbs() - a);
   AABB_corner.noalias() -= Bf * b;
-  return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm();
+  return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm();
 }
 
-inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T,
-                                         const Vec3f& a, const Vec3f& b,
-                                         const Matrix3f& Bf) {
+inline CoalScalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T,
+                                           const Vec3s& a, const Vec3s& b,
+                                           const Matrix3s& Bf) {
   // Bf = |B|
   // | B^T T| - Bf^T * a - b
-  FCL_REAL s, t = 0;
+  CoalScalar s, t = 0;
   s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
   if (s > 0) t += s * s;
   s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
@@ -311,17 +310,17 @@ inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T,
 }
 
 template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
-struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi{static inline bool run(
-    int ia, int ja, int ka, const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-    const Vec3f& b, const Matrix3f& Bf, const FCL_REAL& breakDistance2,
-    FCL_REAL& squaredLowerBoundDistance){FCL_REAL sinus2 =
-                                             1 - Bf(ia, ib) * Bf(ia, ib);
+struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi{static inline bool run(
+    int ia, int ja, int ka, const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+    const Vec3s& b, const Matrix3s& Bf, const CoalScalar& breakDistance2,
+    CoalScalar& squaredLowerBoundDistance){CoalScalar sinus2 =
+                                               1 - Bf(ia, ib) * Bf(ia, ib);
 if (sinus2 < 1e-6) return false;
 
-const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
+const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
 
-const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
-                                 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
+const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
+                                   b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
 // We need to divide by the norm || Aia x Bib ||
 // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2  = cosine^2
 if (diff > 0) {
@@ -332,8 +331,8 @@ if (diff > 0) {
 }
 return false;
 }  // namespace internal
-};  // namespace fcl
-}  // namespace hpp
+};  // namespace coal
+}  // namespace internal
 
 // B, T orientation and position of 2nd OBB in frame of 1st OBB,
 // a extent of 1st OBB,
@@ -341,26 +340,26 @@ return false;
 //
 // This function tests whether bounding boxes should be broken down.
 //
-bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T,
-                                      const Vec3f& a_, const Vec3f& b_,
+bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T,
+                                      const Vec3s& a_, const Vec3s& b_,
                                       const CollisionRequest& request,
-                                      FCL_REAL& squaredLowerBoundDistance) {
+                                      CoalScalar& squaredLowerBoundDistance) {
   assert(request.security_margin >
              -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) -
-                 10 * Eigen::NumTraits<FCL_REAL>::epsilon() &&
+                 10 * Eigen::NumTraits<CoalScalar>::epsilon() &&
          "A negative security margin could not be lower than the OBB extent.");
-  //  const FCL_REAL breakDistance(request.break_distance +
+  //  const CoalScalar breakDistance(request.break_distance +
   //                               request.security_margin);
-  const FCL_REAL breakDistance2 =
+  const CoalScalar breakDistance2 =
       request.break_distance * request.break_distance;
 
-  Matrix3f Bf(B.cwiseAbs());
-  const Vec3f a((a_ + Vec3f::Constant(request.security_margin / 2))
+  Matrix3s Bf(B.cwiseAbs());
+  const Vec3s a((a_ + Vec3s::Constant(request.security_margin / 2))
                     .array()
-                    .max(FCL_REAL(0)));
-  const Vec3f b((b_ + Vec3f::Constant(request.security_margin / 2))
+                    .max(CoalScalar(0)));
+  const Vec3s b((b_ + Vec3s::Constant(request.security_margin / 2))
                     .array()
-                    .max(FCL_REAL(0)));
+                    .max(CoalScalar(0)));
 
   // Corner of b axis aligned bounding box the closest to the origin
   squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf);
@@ -396,35 +395,35 @@ bool OBB::overlap(const OBB& other) const {
   /// compute what transform [R,T] that takes us from cs1 to cs2.
   /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
   /// First compute the rotation part, then translation part
-  Vec3f T(axes.transpose() * (other.To - To));
-  Matrix3f R(axes.transpose() * other.axes);
+  Vec3s T(axes.transpose() * (other.To - To));
+  Matrix3s R(axes.transpose() * other.axes);
 
   return !obbDisjoint(R, T, extent, other.extent);
 }
 
 bool OBB::overlap(const OBB& other, const CollisionRequest& request,
-                  FCL_REAL& sqrDistLowerBound) const {
+                  CoalScalar& sqrDistLowerBound) const {
   /// compute what transform [R,T] that takes us from cs1 to cs2.
   /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
   /// First compute the rotation part, then translation part
-  // Vec3f t = other.To - To; // T2 - T1
-  // Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
-  // Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
+  // Vec3s t = other.To - To; // T2 - T1
+  // Vec3s T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
+  // Matrix3s R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
   // axis[0].dot(other.axis[2]),
   // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
   // axis[1].dot(other.axis[2]),
   // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
   // axis[2].dot(other.axis[2]));
-  Vec3f T(axes.transpose() * (other.To - To));
-  Matrix3f R(axes.transpose() * other.axes);
+  Vec3s T(axes.transpose() * (other.To - To));
+  Matrix3s R(axes.transpose() * other.axes);
 
   return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request,
                                            sqrDistLowerBound);
 }
 
-bool OBB::contain(const Vec3f& p) const {
-  Vec3f local_p(p - To);
-  FCL_REAL proj = local_p.dot(axes.col(0));
+bool OBB::contain(const Vec3s& p) const {
+  Vec3s local_p(p - To);
+  CoalScalar proj = local_p.dot(axes.col(0));
   if ((proj > extent[0]) || (proj < -extent[0])) return false;
 
   proj = local_p.dot(axes.col(1));
@@ -436,7 +435,7 @@ bool OBB::contain(const Vec3f& p) const {
   return true;
 }
 
-OBB& OBB::operator+=(const Vec3f& p) {
+OBB& OBB::operator+=(const Vec3s& p) {
   OBB bvp;
   bvp.To = p;
   bvp.axes.noalias() = axes;
@@ -447,9 +446,9 @@ OBB& OBB::operator+=(const Vec3f& p) {
 }
 
 OBB OBB::operator+(const OBB& other) const {
-  Vec3f center_diff = To - other.To;
-  FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
-  FCL_REAL max_extent2 =
+  Vec3s center_diff = To - other.To;
+  CoalScalar max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
+  CoalScalar max_extent2 =
       std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
   if (center_diff.norm() > 2 * (max_extent + max_extent2)) {
     return merge_largedist(*this, other);
@@ -458,36 +457,35 @@ OBB OBB::operator+(const OBB& other) const {
   }
 }
 
-FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const {
+CoalScalar OBB::distance(const OBB& /*other*/, Vec3s* /*P*/,
+                         Vec3s* /*Q*/) const {
   std::cerr << "OBB distance not implemented!" << std::endl;
   return 0.0;
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
              const OBB& b2) {
-  Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
-  Vec3f T(b1.axes.transpose() * Ttemp);
-  Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
+  Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
+  Vec3s T(b1.axes.transpose() * Ttemp);
+  Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
 
   return !obbDisjoint(R, T, b1.extent, b2.extent);
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,
-             const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) {
-  Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
-  Vec3f T(b1.axes.transpose() * Ttemp);
-  Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,
+             const CollisionRequest& request, CoalScalar& sqrDistLowerBound) {
+  Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
+  Vec3s T(b1.axes.transpose() * Ttemp);
+  Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
 
   return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request,
                                            sqrDistLowerBound);
 }
 
-OBB translate(const OBB& bv, const Vec3f& t) {
+OBB translate(const OBB& bv, const Vec3s& t) {
   OBB res(bv);
   res.To += t;
   return res;
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BV/OBB.h b/src/BV/OBB.h
index 24d6ef8777cd44a7e1ecc827eb00859093b7e301..a7d3a3817dd9b9b64aba29f38c195dbfbef58a03 100644
--- a/src/BV/OBB.h
+++ b/src/BV/OBB.h
@@ -33,21 +33,18 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef HPP_FCL_SRC_OBB_H
-#define HPP_FCL_SRC_OBB_H
+#ifndef COAL_SRC_OBB_H
+#define COAL_SRC_OBB_H
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
-bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T,
-                                      const Vec3f& a, const Vec3f& b,
+bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T,
+                                      const Vec3s& a, const Vec3s& b,
                                       const CollisionRequest& request,
-                                      FCL_REAL& squaredLowerBoundDistance);
+                                      CoalScalar& squaredLowerBoundDistance);
 
-bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-                 const Vec3f& b);
-}  // namespace fcl
+bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                 const Vec3s& b);
+}  // namespace coal
 
-}  // namespace hpp
-
-#endif  // HPP_FCL_SRC_OBB_H
+#endif  // COAL_SRC_OBB_H
diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp
index 87a8494e8d2a1271e218bfb31650b1526e1498c7..1842e60f0662c9af29bbf90941ba34a6252a1780 100644
--- a/src/BV/OBBRSS.cpp
+++ b/src/BV/OBBRSS.cpp
@@ -35,18 +35,15 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/BV/OBBRSS.h>
+#include "coal/BV/OBBRSS.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
-OBBRSS translate(const OBBRSS& bv, const Vec3f& t) {
+OBBRSS translate(const OBBRSS& bv, const Vec3s& t) {
   OBBRSS res(bv);
   res.obb.To += t;
   res.rss.Tr += t;
   return res;
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 5082775af50ce74af78e7d463d38aecc2df2ae50..97d5d1adba16637e2435c6dee4f36cc26ee6854e 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -35,18 +35,17 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/BV/RSS.h>
-#include <hpp/fcl/BVH/BVH_utility.h>
-#include <hpp/fcl/internal/tools.h>
-#include <hpp/fcl/collision_data.h>
+#include "coal/BV/RSS.h"
+#include "coal/BVH/BVH_utility.h"
+#include "coal/internal/tools.h"
+#include "coal/collision_data.h"
 
 #include <iostream>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 /// @brief Clip value between a and b
-void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) {
+void clipToRange(CoalScalar& val, CoalScalar a, CoalScalar b) {
   if (val < a)
     val = a;
   else if (val > b)
@@ -64,9 +63,9 @@ void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) {
 /// of each segment. "T" in the dot products is the vector betweeen Pa and Pb.
 /// Reference: "On fast computation of distance between line segments." Vladimir
 /// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985.
-void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b,
-               FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) {
-  FCL_REAL denom = 1 - A_dot_B * A_dot_B;
+void segCoords(CoalScalar& t, CoalScalar& u, CoalScalar a, CoalScalar b,
+               CoalScalar A_dot_B, CoalScalar A_dot_T, CoalScalar B_dot_T) {
+  CoalScalar denom = 1 - A_dot_B * A_dot_B;
 
   if (denom == 0)
     t = 0;
@@ -92,12 +91,12 @@ void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b,
 /// Pa + A*t, 0 <= t <= a, is within the half space
 /// determined by the point Pa and the direction Anorm.
 /// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb.
-bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B,
-               FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T,
-               FCL_REAL B_dot_T) {
+bool inVoronoi(CoalScalar a, CoalScalar b, CoalScalar Anorm_dot_B,
+               CoalScalar Anorm_dot_T, CoalScalar A_dot_B, CoalScalar A_dot_T,
+               CoalScalar B_dot_T) {
   if (fabs(Anorm_dot_B) < 1e-7) return false;
 
-  FCL_REAL t, u, v;
+  CoalScalar t, u, v;
 
   u = -Anorm_dot_T / Anorm_dot_B;
   clipToRange(u, 0, b);
@@ -118,18 +117,18 @@ bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B,
 /// @brief Distance between two oriented rectangles; P and Q (optional return
 /// values) are the closest points in the rectangles, both are in the local
 /// frame of the first rectangle.
-FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
-                      const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL,
-                      Vec3f* Q = NULL) {
-  FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
+CoalScalar rectDistance(const Matrix3s& Rab, Vec3s const& Tab,
+                        const CoalScalar a[2], const CoalScalar b[2],
+                        Vec3s* P = NULL, Vec3s* Q = NULL) {
+  CoalScalar A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
 
   A0_dot_B0 = Rab(0, 0);
   A0_dot_B1 = Rab(0, 1);
   A1_dot_B0 = Rab(1, 0);
   A1_dot_B1 = Rab(1, 1);
 
-  FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
-  FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
+  CoalScalar aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
+  CoalScalar bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
 
   aA0_dot_B0 = a[0] * A0_dot_B0;
   aA0_dot_B1 = a[0] * A0_dot_B1;
@@ -140,16 +139,16 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
   bA0_dot_B1 = b[1] * A0_dot_B1;
   bA1_dot_B1 = b[1] * A1_dot_B1;
 
-  Vec3f Tba(Rab.transpose() * Tab);
+  Vec3s Tba(Rab.transpose() * Tab);
 
-  Vec3f S;
-  FCL_REAL t, u;
+  Vec3s S;
+  CoalScalar t, u;
 
   // determine if any edge pair contains the closest points
 
-  FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x;
-  FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x;
-  FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
+  CoalScalar ALL_x, ALU_x, AUL_x, AUU_x;
+  CoalScalar BLL_x, BLU_x, BUL_x, BUU_x;
+  CoalScalar LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
 
   ALL_x = -Tba[0];
   ALU_x = ALL_x + aA1_dot_B0;
@@ -278,14 +277,14 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
     }
   }
 
-  FCL_REAL ALL_y, ALU_y, AUL_y, AUU_y;
+  CoalScalar ALL_y, ALU_y, AUL_y, AUU_y;
 
   ALL_y = -Tba[1];
   ALU_y = ALL_y + aA1_dot_B1;
   AUL_y = ALL_y + aA0_dot_B1;
   AUU_y = ALU_y + aA0_dot_B1;
 
-  FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
+  CoalScalar LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
 
   if (ALL_y < ALU_y) {
     LA1_ly = ALL_y;
@@ -405,14 +404,14 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
     }
   }
 
-  FCL_REAL BLL_y, BLU_y, BUL_y, BUU_y;
+  CoalScalar BLL_y, BLU_y, BUL_y, BUU_y;
 
   BLL_y = Tab[1];
   BLU_y = BLL_y + bA1_dot_B1;
   BUL_y = BLL_y + bA1_dot_B0;
   BUU_y = BLU_y + bA1_dot_B0;
 
-  FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
+  CoalScalar LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
 
   if (ALL_x < AUL_x) {
     LA0_lx = ALL_x;
@@ -531,7 +530,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
     }
   }
 
-  FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
+  CoalScalar LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
 
   if (ALL_y < AUL_y) {
     LA0_ly = ALL_y;
@@ -653,7 +652,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
 
   // no edges passed, take max separation along face normals
 
-  FCL_REAL sep1, sep2;
+  CoalScalar sep1, sep2;
 
   if (Tab[2] > 0.0) {
     sep1 = Tab[2];
@@ -688,8 +687,8 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
   }
 
   if (sep2 >= sep1 && sep2 >= 0) {
-    Vec3f Q_(Tab[0], Tab[1], Tab[2]);
-    Vec3f P_;
+    Vec3s Q_(Tab[0], Tab[1], Tab[2]);
+    Vec3s P_;
     if (Tba[2] < 0) {
       P_[0] = Rab(0, 2) * sep2 + Tab[0];
       P_[1] = Rab(1, 2) * sep2 + Tab[1];
@@ -708,7 +707,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
     }
   }
 
-  FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2);
+  CoalScalar sep = (sep1 > sep2 ? sep1 : sep2);
   return (sep > 0 ? sep : 0);
 }
 
@@ -718,56 +717,56 @@ bool RSS::overlap(const RSS& other) const {
   /// First compute the rotation part, then translation part
 
   /// Then compute R1'(T2 - T1)
-  Vec3f T(axes.transpose() * (other.Tr - Tr));
+  Vec3s T(axes.transpose() * (other.Tr - Tr));
 
   /// Now compute R1'R2
-  Matrix3f R(axes.transpose() * other.axes);
+  Matrix3s R(axes.transpose() * other.axes);
 
-  FCL_REAL dist = rectDistance(R, T, length, other.length);
+  CoalScalar dist = rectDistance(R, T, length, other.length);
   return (dist <= (radius + other.radius));
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
              const RSS& b2) {
   // ROb2 = R0 . b2
   // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
 
   // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
   // R = b2^T RO^T b1
-  Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
-  Vec3f T(b1.axes.transpose() * Ttemp);
-  Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
+  Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
+  Vec3s T(b1.axes.transpose() * Ttemp);
+  Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
 
-  FCL_REAL dist = rectDistance(R, T, b1.length, b2.length);
+  CoalScalar dist = rectDistance(R, T, b1.length, b2.length);
   return (dist <= (b1.radius + b2.radius));
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
-             const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) {
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2,
+             const CollisionRequest& request, CoalScalar& sqrDistLowerBound) {
   // ROb2 = R0 . b2
   // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
 
   // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
   // R = b2^T RO^T b1
-  Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
-  Vec3f T(b1.axes.transpose() * Ttemp);
-  Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
+  Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
+  Vec3s T(b1.axes.transpose() * Ttemp);
+  Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
 
-  FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius -
-                  b2.radius - request.security_margin;
+  CoalScalar dist = rectDistance(R, T, b1.length, b2.length) - b1.radius -
+                    b2.radius - request.security_margin;
   if (dist <= 0) return true;
   sqrDistLowerBound = dist * dist;
   return false;
 }
 
-bool RSS::contain(const Vec3f& p) const {
-  Vec3f local_p = p - Tr;
-  // FIXME: Vec3f proj (axes.transpose() * local_p);
-  FCL_REAL proj0 = local_p.dot(axes.col(0));
-  FCL_REAL proj1 = local_p.dot(axes.col(1));
-  FCL_REAL proj2 = local_p.dot(axes.col(2));
-  FCL_REAL abs_proj2 = fabs(proj2);
-  Vec3f proj(proj0, proj1, proj2);
+bool RSS::contain(const Vec3s& p) const {
+  Vec3s local_p = p - Tr;
+  // FIXME: Vec3s proj (axes.transpose() * local_p);
+  CoalScalar proj0 = local_p.dot(axes.col(0));
+  CoalScalar proj1 = local_p.dot(axes.col(1));
+  CoalScalar proj2 = local_p.dot(axes.col(2));
+  CoalScalar abs_proj2 = fabs(proj2);
+  Vec3s proj(proj0, proj1, proj2);
 
   /// projection is within the rectangle
   if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) &&
@@ -775,29 +774,29 @@ bool RSS::contain(const Vec3f& p) const {
     return (abs_proj2 < radius);
   } else if ((proj0 < length[0]) && (proj0 > 0) &&
              ((proj1 < 0) || (proj1 > length[1]))) {
-    FCL_REAL y = (proj1 > 0) ? length[1] : 0;
-    Vec3f v(proj0, y, 0);
+    CoalScalar y = (proj1 > 0) ? length[1] : 0;
+    Vec3s v(proj0, y, 0);
     return ((proj - v).squaredNorm() < radius * radius);
   } else if ((proj1 < length[1]) && (proj1 > 0) &&
              ((proj0 < 0) || (proj0 > length[0]))) {
-    FCL_REAL x = (proj0 > 0) ? length[0] : 0;
-    Vec3f v(x, proj1, 0);
+    CoalScalar x = (proj0 > 0) ? length[0] : 0;
+    Vec3s v(x, proj1, 0);
     return ((proj - v).squaredNorm() < radius * radius);
   } else {
-    FCL_REAL x = (proj0 > 0) ? length[0] : 0;
-    FCL_REAL y = (proj1 > 0) ? length[1] : 0;
-    Vec3f v(x, y, 0);
+    CoalScalar x = (proj0 > 0) ? length[0] : 0;
+    CoalScalar y = (proj1 > 0) ? length[1] : 0;
+    Vec3s v(x, y, 0);
     return ((proj - v).squaredNorm() < radius * radius);
   }
 }
 
-RSS& RSS::operator+=(const Vec3f& p) {
-  Vec3f local_p = p - Tr;
-  FCL_REAL proj0 = local_p.dot(axes.col(0));
-  FCL_REAL proj1 = local_p.dot(axes.col(1));
-  FCL_REAL proj2 = local_p.dot(axes.col(2));
-  FCL_REAL abs_proj2 = fabs(proj2);
-  Vec3f proj(proj0, proj1, proj2);
+RSS& RSS::operator+=(const Vec3s& p) {
+  Vec3s local_p = p - Tr;
+  CoalScalar proj0 = local_p.dot(axes.col(0));
+  CoalScalar proj1 = local_p.dot(axes.col(1));
+  CoalScalar proj2 = local_p.dot(axes.col(2));
+  CoalScalar abs_proj2 = fabs(proj2);
+  Vec3s proj(proj0, proj1, proj2);
 
   // projection is within the rectangle
   if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) &&
@@ -814,19 +813,19 @@ RSS& RSS::operator+=(const Vec3f& p) {
     }
   } else if ((proj0 < length[0]) && (proj0 > 0) &&
              ((proj1 < 0) || (proj1 > length[1]))) {
-    FCL_REAL y = (proj1 > 0) ? length[1] : 0;
-    Vec3f v(proj0, y, 0);
-    FCL_REAL new_r_sqr = (proj - v).squaredNorm();
+    CoalScalar y = (proj1 > 0) ? length[1] : 0;
+    Vec3s v(proj0, y, 0);
+    CoalScalar new_r_sqr = (proj - v).squaredNorm();
     if (new_r_sqr < radius * radius)
       ;  // do nothing
     else {
       if (abs_proj2 < radius) {
-        FCL_REAL delta_y =
+        CoalScalar delta_y =
             -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y);
         length[1] += delta_y;
         if (proj1 < 0) Tr[1] -= delta_y;
       } else {
-        FCL_REAL delta_y = fabs(proj1 - y);
+        CoalScalar delta_y = fabs(proj1 - y);
         length[1] += delta_y;
         if (proj1 < 0) Tr[1] -= delta_y;
 
@@ -838,19 +837,19 @@ RSS& RSS::operator+=(const Vec3f& p) {
     }
   } else if ((proj1 < length[1]) && (proj1 > 0) &&
              ((proj0 < 0) || (proj0 > length[0]))) {
-    FCL_REAL x = (proj0 > 0) ? length[0] : 0;
-    Vec3f v(x, proj1, 0);
-    FCL_REAL new_r_sqr = (proj - v).squaredNorm();
+    CoalScalar x = (proj0 > 0) ? length[0] : 0;
+    Vec3s v(x, proj1, 0);
+    CoalScalar new_r_sqr = (proj - v).squaredNorm();
     if (new_r_sqr < radius * radius)
       ;  // do nothing
     else {
       if (abs_proj2 < radius) {
-        FCL_REAL delta_x =
+        CoalScalar delta_x =
             -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x);
         length[0] += delta_x;
         if (proj0 < 0) Tr[0] -= delta_x;
       } else {
-        FCL_REAL delta_x = fabs(proj0 - x);
+        CoalScalar delta_x = fabs(proj0 - x);
         length[0] += delta_x;
         if (proj0 < 0) Tr[0] -= delta_x;
 
@@ -861,20 +860,20 @@ RSS& RSS::operator+=(const Vec3f& p) {
       }
     }
   } else {
-    FCL_REAL x = (proj0 > 0) ? length[0] : 0;
-    FCL_REAL y = (proj1 > 0) ? length[1] : 0;
-    Vec3f v(x, y, 0);
-    FCL_REAL new_r_sqr = (proj - v).squaredNorm();
+    CoalScalar x = (proj0 > 0) ? length[0] : 0;
+    CoalScalar y = (proj1 > 0) ? length[1] : 0;
+    Vec3s v(x, y, 0);
+    CoalScalar new_r_sqr = (proj - v).squaredNorm();
     if (new_r_sqr < radius * radius)
       ;  // do nothing
     else {
       if (abs_proj2 < radius) {
-        FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2);
-        FCL_REAL delta_diag =
+        CoalScalar diag = std::sqrt(new_r_sqr - proj2 * proj2);
+        CoalScalar delta_diag =
             -std::sqrt(radius * radius - proj2 * proj2) + diag;
 
-        FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
-        FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
+        CoalScalar delta_x = delta_diag / diag * fabs(proj0 - x);
+        CoalScalar delta_y = delta_diag / diag * fabs(proj1 - y);
         length[0] += delta_x;
         length[1] += delta_y;
 
@@ -883,8 +882,8 @@ RSS& RSS::operator+=(const Vec3f& p) {
           Tr[1] -= delta_y;
         }
       } else {
-        FCL_REAL delta_x = fabs(proj0 - x);
-        FCL_REAL delta_y = fabs(proj1 - y);
+        CoalScalar delta_x = fabs(proj0 - x);
+        CoalScalar delta_y = fabs(proj1 - y);
 
         length[0] += delta_x;
         length[1] += delta_y;
@@ -908,13 +907,13 @@ RSS& RSS::operator+=(const Vec3f& p) {
 RSS RSS::operator+(const RSS& other) const {
   RSS bv;
 
-  Vec3f v[16];
-  Vec3f d0_pos(other.axes.col(0) * (other.length[0] + other.radius));
-  Vec3f d1_pos(other.axes.col(1) * (other.length[1] + other.radius));
-  Vec3f d0_neg(other.axes.col(0) * (-other.radius));
-  Vec3f d1_neg(other.axes.col(1) * (-other.radius));
-  Vec3f d2_pos(other.axes.col(2) * other.radius);
-  Vec3f d2_neg(other.axes.col(2) * (-other.radius));
+  Vec3s v[16];
+  Vec3s d0_pos(other.axes.col(0) * (other.length[0] + other.radius));
+  Vec3s d1_pos(other.axes.col(1) * (other.length[1] + other.radius));
+  Vec3s d0_neg(other.axes.col(0) * (-other.radius));
+  Vec3s d1_neg(other.axes.col(1) * (-other.radius));
+  Vec3s d2_pos(other.axes.col(2) * other.radius);
+  Vec3s d2_neg(other.axes.col(2) * (-other.radius));
 
   v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos;
   v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg;
@@ -941,9 +940,9 @@ RSS RSS::operator+(const RSS& other) const {
   v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos;
   v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg;
 
-  Matrix3f M;  // row first matrix
-  Vec3f E[3];  // row first eigen-vectors
-  Matrix3f::Scalar s[3] = {0, 0, 0};
+  Matrix3s M;  // row first matrix
+  Vec3s E[3];  // row first eigen-vectors
+  CoalScalar s[3] = {0, 0, 0};
 
   getCovariance(v, NULL, NULL, NULL, 16, M);
   eigen(M, s, E);
@@ -980,36 +979,34 @@ RSS RSS::operator+(const RSS& other) const {
   return bv;
 }
 
-FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const {
+CoalScalar RSS::distance(const RSS& other, Vec3s* P, Vec3s* Q) const {
   // compute what transform [R,T] that takes us from cs1 to cs2.
   // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
   // First compute the rotation part, then translation part
-  Matrix3f R(axes.transpose() * other.axes);
-  Vec3f T(axes.transpose() * (other.Tr - Tr));
+  Matrix3s R(axes.transpose() * other.axes);
+  Vec3s T(axes.transpose() * (other.Tr - Tr));
 
-  FCL_REAL dist = rectDistance(R, T, length, other.length, P, Q);
+  CoalScalar dist = rectDistance(R, T, length, other.length, P, Q);
   dist -= (radius + other.radius);
-  return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
+  return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist;
 }
 
-FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1,
-                  const RSS& b2, Vec3f* P, Vec3f* Q) {
-  Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
-  Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr);
+CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
+                    const RSS& b2, Vec3s* P, Vec3s* Q) {
+  Matrix3s R(b1.axes.transpose() * R0 * b2.axes);
+  Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr);
 
-  Vec3f T(b1.axes.transpose() * Ttemp);
+  Vec3s T(b1.axes.transpose() * Ttemp);
 
-  FCL_REAL dist = rectDistance(R, T, b1.length, b2.length, P, Q);
+  CoalScalar dist = rectDistance(R, T, b1.length, b2.length, P, Q);
   dist -= (b1.radius + b2.radius);
-  return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
+  return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist;
 }
 
-RSS translate(const RSS& bv, const Vec3f& t) {
+RSS translate(const RSS& bv, const Vec3s& t) {
   RSS res(bv);
   res.Tr += t;
   return res;
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp
index 3e92cb30d96b597ba2fc1c9466a58211f1d6fc0d..d59ccc6b9fe78c4d83a63b2357ee99be172dd99e 100644
--- a/src/BV/kDOP.cpp
+++ b/src/BV/kDOP.cpp
@@ -35,17 +35,17 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/BV/kDOP.h>
+#include "coal/collision_data.h"
+#include "coal/BV/kDOP.h"
+
 #include <limits>
 #include <iostream>
 
-#include <hpp/fcl/collision_data.h>
-
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 /// @brief Find the smaller and larger one of two values
-inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) {
+inline void minmax(CoalScalar a, CoalScalar b, CoalScalar& minv,
+                   CoalScalar& maxv) {
   if (a > b) {
     minv = b;
     maxv = a;
@@ -55,7 +55,7 @@ inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) {
   }
 }
 /// @brief Merge the interval [minv, maxv] and value p/
-inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) {
+inline void minmax(CoalScalar p, CoalScalar& minv, CoalScalar& maxv) {
   if (p > maxv) maxv = p;
   if (p < minv) minv = p;
 }
@@ -63,11 +63,11 @@ inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) {
 /// @brief Compute the distances to planes with normals from KDOP vectors except
 /// those of AABB face planes
 template <short N>
-void getDistances(const Vec3f& /*p*/, FCL_REAL* /*d*/) {}
+void getDistances(const Vec3s& /*p*/, CoalScalar* /*d*/) {}
 
 /// @brief Specification of getDistances
 template <>
-inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) {
+inline void getDistances<5>(const Vec3s& p, CoalScalar* d) {
   d[0] = p[0] + p[1];
   d[1] = p[0] + p[2];
   d[2] = p[1] + p[2];
@@ -76,7 +76,7 @@ inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) {
 }
 
 template <>
-inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) {
+inline void getDistances<6>(const Vec3s& p, CoalScalar* d) {
   d[0] = p[0] + p[1];
   d[1] = p[0] + p[2];
   d[2] = p[1] + p[2];
@@ -86,7 +86,7 @@ inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) {
 }
 
 template <>
-inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) {
+inline void getDistances<9>(const Vec3s& p, CoalScalar* d) {
   d[0] = p[0] + p[1];
   d[1] = p[0] + p[2];
   d[2] = p[1] + p[2];
@@ -100,18 +100,18 @@ inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) {
 
 template <short N>
 KDOP<N>::KDOP() {
-  FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
   dist_.template head<N / 2>().setConstant(real_max);
   dist_.template tail<N / 2>().setConstant(-real_max);
 }
 
 template <short N>
-KDOP<N>::KDOP(const Vec3f& v) {
+KDOP<N>::KDOP(const Vec3s& v) {
   for (short i = 0; i < 3; ++i) {
     dist_[i] = dist_[N / 2 + i] = v[i];
   }
 
-  FCL_REAL d[(N - 6) / 2];
+  CoalScalar d[(N - 6) / 2];
   getDistances<(N - 6) / 2>(v, d);
   for (short i = 0; i < (N - 6) / 2; ++i) {
     dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
@@ -119,12 +119,12 @@ KDOP<N>::KDOP(const Vec3f& v) {
 }
 
 template <short N>
-KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b) {
+KDOP<N>::KDOP(const Vec3s& a, const Vec3s& b) {
   for (short i = 0; i < 3; ++i) {
     minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
   }
 
-  FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
+  CoalScalar ad[(N - 6) / 2], bd[(N - 6) / 2];
   getDistances<(N - 6) / 2>(a, ad);
   getDistances<(N - 6) / 2>(b, bd);
   for (short i = 0; i < (N - 6) / 2; ++i) {
@@ -143,11 +143,11 @@ bool KDOP<N>::overlap(const KDOP<N>& other) const {
 
 template <short N>
 bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
-                      FCL_REAL& sqrDistLowerBound) const {
-  const FCL_REAL breakDistance(request.break_distance +
-                               request.security_margin);
+                      CoalScalar& sqrDistLowerBound) const {
+  const CoalScalar breakDistance(request.break_distance +
+                                 request.security_margin);
 
-  FCL_REAL a =
+  CoalScalar a =
       (dist_.template head<N / 2>() - other.dist_.template tail<N / 2>())
           .minCoeff();
   if (a > breakDistance) {
@@ -155,7 +155,7 @@ bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
     return false;
   }
 
-  FCL_REAL b =
+  CoalScalar b =
       (other.dist_.template head<N / 2>() - dist_.template tail<N / 2>())
           .minCoeff();
   if (b > breakDistance) {
@@ -168,12 +168,12 @@ bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
 }
 
 template <short N>
-bool KDOP<N>::inside(const Vec3f& p) const {
+bool KDOP<N>::inside(const Vec3s& p) const {
   if ((p.array() < dist_.template head<3>()).any()) return false;
   if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false;
 
   enum { P = ((N - 6) / 2) };
-  Eigen::Array<FCL_REAL, P, 1> d;
+  Eigen::Array<CoalScalar, P, 1> d;
   getDistances<P>(p, d.data());
 
   if ((d < dist_.template segment<P>(3)).any()) return false;
@@ -183,12 +183,12 @@ bool KDOP<N>::inside(const Vec3f& p) const {
 }
 
 template <short N>
-KDOP<N>& KDOP<N>::operator+=(const Vec3f& p) {
+KDOP<N>& KDOP<N>::operator+=(const Vec3s& p) {
   for (short i = 0; i < 3; ++i) {
     minmax(p[i], dist_[i], dist_[N / 2 + i]);
   }
 
-  FCL_REAL pd[(N - 6) / 2];
+  CoalScalar pd[(N - 6) / 2];
   getDistances<(N - 6) / 2>(p, pd);
   for (short i = 0; i < (N - 6) / 2; ++i) {
     minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
@@ -213,21 +213,21 @@ KDOP<N> KDOP<N>::operator+(const KDOP<N>& other) const {
 }
 
 template <short N>
-FCL_REAL KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/,
-                           Vec3f* /*Q*/) const {
+CoalScalar KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3s* /*P*/,
+                             Vec3s* /*Q*/) const {
   std::cerr << "KDOP distance not implemented!" << std::endl;
   return 0.0;
 }
 
 template <short N>
-KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t) {
+KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t) {
   KDOP<N> res(bv);
   for (short i = 0; i < 3; ++i) {
     res.dist(i) += t[i];
     res.dist(short(N / 2 + i)) += t[i];
   }
 
-  FCL_REAL d[(N - 6) / 2];
+  CoalScalar d[(N - 6) / 2];
   getDistances<(N - 6) / 2>(t, d);
   for (short i = 0; i < (N - 6) / 2; ++i) {
     res.dist(short(3 + i)) += d[i];
@@ -241,10 +241,8 @@ template class KDOP<16>;
 template class KDOP<18>;
 template class KDOP<24>;
 
-template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&);
-template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&);
-template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&);
-
-}  // namespace fcl
+template KDOP<16> translate<16>(const KDOP<16>&, const Vec3s&);
+template KDOP<18> translate<18>(const KDOP<18>&, const Vec3s&);
+template KDOP<24> translate<24>(const KDOP<24>&, const Vec3s&);
 
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index 72ba3523dfa327eff8364d6d2d16b8e03ed8a811..802496e084261ae9ab9fbbfc86ab28ef03208c15 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -35,21 +35,20 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/BV/kIOS.h>
-#include <hpp/fcl/BVH/BVH_utility.h>
-#include <hpp/fcl/math/transform.h>
+#include "coal/BV/kIOS.h"
+#include "coal/BVH/BVH_utility.h"
+#include "coal/math/transform.h"
 
 #include <iostream>
 #include <limits>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 bool kIOS::overlap(const kIOS& other) const {
   for (unsigned int i = 0; i < num_spheres; ++i) {
     for (unsigned int j = 0; j < other.num_spheres; ++j) {
-      FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
-      FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
+      CoalScalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
+      CoalScalar sum_r = spheres[i].r + other.spheres[j].r;
       if (o_dist > sum_r * sum_r) return false;
     }
   }
@@ -58,11 +57,11 @@ bool kIOS::overlap(const kIOS& other) const {
 }
 
 bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
-                   FCL_REAL& sqrDistLowerBound) const {
+                   CoalScalar& sqrDistLowerBound) const {
   for (unsigned int i = 0; i < num_spheres; ++i) {
     for (unsigned int j = 0; j < other.num_spheres; ++j) {
-      FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
-      FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
+      CoalScalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
+      CoalScalar sum_r = spheres[i].r + other.spheres[j].r;
       if (o_dist > sum_r * sum_r) {
         o_dist = sqrt(o_dist) - sum_r;
         sqrDistLowerBound = o_dist * o_dist;
@@ -74,19 +73,19 @@ bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
   return obb.overlap(other.obb, request, sqrDistLowerBound);
 }
 
-bool kIOS::contain(const Vec3f& p) const {
+bool kIOS::contain(const Vec3s& p) const {
   for (unsigned int i = 0; i < num_spheres; ++i) {
-    FCL_REAL r = spheres[i].r;
+    CoalScalar r = spheres[i].r;
     if ((spheres[i].o - p).squaredNorm() > r * r) return false;
   }
 
   return true;
 }
 
-kIOS& kIOS::operator+=(const Vec3f& p) {
+kIOS& kIOS::operator+=(const Vec3s& p) {
   for (unsigned int i = 0; i < num_spheres; ++i) {
-    FCL_REAL r = spheres[i].r;
-    FCL_REAL new_r_sqr = (p - spheres[i].o).squaredNorm();
+    CoalScalar r = spheres[i].r;
+    CoalScalar new_r_sqr = (p - spheres[i].o).squaredNorm();
     if (new_r_sqr > r * r) {
       spheres[i].r = sqrt(new_r_sqr);
     }
@@ -110,23 +109,23 @@ kIOS kIOS::operator+(const kIOS& other) const {
   return result;
 }
 
-FCL_REAL kIOS::width() const { return obb.width(); }
+CoalScalar kIOS::width() const { return obb.width(); }
 
-FCL_REAL kIOS::height() const { return obb.height(); }
+CoalScalar kIOS::height() const { return obb.height(); }
 
-FCL_REAL kIOS::depth() const { return obb.depth(); }
+CoalScalar kIOS::depth() const { return obb.depth(); }
 
-FCL_REAL kIOS::volume() const { return obb.volume(); }
+CoalScalar kIOS::volume() const { return obb.volume(); }
 
-FCL_REAL kIOS::size() const { return volume(); }
+CoalScalar kIOS::size() const { return volume(); }
 
-FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const {
-  FCL_REAL d_max = 0;
+CoalScalar kIOS::distance(const kIOS& other, Vec3s* P, Vec3s* Q) const {
+  CoalScalar d_max = 0;
   long id_a = -1, id_b = -1;
   for (unsigned int i = 0; i < num_spheres; ++i) {
     for (unsigned int j = 0; j < other.num_spheres; ++j) {
-      FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() -
-                   (spheres[i].r + other.spheres[j].r);
+      CoalScalar d = (spheres[i].o - other.spheres[j].o).norm() -
+                     (spheres[i].r + other.spheres[j].r);
       if (d_max < d) {
         d_max = d;
         if (P && Q) {
@@ -139,8 +138,8 @@ FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const {
 
   if (P && Q) {
     if (id_a != -1 && id_b != -1) {
-      const Vec3f v = spheres[id_a].o - spheres[id_b].o;
-      FCL_REAL len_v = v.norm();
+      const Vec3s v = spheres[id_a].o - spheres[id_b].o;
+      CoalScalar len_v = v.norm();
       *P = spheres[id_a].o - v * (spheres[id_a].r / len_v);
       *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v);
     }
@@ -149,7 +148,7 @@ FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const {
   return d_max;
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
              const kIOS& b2) {
   kIOS b2_temp = b2;
   for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
@@ -163,9 +162,9 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
   return b1.overlap(b2_temp);
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
              const kIOS& b2, const CollisionRequest& request,
-             FCL_REAL& sqrDistLowerBound) {
+             CoalScalar& sqrDistLowerBound) {
   kIOS b2_temp = b2;
   for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
     b2_temp.spheres[i].o.noalias() =
@@ -178,8 +177,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
   return b1.overlap(b2_temp, request, sqrDistLowerBound);
 }
 
-FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
-                  const kIOS& b2, Vec3f* P, Vec3f* Q) {
+CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
+                    const kIOS& b2, Vec3s* P, Vec3s* Q) {
   kIOS b2_temp = b2;
   for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
     b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
@@ -188,7 +187,7 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
   return b1.distance(b2_temp, P, Q);
 }
 
-kIOS translate(const kIOS& bv, const Vec3f& t) {
+kIOS translate(const kIOS& bv, const Vec3s& t) {
   kIOS res(bv);
   for (size_t i = 0; i < res.num_spheres; ++i) {
     res.spheres[i].o += t;
@@ -198,6 +197,4 @@ kIOS translate(const kIOS& bv, const Vec3f& t) {
   return res;
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index 3327fd071dae6df3aae092250ea9a19cdd4444d7..9cfdcab99102f953ebd8324772e7270dc925ad8f 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -36,20 +36,19 @@
 
 /** \author Jia Pan */
 
-#include "hpp/fcl/BV/BV_node.h"
-#include <hpp/fcl/BVH/BVH_model.h>
+#include "coal/BV/BV_node.h"
+#include "coal/BVH/BVH_model.h"
 
-#include <iostream>
-#include <string.h>
+#include "coal/BV/BV.h"
+#include "coal/shape/convex.h"
 
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/shape/convex.h>
+#include "coal/internal/BV_splitter.h"
+#include "coal/internal/BV_fitter.h"
 
-#include <hpp/fcl/internal/BV_splitter.h>
-#include <hpp/fcl/internal/BV_fitter.h>
+#include <iostream>
+#include <string.h>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 BVHModelBase::BVHModelBase()
     : num_tris(0),
@@ -67,7 +66,7 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other)
       num_tris_allocated(other.num_tris),
       num_vertices_allocated(other.num_vertices) {
   if (other.vertices.get() && other.vertices->size() > 0) {
-    vertices.reset(new std::vector<Vec3f>(*(other.vertices)));
+    vertices.reset(new std::vector<Vec3s>(*(other.vertices)));
   } else
     vertices.reset();
 
@@ -77,7 +76,7 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other)
     tri_indices.reset();
 
   if (other.prev_vertices.get() && other.prev_vertices->size() > 0) {
-    prev_vertices.reset(new std::vector<Vec3f>(*(other.prev_vertices)));
+    prev_vertices.reset(new std::vector<Vec3s>(*(other.prev_vertices)));
   } else
     prev_vertices.reset();
 }
@@ -106,8 +105,8 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
       (vertices.get() && !(other.vertices.get())))
     return false;
   if (vertices.get() && other.vertices.get()) {
-    const std::vector<Vec3f>& vertices_ = *(vertices);
-    const std::vector<Vec3f>& other_vertices_ = *(other.vertices);
+    const std::vector<Vec3s>& vertices_ = *(vertices);
+    const std::vector<Vec3s>& other_vertices_ = *(other.vertices);
     for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k)
       if (vertices_[k] != other_vertices_[k]) return false;
   }
@@ -116,8 +115,8 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
       (prev_vertices.get() && !(other.prev_vertices.get())))
     return false;
   if (prev_vertices.get() && other.prev_vertices.get()) {
-    const std::vector<Vec3f>& prev_vertices_ = *(prev_vertices);
-    const std::vector<Vec3f>& other_prev_vertices_ = *(other.prev_vertices);
+    const std::vector<Vec3s>& prev_vertices_ = *(prev_vertices);
+    const std::vector<Vec3s>& other_prev_vertices_ = *(other.prev_vertices);
     for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k) {
       if (prev_vertices_[k] != other_prev_vertices_[k]) return false;
     }
@@ -141,10 +140,10 @@ void BVHModelBase::buildConvexRepresentation(bool share_memory) {
   }
 
   if (!convex) {
-    std::shared_ptr<std::vector<Vec3f>> points = vertices;
+    std::shared_ptr<std::vector<Vec3s>> points = vertices;
     std::shared_ptr<std::vector<Triangle>> polygons = tri_indices;
     if (!share_memory) {
-      points.reset(new std::vector<Vec3f>(*(vertices)));
+      points.reset(new std::vector<Vec3s>(*(vertices)));
       polygons.reset(new std::vector<Triangle>(*(tri_indices)));
     }
     convex.reset(
@@ -207,7 +206,7 @@ int BVHModelBase::beginModel(unsigned int num_tris_,
     tri_indices.reset();
 
   if (num_vertices_allocated > 0) {
-    vertices.reset(new std::vector<Vec3f>(num_vertices_allocated));
+    vertices.reset(new std::vector<Vec3s>(num_vertices_allocated));
     if (!(vertices.get())) {
       std::cerr
           << "BVH Error! Out of memory for vertices array on BeginModel() call!"
@@ -233,7 +232,7 @@ int BVHModelBase::beginModel(unsigned int num_tris_,
   return BVH_OK;
 }
 
-int BVHModelBase::addVertex(const Vec3f& p) {
+int BVHModelBase::addVertex(const Vec3s& p) {
   if (build_state != BVH_BUILD_STATE_BEGUN) {
     std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() "
                  "was ignored. Must do a beginModel() to clear the model for "
@@ -243,8 +242,8 @@ int BVHModelBase::addVertex(const Vec3f& p) {
   }
 
   if (num_vertices >= num_vertices_allocated) {
-    std::shared_ptr<std::vector<Vec3f>> temp(
-        new std::vector<Vec3f>(num_vertices_allocated * 2));
+    std::shared_ptr<std::vector<Vec3s>> temp(
+        new std::vector<Vec3s>(num_vertices_allocated * 2));
     if (!(temp.get())) {
       std::cerr
           << "BVH Error! Out of memory for vertices array on addVertex() call!"
@@ -305,7 +304,7 @@ int BVHModelBase::addTriangles(const Matrixx3i& triangles) {
   return BVH_OK;
 }
 
-int BVHModelBase::addVertices(const Matrixx3f& points) {
+int BVHModelBase::addVertices(const MatrixX3s& points) {
   if (build_state != BVH_BUILD_STATE_BEGUN) {
     std::cerr << "BVH Warning! Call addVertex() in a wrong order. "
                  "addVertices() was ignored. Must do a beginModel() to clear "
@@ -316,8 +315,8 @@ int BVHModelBase::addVertices(const Matrixx3f& points) {
 
   if (num_vertices + points.rows() > num_vertices_allocated) {
     num_vertices_allocated = num_vertices + (unsigned int)points.rows();
-    std::shared_ptr<std::vector<Vec3f>> temp(
-        new std::vector<Vec3f>(num_vertices_allocated));
+    std::shared_ptr<std::vector<Vec3s>> temp(
+        new std::vector<Vec3s>(num_vertices_allocated));
     if (!(temp.get())) {
       std::cerr
           << "BVH Error! Out of memory for vertices array on addVertex() call!"
@@ -331,15 +330,15 @@ int BVHModelBase::addVertices(const Matrixx3f& points) {
     vertices = temp;
   }
 
-  std::vector<Vec3f>& vertices_ = *vertices;
+  std::vector<Vec3s>& vertices_ = *vertices;
   for (Eigen::DenseIndex id = 0; id < points.rows(); ++id)
     vertices_[num_vertices++] = points.row(id).transpose();
 
   return BVH_OK;
 }
 
-int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
-                              const Vec3f& p3) {
+int BVHModelBase::addTriangle(const Vec3s& p1, const Vec3s& p2,
+                              const Vec3s& p3) {
   if (build_state == BVH_BUILD_STATE_PROCESSED) {
     std::cerr << "BVH Warning! Call addTriangle() in a wrong order. "
                  "addTriangle() was ignored. Must do a beginModel() to clear "
@@ -349,8 +348,8 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
   }
 
   if (num_vertices + 2 >= num_vertices_allocated) {
-    std::shared_ptr<std::vector<Vec3f>> temp(
-        new std::vector<Vec3f>(num_vertices_allocated * 2 + 2));
+    std::shared_ptr<std::vector<Vec3s>> temp(
+        new std::vector<Vec3s>(num_vertices_allocated * 2 + 2));
     if (!(temp.get())) {
       std::cerr << "BVH Error! Out of memory for vertices array on "
                    "addTriangle() call!"
@@ -399,7 +398,7 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
   return BVH_OK;
 }
 
-int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
+int BVHModelBase::addSubModel(const std::vector<Vec3s>& ps) {
   if (build_state == BVH_BUILD_STATE_PROCESSED) {
     std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. "
                  "addSubModel() was ignored. Must do a beginModel() to clear "
@@ -411,7 +410,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
   const unsigned int num_vertices_to_add = (unsigned int)ps.size();
 
   if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
-    std::shared_ptr<std::vector<Vec3f>> temp(new std::vector<Vec3f>(
+    std::shared_ptr<std::vector<Vec3s>> temp(new std::vector<Vec3s>(
         num_vertices_allocated * 2 + num_vertices_to_add - 1));
     if (!(temp.get())) {
       std::cerr << "BVH Error! Out of memory for vertices array on "
@@ -428,7 +427,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
         num_vertices_allocated * 2 + num_vertices_to_add - 1;
   }
 
-  std::vector<Vec3f>& vertices_ = *vertices;
+  std::vector<Vec3s>& vertices_ = *vertices;
   for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
     vertices_[num_vertices] = ps[i];
     num_vertices++;
@@ -437,7 +436,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
   return BVH_OK;
 }
 
-int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
+int BVHModelBase::addSubModel(const std::vector<Vec3s>& ps,
                               const std::vector<Triangle>& ts) {
   if (build_state == BVH_BUILD_STATE_PROCESSED) {
     std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. "
@@ -450,7 +449,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
   const unsigned int num_vertices_to_add = (unsigned int)ps.size();
 
   if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
-    std::shared_ptr<std::vector<Vec3f>> temp(new std::vector<Vec3f>(
+    std::shared_ptr<std::vector<Vec3s>> temp(new std::vector<Vec3s>(
         num_vertices_allocated * 2 + num_vertices_to_add - 1));
     if (!(temp.get())) {
       std::cerr << "BVH Error! Out of memory for vertices array on "
@@ -469,7 +468,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
 
   const unsigned int offset = num_vertices;
 
-  std::vector<Vec3f>& vertices_ = *vertices;
+  std::vector<Vec3s>& vertices_ = *vertices;
   for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
     vertices_[num_vertices] = ps[i];
     num_vertices++;
@@ -544,8 +543,8 @@ int BVHModelBase::endModel() {
 
   if (num_vertices_allocated > num_vertices) {
     if (num_vertices > 0) {
-      std::shared_ptr<std::vector<Vec3f>> new_vertices(
-          new std::vector<Vec3f>(num_vertices));
+      std::shared_ptr<std::vector<Vec3s>> new_vertices(
+          new std::vector<Vec3s>(num_vertices));
       if (!(new_vertices.get())) {
         std::cerr
             << "BVH Error! Out of memory for vertices array in endModel() call!"
@@ -592,7 +591,7 @@ int BVHModelBase::beginReplaceModel() {
   return BVH_OK;
 }
 
-int BVHModelBase::replaceVertex(const Vec3f& p) {
+int BVHModelBase::replaceVertex(const Vec3s& p) {
   if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
     std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. "
                  "replaceVertex() was ignored. Must do a beginReplaceModel() "
@@ -607,8 +606,8 @@ int BVHModelBase::replaceVertex(const Vec3f& p) {
   return BVH_OK;
 }
 
-int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2,
-                                  const Vec3f& p3) {
+int BVHModelBase::replaceTriangle(const Vec3s& p1, const Vec3s& p2,
+                                  const Vec3s& p3) {
   if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
     std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. "
                  "replaceTriangle() was ignored. Must do a beginReplaceModel() "
@@ -626,7 +625,7 @@ int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2,
   return BVH_OK;
 }
 
-int BVHModelBase::replaceSubModel(const std::vector<Vec3f>& ps) {
+int BVHModelBase::replaceSubModel(const std::vector<Vec3s>& ps) {
   if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
     std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. "
                  "replaceSubModel() was ignored. Must do a beginReplaceModel() "
@@ -635,7 +634,7 @@ int BVHModelBase::replaceSubModel(const std::vector<Vec3f>& ps) {
     return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
   }
 
-  std::vector<Vec3f>& vertices_ = *vertices;
+  std::vector<Vec3s>& vertices_ = *vertices;
   for (unsigned int i = 0; i < ps.size(); ++i) {
     vertices_[num_vertex_updated] = ps[i];
     num_vertex_updated++;
@@ -681,12 +680,12 @@ int BVHModelBase::beginUpdateModel() {
   }
 
   if (prev_vertices.get()) {
-    std::shared_ptr<std::vector<Vec3f>> temp = prev_vertices;
+    std::shared_ptr<std::vector<Vec3s>> temp = prev_vertices;
     prev_vertices = vertices;
     vertices = temp;
   } else {
     prev_vertices = vertices;
-    vertices.reset(new std::vector<Vec3f>(num_vertices));
+    vertices.reset(new std::vector<Vec3s>(num_vertices));
   }
 
   num_vertex_updated = 0;
@@ -696,7 +695,7 @@ int BVHModelBase::beginUpdateModel() {
   return BVH_OK;
 }
 
-int BVHModelBase::updateVertex(const Vec3f& p) {
+int BVHModelBase::updateVertex(const Vec3s& p) {
   if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
     std::cerr
         << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() "
@@ -711,8 +710,8 @@ int BVHModelBase::updateVertex(const Vec3f& p) {
   return BVH_OK;
 }
 
-int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2,
-                                 const Vec3f& p3) {
+int BVHModelBase::updateTriangle(const Vec3s& p1, const Vec3s& p2,
+                                 const Vec3s& p3) {
   if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
     std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. "
                  "updateTriangle() was ignored. Must do a beginUpdateModel() "
@@ -730,7 +729,7 @@ int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2,
   return BVH_OK;
 }
 
-int BVHModelBase::updateSubModel(const std::vector<Vec3f>& ps) {
+int BVHModelBase::updateSubModel(const std::vector<Vec3s>& ps) {
   if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
     std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. "
                  "updateSubModel() was ignored. Must do a beginUpdateModel() "
@@ -739,7 +738,7 @@ int BVHModelBase::updateSubModel(const std::vector<Vec3f>& ps) {
     return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
   }
 
-  std::vector<Vec3f>& vertices_ = *vertices;
+  std::vector<Vec3s>& vertices_ = *vertices;
   for (unsigned int i = 0; i < ps.size(); ++i) {
     vertices_[num_vertex_updated] = ps[i];
     num_vertex_updated++;
@@ -781,7 +780,7 @@ int BVHModelBase::endUpdateModel(bool refit, bool bottomup) {
 
 void BVHModelBase::computeLocalAABB() {
   AABB aabb_;
-  const std::vector<Vec3f>& vertices_ = *vertices;
+  const std::vector<Vec3s>& vertices_ = *vertices;
   for (unsigned int i = 0; i < num_vertices; ++i) {
     aabb_ += vertices_[i];
   }
@@ -790,7 +789,7 @@ void BVHModelBase::computeLocalAABB() {
 
   aabb_radius = 0;
   for (unsigned int i = 0; i < num_vertices; ++i) {
-    FCL_REAL r = (aabb_center - vertices_[i]).squaredNorm();
+    CoalScalar r = (aabb_center - vertices_[i]).squaredNorm();
     if (r > aabb_radius) aabb_radius = r;
   }
 
@@ -841,7 +840,7 @@ template <typename BV>
 int BVHModel<BV>::memUsage(const bool msg) const {
   unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs;
   unsigned int mem_tri_list = (unsigned int)sizeof(Triangle) * num_tris;
-  unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3f) * num_vertices;
+  unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3s) * num_vertices;
 
   unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list +
                            (unsigned int)sizeof(BVHModel<BV>);
@@ -858,7 +857,7 @@ int BVHModel<BV>::memUsage(const bool msg) const {
 template <typename BV>
 int BVHModel<BV>::buildTree() {
   // set BVFitter
-  Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL;
+  Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL;
   Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
   bv_fitter->set(vertices_, tri_indices_, getModelType());
   // set SplitRule
@@ -912,17 +911,17 @@ int BVHModel<BV>::recursiveBuildTree(int bv_id, unsigned int first_primitive,
     num_bvs += 2;
 
     unsigned int c1 = 0;
-    const std::vector<Vec3f>& vertices_ = *vertices;
+    const std::vector<Vec3s>& vertices_ = *vertices;
     const std::vector<Triangle>& tri_indices_ = *tri_indices;
     for (unsigned int i = 0; i < num_primitives; ++i) {
-      Vec3f p;
+      Vec3s p;
       if (type == BVH_MODEL_POINTCLOUD)
         p = vertices_[cur_primitive_indices[i]];
       else if (type == BVH_MODEL_TRIANGLES) {
         const Triangle& t = tri_indices_[cur_primitive_indices[i]];
-        const Vec3f& p1 = vertices_[t[0]];
-        const Vec3f& p2 = vertices_[t[1]];
-        const Vec3f& p3 = vertices_[t[2]];
+        const Vec3s& p1 = vertices_[t[0]];
+        const Vec3s& p2 = vertices_[t[1]];
+        const Vec3s& p3 = vertices_[t[2]];
 
         p = (p1 + p2 + p3) / 3.;
       } else {
@@ -990,7 +989,7 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
       BV bv;
 
       if (prev_vertices.get()) {
-        Vec3f v[2];
+        Vec3s v[2];
         v[0] = (*prev_vertices)[static_cast<size_t>(primitive_id)];
         v[1] = (*vertices)[static_cast<size_t>(primitive_id)];
         fit(v, 2, bv);
@@ -1004,7 +1003,7 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
           (*tri_indices)[static_cast<size_t>(primitive_id)];
 
       if (prev_vertices.get()) {
-        Vec3f v[6];
+        Vec3s v[6];
         for (Triangle::index_type i = 0; i < 3; ++i) {
           v[i] = (*prev_vertices)[triangle[i]];
           v[i + 3] = (*vertices)[triangle[i]];
@@ -1016,7 +1015,7 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
         // unsigned int* cur_primitive_indices = primitive_indices +
         // bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices,
         // bvnode->num_primitives);
-        Vec3f v[3];
+        Vec3s v[3];
         for (int i = 0; i < 3; ++i) {
           v[i] = (*vertices)[triangle[(Triangle::index_type)i]];
         }
@@ -1045,8 +1044,8 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
 
 template <typename BV>
 int BVHModel<BV>::refitTree_topdown() {
-  Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL;
-  Vec3f* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL;
+  Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL;
+  Vec3s* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL;
   Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
   bv_fitter->set(vertices_, prev_vertices_, tri_indices_, getModelType());
   BVNode<BV>* bvs_ = bvs->data();
@@ -1063,8 +1062,8 @@ int BVHModel<BV>::refitTree_topdown() {
 }
 
 template <>
-void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
-                                              const Vec3f& parent_c) {
+void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+                                              const Vec3s& parent_c) {
   bv_node_vector_t& bvs_ = *bvs;
   OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv;
   if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
@@ -1079,13 +1078,13 @@ void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
   // obb.axes = parent_axes.transpose() * obb.axes;
   obb.axes.applyOnTheLeft(parent_axes.transpose());
 
-  Vec3f t(obb.To - parent_c);
+  Vec3s t(obb.To - parent_c);
   obb.To.noalias() = parent_axes.transpose() * t;
 }
 
 template <>
-void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
-                                              const Vec3f& parent_c) {
+void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+                                              const Vec3s& parent_c) {
   bv_node_vector_t& bvs_ = *bvs;
   RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv;
   if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
@@ -1100,14 +1099,14 @@ void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
   // rss.axes = parent_axes.transpose() * rss.axes;
   rss.axes.applyOnTheLeft(parent_axes.transpose());
 
-  Vec3f t(rss.Tr - parent_c);
+  Vec3s t(rss.Tr - parent_c);
   rss.Tr.noalias() = parent_axes.transpose() * t;
 }
 
 template <>
 void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
-                                                 Matrix3f& parent_axes,
-                                                 const Vec3f& parent_c) {
+                                                 Matrix3s& parent_axes,
+                                                 const Vec3s& parent_c) {
   bv_node_vector_t& bvs_ = *bvs;
   OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv.obb;
   RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv.rss;
@@ -1123,7 +1122,7 @@ void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
   rss.axes.noalias() = parent_axes.transpose() * obb.axes;
   obb.axes = rss.axes;
 
-  Vec3f t(obb.To - parent_c);
+  Vec3s t(obb.To - parent_c);
   obb.To.noalias() = parent_axes.transpose() * t;
   rss.Tr = obb.To;
 }
@@ -1177,6 +1176,4 @@ template class BVHModel<RSS>;
 template class BVHModel<kIOS>;
 template class BVHModel<OBBRSS>;
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index 6f644e307bedcf42729a17031db2a6083b3b3f81..d3a74960c0537b52a8df6d82d2fa88c2935695d9 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -35,23 +35,22 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/BVH/BVH_utility.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/BVH/BVH_utility.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/shape_shape_func.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace details {
 template <typename BV>
-BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
+BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3s& pose,
                          const AABB& _aabb) {
   assert(model.getModelType() == BVH_MODEL_TRIANGLES);
-  const Matrix3f& q = pose.getRotation();
+  const Matrix3s& q = pose.getRotation();
   AABB aabb = translate(_aabb, -pose.getTranslation());
 
-  Transform3f box_pose;
+  Transform3s box_pose;
   Box box;
   constructBox(_aabb, box, box_pose);
   box_pose = pose.inverseTimes(box_pose);
@@ -65,7 +64,7 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
   std::vector<bool> keep_vertex(model.num_vertices, false);
   std::vector<bool> keep_tri(model.num_tris, false);
   unsigned int ntri = 0;
-  const std::vector<Vec3f>& model_vertices_ = *(model.vertices);
+  const std::vector<Vec3s>& model_vertices_ = *(model.vertices);
   const std::vector<Triangle>& model_tri_indices_ = *(model.tri_indices);
   for (unsigned int i = 0; i < model.num_tris; ++i) {
     const Triangle& t = model_tri_indices_[i];
@@ -89,8 +88,8 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
       const DistanceRequest distanceRequest(enable_nearest_points,
                                             compute_penetration);
       DistanceResult distanceResult;
-      const FCL_REAL distance = ShapeShapeDistance<Box, TriangleP>(
-          &box, box_pose, &tri, Transform3f(), &gjk, distanceRequest,
+      const CoalScalar distance = ShapeShapeDistance<Box, TriangleP>(
+          &box, box_pose, &tri, Transform3s(), &gjk, distanceRequest,
           distanceResult);
       bool is_collision =
           (distance <= gjk.getDistancePrecision(compute_penetration));
@@ -112,7 +111,7 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
   new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices));
   std::vector<unsigned int> idxConversion(model.num_vertices);
   assert(new_model->num_vertices == 0);
-  std::vector<Vec3f>& new_model_vertices_ = *(new_model->vertices);
+  std::vector<Vec3s>& new_model_vertices_ = *(new_model->vertices);
   for (unsigned int i = 0; i < keep_vertex.size(); ++i) {
     if (keep_vertex[i]) {
       idxConversion[i] = new_model->num_vertices;
@@ -140,58 +139,58 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
 }  // namespace details
 
 template <>
-BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model, const Transform3f& pose,
+BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model, const Transform3s& pose,
                           const AABB& aabb) {
   return details::BVHExtract(model, pose, aabb);
 }
 template <>
-BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model, const Transform3f& pose,
+BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model, const Transform3s& pose,
                            const AABB& aabb) {
   return details::BVHExtract(model, pose, aabb);
 }
 template <>
-BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model, const Transform3f& pose,
+BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model, const Transform3s& pose,
                           const AABB& aabb) {
   return details::BVHExtract(model, pose, aabb);
 }
 template <>
-BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model, const Transform3f& pose,
+BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model, const Transform3s& pose,
                            const AABB& aabb) {
   return details::BVHExtract(model, pose, aabb);
 }
 template <>
 BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model,
-                             const Transform3f& pose, const AABB& aabb) {
+                             const Transform3s& pose, const AABB& aabb) {
   return details::BVHExtract(model, pose, aabb);
 }
 template <>
 BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model,
-                                const Transform3f& pose, const AABB& aabb) {
+                                const Transform3s& pose, const AABB& aabb) {
   return details::BVHExtract(model, pose, aabb);
 }
 template <>
 BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model,
-                                const Transform3f& pose, const AABB& aabb) {
+                                const Transform3s& pose, const AABB& aabb) {
   return details::BVHExtract(model, pose, aabb);
 }
 template <>
 BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
-                                const Transform3f& pose, const AABB& aabb) {
+                                const Transform3s& pose, const AABB& aabb) {
   return details::BVHExtract(model, pose, aabb);
 }
 
-void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
-                   unsigned int n, Matrix3f& M) {
-  Vec3f S1(Vec3f::Zero());
-  Vec3f S2[3] = {Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero()};
+void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices,
+                   unsigned int n, Matrix3s& M) {
+  Vec3s S1(Vec3s::Zero());
+  Vec3s S2[3] = {Vec3s::Zero(), Vec3s::Zero(), Vec3s::Zero()};
 
   if (ts) {
     for (unsigned int i = 0; i < n; ++i) {
       const Triangle& t = (indices) ? ts[indices[i]] : ts[i];
 
-      const Vec3f& p1 = ps[t[0]];
-      const Vec3f& p2 = ps[t[1]];
-      const Vec3f& p3 = ps[t[2]];
+      const Vec3s& p1 = ps[t[0]];
+      const Vec3s& p2 = ps[t[1]];
+      const Vec3s& p3 = ps[t[2]];
 
       S1[0] += (p1[0] + p2[0] + p3[0]);
       S1[1] += (p1[1] + p2[1] + p3[1]);
@@ -204,9 +203,9 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
       S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
 
       if (ps2) {
-        const Vec3f& p1 = ps2[t[0]];
-        const Vec3f& p2 = ps2[t[1]];
-        const Vec3f& p3 = ps2[t[2]];
+        const Vec3s& p1 = ps2[t[0]];
+        const Vec3s& p2 = ps2[t[1]];
+        const Vec3s& p3 = ps2[t[2]];
 
         S1[0] += (p1[0] + p2[0] + p3[0]);
         S1[1] += (p1[1] + p2[1] + p3[1]);
@@ -222,7 +221,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
     }
   } else {
     for (unsigned int i = 0; i < n; ++i) {
-      const Vec3f& p = (indices) ? ps[indices[i]] : ps[i];
+      const Vec3s& p = (indices) ? ps[indices[i]] : ps[i];
       S1 += p;
       S2[0][0] += (p[0] * p[0]);
       S2[1][1] += (p[1] * p[1]);
@@ -233,7 +232,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
 
       if (ps2)  // another frame
       {
-        const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i];
+        const Vec3s& p = (indices) ? ps2[indices[i]] : ps2[i];
         S1 += p;
         S2[0][0] += (p[0] * p[0]);
         S2[1][1] += (p[1] * p[1]);
@@ -261,16 +260,16 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
 /** @brief Compute the RSS bounding volume parameters: radius, rectangle size
  * and the origin. The bounding volume axes are known.
  */
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
+void getRadiusAndOriginAndRectangleSize(Vec3s* ps, Vec3s* ps2, Triangle* ts,
                                         unsigned int* indices, unsigned int n,
-                                        const Matrix3f& axes, Vec3f& origin,
-                                        FCL_REAL l[2], FCL_REAL& r) {
+                                        const Matrix3s& axes, Vec3s& origin,
+                                        CoalScalar l[2], CoalScalar& r) {
   bool indirect_index = true;
   if (!indices) indirect_index = false;
 
   unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
 
-  FCL_REAL(*P)[3] = new FCL_REAL[size_P][3];
+  CoalScalar(*P)[3] = new CoalScalar[size_P][3];
 
   int P_id = 0;
 
@@ -281,8 +280,8 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
 
       for (Triangle::index_type j = 0; j < 3; ++j) {
         Triangle::index_type point_id = t[j];
-        const Vec3f& p = ps[point_id];
-        Vec3f v(p[0], p[1], p[2]);
+        const Vec3s& p = ps[point_id];
+        Vec3s v(p[0], p[1], p[2]);
         P[P_id][0] = axes.col(0).dot(v);
         P[P_id][1] = axes.col(1).dot(v);
         P[P_id][2] = axes.col(2).dot(v);
@@ -292,9 +291,9 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
       if (ps2) {
         for (Triangle::index_type j = 0; j < 3; ++j) {
           Triangle::index_type point_id = t[j];
-          const Vec3f& p = ps2[point_id];
+          const Vec3s& p = ps2[point_id];
           // FIXME Is this right ?????
-          Vec3f v(p[0], p[1], p[2]);
+          Vec3s v(p[0], p[1], p[2]);
           P[P_id][0] = axes.col(0).dot(v);
           P[P_id][1] = axes.col(0).dot(v);
           P[P_id][2] = axes.col(1).dot(v);
@@ -306,15 +305,15 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     for (unsigned int i = 0; i < n; ++i) {
       unsigned int index = indirect_index ? indices[i] : i;
 
-      const Vec3f& p = ps[index];
-      Vec3f v(p[0], p[1], p[2]);
+      const Vec3s& p = ps[index];
+      Vec3s v(p[0], p[1], p[2]);
       P[P_id][0] = axes.col(0).dot(v);
       P[P_id][1] = axes.col(1).dot(v);
       P[P_id][2] = axes.col(2).dot(v);
       P_id++;
 
       if (ps2) {
-        const Vec3f& v = ps2[index];
+        const Vec3s& v = ps2[index];
         P[P_id][0] = axes.col(0).dot(v);
         P[P_id][1] = axes.col(1).dot(v);
         P[P_id][2] = axes.col(2).dot(v);
@@ -323,34 +322,34 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     }
   }
 
-  FCL_REAL minx, maxx, miny, maxy, minz, maxz;
+  CoalScalar minx, maxx, miny, maxy, minz, maxz;
 
-  FCL_REAL cz, radsqr;
+  CoalScalar cz, radsqr;
 
   minz = maxz = P[0][2];
 
   for (unsigned int i = 1; i < size_P; ++i) {
-    FCL_REAL z_value = P[i][2];
+    CoalScalar z_value = P[i][2];
     if (z_value < minz)
       minz = z_value;
     else if (z_value > maxz)
       maxz = z_value;
   }
 
-  r = (FCL_REAL)0.5 * (maxz - minz);
+  r = (CoalScalar)0.5 * (maxz - minz);
   radsqr = r * r;
-  cz = (FCL_REAL)0.5 * (maxz + minz);
+  cz = (CoalScalar)0.5 * (maxz + minz);
 
   // compute an initial norm of rectangle along x direction
 
   // find minx and maxx as starting points
 
   unsigned int minindex = 0, maxindex = 0;
-  FCL_REAL mintmp, maxtmp;
+  CoalScalar mintmp, maxtmp;
   mintmp = maxtmp = P[0][0];
 
   for (unsigned int i = 1; i < size_P; ++i) {
-    FCL_REAL x_value = P[i][0];
+    CoalScalar x_value = P[i][0];
     if (x_value < mintmp) {
       minindex = i;
       mintmp = x_value;
@@ -360,22 +359,22 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     }
   }
 
-  FCL_REAL x, dz;
+  CoalScalar x, dz;
   dz = P[minindex][2] - cz;
-  minx = P[minindex][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+  minx = P[minindex][0] + std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
   dz = P[maxindex][2] - cz;
-  maxx = P[maxindex][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+  maxx = P[maxindex][0] - std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
 
   // grow minx/maxx
 
   for (unsigned int i = 0; i < size_P; ++i) {
     if (P[i][0] < minx) {
       dz = P[i][2] - cz;
-      x = P[i][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+      x = P[i][0] + std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
       if (x < minx) minx = x;
     } else if (P[i][0] > maxx) {
       dz = P[i][2] - cz;
-      x = P[i][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+      x = P[i][0] - std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
       if (x > maxx) maxx = x;
     }
   }
@@ -387,7 +386,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
   minindex = maxindex = 0;
   mintmp = maxtmp = P[0][1];
   for (unsigned int i = 1; i < size_P; ++i) {
-    FCL_REAL y_value = P[i][1];
+    CoalScalar y_value = P[i][1];
     if (y_value < mintmp) {
       minindex = i;
       mintmp = y_value;
@@ -397,30 +396,30 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     }
   }
 
-  FCL_REAL y;
+  CoalScalar y;
   dz = P[minindex][2] - cz;
-  miny = P[minindex][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+  miny = P[minindex][1] + std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
   dz = P[maxindex][2] - cz;
-  maxy = P[maxindex][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+  maxy = P[maxindex][1] - std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
 
   // grow miny/maxy
 
   for (unsigned int i = 0; i < size_P; ++i) {
     if (P[i][1] < miny) {
       dz = P[i][2] - cz;
-      y = P[i][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+      y = P[i][1] + std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
       if (y < miny) miny = y;
     } else if (P[i][1] > maxy) {
       dz = P[i][2] - cz;
-      y = P[i][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+      y = P[i][1] - std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
       if (y > maxy) maxy = y;
     }
   }
 
   // corners may have some points which are not covered - grow lengths if
   // necessary quite conservative (can be improved)
-  FCL_REAL dx, dy, u, t;
-  FCL_REAL a = std::sqrt((FCL_REAL)0.5);
+  CoalScalar dx, dy, u, t;
+  CoalScalar a = std::sqrt((CoalScalar)0.5);
   for (unsigned int i = 0; i < size_P; ++i) {
     if (P[i][0] > maxx) {
       if (P[i][1] > maxy) {
@@ -429,7 +428,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
         u = dx * a + dy * a;
         t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) +
             (cz - P[i][2]) * (cz - P[i][2]);
-        u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
+        u = u - std::sqrt(std::max<CoalScalar>(radsqr - t, 0));
         if (u > 0) {
           maxx += u * a;
           maxy += u * a;
@@ -440,7 +439,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
         u = dx * a - dy * a;
         t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) +
             (cz - P[i][2]) * (cz - P[i][2]);
-        u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
+        u = u - std::sqrt(std::max<CoalScalar>(radsqr - t, 0));
         if (u > 0) {
           maxx += u * a;
           miny -= u * a;
@@ -453,7 +452,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
         u = dy * a - dx * a;
         t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) +
             (cz - P[i][2]) * (cz - P[i][2]);
-        u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
+        u = u - std::sqrt(std::max<CoalScalar>(radsqr - t, 0));
         if (u > 0) {
           minx -= u * a;
           maxy += u * a;
@@ -464,7 +463,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
         u = -dx * a - dy * a;
         t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) +
             (cz - P[i][2]) * (cz - P[i][2]);
-        u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
+        u = u - std::sqrt(std::max<CoalScalar>(radsqr - t, 0));
         if (u > 0) {
           minx -= u * a;
           miny -= u * a;
@@ -473,10 +472,10 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     }
   }
 
-  origin.noalias() = axes * Vec3f(minx, miny, cz);
+  origin.noalias() = axes * Vec3s(minx, miny, cz);
 
-  l[0] = std::max<FCL_REAL>(maxx - minx, 0);
-  l[1] = std::max<FCL_REAL>(maxy - miny, 0);
+  l[0] = std::max<CoalScalar>(maxx - minx, 0);
+  l[1] = std::max<CoalScalar>(maxy - miny, 0);
 
   delete[] P;
 }
@@ -484,23 +483,23 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
 /** @brief Compute the bounding volume extent and center for a set or subset of
  * points. The bounding volume axes are known.
  */
-static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2,
+static inline void getExtentAndCenter_pointcloud(Vec3s* ps, Vec3s* ps2,
                                                  unsigned int* indices,
-                                                 unsigned int n, Matrix3f& axes,
-                                                 Vec3f& center, Vec3f& extent) {
+                                                 unsigned int n, Matrix3s& axes,
+                                                 Vec3s& center, Vec3s& extent) {
   bool indirect_index = true;
   if (!indices) indirect_index = false;
 
-  FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
 
-  Vec3f min_coord(real_max, real_max, real_max);
-  Vec3f max_coord(-real_max, -real_max, -real_max);
+  Vec3s min_coord(real_max, real_max, real_max);
+  Vec3s max_coord(-real_max, -real_max, -real_max);
 
   for (unsigned int i = 0; i < n; ++i) {
     unsigned int index = indirect_index ? indices[i] : i;
 
-    const Vec3f& p = ps[index];
-    Vec3f proj(axes.transpose() * p);
+    const Vec3s& p = ps[index];
+    Vec3s proj(axes.transpose() * p);
 
     for (int j = 0; j < 3; ++j) {
       if (proj[j] > max_coord[j]) max_coord[j] = proj[j];
@@ -508,7 +507,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2,
     }
 
     if (ps2) {
-      const Vec3f& v = ps2[index];
+      const Vec3s& v = ps2[index];
       proj.noalias() = axes.transpose() * v;
 
       for (int j = 0; j < 3; ++j) {
@@ -526,17 +525,17 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2,
 /** @brief Compute the bounding volume extent and center for a set or subset of
  * points. The bounding volume axes are known.
  */
-static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
+static inline void getExtentAndCenter_mesh(Vec3s* ps, Vec3s* ps2, Triangle* ts,
                                            unsigned int* indices,
-                                           unsigned int n, Matrix3f& axes,
-                                           Vec3f& center, Vec3f& extent) {
+                                           unsigned int n, Matrix3s& axes,
+                                           Vec3s& center, Vec3s& extent) {
   bool indirect_index = true;
   if (!indices) indirect_index = false;
 
-  FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
 
-  Vec3f min_coord(real_max, real_max, real_max);
-  Vec3f max_coord(-real_max, -real_max, -real_max);
+  Vec3s min_coord(real_max, real_max, real_max);
+  Vec3s max_coord(-real_max, -real_max, -real_max);
 
   for (unsigned int i = 0; i < n; ++i) {
     unsigned int index = indirect_index ? indices[i] : i;
@@ -544,8 +543,8 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
 
     for (Triangle::index_type j = 0; j < 3; ++j) {
       Triangle::index_type point_id = t[j];
-      const Vec3f& p = ps[point_id];
-      Vec3f proj(axes.transpose() * p);
+      const Vec3s& p = ps[point_id];
+      Vec3s proj(axes.transpose() * p);
 
       for (int k = 0; k < 3; ++k) {
         if (proj[k] > max_coord[k]) max_coord[k] = proj[k];
@@ -556,8 +555,8 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     if (ps2) {
       for (Triangle::index_type j = 0; j < 3; ++j) {
         Triangle::index_type point_id = t[j];
-        const Vec3f& p = ps2[point_id];
-        Vec3f proj(axes.transpose() * p);
+        const Vec3s& p = ps2[point_id];
+        Vec3s proj(axes.transpose() * p);
 
         for (int k = 0; k < 3; ++k) {
           if (proj[k] > max_coord[k]) max_coord[k] = proj[k];
@@ -567,62 +566,63 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     }
   }
 
-  Vec3f o((max_coord + min_coord) / 2);
+  Vec3s o((max_coord + min_coord) / 2);
 
   center.noalias() = axes * o;
 
   extent.noalias() = (max_coord - min_coord) / 2;
 }
 
-void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts,
-                        unsigned int* indices, unsigned int n, Matrix3f& axes,
-                        Vec3f& center, Vec3f& extent) {
+void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+                        unsigned int* indices, unsigned int n, Matrix3s& axes,
+                        Vec3s& center, Vec3s& extent) {
   if (ts)
     getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent);
   else
     getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent);
 }
 
-void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c,
-                             Vec3f& center, FCL_REAL& radius) {
-  Vec3f e1 = a - c;
-  Vec3f e2 = b - c;
-  FCL_REAL e1_len2 = e1.squaredNorm();
-  FCL_REAL e2_len2 = e2.squaredNorm();
-  Vec3f e3 = e1.cross(e2);
-  FCL_REAL e3_len2 = e3.squaredNorm();
+void circumCircleComputation(const Vec3s& a, const Vec3s& b, const Vec3s& c,
+                             Vec3s& center, CoalScalar& radius) {
+  Vec3s e1 = a - c;
+  Vec3s e2 = b - c;
+  CoalScalar e1_len2 = e1.squaredNorm();
+  CoalScalar e2_len2 = e2.squaredNorm();
+  Vec3s e3 = e1.cross(e2);
+  CoalScalar e3_len2 = e3.squaredNorm();
   radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2;
   radius = std::sqrt(radius) * 0.5;
 
   center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
 }
 
-static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
-                                            unsigned int* indices,
-                                            unsigned int n,
-                                            const Vec3f& query) {
+static inline CoalScalar maximumDistance_mesh(Vec3s* ps, Vec3s* ps2,
+                                              Triangle* ts,
+                                              unsigned int* indices,
+                                              unsigned int n,
+                                              const Vec3s& query) {
   bool indirect_index = true;
   if (!indices) indirect_index = false;
 
-  FCL_REAL maxD = 0;
+  CoalScalar maxD = 0;
   for (unsigned int i = 0; i < n; ++i) {
     unsigned int index = indirect_index ? indices[i] : i;
     const Triangle& t = ts[index];
 
     for (Triangle::index_type j = 0; j < 3; ++j) {
       Triangle::index_type point_id = t[j];
-      const Vec3f& p = ps[point_id];
+      const Vec3s& p = ps[point_id];
 
-      FCL_REAL d = (p - query).squaredNorm();
+      CoalScalar d = (p - query).squaredNorm();
       if (d > maxD) maxD = d;
     }
 
     if (ps2) {
       for (Triangle::index_type j = 0; j < 3; ++j) {
         Triangle::index_type point_id = t[j];
-        const Vec3f& p = ps2[point_id];
+        const Vec3s& p = ps2[point_id];
 
-        FCL_REAL d = (p - query).squaredNorm();
+        CoalScalar d = (p - query).squaredNorm();
         if (d > maxD) maxD = d;
       }
     }
@@ -631,24 +631,24 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
   return std::sqrt(maxD);
 }
 
-static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2,
-                                                  unsigned int* indices,
-                                                  unsigned int n,
-                                                  const Vec3f& query) {
+static inline CoalScalar maximumDistance_pointcloud(Vec3s* ps, Vec3s* ps2,
+                                                    unsigned int* indices,
+                                                    unsigned int n,
+                                                    const Vec3s& query) {
   bool indirect_index = true;
   if (!indices) indirect_index = false;
 
-  FCL_REAL maxD = 0;
+  CoalScalar maxD = 0;
   for (unsigned int i = 0; i < n; ++i) {
     unsigned int index = indirect_index ? indices[i] : i;
 
-    const Vec3f& p = ps[index];
-    FCL_REAL d = (p - query).squaredNorm();
+    const Vec3s& p = ps[index];
+    CoalScalar d = (p - query).squaredNorm();
     if (d > maxD) maxD = d;
 
     if (ps2) {
-      const Vec3f& v = ps2[index];
-      FCL_REAL d = (v - query).squaredNorm();
+      const Vec3s& v = ps2[index];
+      CoalScalar d = (v - query).squaredNorm();
       if (d > maxD) maxD = d;
     }
   }
@@ -656,15 +656,13 @@ static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2,
   return std::sqrt(maxD);
 }
 
-FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts,
-                         unsigned int* indices, unsigned int n,
-                         const Vec3f& query) {
+CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+                           unsigned int* indices, unsigned int n,
+                           const Vec3s& query) {
   if (ts)
     return maximumDistance_mesh(ps, ps2, ts, indices, n, query);
   else
     return maximumDistance_pointcloud(ps, ps2, indices, n, query);
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index 730447a38a477d8c42bd45932083cf9f0dad4d6e..02a8bc8c9b8de0ebee49713f1d2dc400277c1c07 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -35,20 +35,20 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/internal/BV_fitter.h>
-#include <hpp/fcl/BVH/BVH_utility.h>
+#include "coal/internal/BV_fitter.h"
+#include "coal/BVH/BVH_utility.h"
+#include "coal/internal/tools.h"
+
 #include <limits>
-#include <hpp/fcl/internal/tools.h>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 static const double kIOS_RATIO = 1.5;
 static const double invSinA = 2;
 static const double cosA = sqrt(3.0) / 2.0;
 
-static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3],
-                                 Matrix3f& axes) {
+static inline void axisFromEigen(Vec3s eigenV[3], CoalScalar eigenS[3],
+                                 Matrix3s& axes) {
   int min, mid, max;
   if (eigenS[0] > eigenS[1]) {
     max = 0;
@@ -77,17 +77,17 @@ static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3],
 
 namespace OBB_fit_functions {
 
-void fit1(Vec3f* ps, OBB& bv) {
+void fit1(Vec3s* ps, OBB& bv) {
   bv.To.noalias() = ps[0];
   bv.axes.setIdentity();
   bv.extent.setZero();
 }
 
-void fit2(Vec3f* ps, OBB& bv) {
-  const Vec3f& p1 = ps[0];
-  const Vec3f& p2 = ps[1];
-  Vec3f p1p2 = p1 - p2;
-  FCL_REAL len_p1p2 = p1p2.norm();
+void fit2(Vec3s* ps, OBB& bv) {
+  const Vec3s& p1 = ps[0];
+  const Vec3s& p2 = ps[1];
+  Vec3s p1p2 = p1 - p2;
+  CoalScalar len_p1p2 = p1p2.norm();
   p1p2.normalize();
 
   bv.axes.col(0).noalias() = p1p2;
@@ -97,15 +97,15 @@ void fit2(Vec3f* ps, OBB& bv) {
   bv.To.noalias() = (p1 + p2) / 2;
 }
 
-void fit3(Vec3f* ps, OBB& bv) {
-  const Vec3f& p1 = ps[0];
-  const Vec3f& p2 = ps[1];
-  const Vec3f& p3 = ps[2];
-  Vec3f e[3];
+void fit3(Vec3s* ps, OBB& bv) {
+  const Vec3s& p1 = ps[0];
+  const Vec3s& p2 = ps[1];
+  const Vec3s& p3 = ps[2];
+  Vec3s e[3];
   e[0] = p1 - p2;
   e[1] = p2 - p3;
   e[2] = p3 - p1;
-  FCL_REAL len[3];
+  CoalScalar len[3];
   len[0] = e[0].squaredNorm();
   len[1] = e[1].squaredNorm();
   len[2] = e[2].squaredNorm();
@@ -121,17 +121,17 @@ void fit3(Vec3f* ps, OBB& bv) {
   getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent);
 }
 
-void fit6(Vec3f* ps, OBB& bv) {
+void fit6(Vec3s* ps, OBB& bv) {
   OBB bv1, bv2;
   fit3(ps, bv1);
   fit3(ps + 3, bv2);
   bv = bv1 + bv2;
 }
 
-void fitn(Vec3f* ps, unsigned int n, OBB& bv) {
-  Matrix3f M;
-  Vec3f E[3];
-  Matrix3f::Scalar s[3] = {0, 0, 0};  // three eigen values
+void fitn(Vec3s* ps, unsigned int n, OBB& bv) {
+  Matrix3s M;
+  Vec3s E[3];
+  CoalScalar s[3] = {0, 0, 0};  // three eigen values
 
   getCovariance(ps, NULL, NULL, NULL, n, M);
   eigen(M, s, E);
@@ -144,7 +144,7 @@ void fitn(Vec3f* ps, unsigned int n, OBB& bv) {
 }  // namespace OBB_fit_functions
 
 namespace RSS_fit_functions {
-void fit1(Vec3f* ps, RSS& bv) {
+void fit1(Vec3s* ps, RSS& bv) {
   bv.Tr.noalias() = ps[0];
   bv.axes.setIdentity();
   bv.length[0] = 0;
@@ -152,11 +152,11 @@ void fit1(Vec3f* ps, RSS& bv) {
   bv.radius = 0;
 }
 
-void fit2(Vec3f* ps, RSS& bv) {
-  const Vec3f& p1 = ps[0];
-  const Vec3f& p2 = ps[1];
+void fit2(Vec3s* ps, RSS& bv) {
+  const Vec3s& p1 = ps[0];
+  const Vec3s& p2 = ps[1];
   bv.axes.col(0).noalias() = p1 - p2;
-  FCL_REAL len_p1p2 = bv.axes.col(0).norm();
+  CoalScalar len_p1p2 = bv.axes.col(0).norm();
   bv.axes.col(0) /= len_p1p2;
 
   generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2));
@@ -167,15 +167,15 @@ void fit2(Vec3f* ps, RSS& bv) {
   bv.radius = 0;
 }
 
-void fit3(Vec3f* ps, RSS& bv) {
-  const Vec3f& p1 = ps[0];
-  const Vec3f& p2 = ps[1];
-  const Vec3f& p3 = ps[2];
-  Vec3f e[3];
+void fit3(Vec3s* ps, RSS& bv) {
+  const Vec3s& p1 = ps[0];
+  const Vec3s& p2 = ps[1];
+  const Vec3s& p3 = ps[2];
+  Vec3s e[3];
   e[0] = p1 - p2;
   e[1] = p2 - p3;
   e[2] = p3 - p1;
-  FCL_REAL len[3];
+  CoalScalar len[3];
   len[0] = e[0].squaredNorm();
   len[1] = e[1].squaredNorm();
   len[2] = e[2].squaredNorm();
@@ -192,17 +192,17 @@ void fit3(Vec3f* ps, RSS& bv) {
                                      bv.length, bv.radius);
 }
 
-void fit6(Vec3f* ps, RSS& bv) {
+void fit6(Vec3s* ps, RSS& bv) {
   RSS bv1, bv2;
   fit3(ps, bv1);
   fit3(ps + 3, bv2);
   bv = bv1 + bv2;
 }
 
-void fitn(Vec3f* ps, unsigned int n, RSS& bv) {
-  Matrix3f M;  // row first matrix
-  Vec3f E[3];  // row first eigen-vectors
-  Matrix3f::Scalar s[3] = {0, 0, 0};
+void fitn(Vec3s* ps, unsigned int n, RSS& bv) {
+  Matrix3s M;  // row first matrix
+  Vec3s E[3];  // row first eigen-vectors
+  CoalScalar s[3] = {0, 0, 0};
 
   getCovariance(ps, NULL, NULL, NULL, n, M);
   eigen(M, s, E);
@@ -217,7 +217,7 @@ void fitn(Vec3f* ps, unsigned int n, RSS& bv) {
 
 namespace kIOS_fit_functions {
 
-void fit1(Vec3f* ps, kIOS& bv) {
+void fit1(Vec3s* ps, kIOS& bv) {
   bv.num_spheres = 1;
   bv.spheres[0].o.noalias() = ps[0];
   bv.spheres[0].r = 0;
@@ -227,31 +227,31 @@ void fit1(Vec3f* ps, kIOS& bv) {
   bv.obb.To.noalias() = ps[0];
 }
 
-void fit2(Vec3f* ps, kIOS& bv) {
+void fit2(Vec3s* ps, kIOS& bv) {
   bv.num_spheres = 5;
 
-  const Vec3f& p1 = ps[0];
-  const Vec3f& p2 = ps[1];
-  Vec3f p1p2 = p1 - p2;
-  FCL_REAL len_p1p2 = p1p2.norm();
+  const Vec3s& p1 = ps[0];
+  const Vec3s& p2 = ps[1];
+  Vec3s p1p2 = p1 - p2;
+  CoalScalar len_p1p2 = p1p2.norm();
   p1p2.normalize();
 
-  Matrix3f& axes = bv.obb.axes;
+  Matrix3s& axes = bv.obb.axes;
   axes.col(0).noalias() = p1p2;
   generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2));
 
-  FCL_REAL r0 = len_p1p2 * 0.5;
+  CoalScalar r0 = len_p1p2 * 0.5;
   bv.obb.extent << r0, 0, 0;
   bv.obb.To = (p1 + p2) * 0.5;
 
   bv.spheres[0].o = bv.obb.To;
   bv.spheres[0].r = r0;
 
-  FCL_REAL r1 = r0 * invSinA;
-  FCL_REAL r1cosA = r1 * cosA;
+  CoalScalar r1 = r0 * invSinA;
+  CoalScalar r1cosA = r1 * cosA;
   bv.spheres[1].r = r1;
   bv.spheres[2].r = r1;
-  Vec3f delta = axes.col(1) * r1cosA;
+  Vec3s delta = axes.col(1) * r1cosA;
   bv.spheres[1].o = bv.spheres[0].o - delta;
   bv.spheres[2].o = bv.spheres[0].o + delta;
 
@@ -262,17 +262,17 @@ void fit2(Vec3f* ps, kIOS& bv) {
   bv.spheres[4].o = bv.spheres[0].o + delta;
 }
 
-void fit3(Vec3f* ps, kIOS& bv) {
+void fit3(Vec3s* ps, kIOS& bv) {
   bv.num_spheres = 3;
 
-  const Vec3f& p1 = ps[0];
-  const Vec3f& p2 = ps[1];
-  const Vec3f& p3 = ps[2];
-  Vec3f e[3];
+  const Vec3s& p1 = ps[0];
+  const Vec3s& p2 = ps[1];
+  const Vec3s& p3 = ps[2];
+  Vec3s e[3];
   e[0] = p1 - p2;
   e[1] = p2 - p3;
   e[2] = p3 - p1;
-  FCL_REAL len[3];
+  CoalScalar len[3];
   len[0] = e[0].squaredNorm();
   len[1] = e[1].squaredNorm();
   len[2] = e[2].squaredNorm();
@@ -289,15 +289,15 @@ void fit3(Vec3f* ps, kIOS& bv) {
                      bv.obb.extent);
 
   // compute radius and center
-  FCL_REAL r0;
-  Vec3f center;
+  CoalScalar r0;
+  Vec3s center;
   circumCircleComputation(p1, p2, p3, center, r0);
 
   bv.spheres[0].o = center;
   bv.spheres[0].r = r0;
 
-  FCL_REAL r1 = r0 * invSinA;
-  Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA);
+  CoalScalar r1 = r0 * invSinA;
+  Vec3s delta = bv.obb.axes.col(2) * (r1 * cosA);
 
   bv.spheres[1].r = r1;
   bv.spheres[1].o = center - delta;
@@ -305,23 +305,23 @@ void fit3(Vec3f* ps, kIOS& bv) {
   bv.spheres[2].o = center + delta;
 }
 
-void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
-  Matrix3f M;
-  Vec3f E[3];
-  Matrix3f::Scalar s[3] = {0, 0, 0};  // three eigen values;
+void fitn(Vec3s* ps, unsigned int n, kIOS& bv) {
+  Matrix3s M;
+  Vec3s E[3];
+  CoalScalar s[3] = {0, 0, 0};  // three eigen values;
 
   getCovariance(ps, NULL, NULL, NULL, n, M);
   eigen(M, s, E);
 
-  Matrix3f& axes = bv.obb.axes;
+  Matrix3s& axes = bv.obb.axes;
   axisFromEigen(E, s, axes);
 
   getExtentAndCenter(ps, NULL, NULL, NULL, n, axes, bv.obb.To, bv.obb.extent);
 
   // get center and extension
-  const Vec3f& center = bv.obb.To;
-  const Vec3f& extent = bv.obb.extent;
-  FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
+  const Vec3s& center = bv.obb.To;
+  const Vec3s& extent = bv.obb.extent;
+  CoalScalar r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
 
   // decide the k in kIOS
   if (extent[0] > kIOS_RATIO * extent[2]) {
@@ -336,12 +336,12 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
   bv.spheres[0].r = r0;
 
   if (bv.num_spheres >= 3) {
-    FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
-    Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]);
+    CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
+    Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]);
     bv.spheres[1].o = center - delta;
     bv.spheres[2].o = center + delta;
 
-    FCL_REAL r11 = 0, r12 = 0;
+    CoalScalar r11 = 0, r12 = 0;
     r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o);
     r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o);
     bv.spheres[1].o += axes.col(2) * (-r10 + r11);
@@ -352,15 +352,15 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
   }
 
   if (bv.num_spheres >= 5) {
-    FCL_REAL r10 = bv.spheres[1].r;
-    Vec3f delta =
+    CoalScalar r10 = bv.spheres[1].r;
+    Vec3s delta =
         axes.col(1) *
         (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) -
          extent[1]);
     bv.spheres[3].o = bv.spheres[0].o - delta;
     bv.spheres[4].o = bv.spheres[0].o + delta;
 
-    FCL_REAL r21 = 0, r22 = 0;
+    CoalScalar r21 = 0, r22 = 0;
     r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o);
     r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o);
 
@@ -375,22 +375,22 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
 }  // namespace kIOS_fit_functions
 
 namespace OBBRSS_fit_functions {
-void fit1(Vec3f* ps, OBBRSS& bv) {
+void fit1(Vec3s* ps, OBBRSS& bv) {
   OBB_fit_functions::fit1(ps, bv.obb);
   RSS_fit_functions::fit1(ps, bv.rss);
 }
 
-void fit2(Vec3f* ps, OBBRSS& bv) {
+void fit2(Vec3s* ps, OBBRSS& bv) {
   OBB_fit_functions::fit2(ps, bv.obb);
   RSS_fit_functions::fit2(ps, bv.rss);
 }
 
-void fit3(Vec3f* ps, OBBRSS& bv) {
+void fit3(Vec3s* ps, OBBRSS& bv) {
   OBB_fit_functions::fit3(ps, bv.obb);
   RSS_fit_functions::fit3(ps, bv.rss);
 }
 
-void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) {
+void fitn(Vec3s* ps, unsigned int n, OBBRSS& bv) {
   OBB_fit_functions::fitn(ps, n, bv.obb);
   RSS_fit_functions::fitn(ps, n, bv.rss);
 }
@@ -398,7 +398,7 @@ void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) {
 }  // namespace OBBRSS_fit_functions
 
 template <>
-void fit(Vec3f* ps, unsigned int n, OBB& bv) {
+void fit(Vec3s* ps, unsigned int n, OBB& bv) {
   switch (n) {
     case 1:
       OBB_fit_functions::fit1(ps, bv);
@@ -418,7 +418,7 @@ void fit(Vec3f* ps, unsigned int n, OBB& bv) {
 }
 
 template <>
-void fit(Vec3f* ps, unsigned int n, RSS& bv) {
+void fit(Vec3s* ps, unsigned int n, RSS& bv) {
   switch (n) {
     case 1:
       RSS_fit_functions::fit1(ps, bv);
@@ -435,7 +435,7 @@ void fit(Vec3f* ps, unsigned int n, RSS& bv) {
 }
 
 template <>
-void fit(Vec3f* ps, unsigned int n, kIOS& bv) {
+void fit(Vec3s* ps, unsigned int n, kIOS& bv) {
   switch (n) {
     case 1:
       kIOS_fit_functions::fit1(ps, bv);
@@ -452,7 +452,7 @@ void fit(Vec3f* ps, unsigned int n, kIOS& bv) {
 }
 
 template <>
-void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) {
+void fit(Vec3s* ps, unsigned int n, OBBRSS& bv) {
   switch (n) {
     case 1:
       OBBRSS_fit_functions::fit1(ps, bv);
@@ -469,7 +469,7 @@ void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) {
 }
 
 template <>
-void fit(Vec3f* ps, unsigned int n, AABB& bv) {
+void fit(Vec3s* ps, unsigned int n, AABB& bv) {
   if (n <= 0) return;
   bv = AABB(ps[0]);
   for (unsigned int i = 1; i < n; ++i) {
@@ -481,9 +481,9 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices,
                        unsigned int num_primitives) {
   OBB bv;
 
-  Matrix3f M;             // row first matrix
-  Vec3f E[3];             // row first eigen-vectors
-  Matrix3f::Scalar s[3];  // three eigen values
+  Matrix3s M;       // row first matrix
+  Vec3s E[3];       // row first eigen-vectors
+  CoalScalar s[3];  // three eigen values
 
   getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
                 num_primitives, M);
@@ -501,9 +501,9 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices,
 OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices,
                              unsigned int num_primitives) {
   OBBRSS bv;
-  Matrix3f M;
-  Vec3f E[3];
-  Matrix3f::Scalar s[3];
+  Matrix3s M;
+  Vec3s E[3];
+  CoalScalar s[3];
 
   getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
                 num_primitives, M);
@@ -515,9 +515,9 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices,
   getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices,
                      num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent);
 
-  Vec3f origin;
-  FCL_REAL l[2];
-  FCL_REAL r;
+  Vec3s origin;
+  CoalScalar l[2];
+  CoalScalar r;
   getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices,
                                      primitive_indices, num_primitives,
                                      bv.rss.axes, origin, l, r);
@@ -534,9 +534,9 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices,
                        unsigned int num_primitives) {
   RSS bv;
 
-  Matrix3f M;             // row first matrix
-  Vec3f E[3];             // row first eigen-vectors
-  Matrix3f::Scalar s[3];  // three eigen values
+  Matrix3s M;       // row first matrix
+  Vec3s E[3];       // row first eigen-vectors
+  CoalScalar s[3];  // three eigen values
   getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
                 num_primitives, M);
   eigen(M, s, E);
@@ -544,9 +544,9 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices,
 
   // set rss origin, rectangle size and radius
 
-  Vec3f origin;
-  FCL_REAL l[2];
-  FCL_REAL r;
+  Vec3s origin;
+  CoalScalar l[2];
+  CoalScalar r;
   getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices,
                                      primitive_indices, num_primitives, bv.axes,
                                      origin, l, r);
@@ -563,25 +563,25 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices,
                          unsigned int num_primitives) {
   kIOS bv;
 
-  Matrix3f M;  // row first matrix
-  Vec3f E[3];  // row first eigen-vectors
-  Matrix3f::Scalar s[3];
+  Matrix3s M;  // row first matrix
+  Vec3s E[3];  // row first eigen-vectors
+  CoalScalar s[3];
 
   getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
                 num_primitives, M);
   eigen(M, s, E);
 
-  Matrix3f& axes = bv.obb.axes;
+  Matrix3s& axes = bv.obb.axes;
   axisFromEigen(E, s, axes);
 
   // get centers and extensions
   getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices,
                      num_primitives, axes, bv.obb.To, bv.obb.extent);
 
-  const Vec3f& center = bv.obb.To;
-  const Vec3f& extent = bv.obb.extent;
-  FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices,
-                                primitive_indices, num_primitives, center);
+  const Vec3s& center = bv.obb.To;
+  const Vec3s& extent = bv.obb.extent;
+  CoalScalar r0 = maximumDistance(vertices, prev_vertices, tri_indices,
+                                  primitive_indices, num_primitives, center);
 
   // decide k in kIOS
   if (extent[0] > kIOS_RATIO * extent[2]) {
@@ -596,15 +596,15 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices,
   bv.spheres[0].r = r0;
 
   if (bv.num_spheres >= 3) {
-    FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
-    Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]);
+    CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
+    Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]);
     bv.spheres[1].o = center - delta;
     bv.spheres[2].o = center + delta;
 
-    FCL_REAL r11 =
+    CoalScalar r11 =
         maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices,
                         num_primitives, bv.spheres[1].o);
-    FCL_REAL r12 =
+    CoalScalar r12 =
         maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices,
                         num_primitives, bv.spheres[2].o);
 
@@ -616,15 +616,15 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices,
   }
 
   if (bv.num_spheres >= 5) {
-    FCL_REAL r10 = bv.spheres[1].r;
-    Vec3f delta =
+    CoalScalar r10 = bv.spheres[1].r;
+    Vec3s delta =
         axes.col(1) *
         (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) -
          extent[1]);
     bv.spheres[3].o = bv.spheres[0].o - delta;
     bv.spheres[4].o = bv.spheres[0].o + delta;
 
-    FCL_REAL r21 = 0, r22 = 0;
+    CoalScalar r21 = 0, r22 = 0;
     r21 = maximumDistance(vertices, prev_vertices, tri_indices,
                           primitive_indices, num_primitives, bv.spheres[3].o);
     r22 = maximumDistance(vertices, prev_vertices, tri_indices,
@@ -679,6 +679,4 @@ AABB BVFitter<AABB>::fit(unsigned int* primitive_indices,
   return bv;
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
index 64dc26733140f228bd3424603f4599d8665335f2..50d4024298c2c75a297280e6175f9aed11e54f92 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/src/BVH/BV_splitter.cpp
@@ -35,27 +35,26 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/internal/BV_splitter.h>
+#include "coal/internal/BV_splitter.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 template <typename BV>
-void computeSplitVector(const BV& bv, Vec3f& split_vector) {
+void computeSplitVector(const BV& bv, Vec3s& split_vector) {
   split_vector = bv.axes.col(0);
 }
 
 template <>
-void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector) {
+void computeSplitVector<kIOS>(const kIOS& bv, Vec3s& split_vector) {
   /*
     switch(bv.num_spheres)
     {
     case 1:
-    split_vector = Vec3f(1, 0, 0);
+    split_vector = Vec3s(1, 0, 0);
     break;
     case 3:
     {
-    Vec3f v[3];
+    Vec3s v[3];
     v[0] = bv.spheres[1].o - bv.spheres[0].o;
     v[0].normalize();
     generateCoordinateSystem(v[0], v[1], v[2]);
@@ -64,7 +63,7 @@ void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector) {
     break;
     case 5:
     {
-    Vec3f v[2];
+    Vec3s v[2];
     v[0] = bv.spheres[1].o - bv.spheres[0].o;
     v[1] = bv.spheres[3].o - bv.spheres[0].o;
     split_vector = v[0].cross(v[1]);
@@ -79,37 +78,38 @@ void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector) {
 }
 
 template <>
-void computeSplitVector<OBBRSS>(const OBBRSS& bv, Vec3f& split_vector) {
+void computeSplitVector<OBBRSS>(const OBBRSS& bv, Vec3s& split_vector) {
   split_vector = bv.obb.axes.col(0);
 }
 
 template <typename BV>
-void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value) {
-  Vec3f center = bv.center();
+void computeSplitValue_bvcenter(const BV& bv, CoalScalar& split_value) {
+  Vec3s center = bv.center();
   split_value = center[0];
 }
 
 template <typename BV>
-void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles,
+void computeSplitValue_mean(const BV&, Vec3s* vertices, Triangle* triangles,
                             unsigned int* primitive_indices,
                             unsigned int num_primitives, BVHModelType type,
-                            const Vec3f& split_vector, FCL_REAL& split_value) {
+                            const Vec3s& split_vector,
+                            CoalScalar& split_value) {
   if (type == BVH_MODEL_TRIANGLES) {
-    Vec3f c(Vec3f::Zero());
+    Vec3s c(Vec3s::Zero());
 
     for (unsigned int i = 0; i < num_primitives; ++i) {
       const Triangle& t = triangles[primitive_indices[i]];
-      const Vec3f& p1 = vertices[t[0]];
-      const Vec3f& p2 = vertices[t[1]];
-      const Vec3f& p3 = vertices[t[2]];
+      const Vec3s& p1 = vertices[t[0]];
+      const Vec3s& p2 = vertices[t[1]];
+      const Vec3s& p3 = vertices[t[2]];
 
       c += p1 + p2 + p3;
     }
     split_value = c.dot(split_vector) / (3 * num_primitives);
   } else if (type == BVH_MODEL_POINTCLOUD) {
-    FCL_REAL sum = 0;
+    CoalScalar sum = 0;
     for (unsigned int i = 0; i < num_primitives; ++i) {
-      const Vec3f& p = vertices[primitive_indices[i]];
+      const Vec3s& p = vertices[primitive_indices[i]];
       sum += p.dot(split_vector);
     }
 
@@ -118,28 +118,28 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles,
 }
 
 template <typename BV>
-void computeSplitValue_median(const BV&, Vec3f* vertices, Triangle* triangles,
+void computeSplitValue_median(const BV&, Vec3s* vertices, Triangle* triangles,
                               unsigned int* primitive_indices,
                               unsigned int num_primitives, BVHModelType type,
-                              const Vec3f& split_vector,
-                              FCL_REAL& split_value) {
-  std::vector<FCL_REAL> proj(num_primitives);
+                              const Vec3s& split_vector,
+                              CoalScalar& split_value) {
+  std::vector<CoalScalar> proj(num_primitives);
 
   if (type == BVH_MODEL_TRIANGLES) {
     for (unsigned int i = 0; i < num_primitives; ++i) {
       const Triangle& t = triangles[primitive_indices[i]];
-      const Vec3f& p1 = vertices[t[0]];
-      const Vec3f& p2 = vertices[t[1]];
-      const Vec3f& p3 = vertices[t[2]];
-      Vec3f centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1],
+      const Vec3s& p1 = vertices[t[0]];
+      const Vec3s& p2 = vertices[t[1]];
+      const Vec3s& p3 = vertices[t[2]];
+      Vec3s centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1],
                       p1[2] + p2[2] + p3[2]);
 
       proj[i] = centroid3.dot(split_vector) / 3;
     }
   } else if (type == BVH_MODEL_POINTCLOUD) {
     for (unsigned int i = 0; i < num_primitives; ++i) {
-      const Vec3f& p = vertices[primitive_indices[i]];
-      Vec3f v(p[0], p[1], p[2]);
+      const Vec3s& p = vertices[primitive_indices[i]];
+      Vec3s v(p[0], p[1], p[2]);
       proj[i] = v.dot(split_vector);
     }
   }
@@ -259,23 +259,23 @@ void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv,
 }
 
 template <>
-bool BVSplitter<OBB>::apply(const Vec3f& q) const {
-  return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+bool BVSplitter<OBB>::apply(const Vec3s& q) const {
+  return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value;
 }
 
 template <>
-bool BVSplitter<RSS>::apply(const Vec3f& q) const {
-  return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+bool BVSplitter<RSS>::apply(const Vec3s& q) const {
+  return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value;
 }
 
 template <>
-bool BVSplitter<kIOS>::apply(const Vec3f& q) const {
-  return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+bool BVSplitter<kIOS>::apply(const Vec3s& q) const {
+  return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value;
 }
 
 template <>
-bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const {
-  return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+bool BVSplitter<OBBRSS>::apply(const Vec3s& q) const {
+  return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value;
 }
 
 template class BVSplitter<RSS>;
@@ -283,6 +283,4 @@ template class BVSplitter<OBBRSS>;
 template class BVSplitter<OBB>;
 template class BVSplitter<kIOS>;
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 74e7fb304516ec4327daef3af641a53b1a07a646..f1dfb892a57d33a726fa9448e6bf7ea123ce206b 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -110,11 +110,11 @@ set(${LIBRARY_NAME}_SOURCES
   serialization/serialization.cpp
   )
 
-if(HPP_FCL_HAS_OCTOMAP)
+if(COAL_HAS_OCTOMAP)
   list(APPEND ${LIBRARY_NAME}_SOURCES octree.cpp)
-endif(HPP_FCL_HAS_OCTOMAP)
+endif(COAL_HAS_OCTOMAP)
 
-if(HPP_FCL_HAS_QHULL AND NOT HPP_FCL_USE_SYSTEM_QHULL)
+if(COAL_HAS_QHULL AND NOT COAL_USE_SYSTEM_QHULL)
   set(
     libqhullcpp_HEADERS
     ${Qhullcpp_PREFIX}/libqhullcpp/Coordinates.h
@@ -179,14 +179,20 @@ SET(PROJECT_HEADERS_FULL_PATH)
 FOREACH(header ${${PROJECT_NAME}_HEADERS})
   LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_SOURCE_DIR}/${header})
 ENDFOREACH()
-LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/config.hh)
-LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/deprecated.hh)
-LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/warning.hh)
+LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/config.hh)
+LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/deprecated.hh)
+LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/warning.hh)
 add_library(${LIBRARY_NAME}
   SHARED
   ${PROJECT_HEADERS_FULL_PATH}
   ${${LIBRARY_NAME}_SOURCES}
   )
+add_library(${LIBRARY_NAME}::${LIBRARY_NAME} ALIAS ${LIBRARY_NAME})
+
+if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL)
+  add_library(hpp-fcl ALIAS ${LIBRARY_NAME})
+  add_library(hpp-fcl::hpp-fcl ALIAS ${LIBRARY_NAME})
+endif()
 
 if(UNIX)
   get_relative_rpath(${CMAKE_INSTALL_LIBDIR} ${PROJECT_NAME}_INSTALL_RPATH)
@@ -213,10 +219,10 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
   Boost::filesystem
 )
 
-if (HPP_FCL_ENABLE_LOGGING)
+if (COAL_ENABLE_LOGGING)
   TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC Boost::log)
   # The compile flag `BOOST_LOG_DYN_LINK` is required here.
-  target_compile_definitions(${LIBRARY_NAME} PUBLIC HPP_FCL_ENABLE_LOGGING BOOST_LOG_DYN_LINK)
+  target_compile_definitions(${LIBRARY_NAME} PUBLIC COAL_ENABLE_LOGGING BOOST_LOG_DYN_LINK)
 endif()
 
 IF(WIN32)
@@ -230,13 +236,17 @@ IF(WIN32)
   target_compile_definitions(${LIBRARY_NAME} PRIVATE _ENABLE_EXTENDED_ALIGNED_STORAGE)
 ENDIF(WIN32)
 
-if (HPP_FCL_TURN_ASSERT_INTO_EXCEPTION)
-  target_compile_definitions(${LIBRARY_NAME} PUBLIC -DHPP_FCL_TURN_ASSERT_INTO_EXCEPTION)
+if (COAL_TURN_ASSERT_INTO_EXCEPTION)
+  target_compile_definitions(${LIBRARY_NAME} PUBLIC -DCOAL_TURN_ASSERT_INTO_EXCEPTION)
+endif()
+
+if (COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL)
+  target_compile_definitions(${LIBRARY_NAME} PUBLIC -DCOAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL)
 endif()
 
-if(HPP_FCL_HAS_QHULL)
-  target_compile_definitions(${LIBRARY_NAME} PRIVATE -DHPP_FCL_HAS_QHULL)
-  if (HPP_FCL_USE_SYSTEM_QHULL)
+if(COAL_HAS_QHULL)
+  target_compile_definitions(${LIBRARY_NAME} PRIVATE -DCOAL_HAS_QHULL)
+  if (COAL_USE_SYSTEM_QHULL)
     target_link_libraries(${LIBRARY_NAME} PRIVATE Qhull::qhull_r Qhull::qhullcpp)
   else()
     target_include_directories(${LIBRARY_NAME} SYSTEM PRIVATE
@@ -252,6 +262,8 @@ MODERNIZE_TARGET_LINK_LIBRARIES(${PROJECT_NAME} SCOPE PUBLIC
 target_include_directories(${LIBRARY_NAME}
   PUBLIC
   $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+  $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/include>
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   )
 
 IF(octomap_FOUND)
@@ -260,8 +272,8 @@ IF(octomap_FOUND)
     LIBRARIES ${OCTOMAP_LIBRARIES}
     INCLUDE_DIRS ${OCTOMAP_INCLUDE_DIRS})
   target_compile_definitions (${LIBRARY_NAME} PUBLIC
-    -DHPP_FCL_HAS_OCTOMAP
-    -DHPP_FCL_HAVE_OCTOMAP
+    -DCOAL_HAS_OCTOMAP
+    -DCOAL_HAVE_OCTOMAP
     -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION}
     -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION}
     -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION})
diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp
index bc51940cf2114cd3f327312918d08f3bede4d4e2..a39953835370928b1e854d27490ece0da49db169 100644
--- a/src/broadphase/broadphase_SSaP.cpp
+++ b/src/broadphase/broadphase_SSaP.cpp
@@ -35,10 +35,9 @@
 
 /** @author Jia Pan */
 
-#include "hpp/fcl/broadphase/broadphase_SSaP.h"
+#include "coal/broadphase/broadphase_SSaP.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 /** @brief Functor sorting objects according to the AABB lower x bound */
 struct SortByXLow {
@@ -65,7 +64,7 @@ struct SortByZLow {
 };
 
 /** @brief Dummy collision object with a point AABB */
-class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject {
+class COAL_DLLAPI DummyCollisionObject : public CollisionObject {
  public:
   DummyCollisionObject(const AABB& aabb_)
       : CollisionObject(shared_ptr<CollisionGeometry>()) {
@@ -184,7 +183,7 @@ bool SSaPCollisionManager::checkDis(
     typename std::vector<CollisionObject*>::const_iterator pos_start,
     typename std::vector<CollisionObject*>::const_iterator pos_end,
     CollisionObject* obj, DistanceCallBackBase* callback,
-    FCL_REAL& min_dist) const {
+    CoalScalar& min_dist) const {
   while (pos_start < pos_end) {
     if (*pos_start != obj)  // no distance between the same object
     {
@@ -257,19 +256,19 @@ void SSaPCollisionManager::distance(CollisionObject* obj,
   callback->init();
   if (size() == 0) return;
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   distance_(obj, callback, min_dist);
 }
 
 //==============================================================================
 bool SSaPCollisionManager::distance_(CollisionObject* obj,
                                      DistanceCallBackBase* callback,
-                                     FCL_REAL& min_dist) const {
+                                     CoalScalar& min_dist) const {
   static const unsigned int CUTOFF = 100;
-  Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
-  Vec3f dummy_vector = obj->getAABB().max_;
-  if (min_dist < (std::numeric_limits<FCL_REAL>::max)())
-    dummy_vector += Vec3f(min_dist, min_dist, min_dist);
+  Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  Vec3s dummy_vector = obj->getAABB().max_;
+  if (min_dist < (std::numeric_limits<CoalScalar>::max)())
+    dummy_vector += Vec3s(min_dist, min_dist, min_dist);
 
   typename std::vector<CollisionObject*>::const_iterator pos_start1 =
       objs_x.begin();
@@ -285,7 +284,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj,
       objs_z.end();
 
   int status = 1;
-  FCL_REAL old_min_distance;
+  CoalScalar old_min_distance;
 
   while (1) {
     old_min_distance = min_dist;
@@ -329,20 +328,20 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj,
     if (dist_res) return true;
 
     if (status == 1) {
-      if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
+      if (old_min_distance < (std::numeric_limits<CoalScalar>::max)())
         break;
       else {
         // from infinity to a finite one, only need one additional loop
         // to check the possible missed ones to the right of the objs array
         if (min_dist < old_min_distance) {
           dummy_vector =
-              obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
+              obj->getAABB().max_ + Vec3s(min_dist, min_dist, min_dist);
           status = 0;
         } else  // need more loop
         {
           if (dummy_vector.isApprox(
                   obj->getAABB().max_,
-                  std::numeric_limits<FCL_REAL>::epsilon() * 100))
+                  std::numeric_limits<CoalScalar>::epsilon() * 100))
             dummy_vector = dummy_vector + delta;
           else
             dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
@@ -368,12 +367,12 @@ int SSaPCollisionManager::selectOptimalAxis(
     typename std::vector<CollisionObject*>::const_iterator& it_beg,
     typename std::vector<CollisionObject*>::const_iterator& it_end) {
   /// simple sweep and prune method
-  FCL_REAL delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] -
-                     (objs_x[0])->getAABB().min_[0];
-  FCL_REAL delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] -
-                     (objs_y[0])->getAABB().min_[1];
-  FCL_REAL delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] -
-                     (objs_z[0])->getAABB().min_[2];
+  CoalScalar delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] -
+                       (objs_x[0])->getAABB().min_[0];
+  CoalScalar delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] -
+                       (objs_y[0])->getAABB().min_[1];
+  CoalScalar delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] -
+                       (objs_z[0])->getAABB().min_[2];
 
   int axis = 0;
   if (delta_y > delta_x && delta_y > delta_z)
@@ -454,7 +453,7 @@ void SSaPCollisionManager::distance(DistanceCallBackBase* callback) const {
   typename std::vector<CollisionObject*>::const_iterator it, it_end;
   selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   for (; it != it_end; ++it) {
     if (distance_(*it, callback, min_dist)) return;
   }
@@ -499,7 +498,7 @@ void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
     return;
   }
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   typename std::vector<CollisionObject*>::const_iterator it, end;
   if (this->size() < other_manager->size()) {
     for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
@@ -517,5 +516,4 @@ bool SSaPCollisionManager::empty() const { return objs_x.empty(); }
 //==============================================================================
 size_t SSaPCollisionManager::size() const { return objs_x.size(); }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp
index 4da0c995af2667b9d64ddc21907410bbc1d0118f..b2b27f5fe861016a38cda5ad25956056a02aace5 100644
--- a/src/broadphase/broadphase_SaP.cpp
+++ b/src/broadphase/broadphase_SaP.cpp
@@ -35,10 +35,9 @@
 
 /** @author Jia Pan */
 
-#include "hpp/fcl/broadphase/broadphase_SaP.h"
+#include "coal/broadphase/broadphase_SaP.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 //==============================================================================
 void SaPCollisionManager::unregisterObject(CollisionObject* obj) {
@@ -119,15 +118,15 @@ void SaPCollisionManager::registerObjects(
       obj_aabb_map[other_objs[i]] = sapaabb;
     }
 
-    FCL_REAL scale[3];
+    CoalScalar scale[3];
     for (int coord = 0; coord < 3; ++coord) {
       std::sort(
           endpoints.begin(), endpoints.end(),
-          std::bind(std::less<FCL_REAL>(),
-                    std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+          std::bind(std::less<CoalScalar>(),
+                    std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
                                   &EndPoint::getVal),
                               std::placeholders::_1, coord),
-                    std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+                    std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
                                   &EndPoint::getVal),
                               std::placeholders::_2, coord)));
 
@@ -204,7 +203,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) {
     } else  // otherwise, find the correct location in the list and insert
     {
       EndPoint* curr_lo = new_sap->lo;
-      FCL_REAL curr_lo_val = curr_lo->getVal()[coord];
+      CoalScalar curr_lo_val = curr_lo->getVal()[coord];
       while ((current->getVal()[coord] < curr_lo_val) &&
              (current->next[coord] != nullptr))
         current = current->next[coord];
@@ -232,7 +231,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) {
     current = new_sap->lo;
 
     EndPoint* curr_hi = new_sap->hi;
-    FCL_REAL curr_hi_val = curr_hi->getVal()[coord];
+    CoalScalar curr_hi_val = curr_hi->getVal()[coord];
 
     if (coord == 0) {
       while ((current->getVal()[coord] < curr_hi_val) &&
@@ -274,7 +273,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) {
 void SaPCollisionManager::setup() {
   if (size() == 0) return;
 
-  FCL_REAL scale[3];
+  CoalScalar scale[3];
   scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
   scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);
   scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
@@ -293,8 +292,8 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) {
 
   const AABB current_aabb = current->obj->getAABB();
 
-  const Vec3f& new_min = current_aabb.min_;
-  const Vec3f& new_max = current_aabb.max_;
+  const Vec3s& new_min = current_aabb.min_;
+  const Vec3s& new_max = current_aabb.max_;
 
   for (int coord = 0; coord < 3; ++coord) {
     int direction;  // -1 reverse, 0 nochange, 1 forward
@@ -507,8 +506,8 @@ bool SaPCollisionManager::collide_(CollisionObject* obj,
   int axis = optimal_axis;
   const AABB& obj_aabb = obj->getAABB();
 
-  FCL_REAL min_val = obj_aabb.min_[axis];
-  //  FCL_REAL max_val = obj_aabb.max_[axis];
+  CoalScalar min_val = obj_aabb.min_[axis];
+  //  CoalScalar max_val = obj_aabb.max_[axis];
 
   EndPoint dummy;
   SaPAABB dummy_aabb;
@@ -520,11 +519,11 @@ bool SaPCollisionManager::collide_(CollisionObject* obj,
   // iteration linearly
   const auto res_it = std::upper_bound(
       velist[axis].begin(), velist[axis].end(), &dummy,
-      std::bind(std::less<FCL_REAL>(),
-                std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+      std::bind(std::less<CoalScalar>(),
+                std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
                               &EndPoint::getVal),
                           std::placeholders::_1, axis),
-                std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+                std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
                               &EndPoint::getVal),
                           std::placeholders::_2, axis)));
 
@@ -585,26 +584,26 @@ void SaPCollisionManager::collide(CollisionObject* obj,
 //==============================================================================
 bool SaPCollisionManager::distance_(CollisionObject* obj,
                                     DistanceCallBackBase* callback,
-                                    FCL_REAL& min_dist) const {
-  Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+                                    CoalScalar& min_dist) const {
+  Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
   AABB aabb = obj->getAABB();
 
-  if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) {
-    Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+  if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
+    Vec3s min_dist_delta(min_dist, min_dist, min_dist);
     aabb.expand(min_dist_delta);
   }
 
   int axis = optimal_axis;
 
   int status = 1;
-  FCL_REAL old_min_distance;
+  CoalScalar old_min_distance;
 
   EndPoint* start_pos = elist[axis];
 
   while (1) {
     old_min_distance = min_dist;
-    FCL_REAL min_val = aabb.min_[axis];
-    //    FCL_REAL max_val = aabb.max_[axis];
+    CoalScalar min_val = aabb.min_[axis];
+    //    CoalScalar max_val = aabb.max_[axis];
 
     EndPoint dummy;
     SaPAABB dummy_aabb;
@@ -614,11 +613,11 @@ bool SaPCollisionManager::distance_(CollisionObject* obj,
 
     const auto res_it = std::upper_bound(
         velist[axis].begin(), velist[axis].end(), &dummy,
-        std::bind(std::less<FCL_REAL>(),
-                  std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+        std::bind(std::less<CoalScalar>(),
+                  std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
                                 &EndPoint::getVal),
                             std::placeholders::_1, axis),
-                  std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+                  std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
                                 &EndPoint::getVal),
                             std::placeholders::_2, axis)));
 
@@ -653,11 +652,11 @@ bool SaPCollisionManager::distance_(CollisionObject* obj,
     }
 
     if (status == 1) {
-      if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
+      if (old_min_distance < (std::numeric_limits<CoalScalar>::max)())
         break;
       else {
         if (min_dist < old_min_distance) {
-          Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+          Vec3s min_dist_delta(min_dist, min_dist, min_dist);
           aabb = AABB(obj->getAABB(), min_dist_delta);
           status = 0;
         } else {
@@ -680,7 +679,7 @@ void SaPCollisionManager::distance(CollisionObject* obj,
   callback->init();
   if (size() == 0) return;
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
 
   distance_(obj, callback, min_dist);
 }
@@ -707,7 +706,7 @@ void SaPCollisionManager::distance(DistanceCallBackBase* callback) const {
   this->enable_tested_set_ = true;
   this->tested_set.clear();
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
 
   for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) {
     if (distance_((*it)->obj, callback, min_dist)) break;
@@ -758,7 +757,7 @@ void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
     return;
   }
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
 
   if (this->size() < other_manager->size()) {
     for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) {
@@ -780,7 +779,7 @@ bool SaPCollisionManager::empty() const { return AABB_arr.empty(); }
 size_t SaPCollisionManager::size() const { return AABB_arr.size(); }
 
 //==============================================================================
-const Vec3f& SaPCollisionManager::EndPoint::getVal() const {
+const Vec3s& SaPCollisionManager::EndPoint::getVal() const {
   if (minmax)
     return aabb->cached.max_;
   else
@@ -788,7 +787,7 @@ const Vec3f& SaPCollisionManager::EndPoint::getVal() const {
 }
 
 //==============================================================================
-Vec3f& SaPCollisionManager::EndPoint::getVal() {
+Vec3s& SaPCollisionManager::EndPoint::getVal() {
   if (minmax)
     return aabb->cached.max_;
   else
@@ -796,7 +795,7 @@ Vec3f& SaPCollisionManager::EndPoint::getVal() {
 }
 
 //==============================================================================
-FCL_REAL SaPCollisionManager::EndPoint::getVal(int i) const {
+CoalScalar SaPCollisionManager::EndPoint::getVal(int i) const {
   if (minmax)
     return aabb->cached.max_[i];
   else
@@ -804,7 +803,7 @@ FCL_REAL SaPCollisionManager::EndPoint::getVal(int i) const {
 }
 
 //==============================================================================
-FCL_REAL& SaPCollisionManager::EndPoint::getVal(int i) {
+CoalScalar& SaPCollisionManager::EndPoint::getVal(int i) {
   if (minmax)
     return aabb->cached.max_[i];
   else
@@ -849,6 +848,4 @@ bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) {
   return (pair.obj1 == obj1) && (pair.obj2 == obj2);
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp
index 17e50dda1e4ac14fde3c1e8e6f9a4bb59fab0cfe..7d83629bf888bd6f1f890c5136e0cb0ebca40a1e 100644
--- a/src/broadphase/broadphase_bruteforce.cpp
+++ b/src/broadphase/broadphase_bruteforce.cpp
@@ -35,13 +35,12 @@
 
 /** @author Jia Pan */
 
-#include "hpp/fcl/broadphase/broadphase_bruteforce.h"
+#include "coal/broadphase/broadphase_bruteforce.h"
 
 #include <iterator>
 #include <algorithm>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 //==============================================================================
 NaiveCollisionManager::NaiveCollisionManager() {
@@ -101,7 +100,7 @@ void NaiveCollisionManager::distance(CollisionObject* obj,
   callback->init();
   if (size() == 0) return;
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   for (auto* obj2 : objs) {
     if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
       if ((*callback)(obj, obj2, min_dist)) return;
@@ -132,7 +131,7 @@ void NaiveCollisionManager::distance(DistanceCallBackBase* callback) const {
   callback->init();
   if (size() == 0) return;
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   for (typename std::list<CollisionObject*>::const_iterator it1 = objs.begin(),
                                                             end = objs.end();
        it1 != end; ++it1) {
@@ -183,7 +182,7 @@ void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
     return;
   }
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   for (auto* obj1 : objs) {
     for (auto* obj2 : other_manager->objs) {
       if (obj1->getAABB().distance(obj2->getAABB()) < min_dist) {
@@ -199,5 +198,4 @@ bool NaiveCollisionManager::empty() const { return objs.empty(); }
 //==============================================================================
 size_t NaiveCollisionManager::size() const { return objs.size(); }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp
index 90f03249cd1ef020531e36c5e730747e3b42c06e..afb0a868fc0068d3ad54e316e9f539ed064b3297 100644
--- a/src/broadphase/broadphase_collision_manager.cpp
+++ b/src/broadphase/broadphase_collision_manager.cpp
@@ -35,10 +35,9 @@
 
 /** @author Jia Pan */
 
-#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/broadphase_collision_manager.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 //==============================================================================
 BroadPhaseCollisionManager::BroadPhaseCollisionManager()
@@ -59,7 +58,7 @@ void BroadPhaseCollisionManager::registerObjects(
 
 //==============================================================================
 void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) {
-  HPP_FCL_UNUSED_VARIABLE(updated_obj);
+  COAL_UNUSED_VARIABLE(updated_obj);
 
   update();
 }
@@ -67,7 +66,7 @@ void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) {
 //==============================================================================
 void BroadPhaseCollisionManager::update(
     const std::vector<CollisionObject*>& updated_objs) {
-  HPP_FCL_UNUSED_VARIABLE(updated_objs);
+  COAL_UNUSED_VARIABLE(updated_objs);
 
   update();
 }
@@ -90,5 +89,4 @@ void BroadPhaseCollisionManager::insertTestedSet(CollisionObject* a,
     tested_set.insert(std::make_pair(b, a));
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
index 608d4628f694c5c586a674bc1152405b047766ba..755d2763face485f2bb6c038e93ae836c132b31f 100644
--- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp
+++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
@@ -35,28 +35,27 @@
 
 /** @author Jia Pan */
 
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
 
-#include <limits>
-
-#ifdef HPP_FCL_HAVE_OCTOMAP
-#include "hpp/fcl/octree.h"
+#ifdef COAL_HAVE_OCTOMAP
+#include "coal/octree.h"
 #endif
 
-#include "hpp/fcl/BV/BV.h"
-#include "hpp/fcl/shape/geometric_shapes_utility.h"
+#include "coal/BV/BV.h"
+#include "coal/shape/geometric_shapes_utility.h"
+
+#include <limits>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace detail {
 
 namespace dynamic_AABB_tree {
 
-#if HPP_FCL_HAVE_OCTOMAP
+#if COAL_HAVE_OCTOMAP
 //==============================================================================
 bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
                        const OcTree* tree2, const OcTree::OcTreeNode* root2,
-                       const AABB& root2_bv, const Transform3f& tf2,
+                       const AABB& root2_bv, const Transform3s& tf2,
                        CollisionCallBackBase* callback) {
   if (!root2) {
     if (root1->isLeaf()) {
@@ -64,12 +63,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 
       if (!obj1->collisionGeometry()->isFree()) {
         OBB obb1, obb2;
-        convertBV(root1->bv, Transform3f::Identity(), obb1);
+        convertBV(root1->bv, Transform3s::Identity(), obb1);
         convertBV(root2_bv, tf2, obb2);
 
         if (obb1.overlap(obb2)) {
           Box* box = new Box();
-          Transform3f box_tf;
+          Transform3s box_tf;
           constructBox(root2_bv, tf2, *box, box_tf);
 
           box->cost_density = tree2->getDefaultOccupancy();
@@ -93,12 +92,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 
     if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
       OBB obb1, obb2;
-      convertBV(root1->bv, Transform3f::Identity(), obb1);
+      convertBV(root1->bv, Transform3s::Identity(), obb1);
       convertBV(root2_bv, tf2, obb2);
 
       if (obb1.overlap(obb2)) {
         Box* box = new Box();
-        Transform3f box_tf;
+        Transform3s box_tf;
         constructBox(root2_bv, tf2, *box, box_tf);
 
         box->cost_density = root2->getOccupancy();
@@ -113,7 +112,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
   }
 
   OBB obb1, obb2;
-  convertBV(root1->bv, Transform3f::Identity(), obb1);
+  convertBV(root1->bv, Transform3s::Identity(), obb1);
   convertBV(root2_bv, tf2, obb2);
 
   if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
@@ -149,12 +148,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 //==============================================================================
 bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
                       const OcTree* tree2, const OcTree::OcTreeNode* root2,
-                      const AABB& root2_bv, const Transform3f& tf2,
-                      DistanceCallBackBase* callback, FCL_REAL& min_dist) {
+                      const AABB& root2_bv, const Transform3s& tf2,
+                      DistanceCallBackBase* callback, CoalScalar& min_dist) {
   if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
     if (tree2->isNodeOccupied(root2)) {
       Box* box = new Box();
-      Transform3f box_tf;
+      Transform3s box_tf;
       constructBox(root2_bv, tf2, *box, box_tf);
       CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
       return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
@@ -170,8 +169,8 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
     AABB aabb2;
     convertBV(root2_bv, tf2, aabb2);
 
-    FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
-    FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
+    CoalScalar d1 = aabb2.distance(root1->children[0]->bv);
+    CoalScalar d2 = aabb2.distance(root1->children[1]->bv);
 
     if (d2 < d1) {
       if (d2 < min_dist) {
@@ -207,7 +206,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 
         AABB aabb2;
         convertBV(child_bv, tf2, aabb2);
-        FCL_REAL d = root1->bv.distance(aabb2);
+        CoalScalar d = root1->bv.distance(aabb2);
 
         if (d < min_dist) {
           if (distanceRecurse_(root1, tree2, child, child_bv, tf2, callback,
@@ -224,7 +223,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 //==============================================================================
 bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
                       const OcTree* tree2, const OcTree::OcTreeNode* root2,
-                      const AABB& root2_bv, const Transform3f& tf2,
+                      const AABB& root2_bv, const Transform3s& tf2,
                       CollisionCallBackBase* callback) {
   if (tf2.rotation().isIdentity())
     return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(),
@@ -236,8 +235,8 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 //==============================================================================
 bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
                      const OcTree* tree2, const OcTree::OcTreeNode* root2,
-                     const AABB& root2_bv, const Transform3f& tf2,
-                     DistanceCallBackBase* callback, FCL_REAL& min_dist) {
+                     const AABB& root2_bv, const Transform3s& tf2,
+                     DistanceCallBackBase* callback, CoalScalar& min_dist) {
   if (tf2.rotation().isIdentity())
     return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(),
                             callback, min_dist);
@@ -408,7 +407,7 @@ bool selfCollisionRecurse(
 //==============================================================================
 bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
                      DynamicAABBTreeCollisionManager::DynamicAABBNode* root2,
-                     DistanceCallBackBase* callback, FCL_REAL& min_dist) {
+                     DistanceCallBackBase* callback, CoalScalar& min_dist) {
   if (root1->isLeaf() && root2->isLeaf()) {
     CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
     CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
@@ -417,8 +416,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 
   if (root2->isLeaf() ||
       (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
-    FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv);
-    FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv);
+    CoalScalar d1 = root2->bv.distance(root1->children[0]->bv);
+    CoalScalar d2 = root2->bv.distance(root1->children[1]->bv);
 
     if (d2 < d1) {
       if (d2 < min_dist) {
@@ -442,8 +441,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
       }
     }
   } else {
-    FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv);
-    FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv);
+    CoalScalar d1 = root1->bv.distance(root2->children[0]->bv);
+    CoalScalar d2 = root1->bv.distance(root2->children[1]->bv);
 
     if (d2 < d1) {
       if (d2 < min_dist) {
@@ -474,14 +473,14 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 //==============================================================================
 bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
                      CollisionObject* query, DistanceCallBackBase* callback,
-                     FCL_REAL& min_dist) {
+                     CoalScalar& min_dist) {
   if (root->isLeaf()) {
     CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
     return (*callback)(root_obj, query, min_dist);
   }
 
-  FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv);
-  FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv);
+  CoalScalar d1 = query->getAABB().distance(root->children[0]->bv);
+  CoalScalar d2 = query->getAABB().distance(root->children[1]->bv);
 
   if (d2 < d1) {
     if (d2 < min_dist) {
@@ -510,7 +509,7 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
 
 //==============================================================================
 bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
-                         DistanceCallBackBase* callback, FCL_REAL& min_dist) {
+                         DistanceCallBackBase* callback, CoalScalar& min_dist) {
   if (root->isLeaf()) return false;
 
   if (selfDistanceRecurse(root->children[0], callback, min_dist)) return true;
@@ -594,7 +593,7 @@ void DynamicAABBTreeCollisionManager::setup() {
 
     size_t height = dtree.getMaxHeight();
 
-    if (((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0)) <
+    if (((CoalScalar)height - std::log((CoalScalar)num) / std::log(2.0)) <
         max_tree_nonbalanced_level)
       dtree.balanceIncremental(tree_incremental_balance_pass);
     else
@@ -611,8 +610,8 @@ void DynamicAABBTreeCollisionManager::update() {
     DynamicAABBNode* node = it->second;
     node->bv = obj->getAABB();
     if (node->bv.volume() <= 0.)
-      HPP_FCL_THROW_PRETTY("The bounding volume has a negative volume.",
-                           std::invalid_argument)
+      COAL_THROW_PRETTY("The bounding volume has a negative volume.",
+                        std::invalid_argument)
   }
 
   dtree.refit();
@@ -667,7 +666,7 @@ void DynamicAABBTreeCollisionManager::collide(
   callback->init();
   if (size() == 0) return;
   switch (obj->collisionGeometry()->getNodeType()) {
-#if HPP_FCL_HAVE_OCTOMAP
+#if COAL_HAVE_OCTOMAP
     case GEOM_OCTREE: {
       if (!octree_as_geometry_collide) {
         const OcTree* octree =
@@ -691,9 +690,9 @@ void DynamicAABBTreeCollisionManager::distance(
     CollisionObject* obj, DistanceCallBackBase* callback) const {
   callback->init();
   if (size() == 0) return;
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   switch (obj->collisionGeometry()->getNodeType()) {
-#if HPP_FCL_HAVE_OCTOMAP
+#if COAL_HAVE_OCTOMAP
     case GEOM_OCTREE: {
       if (!octree_as_geometry_distance) {
         const OcTree* octree =
@@ -725,7 +724,7 @@ void DynamicAABBTreeCollisionManager::distance(
     DistanceCallBackBase* callback) const {
   callback->init();
   if (size() == 0) return;
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback,
                                                  min_dist);
 }
@@ -750,7 +749,7 @@ void DynamicAABBTreeCollisionManager::distance(
   DynamicAABBTreeCollisionManager* other_manager =
       static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
   if ((size() == 0) || (other_manager->size() == 0)) return;
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   detail::dynamic_AABB_tree::distanceRecurse(
       dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist);
 }
@@ -771,5 +770,4 @@ detail::HierarchyTree<AABB>& DynamicAABBTreeCollisionManager::getTree() {
   return dtree;
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
index 4921408372dd16c780f64029559d1f1073c36e85..9306fa8eb1e03dfb4b975c84eaca8a0cab4bf692 100644
--- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
+++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
@@ -35,23 +35,22 @@
 
 /** @author Jia Pan */
 
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
 
-#ifdef HPP_FCL_HAVE_OCTOMAP
-#include "hpp/fcl/octree.h"
+#ifdef COAL_HAVE_OCTOMAP
+#include "coal/octree.h"
 #endif
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace detail {
 namespace dynamic_AABB_tree_array {
 
-#if HPP_FCL_HAVE_OCTOMAP
+#if COAL_HAVE_OCTOMAP
 
 //==============================================================================
 bool collisionRecurse_(
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
     size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
-    const AABB& root2_bv, const Transform3f& tf2,
+    const AABB& root2_bv, const Transform3s& tf2,
     CollisionCallBackBase* callback) {
   DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
       nodes1 + root1_id;
@@ -61,12 +60,12 @@ bool collisionRecurse_(
 
       if (!obj1->collisionGeometry()->isFree()) {
         OBB obb1, obb2;
-        convertBV(root1->bv, Transform3f::Identity(), obb1);
+        convertBV(root1->bv, Transform3s::Identity(), obb1);
         convertBV(root2_bv, tf2, obb2);
 
         if (obb1.overlap(obb2)) {
           Box* box = new Box();
-          Transform3f box_tf;
+          Transform3s box_tf;
           constructBox(root2_bv, tf2, *box, box_tf);
 
           box->cost_density = tree2->getDefaultOccupancy();
@@ -89,12 +88,12 @@ bool collisionRecurse_(
     CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
     if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
       OBB obb1, obb2;
-      convertBV(root1->bv, Transform3f::Identity(), obb1);
+      convertBV(root1->bv, Transform3s::Identity(), obb1);
       convertBV(root2_bv, tf2, obb2);
 
       if (obb1.overlap(obb2)) {
         Box* box = new Box();
-        Transform3f box_tf;
+        Transform3s box_tf;
         constructBox(root2_bv, tf2, *box, box_tf);
 
         box->cost_density = root2->getOccupancy();
@@ -109,7 +108,7 @@ bool collisionRecurse_(
   }
 
   OBB obb1, obb2;
-  convertBV(root1->bv, Transform3f::Identity(), obb1);
+  convertBV(root1->bv, Transform3s::Identity(), obb1);
   convertBV(root2_bv, tf2, obb2);
 
   if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
@@ -149,14 +148,14 @@ bool collisionRecurse_(
 bool distanceRecurse_(
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
     size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
-    const AABB& root2_bv, const Transform3f& tf2,
-    DistanceCallBackBase* callback, FCL_REAL& min_dist) {
+    const AABB& root2_bv, const Transform3s& tf2,
+    DistanceCallBackBase* callback, CoalScalar& min_dist) {
   DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
       nodes1 + root1_id;
   if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
     if (tree2->isNodeOccupied(root2)) {
       Box* box = new Box();
-      Transform3f box_tf;
+      Transform3s box_tf;
       constructBox(root2_bv, tf2, *box, box_tf);
       CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
       return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
@@ -172,8 +171,8 @@ bool distanceRecurse_(
     AABB aabb2;
     convertBV(root2_bv, tf2, aabb2);
 
-    FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
-    FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
+    CoalScalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
+    CoalScalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
 
     if (d2 < d1) {
       if (d2 < min_dist) {
@@ -209,7 +208,7 @@ bool distanceRecurse_(
 
         AABB aabb2;
         convertBV(child_bv, tf2, aabb2);
-        FCL_REAL d = root1->bv.distance(aabb2);
+        CoalScalar d = root1->bv.distance(aabb2);
 
         if (d < min_dist) {
           if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2,
@@ -263,7 +262,7 @@ bool collisionRecurse(
 }
 
 //==============================================================================
-inline HPP_FCL_DLLAPI bool collisionRecurse(
+inline COAL_DLLAPI bool collisionRecurse(
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes,
     size_t root_id, CollisionObject* query, CollisionCallBackBase* callback) {
   DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id;
@@ -308,7 +307,7 @@ bool distanceRecurse(
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
     size_t root1_id,
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2,
-    size_t root2_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) {
+    size_t root2_id, DistanceCallBackBase* callback, CoalScalar& min_dist) {
   DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
       nodes1 + root1_id;
   DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 =
@@ -321,8 +320,8 @@ bool distanceRecurse(
 
   if (root2->isLeaf() ||
       (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
-    FCL_REAL d1 = root2->bv.distance((nodes1 + root1->children[0])->bv);
-    FCL_REAL d2 = root2->bv.distance((nodes1 + root1->children[1])->bv);
+    CoalScalar d1 = root2->bv.distance((nodes1 + root1->children[0])->bv);
+    CoalScalar d2 = root2->bv.distance((nodes1 + root1->children[1])->bv);
 
     if (d2 < d1) {
       if (d2 < min_dist) {
@@ -350,8 +349,8 @@ bool distanceRecurse(
       }
     }
   } else {
-    FCL_REAL d1 = root1->bv.distance((nodes2 + root2->children[0])->bv);
-    FCL_REAL d2 = root1->bv.distance((nodes2 + root2->children[1])->bv);
+    CoalScalar d1 = root1->bv.distance((nodes2 + root2->children[0])->bv);
+    CoalScalar d2 = root1->bv.distance((nodes2 + root2->children[1])->bv);
 
     if (d2 < d1) {
       if (d2 < min_dist) {
@@ -387,15 +386,15 @@ bool distanceRecurse(
 bool distanceRecurse(
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes,
     size_t root_id, CollisionObject* query, DistanceCallBackBase* callback,
-    FCL_REAL& min_dist) {
+    CoalScalar& min_dist) {
   DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id;
   if (root->isLeaf()) {
     CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
     return (*callback)(root_obj, query, min_dist);
   }
 
-  FCL_REAL d1 = query->getAABB().distance((nodes + root->children[0])->bv);
-  FCL_REAL d2 = query->getAABB().distance((nodes + root->children[1])->bv);
+  CoalScalar d1 = query->getAABB().distance((nodes + root->children[0])->bv);
+  CoalScalar d2 = query->getAABB().distance((nodes + root->children[1])->bv);
 
   if (d2 < d1) {
     if (d2 < min_dist) {
@@ -425,7 +424,7 @@ bool distanceRecurse(
 //==============================================================================
 bool selfDistanceRecurse(
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes,
-    size_t root_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) {
+    size_t root_id, DistanceCallBackBase* callback, CoalScalar& min_dist) {
   DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id;
   if (root->isLeaf()) return false;
 
@@ -442,13 +441,13 @@ bool selfDistanceRecurse(
   return false;
 }
 
-#if HPP_FCL_HAVE_OCTOMAP
+#if COAL_HAVE_OCTOMAP
 
 //==============================================================================
 bool collisionRecurse(
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
     size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
-    const AABB& root2_bv, const Transform3f& tf2,
+    const AABB& root2_bv, const Transform3s& tf2,
     CollisionCallBackBase* callback) {
   if (tf2.rotation().isIdentity())
     return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
@@ -462,8 +461,8 @@ bool collisionRecurse(
 bool distanceRecurse(
     DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
     size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
-    const AABB& root2_bv, const Transform3f& tf2,
-    DistanceCallBackBase* callback, FCL_REAL& min_dist) {
+    const AABB& root2_bv, const Transform3s& tf2,
+    DistanceCallBackBase* callback, CoalScalar& min_dist) {
   if (tf2.rotation().isIdentity())
     return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
                             tf2.translation(), callback, min_dist);
@@ -546,7 +545,7 @@ void DynamicAABBTreeArrayCollisionManager::setup() {
 
     int height = (int)dtree.getMaxHeight();
 
-    if ((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0) <
+    if ((CoalScalar)height - std::log((CoalScalar)num) / std::log(2.0) <
         max_tree_nonbalanced_level)
       dtree.balanceIncremental(tree_incremental_balance_pass);
     else
@@ -618,7 +617,7 @@ void DynamicAABBTreeArrayCollisionManager::collide(
   callback->init();
   if (size() == 0) return;
   switch (obj->collisionGeometry()->getNodeType()) {
-#if HPP_FCL_HAVE_OCTOMAP
+#if COAL_HAVE_OCTOMAP
     case GEOM_OCTREE: {
       if (!octree_as_geometry_collide) {
         const OcTree* octree =
@@ -642,9 +641,9 @@ void DynamicAABBTreeArrayCollisionManager::distance(
     CollisionObject* obj, DistanceCallBackBase* callback) const {
   callback->init();
   if (size() == 0) return;
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   switch (obj->collisionGeometry()->getNodeType()) {
-#if HPP_FCL_HAVE_OCTOMAP
+#if COAL_HAVE_OCTOMAP
     case GEOM_OCTREE: {
       if (!octree_as_geometry_distance) {
         const OcTree* octree =
@@ -677,7 +676,7 @@ void DynamicAABBTreeArrayCollisionManager::distance(
     DistanceCallBackBase* callback) const {
   callback->init();
   if (size() == 0) return;
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   detail::dynamic_AABB_tree_array::selfDistanceRecurse(
       dtree.getNodes(), dtree.getRoot(), callback, min_dist);
 }
@@ -703,7 +702,7 @@ void DynamicAABBTreeArrayCollisionManager::distance(
   DynamicAABBTreeArrayCollisionManager* other_manager =
       static_cast<DynamicAABBTreeArrayCollisionManager*>(other_manager_);
   if ((size() == 0) || (other_manager->size() == 0)) return;
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   detail::dynamic_AABB_tree_array::distanceRecurse(
       dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(),
       other_manager->dtree.getRoot(), callback, min_dist);
@@ -725,5 +724,4 @@ DynamicAABBTreeArrayCollisionManager::getTree() const {
   return dtree;
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp
index aff1c6d25d86ab8694879ffb0274f06160145c18..5f65e1be97df9b1e1a41356d565b288790f7f1f2 100644
--- a/src/broadphase/broadphase_interval_tree.cpp
+++ b/src/broadphase/broadphase_interval_tree.cpp
@@ -35,10 +35,9 @@
 
 /** @author Jia Pan */
 
-#include "hpp/fcl/broadphase/broadphase_interval_tree.h"
+#include "coal/broadphase/broadphase_interval_tree.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 //==============================================================================
 void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) {
@@ -363,25 +362,25 @@ void IntervalTreeCollisionManager::distance(
     CollisionObject* obj, DistanceCallBackBase* callback) const {
   callback->init();
   if (size() == 0) return;
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
   distance_(obj, callback, min_dist);
 }
 
 //==============================================================================
 bool IntervalTreeCollisionManager::distance_(CollisionObject* obj,
                                              DistanceCallBackBase* callback,
-                                             FCL_REAL& min_dist) const {
+                                             CoalScalar& min_dist) const {
   static const unsigned int CUTOFF = 100;
 
-  Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
   AABB aabb = obj->getAABB();
-  if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) {
-    Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+  if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
+    Vec3s min_dist_delta(min_dist, min_dist, min_dist);
     aabb.expand(min_dist_delta);
   }
 
   int status = 1;
-  FCL_REAL old_min_distance;
+  CoalScalar old_min_distance;
 
   while (1) {
     bool dist_res = false;
@@ -426,11 +425,11 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj,
     results2.clear();
 
     if (status == 1) {
-      if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
+      if (old_min_distance < (std::numeric_limits<CoalScalar>::max)())
         break;
       else {
         if (min_dist < old_min_distance) {
-          Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+          Vec3s min_dist_delta(min_dist, min_dist, min_dist);
           aabb = AABB(obj->getAABB(), min_dist_delta);
           status = 0;
         } else {
@@ -456,9 +455,9 @@ void IntervalTreeCollisionManager::collide(
   std::set<CollisionObject*> active;
   std::set<std::pair<CollisionObject*, CollisionObject*> > overlap;
   size_t n = endpoints[0].size();
-  FCL_REAL diff_x = endpoints[0][0].value - endpoints[0][n - 1].value;
-  FCL_REAL diff_y = endpoints[1][0].value - endpoints[1][n - 1].value;
-  FCL_REAL diff_z = endpoints[2][0].value - endpoints[2][n - 1].value;
+  CoalScalar diff_x = endpoints[0][0].value - endpoints[0][n - 1].value;
+  CoalScalar diff_y = endpoints[1][0].value - endpoints[1][n - 1].value;
+  CoalScalar diff_z = endpoints[2][0].value - endpoints[2][n - 1].value;
 
   int axis = 0;
   if (diff_y > diff_x && diff_y > diff_z)
@@ -509,7 +508,7 @@ void IntervalTreeCollisionManager::distance(
 
   this->enable_tested_set_ = true;
   this->tested_set.clear();
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
 
   for (size_t i = 0; i < endpoints[0].size(); ++i)
     if (distance_(endpoints[0][i].obj, callback, min_dist)) break;
@@ -557,7 +556,7 @@ void IntervalTreeCollisionManager::distance(
     return;
   }
 
-  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
 
   if (this->size() < other_manager->size()) {
     for (size_t i = 0, size = endpoints[0].size(); i < size; ++i)
@@ -604,7 +603,7 @@ bool IntervalTreeCollisionManager::checkDist(
     typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
     typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
     CollisionObject* obj, DistanceCallBackBase* callback,
-    FCL_REAL& min_dist) const {
+    CoalScalar& min_dist) const {
   while (pos_start < pos_end) {
     SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
     if (ivl->obj != obj) {
@@ -636,8 +635,8 @@ bool IntervalTreeCollisionManager::EndPoint::operator<(
 }
 
 //==============================================================================
-IntervalTreeCollisionManager::SAPInterval::SAPInterval(FCL_REAL low_,
-                                                       FCL_REAL high_,
+IntervalTreeCollisionManager::SAPInterval::SAPInterval(CoalScalar low_,
+                                                       CoalScalar high_,
                                                        CollisionObject* obj_)
     : detail::SimpleInterval() {
   this->low = low_;
@@ -645,5 +644,4 @@ IntervalTreeCollisionManager::SAPInterval::SAPInterval(FCL_REAL low_,
   obj = obj_;
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/default_broadphase_callbacks.cpp b/src/broadphase/default_broadphase_callbacks.cpp
index ea88687d4edad880639477d9205e296731907c46..8e54a47e2b8fc4c84c3777a102fa9cc67666fc10 100644
--- a/src/broadphase/default_broadphase_callbacks.cpp
+++ b/src/broadphase/default_broadphase_callbacks.cpp
@@ -34,11 +34,10 @@
 
 /** @author Sean Curtis (sean@tri.global) */
 
-#include "hpp/fcl/broadphase/default_broadphase_callbacks.h"
+#include "coal/broadphase/default_broadphase_callbacks.h"
 #include <algorithm>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
                               void* data) {
@@ -65,7 +64,7 @@ bool CollisionCallBackDefault::collide(CollisionObject* o1,
 }
 
 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
-                             void* data, FCL_REAL& dist) {
+                             void* data, CoalScalar& dist) {
   assert(data != nullptr);
   auto* cdata = static_cast<DistanceData*>(data);
   const DistanceRequest& request = cdata->request;
@@ -86,7 +85,7 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
 }
 
 bool DistanceCallBackDefault::distance(CollisionObject* o1, CollisionObject* o2,
-                                       FCL_REAL& dist) {
+                                       CoalScalar& dist) {
   return defaultDistanceFunction(o1, o2, &data, dist);
 }
 
@@ -122,5 +121,4 @@ bool CollisionCallBackCollect::exist(const CollisionPair& pair) const {
          collision_pairs.end();
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/detail/interval_tree.cpp b/src/broadphase/detail/interval_tree.cpp
index 5671959889bbab7a99b4fba25dae120ad6593a69..34a2326c42772d928f2b1ec72886743c9be8e59d 100644
--- a/src/broadphase/detail/interval_tree.cpp
+++ b/src/broadphase/detail/interval_tree.cpp
@@ -35,15 +35,14 @@
 
 /** @author Jia Pan */
 
-#ifndef HPP_FCL_INTERVAL_TREE_INL_H
-#define HPP_FCL_INTERVAL_TREE_INL_H
+#ifndef COAL_INTERVAL_TREE_INL_H
+#define COAL_INTERVAL_TREE_INL_H
 
-#include "hpp/fcl/broadphase/detail/interval_tree.h"
+#include "coal/broadphase/detail/interval_tree.h"
 
 #include <algorithm>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace detail {
 
 //==============================================================================
@@ -53,13 +52,13 @@ IntervalTree::IntervalTree() {
       invalid_node;
   invalid_node->red = false;
   invalid_node->key = invalid_node->high = invalid_node->max_high =
-      -(std::numeric_limits<FCL_REAL>::max)();
+      -(std::numeric_limits<CoalScalar>::max)();
   invalid_node->stored_interval = nullptr;
 
   root = new IntervalTreeNode;
   root->parent = root->left = root->right = invalid_node;
   root->key = root->high = root->max_high =
-      (std::numeric_limits<FCL_REAL>::max)();
+      (std::numeric_limits<CoalScalar>::max)();
   root->red = false;
   root->stored_interval = nullptr;
 
@@ -382,7 +381,7 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) {
   /// @brief y should not be invalid_node in this case
   /// y is the node to splice out and x is its child
   if (y != z) {
-    y->max_high = -(std::numeric_limits<FCL_REAL>::max)();
+    y->max_high = -(std::numeric_limits<CoalScalar>::max)();
     y->left = z->left;
     y->right = z->right;
     y->parent = z->parent;
@@ -410,7 +409,7 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) {
 
 //==============================================================================
 /// @brief returns 1 if the intervals overlap, and 0 otherwise
-bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) {
+bool overlap(CoalScalar a1, CoalScalar a2, CoalScalar b1, CoalScalar b2) {
   if (a1 <= b1) {
     return (b1 <= a2);
   } else {
@@ -419,7 +418,8 @@ bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) {
 }
 
 //==============================================================================
-std::deque<SimpleInterval*> IntervalTree::query(FCL_REAL low, FCL_REAL high) {
+std::deque<SimpleInterval*> IntervalTree::query(CoalScalar low,
+                                                CoalScalar high) {
   std::deque<SimpleInterval*> result_stack;
   IntervalTreeNode* x = root->left;
   bool run = (x != invalid_node);
@@ -463,7 +463,6 @@ std::deque<SimpleInterval*> IntervalTree::query(FCL_REAL low, FCL_REAL high) {
 }
 
 }  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
 #endif
diff --git a/src/broadphase/detail/interval_tree_node.cpp b/src/broadphase/detail/interval_tree_node.cpp
index beb9e28d6ebce31d97ec0ae4d82a6ed6444a23be..51a1a457ae985a9838f9c7f69a254276a157b96e 100644
--- a/src/broadphase/detail/interval_tree_node.cpp
+++ b/src/broadphase/detail/interval_tree_node.cpp
@@ -35,16 +35,15 @@
 
 /** @author Jia Pan */
 
-#ifndef HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
-#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
+#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
+#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
 
-#include "hpp/fcl/broadphase/detail/interval_tree_node.h"
+#include "coal/broadphase/detail/interval_tree_node.h"
 
 #include <iostream>
 #include <algorithm>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace detail {
 
 //==============================================================================
@@ -90,7 +89,6 @@ void IntervalTreeNode::print(IntervalTreeNode* invalid_node,
 }
 
 }  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
 #endif
diff --git a/src/broadphase/detail/morton-inl.h b/src/broadphase/detail/morton-inl.h
index 53d7d90f77d6a58f314933b2e5bbea333b5595f9..b646f299e8e4f5220ed1ba4f1cdb680757d5bad6 100644
--- a/src/broadphase/detail/morton-inl.h
+++ b/src/broadphase/detail/morton-inl.h
@@ -36,13 +36,12 @@
 
 /** @author Jia Pan */
 
-#ifndef HPP_FCL_MORTON_INL_H
-#define HPP_FCL_MORTON_INL_H
+#ifndef COAL_MORTON_INL_H
+#define COAL_MORTON_INL_H
 
-#include "hpp/fcl/broadphase/detail/morton.h"
+#include "coal/broadphase/detail/morton.h"
 
-namespace hpp {
-namespace fcl {  /// @cond IGNORE
+namespace coal {  /// @cond IGNORE
 namespace detail {
 
 //==============================================================================
@@ -63,7 +62,7 @@ morton_functor<S, uint32_t>::morton_functor(const AABB& bbox)
 
 //==============================================================================
 template <typename S>
-uint32_t morton_functor<S, uint32_t>::operator()(const Vec3f& point) const {
+uint32_t morton_functor<S, uint32_t>::operator()(const Vec3s& point) const {
   uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
   uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
   uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
@@ -83,7 +82,7 @@ morton_functor<S, uint64_t>::morton_functor(const AABB& bbox)
 
 //==============================================================================
 template <typename S>
-uint64_t morton_functor<S, uint64_t>::operator()(const Vec3f& point) const {
+uint64_t morton_functor<S, uint64_t>::operator()(const Vec3s& point) const {
   uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
   uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
   uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20);
@@ -116,7 +115,7 @@ morton_functor<S, std::bitset<N>>::morton_functor(const AABB& bbox)
 //==============================================================================
 template <typename S, size_t N>
 std::bitset<N> morton_functor<S, std::bitset<N>>::operator()(
-    const Vec3f& point) const {
+    const Vec3s& point) const {
   S x = (point[0] - base[0]) * inv[0];
   S y = (point[1] - base[1]) * inv[1];
   S z = (point[2] - base[2]) * inv[2];
@@ -147,7 +146,6 @@ constexpr size_t morton_functor<S, std::bitset<N>>::bits() {
 
 }  // namespace detail
 /// @endcond
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
 #endif
diff --git a/src/broadphase/detail/morton.cpp b/src/broadphase/detail/morton.cpp
index 54beaa104a86ccd4b62f7db075e901539b517fd7..17e554f0e7ee93ed5df524d70e03b3ab0a45702c 100644
--- a/src/broadphase/detail/morton.cpp
+++ b/src/broadphase/detail/morton.cpp
@@ -36,10 +36,9 @@
 
 /** @author Jia Pan */
 
-#include "hpp/fcl/broadphase/detail/morton-inl.h"
+#include "coal/broadphase/detail/morton-inl.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 /// @cond IGNORE
 namespace detail {
@@ -79,5 +78,4 @@ uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z) {
 
 }  // namespace detail
 /// @endcond
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/broadphase/detail/simple_interval.cpp b/src/broadphase/detail/simple_interval.cpp
index 78719156c9f83e9ae72fd3cdd2d2d5d1ac4013bb..95075cbc19426df136380ae47c42fef70914db8c 100644
--- a/src/broadphase/detail/simple_interval.cpp
+++ b/src/broadphase/detail/simple_interval.cpp
@@ -35,14 +35,13 @@
 
 /** @author Jia Pan */
 
-#ifndef HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
-#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
+#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
+#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
 
-#include "hpp/fcl/broadphase/detail/simple_interval.h"
+#include "coal/broadphase/detail/simple_interval.h"
 #include <algorithm>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace detail {
 
 //==============================================================================
@@ -56,7 +55,6 @@ void SimpleInterval::print() {
 }
 
 }  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
 #endif
diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp
index 07f7b663178f157de4b252207e3616051eeab993..dd92fb7c58b20cfc70f718e502420a5a2a3f1344 100644
--- a/src/broadphase/detail/spatial_hash.cpp
+++ b/src/broadphase/detail/spatial_hash.cpp
@@ -35,18 +35,17 @@
 
 /** @author Jia Pan */
 
-#ifndef HPP_FCL_BROADPHASE_SPATIALHASH_INL_H
-#define HPP_FCL_BROADPHASE_SPATIALHASH_INL_H
+#ifndef COAL_BROADPHASE_SPATIALHASH_INL_H
+#define COAL_BROADPHASE_SPATIALHASH_INL_H
 
-#include "hpp/fcl/broadphase/detail/spatial_hash.h"
+#include "coal/broadphase/detail/spatial_hash.h"
 #include <algorithm>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace detail {
 
 //==============================================================================
-SpatialHash::SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_)
+SpatialHash::SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_)
     : cell_size(cell_size_), scene_limit(scene_limit_) {
   width[0] =
       static_cast<unsigned int>(std::ceil(scene_limit.width() / cell_size));
@@ -85,7 +84,6 @@ std::vector<unsigned int> SpatialHash::operator()(const AABB& aabb) const {
 }
 
 }  // namespace detail
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
 #endif
diff --git a/src/collision.cpp b/src/collision.cpp
index 9db29dd7b2e0215c853b1dd78f49bff69cbacf24..95085f3be85b523992d62b2090137b89c4f8653f 100644
--- a/src/collision.cpp
+++ b/src/collision.cpp
@@ -35,13 +35,12 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/collision_utility.h>
-#include <hpp/fcl/collision_func_matrix.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
+#include "coal/collision.h"
+#include "coal/collision_utility.h"
+#include "coal/collision_func_matrix.h"
+#include "coal/narrowphase/narrowphase.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 CollisionFunctionMatrix& getCollisionFunctionLookTable() {
   static CollisionFunctionMatrix table;
@@ -66,11 +65,11 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
                  result);
 }
 
-std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
-                    const CollisionGeometry* o2, const Transform3f& tf2,
+std::size_t collide(const CollisionGeometry* o1, const Transform3s& tf1,
+                    const CollisionGeometry* o2, const Transform3s& tf2,
                     const CollisionRequest& request, CollisionResult& result) {
   // If security margin is set to -infinity, return that there is no collision
-  if (request.security_margin == -std::numeric_limits<FCL_REAL>::infinity()) {
+  if (request.security_margin == -std::numeric_limits<CoalScalar>::infinity()) {
     result.clear();
     return false;
   }
@@ -80,8 +79,8 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
   const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
   std::size_t res;
   if (request.num_max_contacts == 0) {
-    HPP_FCL_THROW_PRETTY("Invalid number of max contacts (current value is 0).",
-                         std::invalid_argument);
+    COAL_THROW_PRETTY("Invalid number of max contacts (current value is 0).",
+                      std::invalid_argument);
     res = 0;
   } else {
     OBJECT_TYPE object_type1 = o1->getObjectType();
@@ -92,12 +91,12 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
     if (object_type1 == OT_GEOM &&
         (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
       if (!looktable.collision_matrix[node_type2][node_type1]) {
-        HPP_FCL_THROW_PRETTY("Collision function between node type "
-                                 << std::string(get_node_type_name(node_type1))
-                                 << " and node type "
-                                 << std::string(get_node_type_name(node_type2))
-                                 << " is not yet supported.",
-                             std::invalid_argument);
+        COAL_THROW_PRETTY("Collision function between node type "
+                              << std::string(get_node_type_name(node_type1))
+                              << " and node type "
+                              << std::string(get_node_type_name(node_type2))
+                              << " is not yet supported.",
+                          std::invalid_argument);
         res = 0;
       } else {
         res = looktable.collision_matrix[node_type2][node_type1](
@@ -108,12 +107,12 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
       }
     } else {
       if (!looktable.collision_matrix[node_type1][node_type2]) {
-        HPP_FCL_THROW_PRETTY("Collision function between node type "
-                                 << std::string(get_node_type_name(node_type1))
-                                 << " and node type "
-                                 << std::string(get_node_type_name(node_type2))
-                                 << " is not yet supported.",
-                             std::invalid_argument);
+        COAL_THROW_PRETTY("Collision function between node type "
+                              << std::string(get_node_type_name(node_type1))
+                              << " and node type "
+                              << std::string(get_node_type_name(node_type2))
+                              << " is not yet supported.",
+                          std::invalid_argument);
         res = 0;
       } else
         res = looktable.collision_matrix[node_type1][node_type2](
@@ -144,12 +143,12 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1,
 
   if ((swap_geoms && !looktable.collision_matrix[node_type2][node_type1]) ||
       (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2])) {
-    HPP_FCL_THROW_PRETTY("Collision function between node type "
-                             << std::string(get_node_type_name(node_type1))
-                             << " and node type "
-                             << std::string(get_node_type_name(node_type2))
-                             << " is not yet supported.",
-                         std::invalid_argument);
+    COAL_THROW_PRETTY("Collision function between node type "
+                          << std::string(get_node_type_name(node_type1))
+                          << " and node type "
+                          << std::string(get_node_type_name(node_type2))
+                          << " is not yet supported.",
+                      std::invalid_argument);
   }
   if (swap_geoms)
     func = looktable.collision_matrix[node_type2][node_type1];
@@ -157,12 +156,12 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1,
     func = looktable.collision_matrix[node_type1][node_type2];
 }
 
-std::size_t ComputeCollision::run(const Transform3f& tf1,
-                                  const Transform3f& tf2,
+std::size_t ComputeCollision::run(const Transform3s& tf1,
+                                  const Transform3s& tf2,
                                   const CollisionRequest& request,
                                   CollisionResult& result) const {
   // If security margin is set to -infinity, return that there is no collision
-  if (request.security_margin == -std::numeric_limits<FCL_REAL>::infinity()) {
+  if (request.security_margin == -std::numeric_limits<CoalScalar>::infinity()) {
     result.clear();
     return false;
   }
@@ -184,8 +183,8 @@ std::size_t ComputeCollision::run(const Transform3f& tf1,
   return res;
 }
 
-std::size_t ComputeCollision::operator()(const Transform3f& tf1,
-                                         const Transform3f& tf2,
+std::size_t ComputeCollision::operator()(const Transform3s& tf1,
+                                         const Transform3s& tf2,
                                          const CollisionRequest& request,
                                          CollisionResult& result) const
 
@@ -203,5 +202,4 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1,
   return res;
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/collision_data.cpp b/src/collision_data.cpp
index 2572014df926f5ed692ad90c0eb2d29bf499feeb..7561ba8310a71f546697a82a57011f28222cd567 100644
--- a/src/collision_data.cpp
+++ b/src/collision_data.cpp
@@ -36,10 +36,9 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/collision_data.h>
+#include "coal/collision_data.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 bool CollisionRequest::isSatisfied(const CollisionResult& result) const {
   return result.isCollision() && (num_max_contacts <= result.numContacts());
@@ -49,5 +48,4 @@ bool DistanceRequest::isSatisfied(const DistanceResult& result) const {
   return (result.min_distance <= 0);
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index d9fd859e1c478e72a565f49b18bca85d5a040de6..b654cfbda11ef51db4eafff44c04afe0eb214a00 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -35,32 +35,30 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/collision_func_matrix.h>
+#include "coal/collision_func_matrix.h"
 
-#include <hpp/fcl/internal/traversal_node_setup.h>
+#include "coal/internal/traversal_node_setup.h"
 #include <../src/collision_node.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
-#include <hpp/fcl/shape/geometric_shapes_traits.h>
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/internal/shape_shape_func.h"
+#include "coal/shape/geometric_shapes_traits.h"
 #include <../src/traits_traversal.h>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
 
 template <typename TypeA, typename TypeB>
-std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1,
-                          const CollisionGeometry* o2, const Transform3f& tf2,
+std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3s& tf1,
+                          const CollisionGeometry* o2, const Transform3s& tf2,
                           const GJKSolver* nsolver,
                           const CollisionRequest& request,
                           CollisionResult& result) {
   if (request.isSatisfied(result)) return result.numContacts();
 
   if (request.security_margin < 0)
-    HPP_FCL_THROW_PRETTY(
-        "Negative security margin are not handled yet for Octree",
-        std::invalid_argument);
+    COAL_THROW_PRETTY("Negative security margin are not handled yet for Octree",
+                      std::invalid_argument);
 
   typename TraversalTraitsCollision<TypeA, TypeB>::CollisionTraversal_t node(
       request);
@@ -99,26 +97,25 @@ BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS);
 ///         - 0 if the query should be made with non-aligned object frames.
 template <typename T_BVH, typename T_SH,
           int _Options = details::bvh_shape_traits<T_BVH, T_SH>::Options>
-struct HPP_FCL_LOCAL BVHShapeCollider{static std::size_t collide(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2,
+struct COAL_LOCAL BVHShapeCollider{static std::size_t collide(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2,
     const GJKSolver* nsolver, const CollisionRequest& request,
     CollisionResult& result){
     if (request.isSatisfied(result)) return result.numContacts();
 
 if (request.security_margin < 0)
-  HPP_FCL_THROW_PRETTY(
-      "Negative security margin are not handled yet for BVHModel",
-      std::invalid_argument);
+  COAL_THROW_PRETTY("Negative security margin are not handled yet for BVHModel",
+                    std::invalid_argument);
 
 if (_Options & RelativeTransformationIsIdentity)
   return aligned(o1, tf1, o2, tf2, nsolver, request, result);
 else
   return oriented(o1, tf1, o2, tf2, nsolver, request, result);
-}  // namespace fcl
+}  // namespace coal
 
-static std::size_t aligned(const CollisionGeometry* o1, const Transform3f& tf1,
-                           const CollisionGeometry* o2, const Transform3f& tf2,
+static std::size_t aligned(const CollisionGeometry* o1, const Transform3s& tf1,
+                           const CollisionGeometry* o2, const Transform3s& tf2,
                            const GJKSolver* nsolver,
                            const CollisionRequest& request,
                            CollisionResult& result) {
@@ -128,18 +125,18 @@ static std::size_t aligned(const CollisionGeometry* o1, const Transform3f& tf1,
       node(request);
   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
   BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
-  Transform3f tf1_tmp = tf1;
+  Transform3s tf1_tmp = tf1;
   const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
   initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result);
-  fcl::collide(&node, request, result);
+  coal::collide(&node, request, result);
 
   delete obj1_tmp;
   return result.numContacts();
 }
 
-static std::size_t oriented(const CollisionGeometry* o1, const Transform3f& tf1,
-                            const CollisionGeometry* o2, const Transform3f& tf2,
+static std::size_t oriented(const CollisionGeometry* o1, const Transform3s& tf1,
+                            const CollisionGeometry* o2, const Transform3s& tf2,
                             const GJKSolver* nsolver,
                             const CollisionRequest& request,
                             CollisionResult& result) {
@@ -150,10 +147,11 @@ static std::size_t oriented(const CollisionGeometry* o1, const Transform3f& tf1,
   const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
   initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
-  fcl::collide(&node, request, result);
+  coal::collide(&node, request, result);
   return result.numContacts();
 }
-};  // namespace hpp
+}
+;
 
 /// @brief Collider functor for HeightField data structure
 /// \tparam _Options takes two values.
@@ -161,13 +159,13 @@ static std::size_t oriented(const CollisionGeometry* o1, const Transform3f& tf1,
 ///           into the frame of object 2 before computing collisions.
 ///         - 0 if the query should be made with non-aligned object frames.
 template <typename BV, typename Shape>
-struct HPP_FCL_LOCAL HeightFieldShapeCollider {
+struct COAL_LOCAL HeightFieldShapeCollider {
   typedef HeightField<BV> HF;
 
   static std::size_t collide(const CollisionGeometry* o1,
-                             const Transform3f& tf1,
+                             const Transform3s& tf1,
                              const CollisionGeometry* o2,
-                             const Transform3f& tf2, const GJKSolver* nsolver,
+                             const Transform3s& tf2, const GJKSolver* nsolver,
                              const CollisionRequest& request,
                              CollisionResult& result) {
     if (request.isSatisfied(result)) return result.numContacts();
@@ -178,7 +176,7 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider {
     HeightFieldShapeCollisionTraversalNode<BV, Shape, 0> node(request);
 
     initialize(node, height_field, tf1, shape, tf2, nsolver, result);
-    fcl::collide(&node, request, result);
+    coal::collide(&node, request, result);
     return result.numContacts();
   }
 };
@@ -186,9 +184,9 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider {
 namespace details {
 template <typename OrientedMeshCollisionTraversalNode, typename T_BVH>
 std::size_t orientedMeshCollide(const CollisionGeometry* o1,
-                                const Transform3f& tf1,
+                                const Transform3s& tf1,
                                 const CollisionGeometry* o2,
-                                const Transform3f& tf2,
+                                const Transform3s& tf2,
                                 const CollisionRequest& request,
                                 CollisionResult& result) {
   if (request.isSatisfied(result)) return result.numContacts();
@@ -206,8 +204,8 @@ std::size_t orientedMeshCollide(const CollisionGeometry* o1,
 }  // namespace details
 
 template <typename T_BVH>
-std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1,
-                       const CollisionGeometry* o2, const Transform3f& tf2,
+std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1,
+                       const CollisionGeometry* o2, const Transform3s& tf2,
                        const CollisionRequest& request,
                        CollisionResult& result) {
   if (request.isSatisfied(result)) return result.numContacts();
@@ -223,12 +221,12 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1,
   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
   BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
-  Transform3f tf1_tmp = tf1;
+  Transform3s tf1_tmp = tf1;
   BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
-  Transform3f tf2_tmp = tf2;
+  Transform3s tf2_tmp = tf2;
 
   initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result);
-  fcl::collide(&node, request, result);
+  coal::collide(&node, request, result);
 
   delete obj1_tmp;
   delete obj2_tmp;
@@ -237,8 +235,8 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1,
 }
 
 template <>
-std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1,
-                            const CollisionGeometry* o2, const Transform3f& tf2,
+std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3s& tf1,
+                            const CollisionGeometry* o2, const Transform3s& tf2,
                             const CollisionRequest& request,
                             CollisionResult& result) {
   return details::orientedMeshCollide<MeshCollisionTraversalNodeOBB, OBB>(
@@ -247,9 +245,9 @@ std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1,
 
 template <>
 std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1,
-                               const Transform3f& tf1,
+                               const Transform3s& tf1,
                                const CollisionGeometry* o2,
-                               const Transform3f& tf2,
+                               const Transform3s& tf2,
                                const CollisionRequest& request,
                                CollisionResult& result) {
   return details::orientedMeshCollide<MeshCollisionTraversalNodeOBBRSS, OBBRSS>(
@@ -258,9 +256,9 @@ std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1,
 
 template <>
 std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1,
-                             const Transform3f& tf1,
+                             const Transform3s& tf1,
                              const CollisionGeometry* o2,
-                             const Transform3f& tf2,
+                             const Transform3s& tf2,
                              const CollisionRequest& request,
                              CollisionResult& result) {
   return details::orientedMeshCollide<MeshCollisionTraversalNodekIOS, kIOS>(
@@ -268,8 +266,8 @@ std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1,
 }
 
 template <typename T_BVH>
-std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1,
-                       const CollisionGeometry* o2, const Transform3f& tf2,
+std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1,
+                       const CollisionGeometry* o2, const Transform3s& tf2,
                        const GJKSolver* /*nsolver*/,
                        const CollisionRequest& request,
                        CollisionResult& result) {
@@ -657,7 +655,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() {
   collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS>;
   collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS>;
 
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
   collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OctreeCollide<OcTree, Box>;
   collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OctreeCollide<OcTree, Sphere>;
   collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OctreeCollide<OcTree, Capsule>;
@@ -728,6 +726,4 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() {
 #endif
 }
 // template struct CollisionFunctionMatrix;
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/collision_node.cpp b/src/collision_node.cpp
index 4a016916c7b66d75495174e01cffbbcbe32c66b2..77c6adb04e594cdb8e4a7010eafc28314a1c7447 100644
--- a/src/collision_node.cpp
+++ b/src/collision_node.cpp
@@ -36,28 +36,26 @@
 /** \author Jia Pan */
 
 #include <../src/collision_node.h>
-#include <hpp/fcl/internal/traversal_recurse.h>
+#include "coal/internal/traversal_recurse.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 void checkResultLowerBound(const CollisionResult& result,
-                           FCL_REAL sqrDistLowerBound) {
-  HPP_FCL_UNUSED_VARIABLE(result);
-  const FCL_REAL dummy_precision =
-      std::sqrt(Eigen::NumTraits<FCL_REAL>::epsilon());
-  HPP_FCL_UNUSED_VARIABLE(dummy_precision);
+                           CoalScalar sqrDistLowerBound) {
+  COAL_UNUSED_VARIABLE(result);
+  const CoalScalar dummy_precision =
+      std::sqrt(Eigen::NumTraits<CoalScalar>::epsilon());
+  COAL_UNUSED_VARIABLE(dummy_precision);
   if (sqrDistLowerBound == 0) {
-    HPP_FCL_ASSERT(result.distance_lower_bound <= dummy_precision,
-                   "Distance lower bound should not be positive.",
-                   std::logic_error);
+    COAL_ASSERT(result.distance_lower_bound <= dummy_precision,
+                "Distance lower bound should not be positive.",
+                std::logic_error);
   } else {
-    HPP_FCL_ASSERT(
-        result.distance_lower_bound * result.distance_lower_bound -
-                sqrDistLowerBound <
-            dummy_precision,
-        "Distance lower bound and sqrDistLowerBound should coincide.",
-        std::logic_error);
+    COAL_ASSERT(result.distance_lower_bound * result.distance_lower_bound -
+                        sqrDistLowerBound <
+                    dummy_precision,
+                "Distance lower bound and sqrDistLowerBound should coincide.",
+                std::logic_error);
   }
 }
 
@@ -67,7 +65,7 @@ void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request,
   if (front_list && front_list->size() > 0) {
     propagateBVHFrontListCollisionRecurse(node, request, result, front_list);
   } else {
-    FCL_REAL sqrDistLowerBound = 0;
+    CoalScalar sqrDistLowerBound = 0;
     if (recursive)
       collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
     else
@@ -90,6 +88,4 @@ void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list,
   node->postprocess();
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/collision_node.h b/src/collision_node.h
index 8a72be9cf006745ec15843b1f880699689275a62..b6be94f5df820c19a4063ec14568128b4ef2797f 100644
--- a/src/collision_node.h
+++ b/src/collision_node.h
@@ -35,20 +35,19 @@
 
 /** \author Jia Pan */
 
-#ifndef HPP_FCL_COLLISION_NODE_H
-#define HPP_FCL_COLLISION_NODE_H
+#ifndef COAL_COLLISION_NODE_H
+#define COAL_COLLISION_NODE_H
 
 /// @cond INTERNAL
 
-#include <hpp/fcl/BVH/BVH_front.h>
-#include <hpp/fcl/internal/traversal_node_base.h>
-#include <hpp/fcl/internal/traversal_node_bvhs.h>
+#include "coal/BVH/BVH_front.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal_node_bvhs.h"
 
 /// @brief collision and distance function on traversal nodes. these functions
 /// provide a higher level abstraction for collision functions provided in
 /// collision_func_matrix
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 /// collision on collision traversal node
 ///
@@ -57,21 +56,19 @@ namespace fcl {
 ///         do not collide.
 /// @param front_list list of nodes visited by the query, can be used to
 ///        accelerate computation
-/// \todo should be HPP_FCL_LOCAL but used in unit test.
-HPP_FCL_DLLAPI void collide(CollisionTraversalNodeBase* node,
-                            const CollisionRequest& request,
-                            CollisionResult& result,
-                            BVHFrontList* front_list = NULL,
-                            bool recursive = true);
+/// \todo should be COAL_LOCAL but used in unit test.
+COAL_DLLAPI void collide(CollisionTraversalNodeBase* node,
+                         const CollisionRequest& request,
+                         CollisionResult& result,
+                         BVHFrontList* front_list = NULL,
+                         bool recursive = true);
 
 /// @brief distance computation on distance traversal node; can use front list
-/// to accelerate \todo should be HPP_FCL_LOCAL but used in unit test.
-HPP_FCL_DLLAPI void distance(DistanceTraversalNodeBase* node,
-                             BVHFrontList* front_list = NULL,
-                             unsigned int qsize = 2);
-}  // namespace fcl
-
-}  // namespace hpp
+/// to accelerate \todo should be COAL_LOCAL but used in unit test.
+COAL_DLLAPI void distance(DistanceTraversalNodeBase* node,
+                          BVHFrontList* front_list = NULL,
+                          unsigned int qsize = 2);
+}  // namespace coal
 
 /// @endcond
 
diff --git a/src/collision_object.cpp b/src/collision_object.cpp
index 1b0ec1b71b7059850836fdd9214c496333f4d908..a0501b146e8745f08079e07d605db866bb6477c2 100644
--- a/src/collision_object.cpp
+++ b/src/collision_object.cpp
@@ -35,13 +35,10 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/collision_object.h>
+#include "coal/collision_object.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 bool CollisionGeometry::isUncertain() const {
   return !isOccupied() && !isFree();
 }
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp
index 05fd42969192210617b974b995175a0c970f276b..52839d10052313665092142bb6ecf7f6d12589df 100644
--- a/src/collision_utility.cpp
+++ b/src/collision_utility.cpp
@@ -1,35 +1,33 @@
 // Copyright (c) 2017, Joseph Mirabel
 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
 //
-// This file is part of hpp-fcl.
-// hpp-fcl is free software: you can redistribute it
+// This file is part of Coal.
+// Coal is free software: you can redistribute it
 // and/or modify it under the terms of the GNU Lesser General Public
 // License as published by the Free Software Foundation, either version
 // 3 of the License, or (at your option) any later version.
 //
-// hpp-fcl is distributed in the hope that it will be
+// Coal is distributed in the hope that it will be
 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 // General Lesser Public License for more details.  You should have
 // received a copy of the GNU Lesser General Public License along with
-// hpp-fcl. If not, see <http://www.gnu.org/licenses/>.
+// Coal. If not, see <http://www.gnu.org/licenses/>.
 
-#include <hpp/fcl/collision_utility.h>
+#include "coal/collision_utility.h"
+#include "coal/BVH/BVH_utility.h"
 
-#include <hpp/fcl/BVH/BVH_utility.h>
-
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace details {
 
 template <typename NT>
 inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model,
-                                        const Transform3f& pose,
+                                        const Transform3s& pose,
                                         const AABB& aabb) {
   // Ensure AABB is already computed
   if (model->aabb_radius < 0)
-    HPP_FCL_THROW_PRETTY("Collision geometry AABB should be computed first.",
-                         std::invalid_argument);
+    COAL_THROW_PRETTY("Collision geometry AABB should be computed first.",
+                      std::invalid_argument);
   AABB objAabb = rotate(translate(model->aabb_local, pose.getTranslation()),
                         pose.getRotation());
   if (!objAabb.overlap(aabb)) {
@@ -41,7 +39,7 @@ inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model,
 }
 
 CollisionGeometry* extractBVH(const CollisionGeometry* model,
-                              const Transform3f& pose, const AABB& aabb) {
+                              const Transform3s& pose, const AABB& aabb) {
   switch (model->getNodeType()) {
     case BV_AABB:
       return extractBVHtpl<AABB>(model, pose, aabb);
@@ -60,24 +58,20 @@ CollisionGeometry* extractBVH(const CollisionGeometry* model,
     case BV_KDOP24:
       return extractBVHtpl<KDOP<24> >(model, pose, aabb);
     default:
-      HPP_FCL_THROW_PRETTY("Unknown type of bounding volume",
-                           std::runtime_error);
+      COAL_THROW_PRETTY("Unknown type of bounding volume", std::runtime_error);
   }
 }
 }  // namespace details
 
 CollisionGeometry* extract(const CollisionGeometry* model,
-                           const Transform3f& pose, const AABB& aabb) {
+                           const Transform3s& pose, const AABB& aabb) {
   switch (model->getObjectType()) {
     case OT_BVH:
       return details::extractBVH(model, pose, aabb);
     // case OT_GEOM: return model;
     default:
-      HPP_FCL_THROW_PRETTY(
-          "Extraction is not implemented for this type of object",
-          std::runtime_error);
+      COAL_THROW_PRETTY("Extraction is not implemented for this type of object",
+                        std::runtime_error);
   }
 }
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/contact_patch.cpp b/src/contact_patch.cpp
index de46983ef0e2e84d05431c3b1960cb1361be6c63..bfc8c7c7d12b78fe2522055980a6a09aa18cb888 100644
--- a/src/contact_patch.cpp
+++ b/src/contact_patch.cpp
@@ -34,19 +34,18 @@
 
 /** \author Louis Montaut */
 
-#include "hpp/fcl/contact_patch.h"
-#include "hpp/fcl/collision_utility.h"
+#include "coal/contact_patch.h"
+#include "coal/collision_utility.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 ContactPatchFunctionMatrix& getContactPatchFunctionLookTable() {
   static ContactPatchFunctionMatrix table;
   return table;
 }
 
-void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
-                         const CollisionGeometry* o2, const Transform3f& tf2,
+void computeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1,
+                         const CollisionGeometry* o2, const Transform3s& tf2,
                          const CollisionResult& collision_result,
                          const ContactPatchRequest& request,
                          ContactPatchResult& result) {
@@ -70,12 +69,12 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
   if (object_type1 == OT_GEOM &&
       (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
     if (!looktable.contact_patch_matrix[node_type2][node_type1]) {
-      HPP_FCL_THROW_PRETTY("Computing contact patches between node type "
-                               << std::string(get_node_type_name(node_type1))
-                               << " and node type "
-                               << std::string(get_node_type_name(node_type2))
-                               << " is not yet supported.",
-                           std::invalid_argument);
+      COAL_THROW_PRETTY("Computing contact patches between node type "
+                            << std::string(get_node_type_name(node_type1))
+                            << " and node type "
+                            << std::string(get_node_type_name(node_type2))
+                            << " is not yet supported.",
+                        std::invalid_argument);
     }
     looktable.contact_patch_matrix[node_type2][node_type1](
         o2, tf2, o1, tf1, collision_result, &csolver, request, result);
@@ -84,12 +83,12 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
   }
 
   if (!looktable.contact_patch_matrix[node_type1][node_type2]) {
-    HPP_FCL_THROW_PRETTY("Contact patch computation between node type "
-                             << std::string(get_node_type_name(node_type1))
-                             << " and node type "
-                             << std::string(get_node_type_name(node_type2))
-                             << " is not yet supported.",
-                         std::invalid_argument);
+    COAL_THROW_PRETTY("Contact patch computation between node type "
+                          << std::string(get_node_type_name(node_type1))
+                          << " and node type "
+                          << std::string(get_node_type_name(node_type2))
+                          << " is not yet supported.",
+                      std::invalid_argument);
   }
 
   return looktable.contact_patch_matrix[node_type1][node_type2](
@@ -122,12 +121,12 @@ ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1,
        !looktable.contact_patch_matrix[node_type2][node_type1]) ||
       (!this->swap_geoms &&
        !looktable.contact_patch_matrix[node_type1][node_type2])) {
-    HPP_FCL_THROW_PRETTY("Collision function between node type "
-                             << std::string(get_node_type_name(node_type1))
-                             << " and node type "
-                             << std::string(get_node_type_name(node_type2))
-                             << " is not yet supported.",
-                         std::invalid_argument);
+    COAL_THROW_PRETTY("Collision function between node type "
+                          << std::string(get_node_type_name(node_type1))
+                          << " and node type "
+                          << std::string(get_node_type_name(node_type2))
+                          << " is not yet supported.",
+                      std::invalid_argument);
   }
 
   if (this->swap_geoms) {
@@ -137,7 +136,7 @@ ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1,
   }
 }
 
-void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2,
+void ComputeContactPatch::run(const Transform3s& tf1, const Transform3s& tf2,
                               const CollisionResult& collision_result,
                               const ContactPatchRequest& request,
                               ContactPatchResult& result) const {
@@ -158,8 +157,8 @@ void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2,
   }
 }
 
-void ComputeContactPatch::operator()(const Transform3f& tf1,
-                                     const Transform3f& tf2,
+void ComputeContactPatch::operator()(const Transform3s& tf1,
+                                     const Transform3s& tf2,
                                      const CollisionResult& collision_result,
                                      const ContactPatchRequest& request,
                                      ContactPatchResult& result) const
@@ -169,5 +168,4 @@ void ComputeContactPatch::operator()(const Transform3f& tf1,
   this->run(tf1, tf2, collision_result, request, result);
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/contact_patch/contact_patch_solver.cpp b/src/contact_patch/contact_patch_solver.cpp
index 9593c5cd5106d399ed2635d250f56fe4891914e4..bf6418d51ed429af2d30c391e173505186a48599 100644
--- a/src/contact_patch/contact_patch_solver.cpp
+++ b/src/contact_patch/contact_patch_solver.cpp
@@ -34,10 +34,9 @@
 
 /** \authors Louis Montaut */
 
-#include "hpp/fcl/contact_patch/contact_patch_solver.h"
+#include "coal/contact_patch/contact_patch_solver.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace details {
 
@@ -47,7 +46,7 @@ template <typename ShapeType,
 void getShapeSupportSetTpl(const ShapeBase* shape, SupportSet& support_set,
                            int& hint, ShapeSupportData& support_data,
                            size_t num_sampled_supports = 6,
-                           FCL_REAL tol = 1e-3) {
+                           CoalScalar tol = 1e-3) {
   const ShapeType* shape_ = static_cast<const ShapeType*>(shape);
   getShapeSupportSet<_SupportOptions>(shape_, support_set, hint, support_data,
                                       num_sampled_supports, tol);
@@ -99,9 +98,8 @@ ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape,
       }
     }
     default:
-      HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
+      COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
   }
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/contact_patch_func_matrix.cpp b/src/contact_patch_func_matrix.cpp
index 8357702a6bd2294749c31f94606d63bbef74883b..1a6ad2106a0868a5936307e57772df749c48d308 100644
--- a/src/contact_patch_func_matrix.cpp
+++ b/src/contact_patch_func_matrix.cpp
@@ -34,28 +34,26 @@
 
 /** \author Louis Montaut */
 
-#include "hpp/fcl/contact_patch_func_matrix.h"
-#include "hpp/fcl/shape/geometric_shapes.h"
-#include "hpp/fcl/internal/shape_shape_contact_patch_func.h"
+#include "coal/contact_patch_func_matrix.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/internal/shape_shape_contact_patch_func.h"
+#include "coal/BV/BV.h"
 
-#include "hpp/fcl/BV/BV.h"
-
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 template <typename T_BVH, typename T_SH>
 struct BVHShapeComputeContactPatch {
-  static void run(const CollisionGeometry* o1, const Transform3f& tf1,
-                  const CollisionGeometry* o2, const Transform3f& tf2,
+  static void run(const CollisionGeometry* o1, const Transform3s& tf1,
+                  const CollisionGeometry* o2, const Transform3s& tf2,
                   const CollisionResult& collision_result,
                   const ContactPatchSolver* csolver,
                   const ContactPatchRequest& request,
                   ContactPatchResult& result) {
-    HPP_FCL_UNUSED_VARIABLE(o1);
-    HPP_FCL_UNUSED_VARIABLE(tf1);
-    HPP_FCL_UNUSED_VARIABLE(o2);
-    HPP_FCL_UNUSED_VARIABLE(tf2);
-    HPP_FCL_UNUSED_VARIABLE(csolver);
+    COAL_UNUSED_VARIABLE(o1);
+    COAL_UNUSED_VARIABLE(tf1);
+    COAL_UNUSED_VARIABLE(o2);
+    COAL_UNUSED_VARIABLE(tf2);
+    COAL_UNUSED_VARIABLE(csolver);
     for (size_t i = 0; i < collision_result.numContacts(); ++i) {
       if (i >= request.max_num_patch) {
         break;
@@ -70,17 +68,17 @@ struct BVHShapeComputeContactPatch {
 
 template <typename BV, typename Shape>
 struct HeightFieldShapeComputeContactPatch {
-  static void run(const CollisionGeometry* o1, const Transform3f& tf1,
-                  const CollisionGeometry* o2, const Transform3f& tf2,
+  static void run(const CollisionGeometry* o1, const Transform3s& tf1,
+                  const CollisionGeometry* o2, const Transform3s& tf2,
                   const CollisionResult& collision_result,
                   const ContactPatchSolver* csolver,
                   const ContactPatchRequest& request,
                   ContactPatchResult& result) {
-    HPP_FCL_UNUSED_VARIABLE(o1);
-    HPP_FCL_UNUSED_VARIABLE(tf1);
-    HPP_FCL_UNUSED_VARIABLE(o2);
-    HPP_FCL_UNUSED_VARIABLE(tf2);
-    HPP_FCL_UNUSED_VARIABLE(csolver);
+    COAL_UNUSED_VARIABLE(o1);
+    COAL_UNUSED_VARIABLE(tf1);
+    COAL_UNUSED_VARIABLE(o2);
+    COAL_UNUSED_VARIABLE(tf2);
+    COAL_UNUSED_VARIABLE(csolver);
     for (size_t i = 0; i < collision_result.numContacts(); ++i) {
       if (i >= request.max_num_patch) {
         break;
@@ -95,17 +93,17 @@ struct HeightFieldShapeComputeContactPatch {
 
 template <typename BV>
 struct BVHComputeContactPatch {
-  static void run(const CollisionGeometry* o1, const Transform3f& tf1,
-                  const CollisionGeometry* o2, const Transform3f& tf2,
+  static void run(const CollisionGeometry* o1, const Transform3s& tf1,
+                  const CollisionGeometry* o2, const Transform3s& tf2,
                   const CollisionResult& collision_result,
                   const ContactPatchSolver* csolver,
                   const ContactPatchRequest& request,
                   ContactPatchResult& result) {
-    HPP_FCL_UNUSED_VARIABLE(o1);
-    HPP_FCL_UNUSED_VARIABLE(tf1);
-    HPP_FCL_UNUSED_VARIABLE(o2);
-    HPP_FCL_UNUSED_VARIABLE(tf2);
-    HPP_FCL_UNUSED_VARIABLE(csolver);
+    COAL_UNUSED_VARIABLE(o1);
+    COAL_UNUSED_VARIABLE(tf1);
+    COAL_UNUSED_VARIABLE(o2);
+    COAL_UNUSED_VARIABLE(tf2);
+    COAL_UNUSED_VARIABLE(csolver);
     for (size_t i = 0; i < collision_result.numContacts(); ++i) {
       if (i >= request.max_num_patch) {
         break;
@@ -118,21 +116,21 @@ struct BVHComputeContactPatch {
   }
 };
 
-HPP_FCL_LOCAL void contact_patch_function_not_implemented(
-    const CollisionGeometry* o1, const Transform3f& /*tf1*/,
-    const CollisionGeometry* o2, const Transform3f& /*tf2*/,
+COAL_LOCAL void contact_patch_function_not_implemented(
+    const CollisionGeometry* o1, const Transform3s& /*tf1*/,
+    const CollisionGeometry* o2, const Transform3s& /*tf2*/,
     const CollisionResult& /*collision_result*/,
     const ContactPatchSolver* /*csolver*/,
     const ContactPatchRequest& /*request*/, ContactPatchResult& /*result*/) {
   NODE_TYPE node_type1 = o1->getNodeType();
   NODE_TYPE node_type2 = o2->getNodeType();
 
-  HPP_FCL_THROW_PRETTY("Contact patch function between node type "
-                           << std::string(get_node_type_name(node_type1))
-                           << " and node type "
-                           << std::string(get_node_type_name(node_type2))
-                           << " is not yet supported.",
-                       std::invalid_argument);
+  COAL_THROW_PRETTY("Contact patch function between node type "
+                        << std::string(get_node_type_name(node_type1))
+                        << " and node type "
+                        << std::string(get_node_type_name(node_type2))
+                        << " is not yet supported.",
+                    std::invalid_argument);
 }
 
 ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() {
@@ -364,7 +362,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() {
   contact_patch_matrix[BV_OBBRSS][BV_OBBRSS]      = &BVHComputeContactPatch<OBBRSS>::run;
 
   // TODO(louis): octrees
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
   contact_patch_matrix[GEOM_OCTREE][GEOM_OCTREE] = &contact_patch_function_not_implemented;
   contact_patch_matrix[GEOM_OCTREE][GEOM_BOX] = &contact_patch_function_not_implemented;
   contact_patch_matrix[GEOM_OCTREE][GEOM_SPHERE] = &contact_patch_function_not_implemented;
@@ -411,5 +409,4 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() {
   // clang-format on
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance.cpp b/src/distance.cpp
index a234b5ff59d2263a99caa7451ffd1efc7bb5b1a9..42cc52e09a9ece68d4a3bb812cdf1a944372a47b 100644
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -35,31 +35,30 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/collision_utility.h>
-#include <hpp/fcl/distance_func_matrix.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
+#include "coal/distance.h"
+#include "coal/collision_utility.h"
+#include "coal/distance_func_matrix.h"
+#include "coal/narrowphase/narrowphase.h"
 
 #include <iostream>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 DistanceFunctionMatrix& getDistanceFunctionLookTable() {
   static DistanceFunctionMatrix table;
   return table;
 }
 
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
-                  const DistanceRequest& request, DistanceResult& result) {
+CoalScalar distance(const CollisionObject* o1, const CollisionObject* o2,
+                    const DistanceRequest& request, DistanceResult& result) {
   return distance(o1->collisionGeometryPtr(), o1->getTransform(),
                   o2->collisionGeometryPtr(), o2->getTransform(), request,
                   result);
 }
 
-FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
-                  const CollisionGeometry* o2, const Transform3f& tf2,
-                  const DistanceRequest& request, DistanceResult& result) {
+CoalScalar distance(const CollisionGeometry* o1, const Transform3s& tf1,
+                    const CollisionGeometry* o2, const Transform3s& tf2,
+                    const DistanceRequest& request, DistanceResult& result) {
   GJKSolver solver(request);
 
   const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable();
@@ -69,17 +68,17 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
   OBJECT_TYPE object_type2 = o2->getObjectType();
   NODE_TYPE node_type2 = o2->getNodeType();
 
-  FCL_REAL res = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar res = (std::numeric_limits<CoalScalar>::max)();
 
   if (object_type1 == OT_GEOM &&
       (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
     if (!looktable.distance_matrix[node_type2][node_type1]) {
-      HPP_FCL_THROW_PRETTY("Distance function between node type "
-                               << std::string(get_node_type_name(node_type1))
-                               << " and node type "
-                               << std::string(get_node_type_name(node_type2))
-                               << " is not yet supported.",
-                           std::invalid_argument);
+      COAL_THROW_PRETTY("Distance function between node type "
+                            << std::string(get_node_type_name(node_type1))
+                            << " and node type "
+                            << std::string(get_node_type_name(node_type2))
+                            << " is not yet supported.",
+                        std::invalid_argument);
     } else {
       res = looktable.distance_matrix[node_type2][node_type1](
           o2, tf2, o1, tf1, &solver, request, result);
@@ -89,12 +88,12 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
     }
   } else {
     if (!looktable.distance_matrix[node_type1][node_type2]) {
-      HPP_FCL_THROW_PRETTY("Distance function between node type "
-                               << std::string(get_node_type_name(node_type1))
-                               << " and node type "
-                               << std::string(get_node_type_name(node_type2))
-                               << " is not yet supported.",
-                           std::invalid_argument);
+      COAL_THROW_PRETTY("Distance function between node type "
+                            << std::string(get_node_type_name(node_type1))
+                            << " and node type "
+                            << std::string(get_node_type_name(node_type2))
+                            << " is not yet supported.",
+                        std::invalid_argument);
     } else {
       res = looktable.distance_matrix[node_type1][node_type2](
           o1, tf1, o2, tf2, &solver, request, result);
@@ -123,12 +122,12 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1,
 
   if ((swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) ||
       (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) {
-    HPP_FCL_THROW_PRETTY("Distance function between node type "
-                             << std::string(get_node_type_name(node_type1))
-                             << " and node type "
-                             << std::string(get_node_type_name(node_type2))
-                             << " is not yet supported.",
-                         std::invalid_argument);
+    COAL_THROW_PRETTY("Distance function between node type "
+                          << std::string(get_node_type_name(node_type1))
+                          << " and node type "
+                          << std::string(get_node_type_name(node_type2))
+                          << " is not yet supported.",
+                      std::invalid_argument);
   }
   if (swap_geoms)
     func = looktable.distance_matrix[node_type2][node_type1];
@@ -136,10 +135,10 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1,
     func = looktable.distance_matrix[node_type1][node_type2];
 }
 
-FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2,
-                              const DistanceRequest& request,
-                              DistanceResult& result) const {
-  FCL_REAL res;
+CoalScalar ComputeDistance::run(const Transform3s& tf1, const Transform3s& tf2,
+                                const DistanceRequest& request,
+                                DistanceResult& result) const {
+  CoalScalar res;
 
   if (swap_geoms) {
     res = func(o2, tf2, o1, tf1, &solver, request, result);
@@ -157,13 +156,13 @@ FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2,
   return res;
 }
 
-FCL_REAL ComputeDistance::operator()(const Transform3f& tf1,
-                                     const Transform3f& tf2,
-                                     const DistanceRequest& request,
-                                     DistanceResult& result) const {
+CoalScalar ComputeDistance::operator()(const Transform3s& tf1,
+                                       const Transform3s& tf2,
+                                       const DistanceRequest& request,
+                                       DistanceResult& result) const {
   solver.set(request);
 
-  FCL_REAL res;
+  CoalScalar res;
   if (request.enable_timings) {
     Timer timer;
     res = run(tf1, tf2, request, result);
@@ -173,5 +172,4 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1,
   return res;
 }
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp
index 642bd3cb323a5f280d3f2ff38fb3f6e721e83d2f..913f626e3f9f0e76344f519935d9105f7210a13e 100644
--- a/src/distance/box_halfspace.cpp
+++ b/src/distance/box_halfspace.cpp
@@ -36,36 +36,35 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 
 template <>
-FCL_REAL ShapeShapeDistance<Box, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Box, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Box& s1 = static_cast<const Box&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, Box>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, Box>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const Box& s2 = static_cast<const Box&>(*o2);
   return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
@@ -73,5 +72,4 @@ FCL_REAL ShapeShapeDistance<Halfspace, Box>(
 
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp
index fbbffbcf02d6d33f81ccc42e3b0afb30a67c4828..b3cb6015e4296455bd6ce3bda5754590a2802f18 100644
--- a/src/distance/box_plane.cpp
+++ b/src/distance/box_plane.cpp
@@ -36,44 +36,42 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Box, Plane>(const CollisionGeometry* o1,
-                                        const Transform3f& tf1,
-                                        const CollisionGeometry* o2,
-                                        const Transform3f& tf2,
-                                        const GJKSolver*, const bool, Vec3f& p1,
-                                        Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Box, Plane>(const CollisionGeometry* o1,
+                                          const Transform3s& tf1,
+                                          const CollisionGeometry* o2,
+                                          const Transform3s& tf2,
+                                          const GJKSolver*, const bool,
+                                          Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Box& s1 = static_cast<const Box&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, Box>(const CollisionGeometry* o1,
-                                        const Transform3f& tf1,
-                                        const CollisionGeometry* o2,
-                                        const Transform3f& tf2,
-                                        const GJKSolver*, const bool, Vec3f& p1,
-                                        Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Box>(const CollisionGeometry* o1,
+                                          const Transform3s& tf1,
+                                          const CollisionGeometry* o2,
+                                          const Transform3s& tf2,
+                                          const GJKSolver*, const bool,
+                                          Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const Box& s2 = static_cast<const Box&>(*o2);
   return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/box_sphere.cpp b/src/distance/box_sphere.cpp
index cdc8e9495a716d93a00a702063b39a34af6f5768..0b6dbfb2499f8eaed32ee8b09338592ecf4764f8 100644
--- a/src/distance/box_sphere.cpp
+++ b/src/distance/box_sphere.cpp
@@ -36,44 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Box, Sphere>(const CollisionGeometry* o1,
-                                         const Transform3f& tf1,
-                                         const CollisionGeometry* o2,
-                                         const Transform3f& tf2,
-                                         const GJKSolver*, const bool,
-                                         Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Box, Sphere>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Box& s1 = static_cast<const Box&>(*o1);
   const Sphere& s2 = static_cast<const Sphere&>(*o2);
   return details::boxSphereDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Sphere, Box>(const CollisionGeometry* o1,
-                                         const Transform3f& tf1,
-                                         const CollisionGeometry* o2,
-                                         const Transform3f& tf2,
-                                         const GJKSolver*, const bool,
-                                         Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Sphere, Box>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Sphere& s1 = static_cast<const Sphere&>(*o1);
   const Box& s2 = static_cast<const Box&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::boxSphereDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp
index 635a0c4c7ca707fdf531d35184da2d921a960b1f..0ec5137f6e03cfe60648e934911f0913bbcabe26 100644
--- a/src/distance/capsule_capsule.cpp
+++ b/src/distance/capsule_capsule.cpp
@@ -31,9 +31,9 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/internal/shape_shape_func.h"
 
 // Note that partial specialization of template functions is not allowed.
 // Therefore, two implementations with the default narrow phase solvers are
@@ -43,13 +43,12 @@
 //
 // One solution would be to make narrow phase solvers derive from an abstract
 // class and specialize the template for this abstract class.
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 /// Clamp num / denom in [0, 1]
-FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) {
+CoalScalar clamp(const CoalScalar& num, const CoalScalar& denom) {
   assert(denom >= 0.);
   if (num <= 0.)
     return 0.;
@@ -60,8 +59,8 @@ FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) {
 }
 
 /// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd
-void clamped_linear(Vec3f& a_sd, const Vec3f& a, const FCL_REAL& s_n,
-                    const FCL_REAL& s_d, const Vec3f& d) {
+void clamped_linear(Vec3s& a_sd, const Vec3s& a, const CoalScalar& s_n,
+                    const CoalScalar& s_d, const Vec3s& d) {
   assert(s_d >= 0.);
   if (s_n <= 0.)
     a_sd = a;
@@ -78,42 +77,42 @@ void clamped_linear(Vec3f& a_sd, const Vec3f& a, const FCL_REAL& s_n,
 /// @param wp1, wp2: witness points on the capsules
 /// @param normal: normal pointing from capsule1 to capsule2
 template <>
-FCL_REAL ShapeShapeDistance<Capsule, Capsule>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& wp1, Vec3f& wp2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Capsule, Capsule>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& wp1, Vec3s& wp2, Vec3s& normal) {
   const Capsule* capsule1 = static_cast<const Capsule*>(o1);
   const Capsule* capsule2 = static_cast<const Capsule*>(o2);
 
-  FCL_REAL EPSILON = std::numeric_limits<FCL_REAL>::epsilon() * 100;
+  CoalScalar EPSILON = std::numeric_limits<CoalScalar>::epsilon() * 100;
 
   // We assume that capsules are centered at the origin.
-  const fcl::Vec3f& c1 = tf1.getTranslation();
-  const fcl::Vec3f& c2 = tf2.getTranslation();
+  const coal::Vec3s& c1 = tf1.getTranslation();
+  const coal::Vec3s& c2 = tf2.getTranslation();
   // We assume that capsules are oriented along z-axis.
-  FCL_REAL halfLength1 = capsule1->halfLength;
-  FCL_REAL halfLength2 = capsule2->halfLength;
-  FCL_REAL radius1 = (capsule1->radius + capsule1->getSweptSphereRadius());
-  FCL_REAL radius2 = (capsule2->radius + capsule2->getSweptSphereRadius());
+  CoalScalar halfLength1 = capsule1->halfLength;
+  CoalScalar halfLength2 = capsule2->halfLength;
+  CoalScalar radius1 = (capsule1->radius + capsule1->getSweptSphereRadius());
+  CoalScalar radius2 = (capsule2->radius + capsule2->getSweptSphereRadius());
   // direction of capsules
   // ||d1|| = 2 * halfLength1
-  const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2);
-  const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2);
+  const coal::Vec3s d1 = 2 * halfLength1 * tf1.getRotation().col(2);
+  const coal::Vec3s d2 = 2 * halfLength2 * tf2.getRotation().col(2);
 
   // Starting point of the segments
   // p1 + d1 is the end point of the segment
-  const fcl::Vec3f p1 = c1 - d1 / 2;
-  const fcl::Vec3f p2 = c2 - d2 / 2;
-  const fcl::Vec3f r = p1 - p2;
-  FCL_REAL a = d1.dot(d1);
-  FCL_REAL b = d1.dot(d2);
-  FCL_REAL c = d1.dot(r);
-  FCL_REAL e = d2.dot(d2);
-  FCL_REAL f = d2.dot(r);
+  const coal::Vec3s p1 = c1 - d1 / 2;
+  const coal::Vec3s p2 = c2 - d2 / 2;
+  const coal::Vec3s r = p1 - p2;
+  CoalScalar a = d1.dot(d1);
+  CoalScalar b = d1.dot(d2);
+  CoalScalar c = d1.dot(r);
+  CoalScalar e = d2.dot(d2);
+  CoalScalar f = d2.dot(r);
   // S1 is parametrized by the equation p1 + s * d1
   // S2 is parametrized by the equation p2 + t * d2
 
-  Vec3f w1, w2;
+  Vec3s w1, w2;
   if (a <= EPSILON) {
     w1 = p1;
     if (e <= EPSILON)
@@ -128,10 +127,10 @@ FCL_REAL ShapeShapeDistance<Capsule, Capsule>(
     w2 = p2;
   } else {
     // Always non-negative, equal 0 if the segments are colinear
-    FCL_REAL denom = fmax(a * e - b * b, 0);
+    CoalScalar denom = fmax(a * e - b * b, 0);
 
-    FCL_REAL s;
-    FCL_REAL t;
+    CoalScalar s;
+    CoalScalar t;
     if (denom > EPSILON) {
       s = clamp((b * f - c * e), denom);
       t = b * s + f;
@@ -153,7 +152,7 @@ FCL_REAL ShapeShapeDistance<Capsule, Capsule>(
   }
 
   // witness points achieving the distance between the two segments
-  FCL_REAL distance = (w1 - w2).norm();
+  CoalScalar distance = (w1 - w2).norm();
 
   // capsule spcecific distance computation
   distance = distance - (radius1 + radius2);
@@ -167,5 +166,4 @@ FCL_REAL ShapeShapeDistance<Capsule, Capsule>(
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp
index 7e32dcdb50bd5978dd805c849cd6c6d137203798..70c53b918f04fb93354aac47f030f3466758fcbe 100644
--- a/src/distance/capsule_halfspace.cpp
+++ b/src/distance/capsule_halfspace.cpp
@@ -36,40 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Capsule, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Capsule, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Capsule& s1 = static_cast<const Capsule&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, Capsule>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, Capsule>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const Capsule& s2 = static_cast<const Capsule&>(*o2);
   return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp
index 3d20410ec4559f33495a8ffc46b4c417168100e2..27fe19778f2b66211b86c25fdeb6ef6a29732c92 100644
--- a/src/distance/capsule_plane.cpp
+++ b/src/distance/capsule_plane.cpp
@@ -36,40 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Capsule, Plane>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Capsule, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Capsule& s1 = static_cast<const Capsule&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, Capsule>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Capsule>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const Capsule& s2 = static_cast<const Capsule&>(*o2);
   return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp
index b201ce9afcc0d283d7a07c998f89515ccfd04f42..38276b539b9a4f4b34c82959418b28da821b0761 100644
--- a/src/distance/cone_halfspace.cpp
+++ b/src/distance/cone_halfspace.cpp
@@ -36,40 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Cone, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Cone, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Cone& s1 = static_cast<const Cone&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, Cone>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, Cone>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const Cone& s2 = static_cast<const Cone&>(*o2);
   return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp
index 83f61b5eee3991dfa13a06ec6ce6d0822f1fe917..b26832a965662cdc50ad96d72f5935a35673c4da 100644
--- a/src/distance/cone_plane.cpp
+++ b/src/distance/cone_plane.cpp
@@ -36,44 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Cone, Plane>(const CollisionGeometry* o1,
-                                         const Transform3f& tf1,
-                                         const CollisionGeometry* o2,
-                                         const Transform3f& tf2,
-                                         const GJKSolver*, const bool,
-                                         Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Cone, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Cone& s1 = static_cast<const Cone&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, Cone>(const CollisionGeometry* o1,
-                                         const Transform3f& tf1,
-                                         const CollisionGeometry* o2,
-                                         const Transform3f& tf2,
-                                         const GJKSolver*, const bool,
-                                         Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Cone>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const Cone& s2 = static_cast<const Cone&>(*o2);
   return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp
index 5a25fd2ed914dea7bc8277597fd4d1ea9d9c6894..bda32965d8ef459bc04ef0759bc4824ed795dd00 100644
--- a/src/distance/convex_halfspace.cpp
+++ b/src/distance/convex_halfspace.cpp
@@ -34,38 +34,36 @@
 
 /** \author Joseph Mirabel */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<ConvexBase, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<ConvexBase, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const ConvexBase& s1 = static_cast<const ConvexBase&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, ConvexBase>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, ConvexBase>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const ConvexBase& s2 = static_cast<const ConvexBase&>(*o2);
   return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/convex_plane.cpp b/src/distance/convex_plane.cpp
index e3acc9e2dd4cc82d2631638f7a2dce1666f32b03..c89b57c7ff8c5a251d4d02641d78809680f509c6 100644
--- a/src/distance/convex_plane.cpp
+++ b/src/distance/convex_plane.cpp
@@ -34,38 +34,36 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<ConvexBase, Plane>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<ConvexBase, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const ConvexBase& s1 = static_cast<const ConvexBase&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, ConvexBase>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, ConvexBase>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const ConvexBase& s2 = static_cast<const ConvexBase&>(*o2);
   return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp
index 3d998e8f5f4e20a3902ac46dac1d5633f122bc3b..c29e6f7ca12e1a53a8f511d9b8f8592b3c14c3e5 100644
--- a/src/distance/cylinder_halfspace.cpp
+++ b/src/distance/cylinder_halfspace.cpp
@@ -36,40 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Cylinder, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Cylinder, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Cylinder& s1 = static_cast<const Cylinder&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, Cylinder>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, Cylinder>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const Cylinder& s2 = static_cast<const Cylinder&>(*o2);
   return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp
index 1147d1bdad2903f642bd992db399240aee50b321..2d3db927c934e263e36f45216f6cd918430fc866 100644
--- a/src/distance/cylinder_plane.cpp
+++ b/src/distance/cylinder_plane.cpp
@@ -36,40 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Cylinder, Plane>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Cylinder, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Cylinder& s1 = static_cast<const Cylinder&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, Cylinder>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Cylinder>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const Cylinder& s2 = static_cast<const Cylinder&>(*o2);
   return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp
index 0cd5c09a87e20a761fe7c704394551c6b740106c..d69e3db97899fbfbb7cbbff7c9de783865e7079e 100644
--- a/src/distance/ellipsoid_halfspace.cpp
+++ b/src/distance/ellipsoid_halfspace.cpp
@@ -34,40 +34,38 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Ellipsoid, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Ellipsoid, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Ellipsoid& s1 = static_cast<const Ellipsoid&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, Ellipsoid>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, Ellipsoid>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const Ellipsoid& s2 = static_cast<const Ellipsoid&>(*o2);
   return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp
index 797d4b424ea572b2bb2b12c4e958383593e42175..eae31b84445595cdf8487a4827ae9a779d7e8837 100644
--- a/src/distance/ellipsoid_plane.cpp
+++ b/src/distance/ellipsoid_plane.cpp
@@ -34,39 +34,37 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Ellipsoid, Plane>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Ellipsoid, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Ellipsoid& s1 = static_cast<const Ellipsoid&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, Ellipsoid>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Ellipsoid>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const Ellipsoid& s2 = static_cast<const Ellipsoid&>(*o2);
   return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/halfspace_halfspace.cpp b/src/distance/halfspace_halfspace.cpp
index 1abdb6fd93478d6f34776a77d8530275bee1ea40..9bad0a5966a9d97a9efd77990b39f40bafaa22af 100644
--- a/src/distance/halfspace_halfspace.cpp
+++ b/src/distance/halfspace_halfspace.cpp
@@ -34,26 +34,24 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
   return details::halfspaceHalfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/halfspace_plane.cpp b/src/distance/halfspace_plane.cpp
index 1608b9af1b234435d14459bdbe7b3c91fc254c6f..b0bd3c079dbb62db0054b44cd802f334ffa77566 100644
--- a/src/distance/halfspace_plane.cpp
+++ b/src/distance/halfspace_plane.cpp
@@ -34,39 +34,37 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, Plane>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
   return details::halfspacePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  FCL_REAL distance =
+  CoalScalar distance =
       details::halfspacePlaneDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/plane_plane.cpp b/src/distance/plane_plane.cpp
index 0d32413aeb7a9083c513a72aad67731a13fb55b0..b14bb30996dbee7f0b134a64b62e6417ce9d2bc0 100644
--- a/src/distance/plane_plane.cpp
+++ b/src/distance/plane_plane.cpp
@@ -34,28 +34,24 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Plane, Plane>(const CollisionGeometry* o1,
-                                          const Transform3f& tf1,
-                                          const CollisionGeometry* o2,
-                                          const Transform3f& tf2,
-                                          const GJKSolver*, const bool,
-                                          Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
   return details::planePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/sphere_capsule.cpp b/src/distance/sphere_capsule.cpp
index c205ad34e3917f688d360a210f30fb5c36658621..719041b6ecde5ca586405007494d5ab02088df65 100644
--- a/src/distance/sphere_capsule.cpp
+++ b/src/distance/sphere_capsule.cpp
@@ -33,35 +33,34 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Sphere, Capsule>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Sphere, Capsule>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Sphere& s1 = static_cast<const Sphere&>(*o1);
   const Capsule& s2 = static_cast<const Capsule&>(*o2);
   return details::sphereCapsuleDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Capsule, Sphere>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Capsule, Sphere>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Capsule& s1 = static_cast<const Capsule&>(*o1);
   const Sphere& s2 = static_cast<const Sphere&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::sphereCapsuleDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
@@ -69,5 +68,4 @@ FCL_REAL ShapeShapeDistance<Capsule, Sphere>(
 
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp
index e050a342141a9c0db417f65bc8d9f65e08afc4bf..743989f198b777ef41f5a67c1ab7f8a441e075ab 100644
--- a/src/distance/sphere_cylinder.cpp
+++ b/src/distance/sphere_cylinder.cpp
@@ -36,41 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Sphere, Cylinder>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Sphere, Cylinder>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Sphere& s1 = static_cast<const Sphere&>(*o1);
   const Cylinder& s2 = static_cast<const Cylinder&>(*o2);
   return details::sphereCylinderDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Cylinder, Sphere>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Cylinder, Sphere>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Cylinder& s1 = static_cast<const Cylinder&>(*o1);
   const Sphere& s2 = static_cast<const Sphere&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::sphereCylinderDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 }  // namespace internal
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp
index 2983ebd436d30dd0b6279b3c5f3889432e9d9682..9a6fbe3054018b9cdd841efe57e268a669a0673e 100644
--- a/src/distance/sphere_halfspace.cpp
+++ b/src/distance/sphere_halfspace.cpp
@@ -36,41 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Sphere, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Sphere, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Sphere& s1 = static_cast<const Sphere&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, Sphere>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, Sphere>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const Sphere& s2 = static_cast<const Sphere&>(*o2);
   return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp
index 7c95f2312a3a18fbf350cc4befde82ebf2cd2175..21438a54995d1c8816cf7bae8ddcfad50c0d6ba5 100644
--- a/src/distance/sphere_plane.cpp
+++ b/src/distance/sphere_plane.cpp
@@ -36,40 +36,38 @@
 
 /** \author Florent Lamiraux */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Sphere, Plane>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Sphere, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Sphere& s1 = static_cast<const Sphere&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, Sphere>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Sphere>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const Sphere& s2 = static_cast<const Sphere&>(*o2);
   return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp
index 5f62d317d340eaaac0b3bb4e9b46dd1451a2313d..ac72a8c74c6c40a9eb45d19a0e49fa71282fda0c 100644
--- a/src/distance/sphere_sphere.cpp
+++ b/src/distance/sphere_sphere.cpp
@@ -33,10 +33,10 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
-#include <hpp/fcl/internal/traversal_node_base.h>
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/internal/shape_shape_func.h"
+#include "coal/internal/traversal_node_base.h"
 
 #include "../narrowphase/details.h"
 
@@ -48,21 +48,19 @@
 //
 // One solution would be to make narrow phase solvers derive from an abstract
 // class and specialize the template for this abstract class.
-namespace hpp {
-namespace fcl {
+namespace coal {
 struct GJKSolver;
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<Sphere, Sphere>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Sphere, Sphere>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Sphere& s1 = static_cast<const Sphere&>(*o1);
   const Sphere& s2 = static_cast<const Sphere&>(*o2);
   return details::sphereSphereDistance(s1, tf1, s2, tf2, p1, p2, normal);
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp
index b260cccc2a0d417dadac87c76aec73d0f19bb853..5f4da480722b5feee25b987992537dd13a8006d8 100644
--- a/src/distance/triangle_halfspace.cpp
+++ b/src/distance/triangle_halfspace.cpp
@@ -34,33 +34,32 @@
 
 /** \author Joseph Mirabel */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<TriangleP, Halfspace>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<TriangleP, Halfspace>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Halfspace, TriangleP>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Halfspace, TriangleP>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
   return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
@@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance<Halfspace, TriangleP>(
 
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/triangle_plane.cpp b/src/distance/triangle_plane.cpp
index b7eeb14f5af60877ccde806e4c97c4ee54d2d1c2..87a90ae5d465c9499b0f54c5b0d221887495a066 100644
--- a/src/distance/triangle_plane.cpp
+++ b/src/distance/triangle_plane.cpp
@@ -34,33 +34,32 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<TriangleP, Plane>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<TriangleP, Plane>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
   const Plane& s2 = static_cast<const Plane&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Plane, TriangleP>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, TriangleP>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Plane& s1 = static_cast<const Plane&>(*o1);
   const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
   return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
@@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance<Plane, TriangleP>(
 
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/triangle_sphere.cpp b/src/distance/triangle_sphere.cpp
index e7af8bfca723407a2a9c1a6a980a877170acda43..2bcb6beb94e7fa2702eee9ea3d0dff6680c1a5dd 100644
--- a/src/distance/triangle_sphere.cpp
+++ b/src/distance/triangle_sphere.cpp
@@ -34,33 +34,32 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<TriangleP, Sphere>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<TriangleP, Sphere>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
   const Sphere& s2 = static_cast<const Sphere&>(*o2);
-  const FCL_REAL distance =
+  const CoalScalar distance =
       details::sphereTriangleDistance(s2, tf2, s1, tf1, p2, p1, normal);
   normal = -normal;
   return distance;
 }
 
 template <>
-FCL_REAL ShapeShapeDistance<Sphere, TriangleP>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Sphere, TriangleP>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
+    const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   const Sphere& s1 = static_cast<const Sphere&>(*o1);
   const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
   return details::sphereTriangleDistance(s1, tf1, s2, tf2, p1, p2, normal);
@@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance<Sphere, TriangleP>(
 
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp
index 2f7302ebc702b667a09a797e8fa006470d6f22a7..ccbe2b7f9b83cc352fc39c476ed1e2111c57906d 100644
--- a/src/distance/triangle_triangle.cpp
+++ b/src/distance/triangle_triangle.cpp
@@ -34,20 +34,19 @@
 
 /** \author Louis Montaut */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/shape/geometric_shapes.h"
 
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "../narrowphase/details.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace internal {
 template <>
-FCL_REAL ShapeShapeDistance<TriangleP, TriangleP>(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2,
-    const GJKSolver* solver, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<TriangleP, TriangleP>(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2,
+    const GJKSolver* solver, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   // Transform the triangles in world frame
   const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
   const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b),
@@ -61,11 +60,11 @@ FCL_REAL ShapeShapeDistance<TriangleP, TriangleP>(
   //   We don't need to take into account swept-sphere radius in GJK iterations;
   //   the result will be corrected after GJK terminates.
   solver->minkowski_difference
-      .set<::hpp::fcl::details::SupportOptions::NoSweptSphere>(&t1, &t2);
+      .set<::coal::details::SupportOptions::NoSweptSphere>(&t1, &t2);
   solver->gjk.reset(solver->gjk_max_iterations, solver->gjk_tolerance);
 
   // Get GJK initial guess
-  Vec3f guess;
+  Vec3s guess;
   if (solver->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
       solver->enable_cached_guess) {
     guess = solver->cached_guess;
@@ -85,10 +84,10 @@ FCL_REAL ShapeShapeDistance<TriangleP, TriangleP>(
   // Retrieve witness points and normal
   solver->gjk.getWitnessPointsAndNormal(solver->minkowski_difference, p1, p2,
                                         normal);
-  FCL_REAL distance = solver->gjk.distance;
+  CoalScalar distance = solver->gjk.distance;
 
   if (gjk_status == details::GJK::Collision) {
-    FCL_REAL penetrationDepth =
+    CoalScalar penetrationDepth =
         details::computePenetration(t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
     distance = -penetrationDepth;
   } else {
@@ -105,5 +104,4 @@ FCL_REAL ShapeShapeDistance<TriangleP, TriangleP>(
 }
 }  // namespace internal
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index 4c253cd1f6dcedab38d83a9ee34696a911c088d9..d9aae3ba3d5c27327c341aa9ebdfba12d4e582e2 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -35,24 +35,23 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/distance_func_matrix.h>
+#include "coal/distance_func_matrix.h"
 
 #include <../src/collision_node.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
-#include <hpp/fcl/internal/traversal_node_setup.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
+#include "coal/internal/traversal_node_setup.h"
+#include "coal/internal/shape_shape_func.h"
 #include <../src/traits_traversal.h>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
 
 template <typename TypeA, typename TypeB>
-FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1,
-                  const CollisionGeometry* o2, const Transform3f& tf2,
-                  const GJKSolver* nsolver, const DistanceRequest& request,
-                  DistanceResult& result) {
+CoalScalar Distance(const CollisionGeometry* o1, const Transform3s& tf1,
+                    const CollisionGeometry* o2, const Transform3s& tf2,
+                    const GJKSolver* nsolver, const DistanceRequest& request,
+                    DistanceResult& result) {
   if (request.isSatisfied(result)) return result.min_distance;
   typename TraversalTraitsDistance<TypeA, TypeB>::CollisionTraversal_t node;
   const TypeA* obj1 = static_cast<const TypeA*>(o1);
@@ -67,61 +66,62 @@ FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1,
 
 #endif
 
-HPP_FCL_LOCAL FCL_REAL distance_function_not_implemented(
-    const CollisionGeometry* o1, const Transform3f& /*tf1*/,
-    const CollisionGeometry* o2, const Transform3f& /*tf2*/,
+COAL_LOCAL CoalScalar distance_function_not_implemented(
+    const CollisionGeometry* o1, const Transform3s& /*tf1*/,
+    const CollisionGeometry* o2, const Transform3s& /*tf2*/,
     const GJKSolver* /*nsolver*/, const DistanceRequest& /*request*/,
     DistanceResult& /*result*/) {
   NODE_TYPE node_type1 = o1->getNodeType();
   NODE_TYPE node_type2 = o2->getNodeType();
 
-  HPP_FCL_THROW_PRETTY("Distance function between node type "
-                           << std::string(get_node_type_name(node_type1))
-                           << " and node type "
-                           << std::string(get_node_type_name(node_type2))
-                           << " is not yet supported.",
-                       std::invalid_argument);
+  COAL_THROW_PRETTY("Distance function between node type "
+                        << std::string(get_node_type_name(node_type1))
+                        << " and node type "
+                        << std::string(get_node_type_name(node_type2))
+                        << " is not yet supported.",
+                    std::invalid_argument);
 }
 
 template <typename T_BVH, typename T_SH>
-struct HPP_FCL_LOCAL BVHShapeDistancer{static FCL_REAL distance(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2,
+struct COAL_LOCAL BVHShapeDistancer{static CoalScalar distance(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2,
     const GJKSolver* nsolver, const DistanceRequest& request,
     DistanceResult& result){
     if (request.isSatisfied(result)) return result.min_distance;
 MeshShapeDistanceTraversalNode<T_BVH, T_SH> node;
 const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
 BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
-Transform3f tf1_tmp = tf1;
+Transform3s tf1_tmp = tf1;
 const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
 initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
-fcl::distance(&node);
+::coal::distance(&node);
 
 delete obj1_tmp;
 return result.min_distance;
-}  // namespace fcl
-};  // namespace hpp
+}  // namespace coal
+}
+;
 
 namespace details {
 
 template <typename OrientedMeshShapeDistanceTraversalNode, typename T_BVH,
           typename T_SH>
-FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1,
-                                  const Transform3f& tf1,
-                                  const CollisionGeometry* o2,
-                                  const Transform3f& tf2,
-                                  const GJKSolver* nsolver,
-                                  const DistanceRequest& request,
-                                  DistanceResult& result) {
+CoalScalar orientedBVHShapeDistance(const CollisionGeometry* o1,
+                                    const Transform3s& tf1,
+                                    const CollisionGeometry* o2,
+                                    const Transform3s& tf2,
+                                    const GJKSolver* nsolver,
+                                    const DistanceRequest& request,
+                                    DistanceResult& result) {
   if (request.isSatisfied(result)) return result.min_distance;
   OrientedMeshShapeDistanceTraversalNode node;
   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
   const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
   initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
-  fcl::distance(&node);
+  ::coal::distance(&node);
 
   return result.min_distance;
 }
@@ -129,12 +129,13 @@ FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1,
 }  // namespace details
 
 template <typename T_SH>
-struct HPP_FCL_LOCAL BVHShapeDistancer<RSS, T_SH> {
-  static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
-                           const CollisionGeometry* o2, const Transform3f& tf2,
-                           const GJKSolver* nsolver,
-                           const DistanceRequest& request,
-                           DistanceResult& result) {
+struct COAL_LOCAL BVHShapeDistancer<RSS, T_SH> {
+  static CoalScalar distance(const CollisionGeometry* o1,
+                             const Transform3s& tf1,
+                             const CollisionGeometry* o2,
+                             const Transform3s& tf2, const GJKSolver* nsolver,
+                             const DistanceRequest& request,
+                             DistanceResult& result) {
     return details::orientedBVHShapeDistance<
         MeshShapeDistanceTraversalNodeRSS<T_SH>, RSS, T_SH>(
         o1, tf1, o2, tf2, nsolver, request, result);
@@ -142,12 +143,13 @@ struct HPP_FCL_LOCAL BVHShapeDistancer<RSS, T_SH> {
 };
 
 template <typename T_SH>
-struct HPP_FCL_LOCAL BVHShapeDistancer<kIOS, T_SH> {
-  static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
-                           const CollisionGeometry* o2, const Transform3f& tf2,
-                           const GJKSolver* nsolver,
-                           const DistanceRequest& request,
-                           DistanceResult& result) {
+struct COAL_LOCAL BVHShapeDistancer<kIOS, T_SH> {
+  static CoalScalar distance(const CollisionGeometry* o1,
+                             const Transform3s& tf1,
+                             const CollisionGeometry* o2,
+                             const Transform3s& tf2, const GJKSolver* nsolver,
+                             const DistanceRequest& request,
+                             DistanceResult& result) {
     return details::orientedBVHShapeDistance<
         MeshShapeDistanceTraversalNodekIOS<T_SH>, kIOS, T_SH>(
         o1, tf1, o2, tf2, nsolver, request, result);
@@ -155,12 +157,13 @@ struct HPP_FCL_LOCAL BVHShapeDistancer<kIOS, T_SH> {
 };
 
 template <typename T_SH>
-struct HPP_FCL_LOCAL BVHShapeDistancer<OBBRSS, T_SH> {
-  static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
-                           const CollisionGeometry* o2, const Transform3f& tf2,
-                           const GJKSolver* nsolver,
-                           const DistanceRequest& request,
-                           DistanceResult& result) {
+struct COAL_LOCAL BVHShapeDistancer<OBBRSS, T_SH> {
+  static CoalScalar distance(const CollisionGeometry* o1,
+                             const Transform3s& tf1,
+                             const CollisionGeometry* o2,
+                             const Transform3s& tf2, const GJKSolver* nsolver,
+                             const DistanceRequest& request,
+                             DistanceResult& result) {
     return details::orientedBVHShapeDistance<
         MeshShapeDistanceTraversalNodeOBBRSS<T_SH>, OBBRSS, T_SH>(
         o1, tf1, o2, tf2, nsolver, request, result);
@@ -168,18 +171,18 @@ struct HPP_FCL_LOCAL BVHShapeDistancer<OBBRSS, T_SH> {
 };
 
 template <typename T_HF, typename T_SH>
-struct HPP_FCL_LOCAL HeightFieldShapeDistancer{static FCL_REAL distance(
-    const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2,
+struct COAL_LOCAL HeightFieldShapeDistancer{static CoalScalar distance(
+    const CollisionGeometry* o1, const Transform3s& tf1,
+    const CollisionGeometry* o2, const Transform3s& tf2,
     const GJKSolver* nsolver, const DistanceRequest& request,
-    DistanceResult& result){HPP_FCL_UNUSED_VARIABLE(o1);
-HPP_FCL_UNUSED_VARIABLE(tf1);
-HPP_FCL_UNUSED_VARIABLE(o2);
-HPP_FCL_UNUSED_VARIABLE(tf2);
-HPP_FCL_UNUSED_VARIABLE(nsolver);
-HPP_FCL_UNUSED_VARIABLE(request);
+    DistanceResult& result){COAL_UNUSED_VARIABLE(o1);
+COAL_UNUSED_VARIABLE(tf1);
+COAL_UNUSED_VARIABLE(o2);
+COAL_UNUSED_VARIABLE(tf2);
+COAL_UNUSED_VARIABLE(nsolver);
+COAL_UNUSED_VARIABLE(request);
 // TODO(jcarpent)
-HPP_FCL_THROW_PRETTY(
+COAL_THROW_PRETTY(
     "Distance between a height field and a shape is not implemented",
     std::invalid_argument);
 //    if(request.isSatisfied(result)) return result.min_distance;
@@ -197,17 +200,17 @@ return result.min_distance;
 ;
 
 template <typename T_BVH>
-FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1,
-                     const CollisionGeometry* o2, const Transform3f& tf2,
-                     const DistanceRequest& request, DistanceResult& result) {
+CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1,
+                       const CollisionGeometry* o2, const Transform3s& tf2,
+                       const DistanceRequest& request, DistanceResult& result) {
   if (request.isSatisfied(result)) return result.min_distance;
   MeshDistanceTraversalNode<T_BVH> node;
   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
   BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
-  Transform3f tf1_tmp = tf1;
+  Transform3s tf1_tmp = tf1;
   BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
-  Transform3f tf2_tmp = tf2;
+  Transform3s tf2_tmp = tf2;
 
   initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
   distance(&node);
@@ -219,12 +222,12 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1,
 
 namespace details {
 template <typename OrientedMeshDistanceTraversalNode, typename T_BVH>
-FCL_REAL orientedMeshDistance(const CollisionGeometry* o1,
-                              const Transform3f& tf1,
-                              const CollisionGeometry* o2,
-                              const Transform3f& tf2,
-                              const DistanceRequest& request,
-                              DistanceResult& result) {
+CoalScalar orientedMeshDistance(const CollisionGeometry* o1,
+                                const Transform3s& tf1,
+                                const CollisionGeometry* o2,
+                                const Transform3s& tf2,
+                                const DistanceRequest& request,
+                                DistanceResult& result) {
   if (request.isSatisfied(result)) return result.min_distance;
   OrientedMeshDistanceTraversalNode node;
   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
@@ -239,39 +242,41 @@ FCL_REAL orientedMeshDistance(const CollisionGeometry* o1,
 }  // namespace details
 
 template <>
-FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1,
-                          const CollisionGeometry* o2, const Transform3f& tf2,
-                          const DistanceRequest& request,
-                          DistanceResult& result) {
+CoalScalar BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3s& tf1,
+                            const CollisionGeometry* o2, const Transform3s& tf2,
+                            const DistanceRequest& request,
+                            DistanceResult& result) {
   return details::orientedMeshDistance<MeshDistanceTraversalNodeRSS, RSS>(
       o1, tf1, o2, tf2, request, result);
 }
 
 template <>
-FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1,
-                           const CollisionGeometry* o2, const Transform3f& tf2,
-                           const DistanceRequest& request,
-                           DistanceResult& result) {
+CoalScalar BVHDistance<kIOS>(const CollisionGeometry* o1,
+                             const Transform3s& tf1,
+                             const CollisionGeometry* o2,
+                             const Transform3s& tf2,
+                             const DistanceRequest& request,
+                             DistanceResult& result) {
   return details::orientedMeshDistance<MeshDistanceTraversalNodekIOS, kIOS>(
       o1, tf1, o2, tf2, request, result);
 }
 
 template <>
-FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1,
-                             const Transform3f& tf1,
-                             const CollisionGeometry* o2,
-                             const Transform3f& tf2,
-                             const DistanceRequest& request,
-                             DistanceResult& result) {
+CoalScalar BVHDistance<OBBRSS>(const CollisionGeometry* o1,
+                               const Transform3s& tf1,
+                               const CollisionGeometry* o2,
+                               const Transform3s& tf2,
+                               const DistanceRequest& request,
+                               DistanceResult& result) {
   return details::orientedMeshDistance<MeshDistanceTraversalNodeOBBRSS, OBBRSS>(
       o1, tf1, o2, tf2, request, result);
 }
 
 template <typename T_BVH>
-FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1,
-                     const CollisionGeometry* o2, const Transform3f& tf2,
-                     const GJKSolver* /*nsolver*/,
-                     const DistanceRequest& request, DistanceResult& result) {
+CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1,
+                       const CollisionGeometry* o2, const Transform3s& tf2,
+                       const GJKSolver* /*nsolver*/,
+                       const DistanceRequest& request, DistanceResult& result) {
   return BVHDistance<T_BVH>(o1, tf1, o2, tf2, request, result);
 }
 
@@ -601,7 +606,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() {
   distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS>;
   distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS>;
 
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
   distance_matrix[GEOM_OCTREE][GEOM_BOX] = &Distance<OcTree, Box>;
   distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Distance<OcTree, Sphere>;
   distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Distance<OcTree, Capsule>;
@@ -654,6 +659,4 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() {
 #endif
 }
 // template struct DistanceFunctionMatrix;
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/hfield.cpp b/src/hfield.cpp
index ba771ec657db87cbfe70d6a39fd6b8eb8dbe4dae..af53913798953d68ab154c752728b05cda79969e 100644
--- a/src/hfield.cpp
+++ b/src/hfield.cpp
@@ -34,19 +34,18 @@
 
 /** \author Justin Carpentier */
 
-#include <hpp/fcl/hfield.h>
+#include "coal/hfield.h"
 
-#include <iostream>
-#include <string.h>
+#include "coal/BV/BV.h"
+#include "coal/shape/convex.h"
 
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/shape/convex.h>
+#include "coal/internal/BV_splitter.h"
+#include "coal/internal/BV_fitter.h"
 
-#include <hpp/fcl/internal/BV_splitter.h>
-#include <hpp/fcl/internal/BV_fitter.h>
+#include <iostream>
+#include <string.h>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 template <>
 NODE_TYPE HeightField<AABB>::getNodeType() const {
@@ -97,6 +96,4 @@ template class HeightField<RSS>;
 // template class HeightField<kIOS>;
 template class HeightField<OBBRSS>;
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/intersect.cpp b/src/intersect.cpp
index ca886a1d86f93f3d9194819f742fd2a4e93d118d..cd1320dd352d9555d22443680b86e80697c50fad 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -35,20 +35,20 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/internal/intersect.h>
+#include "coal/internal/intersect.h"
+#include "coal/internal/tools.h"
+
 #include <iostream>
 #include <limits>
 #include <vector>
 #include <cmath>
-#include <hpp/fcl/internal/tools.h>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
-bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2,
-                                   const Vec3f& v3, Vec3f* n, FCL_REAL* t) {
-  Vec3f n_ = (v2 - v1).cross(v3 - v1);
-  FCL_REAL norm2 = n_.squaredNorm();
+bool Intersect::buildTrianglePlane(const Vec3s& v1, const Vec3s& v2,
+                                   const Vec3s& v3, Vec3s* n, CoalScalar* t) {
+  Vec3s n_ = (v2 - v1).cross(v3 - v1);
+  CoalScalar norm2 = n_.squaredNorm();
   if (norm2 > 0) {
     *n = n_ / sqrt(norm2);
     *t = n->dot(v1);
@@ -57,12 +57,12 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2,
   return false;
 }
 
-void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
-                                 const Vec3f& B, Vec3f& VEC, Vec3f& X,
-                                 Vec3f& Y) {
-  Vec3f T;
-  FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
-  Vec3f TMP;
+void TriangleDistance::segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q,
+                                 const Vec3s& B, Vec3s& VEC, Vec3s& X,
+                                 Vec3s& Y) {
+  Vec3s T;
+  CoalScalar A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
+  Vec3s TMP;
 
   T = Q - P;
   A_dot_A = A.dot(A);
@@ -74,12 +74,12 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
   // t parameterizes ray P,A
   // u parameterizes ray Q,B
 
-  FCL_REAL t, u;
+  CoalScalar t, u;
 
   // compute t for the closest point on ray P,A to
   // ray Q,B
 
-  FCL_REAL denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B;
+  CoalScalar denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B;
 
   t = (A_dot_T * B_dot_B - B_dot_T * A_dot_B) / denom;
 
@@ -153,13 +153,13 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
   }
 }
 
-FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
-                                          Vec3f& P, Vec3f& Q) {
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                            Vec3s& P, Vec3s& Q) {
   // Compute vectors along the 6 sides
 
-  Vec3f Sv[3];
-  Vec3f Tv[3];
-  Vec3f VEC;
+  Vec3s Sv[3];
+  Vec3s Tv[3];
+  Vec3s VEC;
 
   Sv[0] = S[1] - S[0];
   Sv[1] = S[2] - S[1];
@@ -177,8 +177,8 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
   // Even if these tests fail, it may be helpful to know the closest
   // points found, and whether the triangles were shown disjoint
 
-  Vec3f V, Z, minP, minQ;
-  FCL_REAL mindd;
+  Vec3s V, Z, minP, minQ;
+  CoalScalar mindd;
   int shown_disjoint = 0;
 
   mindd = (S[0] - T[0]).squaredNorm() + 1;  // Set first minimum safely high
@@ -190,7 +190,7 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
       segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
 
       V = Q - P;
-      FCL_REAL dd = V.dot(V);
+      CoalScalar dd = V.dot(V);
 
       // Verify this closest point pair only if the distance
       // squared is less than the minimum found thus far.
@@ -201,13 +201,13 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
         mindd = dd;
 
         Z = S[(i + 2) % 3] - P;
-        FCL_REAL a = Z.dot(VEC);
+        CoalScalar a = Z.dot(VEC);
         Z = T[(j + 2) % 3] - Q;
-        FCL_REAL b = Z.dot(VEC);
+        CoalScalar b = Z.dot(VEC);
 
         if ((a <= 0) && (b >= 0)) return dd;
 
-        FCL_REAL p = V.dot(VEC);
+        CoalScalar p = V.dot(VEC);
 
         if (a < 0) a = 0;
         if (b > 0) b = 0;
@@ -232,8 +232,8 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
 
   // First check for case 1
 
-  Vec3f Sn;
-  FCL_REAL Snl;
+  Vec3s Sn;
+  CoalScalar Snl;
 
   Sn = Sv[0].cross(Sv[1]);  // Compute normal to S triangle
   Snl = Sn.dot(Sn);         // Compute square of length of normal
@@ -243,7 +243,7 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
   if (Snl > 1e-15) {
     // Get projection lengths of T points
 
-    Vec3f Tp;
+    Vec3s Tp;
 
     V = S[0] - T[0];
     Tp[0] = V.dot(Sn);
@@ -300,14 +300,14 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
     }
   }
 
-  Vec3f Tn;
-  FCL_REAL Tnl;
+  Vec3s Tn;
+  CoalScalar Tnl;
 
   Tn = Tv[0].cross(Tv[1]);
   Tnl = Tn.dot(Tn);
 
   if (Tnl > 1e-15) {
-    Vec3f Sp;
+    Vec3s Sp;
 
     V = T[0] - S[0];
     Sp[0] = V.dot(Tn);
@@ -367,12 +367,12 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
     return 0;
 }
 
-FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
-                                          const Vec3f& S3, const Vec3f& T1,
-                                          const Vec3f& T2, const Vec3f& T3,
-                                          Vec3f& P, Vec3f& Q) {
-  Vec3f S[3];
-  Vec3f T[3];
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                            const Vec3s& S3, const Vec3s& T1,
+                                            const Vec3s& T2, const Vec3s& T3,
+                                            Vec3s& P, Vec3s& Q) {
+  Vec3s S[3];
+  Vec3s T[3];
   S[0] = S1;
   S[1] = S2;
   S[2] = S3;
@@ -383,10 +383,10 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
   return sqrTriDistance(S, T, P, Q);
 }
 
-FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
-                                          const Matrix3f& R, const Vec3f& Tl,
-                                          Vec3f& P, Vec3f& Q) {
-  Vec3f T_transformed[3];
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                            const Matrix3s& R, const Vec3s& Tl,
+                                            Vec3s& P, Vec3s& Q) {
+  Vec3s T_transformed[3];
   T_transformed[0] = R * T[0] + Tl;
   T_transformed[1] = R * T[1] + Tl;
   T_transformed[2] = R * T[2] + Tl;
@@ -394,10 +394,10 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
   return sqrTriDistance(S, T_transformed, P, Q);
 }
 
-FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
-                                          const Transform3f& tf, Vec3f& P,
-                                          Vec3f& Q) {
-  Vec3f T_transformed[3];
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                            const Transform3s& tf, Vec3s& P,
+                                            Vec3s& Q) {
+  Vec3s T_transformed[3];
   T_transformed[0] = tf.transform(T[0]);
   T_transformed[1] = tf.transform(T[1]);
   T_transformed[2] = tf.transform(T[2]);
@@ -405,39 +405,39 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
   return sqrTriDistance(S, T_transformed, P, Q);
 }
 
-FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
-                                          const Vec3f& S3, const Vec3f& T1,
-                                          const Vec3f& T2, const Vec3f& T3,
-                                          const Matrix3f& R, const Vec3f& Tl,
-                                          Vec3f& P, Vec3f& Q) {
-  Vec3f T1_transformed = R * T1 + Tl;
-  Vec3f T2_transformed = R * T2 + Tl;
-  Vec3f T3_transformed = R * T3 + Tl;
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                            const Vec3s& S3, const Vec3s& T1,
+                                            const Vec3s& T2, const Vec3s& T3,
+                                            const Matrix3s& R, const Vec3s& Tl,
+                                            Vec3s& P, Vec3s& Q) {
+  Vec3s T1_transformed = R * T1 + Tl;
+  Vec3s T2_transformed = R * T2 + Tl;
+  Vec3s T3_transformed = R * T3 + Tl;
   return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
                         T3_transformed, P, Q);
 }
 
-FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
-                                          const Vec3f& S3, const Vec3f& T1,
-                                          const Vec3f& T2, const Vec3f& T3,
-                                          const Transform3f& tf, Vec3f& P,
-                                          Vec3f& Q) {
-  Vec3f T1_transformed = tf.transform(T1);
-  Vec3f T2_transformed = tf.transform(T2);
-  Vec3f T3_transformed = tf.transform(T3);
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                            const Vec3s& S3, const Vec3s& T1,
+                                            const Vec3s& T2, const Vec3s& T3,
+                                            const Transform3s& tf, Vec3s& P,
+                                            Vec3s& Q) {
+  Vec3s T1_transformed = tf.transform(T1);
+  Vec3s T2_transformed = tf.transform(T2);
+  Vec3s T3_transformed = tf.transform(T3);
   return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
                         T3_transformed, P, Q);
 }
 
-Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b,
-                                            const Vec3f& p) {
+Project::ProjectResult Project::projectLine(const Vec3s& a, const Vec3s& b,
+                                            const Vec3s& p) {
   ProjectResult res;
 
-  const Vec3f d = b - a;
-  const FCL_REAL l = d.squaredNorm();
+  const Vec3s d = b - a;
+  const CoalScalar l = d.squaredNorm();
 
   if (l > 0) {
-    const FCL_REAL t = (p - a).dot(d);
+    const CoalScalar t = (p - a).dot(d);
     res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
     res.parameterization[0] = 1 - res.parameterization[1];
     if (t >= l) {
@@ -455,19 +455,19 @@ Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b,
   return res;
 }
 
-Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
-                                                const Vec3f& c,
-                                                const Vec3f& p) {
+Project::ProjectResult Project::projectTriangle(const Vec3s& a, const Vec3s& b,
+                                                const Vec3s& c,
+                                                const Vec3s& p) {
   ProjectResult res;
 
   static const size_t nexti[3] = {1, 2, 0};
-  const Vec3f* vt[] = {&a, &b, &c};
-  const Vec3f dl[] = {a - b, b - c, c - a};
-  const Vec3f& n = dl[0].cross(dl[1]);
-  const FCL_REAL l = n.squaredNorm();
+  const Vec3s* vt[] = {&a, &b, &c};
+  const Vec3s dl[] = {a - b, b - c, c - a};
+  const Vec3s& n = dl[0].cross(dl[1]);
+  const CoalScalar l = n.squaredNorm();
 
   if (l > 0) {
-    FCL_REAL mindist = -1;
+    CoalScalar mindist = -1;
     for (size_t i = 0; i < 3; ++i) {
       if ((*vt[i] - p).dot(dl[i].cross(n)) >
           0)  // origin is to the outside part of the triangle edge, then the
@@ -490,9 +490,9 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
 
     if (mindist < 0)  // the origin project is within the triangle
     {
-      FCL_REAL d = (a - p).dot(n);
-      FCL_REAL s = sqrt(l);
-      Vec3f p_to_project = n * (d / l);
+      CoalScalar d = (a - p).dot(n);
+      CoalScalar s = sqrt(l);
+      Vec3s p_to_project = n * (d / l);
       mindist = p_to_project.squaredNorm();
       res.encode = 7;  // m = 0x111
       res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s;
@@ -507,17 +507,17 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
   return res;
 }
 
-Project::ProjectResult Project::projectTetrahedra(const Vec3f& a,
-                                                  const Vec3f& b,
-                                                  const Vec3f& c,
-                                                  const Vec3f& d,
-                                                  const Vec3f& p) {
+Project::ProjectResult Project::projectTetrahedra(const Vec3s& a,
+                                                  const Vec3s& b,
+                                                  const Vec3s& c,
+                                                  const Vec3s& d,
+                                                  const Vec3s& p) {
   ProjectResult res;
 
   static const size_t nexti[] = {1, 2, 0};
-  const Vec3f* vt[] = {&a, &b, &c, &d};
-  const Vec3f dl[3] = {a - d, b - d, c - d};
-  FCL_REAL vl = triple(dl[0], dl[1], dl[2]);
+  const Vec3s* vt[] = {&a, &b, &c, &d};
+  const Vec3s dl[3] = {a - d, b - d, c - d};
+  CoalScalar vl = triple(dl[0], dl[1], dl[2]);
   bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0;
   if (ng &&
       std::abs(vl) > 0)  // abs(vl) == 0, the tetrahedron is degenerated; if ng
@@ -525,11 +525,11 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a,
                          // does not grow toward the origin (in fact origin is
                          // on the other side of the abc face)
   {
-    FCL_REAL mindist = -1;
+    CoalScalar mindist = -1;
 
     for (size_t i = 0; i < 3; ++i) {
       size_t j = nexti[i];
-      FCL_REAL s = vl * (d - p).dot(dl[i].cross(dl[j]));
+      CoalScalar s = vl * (d - p).dot(dl[i].cross(dl[j]));
       if (s > 0)  // the origin is to the outside part of a triangle face, then
                   // the optimal can only be on the triangle face
       {
@@ -567,15 +567,15 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a,
   return res;
 }
 
-Project::ProjectResult Project::projectLineOrigin(const Vec3f& a,
-                                                  const Vec3f& b) {
+Project::ProjectResult Project::projectLineOrigin(const Vec3s& a,
+                                                  const Vec3s& b) {
   ProjectResult res;
 
-  const Vec3f d = b - a;
-  const FCL_REAL l = d.squaredNorm();
+  const Vec3s d = b - a;
+  const CoalScalar l = d.squaredNorm();
 
   if (l > 0) {
-    const FCL_REAL t = -a.dot(d);
+    const CoalScalar t = -a.dot(d);
     res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
     res.parameterization[0] = 1 - res.parameterization[1];
     if (t >= l) {
@@ -593,19 +593,19 @@ Project::ProjectResult Project::projectLineOrigin(const Vec3f& a,
   return res;
 }
 
-Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a,
-                                                      const Vec3f& b,
-                                                      const Vec3f& c) {
+Project::ProjectResult Project::projectTriangleOrigin(const Vec3s& a,
+                                                      const Vec3s& b,
+                                                      const Vec3s& c) {
   ProjectResult res;
 
   static const size_t nexti[3] = {1, 2, 0};
-  const Vec3f* vt[] = {&a, &b, &c};
-  const Vec3f dl[] = {a - b, b - c, c - a};
-  const Vec3f& n = dl[0].cross(dl[1]);
-  const FCL_REAL l = n.squaredNorm();
+  const Vec3s* vt[] = {&a, &b, &c};
+  const Vec3s dl[] = {a - b, b - c, c - a};
+  const Vec3s& n = dl[0].cross(dl[1]);
+  const CoalScalar l = n.squaredNorm();
 
   if (l > 0) {
-    FCL_REAL mindist = -1;
+    CoalScalar mindist = -1;
     for (size_t i = 0; i < 3; ++i) {
       if (vt[i]->dot(dl[i].cross(n)) >
           0)  // origin is to the outside part of the triangle edge, then the
@@ -628,9 +628,9 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a,
 
     if (mindist < 0)  // the origin project is within the triangle
     {
-      FCL_REAL d = a.dot(n);
-      FCL_REAL s = sqrt(l);
-      Vec3f o_to_project = n * (d / l);
+      CoalScalar d = a.dot(n);
+      CoalScalar s = sqrt(l);
+      Vec3s o_to_project = n * (d / l);
       mindist = o_to_project.squaredNorm();
       res.encode = 7;  // m = 0x111
       res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s;
@@ -645,16 +645,16 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a,
   return res;
 }
 
-Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a,
-                                                        const Vec3f& b,
-                                                        const Vec3f& c,
-                                                        const Vec3f& d) {
+Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3s& a,
+                                                        const Vec3s& b,
+                                                        const Vec3s& c,
+                                                        const Vec3s& d) {
   ProjectResult res;
 
   static const size_t nexti[] = {1, 2, 0};
-  const Vec3f* vt[] = {&a, &b, &c, &d};
-  const Vec3f dl[3] = {a - d, b - d, c - d};
-  FCL_REAL vl = triple(dl[0], dl[1], dl[2]);
+  const Vec3s* vt[] = {&a, &b, &c, &d};
+  const Vec3s dl[3] = {a - d, b - d, c - d};
+  CoalScalar vl = triple(dl[0], dl[1], dl[2]);
   bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0;
   if (ng &&
       std::abs(vl) > 0)  // abs(vl) == 0, the tetrahedron is degenerated; if ng
@@ -662,11 +662,11 @@ Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a,
                          // does not grow toward the origin (in fact origin is
                          // on the other side of the abc face)
   {
-    FCL_REAL mindist = -1;
+    CoalScalar mindist = -1;
 
     for (size_t i = 0; i < 3; ++i) {
       size_t j = nexti[i];
-      FCL_REAL s = vl * d.dot(dl[i].cross(dl[j]));
+      CoalScalar s = vl * d.dot(dl[i].cross(dl[j]));
       if (s > 0)  // the origin is to the outside part of a triangle face, then
                   // the optimal can only be on the triangle face
       {
@@ -704,6 +704,4 @@ Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a,
   return res;
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/math/transform.cpp b/src/math/transform.cpp
index ee6237c179c3d108bc97ca3fd4c5f18e2afef857..502b22ab15835879963eba8548e396f4c9974998 100644
--- a/src/math/transform.cpp
+++ b/src/math/transform.cpp
@@ -35,22 +35,19 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/math/transform.h>
+#include "coal/math/transform.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
-void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
-                       Transform3f& tf) {
+void relativeTransform(const Transform3s& tf1, const Transform3s& tf2,
+                       Transform3s& tf) {
   tf = tf1.inverseTimes(tf2);
 }
 
-void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2,
-                        Transform3f& tf) {
-  Matrix3f R(tf2.getRotation() * tf1.getRotation().transpose());
-  tf = Transform3f(R, tf2.getTranslation() - R * tf1.getTranslation());
+void relativeTransform2(const Transform3s& tf1, const Transform3s& tf2,
+                        Transform3s& tf) {
+  Matrix3s R(tf2.getRotation() * tf1.getRotation().transpose());
+  tf = Transform3s(R, tf2.getTranslation() - R * tf1.getTranslation());
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp
index 495ace6024066c5cc3cdc535d0b60242c4dd5036..a0012d7649425f3fc2fb5390611963447926bf5a 100644
--- a/src/mesh_loader/assimp.cpp
+++ b/src/mesh_loader/assimp.cpp
@@ -35,11 +35,11 @@
 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
 #define nullptr NULL
 #endif
-#include <hpp/fcl/mesh_loader/assimp.h>
+#include "coal/mesh_loader/assimp.h"
 
 // Assimp >= 5.0 is forcing the use of C++11 keywords. A fix has been submitted
 // https://github.com/assimp/assimp/pull/2758. The next lines fixes the bug for
-// current version of hpp-fcl.
+// current version of Coal.
 #include <assimp/defs.h>
 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) && \
     defined(AI_NO_EXCEPT)
@@ -54,8 +54,7 @@
 #include <assimp/postprocess.h>
 #include <assimp/scene.h>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace internal {
 
@@ -89,11 +88,11 @@ void Loader::load(const std::string& resource_path) {
         std::string("Could not load resource ") + resource_path +
         std::string("\n") + importer->GetErrorString() + std::string("\n") +
         "Hint: the mesh directory may be wrong.");
-    HPP_FCL_THROW_PRETTY(exception_message.c_str(), std::invalid_argument);
+    COAL_THROW_PRETTY(exception_message.c_str(), std::invalid_argument);
   }
 
   if (!scene->HasMeshes())
-    HPP_FCL_THROW_PRETTY(
+    COAL_THROW_PRETTY(
         (std::string("No meshes found in file ") + resource_path).c_str(),
         std::invalid_argument);
 }
@@ -107,7 +106,7 @@ void Loader::load(const std::string& resource_path) {
  * @param[in]  vertices_offset Current number of vertices in the model
  * @param      tv              Triangles and Vertices of the mesh submodels
  */
-unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene,
+unsigned recurseBuildMesh(const coal::Vec3s& scale, const aiScene* scene,
                           const aiNode* node, unsigned vertices_offset,
                           TriangleAndVertices& tv) {
   if (!node) return 0;
@@ -132,7 +131,7 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene,
       aiVector3D p = input_mesh->mVertices[j];
       p *= transform;
       tv.vertices_.push_back(
-          fcl::Vec3f(p.x * scale[0], p.y * scale[1], p.z * scale[2]));
+          coal::Vec3s(p.x * scale[0], p.y * scale[1], p.z * scale[2]));
     }
 
     // add the indices
@@ -140,9 +139,9 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene,
       aiFace& face = input_mesh->mFaces[j];
       assert(face.mNumIndices == 3 && "The size of the face is not valid.");
       tv.triangles_.push_back(
-          fcl::Triangle(vertices_offset + face.mIndices[0],
-                        vertices_offset + face.mIndices[1],
-                        vertices_offset + face.mIndices[2]));
+          coal::Triangle(vertices_offset + face.mIndices[0],
+                         vertices_offset + face.mIndices[1],
+                         vertices_offset + face.mIndices[2]));
     }
 
     nbVertices += input_mesh->mNumVertices;
@@ -156,11 +155,10 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene,
   return nbVertices;
 }
 
-void buildMesh(const fcl::Vec3f& scale, const aiScene* scene,
+void buildMesh(const coal::Vec3s& scale, const aiScene* scene,
                unsigned vertices_offset, TriangleAndVertices& tv) {
   recurseBuildMesh(scale, scene, scene->mRootNode, vertices_offset, tv);
 }
 
 }  // namespace internal
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp
index cc5543e1ea30bba687a62468a4ea44c9b909e087..9b38398ba42fa04ead401de68ba54c389d1bd0f4 100644
--- a/src/mesh_loader/loader.cpp
+++ b/src/mesh_loader/loader.cpp
@@ -34,19 +34,18 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <hpp/fcl/mesh_loader/loader.h>
-#include <hpp/fcl/mesh_loader/assimp.h>
+#include "coal/mesh_loader/loader.h"
+#include "coal/mesh_loader/assimp.h"
 
 #include <boost/filesystem.hpp>
 
-#ifdef HPP_FCL_HAS_OCTOMAP
-#include <hpp/fcl/octree.h>
+#ifdef COAL_HAS_OCTOMAP
+#include "coal/octree.h"
 #endif
 
-#include <hpp/fcl/BV/BV.h>
+#include "coal/BV/BV.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const {
   const CachedMeshLoader::Key& a = *this;
   for (int i = 0; i < 3; ++i) {
@@ -59,14 +58,14 @@ bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const {
 }
 
 template <typename BV>
-BVHModelPtr_t _load(const std::string& filename, const Vec3f& scale) {
+BVHModelPtr_t _load(const std::string& filename, const Vec3s& scale) {
   shared_ptr<BVHModel<BV> > polyhedron(new BVHModel<BV>);
   loadPolyhedronFromResource(filename, scale, polyhedron);
   return polyhedron;
 }
 
 BVHModelPtr_t MeshLoader::load(const std::string& filename,
-                               const Vec3f& scale) {
+                               const Vec3s& scale) {
   switch (bvType_) {
     case BV_AABB:
       return _load<AABB>(filename, scale);
@@ -85,24 +84,23 @@ BVHModelPtr_t MeshLoader::load(const std::string& filename,
     case BV_KDOP24:
       return _load<KDOP<24> >(filename, scale);
     default:
-      HPP_FCL_THROW_PRETTY("Unhandled bouding volume type.",
-                           std::invalid_argument);
+      COAL_THROW_PRETTY("Unhandled bouding volume type.",
+                        std::invalid_argument);
   }
 }
 
 CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) {
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
   shared_ptr<octomap::OcTree> octree(new octomap::OcTree(filename));
-  return CollisionGeometryPtr_t(new hpp::fcl::OcTree(octree));
+  return CollisionGeometryPtr_t(new coal::OcTree(octree));
 #else
-  HPP_FCL_THROW_PRETTY(
-      "hpp-fcl compiled without OctoMap. Cannot create OcTrees.",
-      std::logic_error);
+  COAL_THROW_PRETTY("Coal compiled without OctoMap. Cannot create OcTrees.",
+                    std::logic_error);
 #endif
 }
 
 BVHModelPtr_t CachedMeshLoader::load(const std::string& filename,
-                                     const Vec3f& scale) {
+                                     const Vec3s& scale) {
   Key key(filename, scale);
 
   std::time_t mtime = 0;
@@ -125,6 +123,4 @@ BVHModelPtr_t CachedMeshLoader::load(const std::string& filename,
   cache_[key] = val;
   return geom;
 }
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
index 17d505058ced4ee545457544acf00a7a3f373bc8..628953b029d6094de371ac90a439445dfbcfc574 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -36,35 +36,34 @@
  */
 /** \author Jia Pan, Florent Lamiraux */
 
-#ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H
-#define HPP_FCL_SRC_NARROWPHASE_DETAILS_H
+#ifndef COAL_SRC_NARROWPHASE_DETAILS_H
+#define COAL_SRC_NARROWPHASE_DETAILS_H
 
-#include <hpp/fcl/internal/traversal_node_setup.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
+#include "coal/internal/traversal_node_setup.h"
+#include "coal/narrowphase/narrowphase.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace details {
 // Compute the point on a line segment that is the closest point on the
 // segment to to another point. The code is inspired by the explanation
 // given by Dan Sunday's page:
 //   http://geomalgorithms.com/a02-_lines.html
-static inline void lineSegmentPointClosestToPoint(const Vec3f& p,
-                                                  const Vec3f& s1,
-                                                  const Vec3f& s2, Vec3f& sp) {
-  Vec3f v = s2 - s1;
-  Vec3f w = p - s1;
+static inline void lineSegmentPointClosestToPoint(const Vec3s& p,
+                                                  const Vec3s& s1,
+                                                  const Vec3s& s2, Vec3s& sp) {
+  Vec3s v = s2 - s1;
+  Vec3s w = p - s1;
 
-  FCL_REAL c1 = w.dot(v);
-  FCL_REAL c2 = v.dot(v);
+  CoalScalar c1 = w.dot(v);
+  CoalScalar c2 = v.dot(v);
 
   if (c1 <= 0) {
     sp = s1;
   } else if (c2 <= c1) {
     sp = s2;
   } else {
-    FCL_REAL b = c1 / c2;
-    Vec3f Pb = s1 + v * b;
+    CoalScalar b = c1 / c2;
+    Vec3s Pb = s1 + v * b;
     sp = Pb;
   }
 }
@@ -73,23 +72,25 @@ static inline void lineSegmentPointClosestToPoint(const Vec3f& p,
 /// @param p2 witness point on the Capsule.
 /// @param normal pointing from shape 1 to shape 2 (sphere to capsule).
 /// @return the distance between the two shapes (negative if penetration).
-inline FCL_REAL sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
-                                      const Capsule& s2, const Transform3f& tf2,
-                                      Vec3f& p1, Vec3f& p2, Vec3f& normal) {
-  Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength)));
-  Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength)));
-  Vec3f s_c = tf1.getTranslation();
+inline CoalScalar sphereCapsuleDistance(const Sphere& s1,
+                                        const Transform3s& tf1,
+                                        const Capsule& s2,
+                                        const Transform3s& tf2, Vec3s& p1,
+                                        Vec3s& p2, Vec3s& normal) {
+  Vec3s pos1(tf2.transform(Vec3s(0., 0., s2.halfLength)));
+  Vec3s pos2(tf2.transform(Vec3s(0., 0., -s2.halfLength)));
+  Vec3s s_c = tf1.getTranslation();
 
-  Vec3f segment_point;
+  Vec3s segment_point;
 
   lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point);
   normal = segment_point - s_c;
-  FCL_REAL norm(normal.norm());
-  FCL_REAL r1 = s1.radius + s1.getSweptSphereRadius();
-  FCL_REAL r2 = s2.radius + s2.getSweptSphereRadius();
-  FCL_REAL dist = norm - r1 - r2;
+  CoalScalar norm(normal.norm());
+  CoalScalar r1 = s1.radius + s1.getSweptSphereRadius();
+  CoalScalar r2 = s2.radius + s2.getSweptSphereRadius();
+  CoalScalar dist = norm - r1 - r2;
 
-  static const FCL_REAL eps(std::numeric_limits<FCL_REAL>::epsilon());
+  static const CoalScalar eps(std::numeric_limits<CoalScalar>::epsilon());
   if (norm > eps) {
     normal.normalize();
   } else {
@@ -104,34 +105,35 @@ inline FCL_REAL sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
 /// @param p2 witness point on the Cylinder.
 /// @param normal pointing from shape 1 to shape 2 (sphere to cylinder).
 /// @return the distance between the two shapes (negative if penetration).
-inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
-                                       const Cylinder& s2,
-                                       const Transform3f& tf2, Vec3f& p1,
-                                       Vec3f& p2, Vec3f& normal) {
-  static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
-  FCL_REAL r1(s1.radius);
-  FCL_REAL r2(s2.radius);
-  FCL_REAL lz2(s2.halfLength);
+inline CoalScalar sphereCylinderDistance(const Sphere& s1,
+                                         const Transform3s& tf1,
+                                         const Cylinder& s2,
+                                         const Transform3s& tf2, Vec3s& p1,
+                                         Vec3s& p2, Vec3s& normal) {
+  static const CoalScalar eps(sqrt(std::numeric_limits<CoalScalar>::epsilon()));
+  CoalScalar r1(s1.radius);
+  CoalScalar r2(s2.radius);
+  CoalScalar lz2(s2.halfLength);
   // boundaries of the cylinder axis
-  Vec3f A(tf2.transform(Vec3f(0, 0, -lz2)));
-  Vec3f B(tf2.transform(Vec3f(0, 0, lz2)));
+  Vec3s A(tf2.transform(Vec3s(0, 0, -lz2)));
+  Vec3s B(tf2.transform(Vec3s(0, 0, lz2)));
   // Position of the center of the sphere
-  Vec3f S(tf1.getTranslation());
+  Vec3s S(tf1.getTranslation());
   // axis of the cylinder
-  Vec3f u(tf2.getRotation().col(2));
+  Vec3s u(tf2.getRotation().col(2));
   /// @todo a tiny performance improvement could be achieved using the abscissa
   /// with S as the origin
   assert((B - A - (s2.halfLength * 2) * u).norm() < eps);
-  Vec3f AS(S - A);
+  Vec3s AS(S - A);
   // abscissa of S on cylinder axis with A as the origin
-  FCL_REAL s(u.dot(AS));
-  Vec3f P(A + s * u);
-  Vec3f PS(S - P);
-  FCL_REAL dPS = PS.norm();
+  CoalScalar s(u.dot(AS));
+  Vec3s P(A + s * u);
+  Vec3s PS(S - P);
+  CoalScalar dPS = PS.norm();
   // Normal to cylinder axis such that plane (A, u, v) contains sphere
   // center
-  Vec3f v(0, 0, 0);
-  FCL_REAL dist;
+  Vec3s v(0, 0, 0);
+  CoalScalar dist;
   if (dPS > eps) {
     // S is not on cylinder axis
     v = (1 / dPS) * PS;
@@ -146,8 +148,8 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
     } else {
       // closest point on cylinder is on cylinder circle basis
       p2 = A + r2 * v;
-      Vec3f Sp2(p2 - S);
-      FCL_REAL dSp2 = Sp2.norm();
+      Vec3s Sp2(p2 - S);
+      CoalScalar dSp2 = Sp2.norm();
       if (dSp2 > eps) {
         normal = (1 / dSp2) * Sp2;
         p1 = S + r1 * normal;
@@ -179,8 +181,8 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
     } else {
       // closest point on cylinder is on cylinder circle basis
       p2 = B + r2 * v;
-      Vec3f Sp2(p2 - S);
-      FCL_REAL dSp2 = Sp2.norm();
+      Vec3s Sp2(p2 - S);
+      CoalScalar dSp2 = Sp2.norm();
       if (dSp2 > eps) {
         normal = (1 / dSp2) * Sp2;
         p1 = S + r1 * normal;
@@ -197,8 +199,8 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
   }
 
   // Take swept-sphere radius into account
-  const FCL_REAL ssr1 = s1.getSweptSphereRadius();
-  const FCL_REAL ssr2 = s2.getSweptSphereRadius();
+  const CoalScalar ssr1 = s1.getSweptSphereRadius();
+  const CoalScalar ssr2 = s2.getSweptSphereRadius();
   if (ssr1 > 0 || ssr2 > 0) {
     p1 += ssr1 * normal;
     p2 -= ssr2 * normal;
@@ -212,19 +214,19 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
 /// @param p2 witness point on the second Sphere.
 /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2).
 /// @return the distance between the two spheres (negative if penetration).
-inline FCL_REAL sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
-                                     const Sphere& s2, const Transform3f& tf2,
-                                     Vec3f& p1, Vec3f& p2, Vec3f& normal) {
-  const fcl::Vec3f& center1 = tf1.getTranslation();
-  const fcl::Vec3f& center2 = tf2.getTranslation();
-  FCL_REAL r1 = (s1.radius + s1.getSweptSphereRadius());
-  FCL_REAL r2 = (s2.radius + s2.getSweptSphereRadius());
-
-  Vec3f c1c2 = center2 - center1;
-  FCL_REAL cdist = c1c2.norm();
-  Vec3f unit(1, 0, 0);
-  if (cdist > Eigen::NumTraits<FCL_REAL>::epsilon()) unit = c1c2 / cdist;
-  FCL_REAL dist = cdist - r1 - r2;
+inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3s& tf1,
+                                       const Sphere& s2, const Transform3s& tf2,
+                                       Vec3s& p1, Vec3s& p2, Vec3s& normal) {
+  const coal::Vec3s& center1 = tf1.getTranslation();
+  const coal::Vec3s& center2 = tf2.getTranslation();
+  CoalScalar r1 = (s1.radius + s1.getSweptSphereRadius());
+  CoalScalar r2 = (s2.radius + s2.getSweptSphereRadius());
+
+  Vec3s c1c2 = center2 - center1;
+  CoalScalar cdist = c1c2.norm();
+  Vec3s unit(1, 0, 0);
+  if (cdist > Eigen::NumTraits<CoalScalar>::epsilon()) unit = c1c2 / cdist;
+  CoalScalar dist = cdist - r1 - r2;
   normal = unit;
   p1.noalias() = center1 + r1 * unit;
   p2.noalias() = center2 - r2 * unit;
@@ -232,14 +234,14 @@ inline FCL_REAL sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
 }
 
 /** @brief the minimum distance from a point to a line */
-inline FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,
-                                   const Vec3f& p, Vec3f& nearest) {
-  Vec3f diff = p - from;
-  Vec3f v = to - from;
-  FCL_REAL t = v.dot(diff);
+inline CoalScalar segmentSqrDistance(const Vec3s& from, const Vec3s& to,
+                                     const Vec3s& p, Vec3s& nearest) {
+  Vec3s diff = p - from;
+  Vec3s v = to - from;
+  CoalScalar t = v.dot(diff);
 
   if (t > 0) {
-    FCL_REAL dotVV = v.squaredNorm();
+    CoalScalar dotVV = v.squaredNorm();
     if (t < dotVV) {
       t /= dotVV;
       diff -= v * t;
@@ -255,21 +257,21 @@ inline FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,
 }
 
 /// @brief Whether a point's projection is in a triangle
-inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
-                              const Vec3f& normal, const Vec3f& p) {
-  Vec3f edge1(p2 - p1);
-  Vec3f edge2(p3 - p2);
-  Vec3f edge3(p1 - p3);
+inline bool projectInTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3,
+                              const Vec3s& normal, const Vec3s& p) {
+  Vec3s edge1(p2 - p1);
+  Vec3s edge2(p3 - p2);
+  Vec3s edge3(p1 - p3);
 
-  Vec3f p1_to_p(p - p1);
-  Vec3f p2_to_p(p - p2);
-  Vec3f p3_to_p(p - p3);
+  Vec3s p1_to_p(p - p1);
+  Vec3s p2_to_p(p - p2);
+  Vec3s p3_to_p(p - p3);
 
-  Vec3f edge1_normal(edge1.cross(normal));
-  Vec3f edge2_normal(edge2.cross(normal));
-  Vec3f edge3_normal(edge3.cross(normal));
+  Vec3s edge1_normal(edge1.cross(normal));
+  Vec3s edge2_normal(edge2.cross(normal));
+  Vec3s edge3_normal(edge3.cross(normal));
 
-  FCL_REAL r1, r2, r3;
+  CoalScalar r1, r2, r3;
   r1 = edge1_normal.dot(p1_to_p);
   r2 = edge2_normal.dot(p2_to_p);
   r3 = edge3_normal.dot(p3_to_p);
@@ -283,30 +285,31 @@ inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
 /// @param p2 witness point on the second Sphere.
 /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2).
 /// @return the distance between the two shapes (negative if penetration).
-inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1,
-                                       const TriangleP& tri,
-                                       const Transform3f& tf2, Vec3f& p1,
-                                       Vec3f& p2, Vec3f& normal) {
-  const Vec3f& P1 = tf2.transform(tri.a);
-  const Vec3f& P2 = tf2.transform(tri.b);
-  const Vec3f& P3 = tf2.transform(tri.c);
-
-  Vec3f tri_normal = (P2 - P1).cross(P3 - P1);
+inline CoalScalar sphereTriangleDistance(const Sphere& s,
+                                         const Transform3s& tf1,
+                                         const TriangleP& tri,
+                                         const Transform3s& tf2, Vec3s& p1,
+                                         Vec3s& p2, Vec3s& normal) {
+  const Vec3s& P1 = tf2.transform(tri.a);
+  const Vec3s& P2 = tf2.transform(tri.b);
+  const Vec3s& P3 = tf2.transform(tri.c);
+
+  Vec3s tri_normal = (P2 - P1).cross(P3 - P1);
   tri_normal.normalize();
-  const Vec3f& center = tf1.getTranslation();
+  const Vec3s& center = tf1.getTranslation();
   // Note: comparing an object with a swept-sphere radius of r1 against another
   // object with a swept-sphere radius of r2 is equivalent to comparing the
   // first object with a swept-sphere radius of r1 + r2 against the second
   // object with a swept-sphere radius of 0.
-  const FCL_REAL& radius =
+  const CoalScalar& radius =
       s.radius + s.getSweptSphereRadius() + tri.getSweptSphereRadius();
   assert(radius >= 0);
   assert(s.radius >= 0);
-  Vec3f p1_to_center = center - P1;
-  FCL_REAL distance_from_plane = p1_to_center.dot(tri_normal);
-  Vec3f closest_point(
-      Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
-  FCL_REAL min_distance_sqr, distance_sqr;
+  Vec3s p1_to_center = center - P1;
+  CoalScalar distance_from_plane = p1_to_center.dot(tri_normal);
+  Vec3s closest_point(
+      Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+  CoalScalar min_distance_sqr, distance_sqr;
 
   if (distance_from_plane < 0) {
     distance_from_plane *= -1;
@@ -318,7 +321,7 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1,
     min_distance_sqr = distance_from_plane * distance_from_plane;
   } else {
     // Compute distance to each edge and take minimal distance
-    Vec3f nearest_on_edge;
+    Vec3s nearest_on_edge;
     min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point);
 
     distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
@@ -336,7 +339,7 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1,
   normal = (closest_point - center).normalized();
   p1 = center + normal * (s.radius + s.getSweptSphereRadius());
   p2 = closest_point - normal * tri.getSweptSphereRadius();
-  const FCL_REAL distance = std::sqrt(min_distance_sqr) - radius;
+  const CoalScalar distance = std::sqrt(min_distance_sqr) - radius;
   return distance;
 }
 
@@ -344,9 +347,9 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1,
 /// @param p2 closest (or most penetrating) point on the shape,
 /// @param normal the halfspace normal.
 /// @return the distance between the two shapes (negative if penetration).
-inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
-                                  const ShapeBase& s, const Transform3f& tf2,
-                                  Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3s& tf1,
+                                    const ShapeBase& s, const Transform3s& tf2,
+                                    Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   // TODO(louis): handle multiple contact points when the halfspace normal is
   // parallel to the shape's surface (every primitive except sphere and
   // ellipsoid).
@@ -355,7 +358,7 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
   Halfspace new_h = transform(h, tf1);
 
   // Express halfspace normal in shape frame
-  Vec3f n_2(tf2.getRotation().transpose() * new_h.n);
+  Vec3s n_2(tf2.getRotation().transpose() * new_h.n);
 
   // Compute support of shape in direction of halfspace normal
   int hint = 0;
@@ -363,13 +366,13 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
       getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_2, hint);
   p2 = tf2.transform(p2);
 
-  const FCL_REAL dist = new_h.signedDistance(p2);
+  const CoalScalar dist = new_h.signedDistance(p2);
   p1.noalias() = p2 - dist * new_h.n;
   normal.noalias() = new_h.n;
 
-  const FCL_REAL dummy_precision =
-      std::sqrt(Eigen::NumTraits<FCL_REAL>::dummy_precision());
-  HPP_FCL_UNUSED_VARIABLE(dummy_precision);
+  const CoalScalar dummy_precision =
+      std::sqrt(Eigen::NumTraits<CoalScalar>::dummy_precision());
+  COAL_UNUSED_VARIABLE(dummy_precision);
   assert(new_h.distance(p1) <= dummy_precision);
   return dist;
 }
@@ -378,9 +381,9 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
 /// @param p2 closest (or most penetrating) point on the shape,
 /// @param normal the halfspace normal.
 /// @return the distance between the two shapes (negative if penetration).
-inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1,
-                              const ShapeBase& s, const Transform3f& tf2,
-                              Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+inline CoalScalar planeDistance(const Plane& plane, const Transform3s& tf1,
+                                const ShapeBase& s, const Transform3s& tf2,
+                                Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   // TODO(louis): handle multiple contact points when the plane normal is
   // parallel to the shape's surface (every primitive except sphere and
   // ellipsoid).
@@ -389,28 +392,28 @@ inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1,
   std::array<Halfspace, 2> new_h = transformToHalfspaces(plane, tf1);
 
   // Express halfspace normals in shape frame
-  Vec3f n_h1(tf2.getRotation().transpose() * new_h[0].n);
-  Vec3f n_h2(tf2.getRotation().transpose() * new_h[1].n);
+  Vec3s n_h1(tf2.getRotation().transpose() * new_h[0].n);
+  Vec3s n_h2(tf2.getRotation().transpose() * new_h[1].n);
 
   // Compute support of shape in direction of halfspace normal and its opposite
   int hint = 0;
-  Vec3f p2h1 =
+  Vec3s p2h1 =
       getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_h1, hint);
   p2h1 = tf2.transform(p2h1);
 
   hint = 0;
-  Vec3f p2h2 =
+  Vec3s p2h2 =
       getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_h2, hint);
   p2h2 = tf2.transform(p2h2);
 
-  FCL_REAL dist1 = new_h[0].signedDistance(p2h1);
-  FCL_REAL dist2 = new_h[1].signedDistance(p2h2);
+  CoalScalar dist1 = new_h[0].signedDistance(p2h1);
+  CoalScalar dist2 = new_h[1].signedDistance(p2h2);
 
-  const FCL_REAL dummy_precision =
-      std::sqrt(Eigen::NumTraits<FCL_REAL>::dummy_precision());
-  HPP_FCL_UNUSED_VARIABLE(dummy_precision);
+  const CoalScalar dummy_precision =
+      std::sqrt(Eigen::NumTraits<CoalScalar>::dummy_precision());
+  COAL_UNUSED_VARIABLE(dummy_precision);
 
-  FCL_REAL dist;
+  CoalScalar dist;
   if (dist1 >= dist2) {
     dist = dist1;
     p2.noalias() = p2h1;
@@ -432,21 +435,21 @@ inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1,
 /// @param ps the witness point on the sphere.
 /// @param normal pointing from box to sphere
 /// @return the distance between the two shapes (negative if penetration).
-inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb,
-                                  const Sphere& s, const Transform3f& tfs,
-                                  Vec3f& pb, Vec3f& ps, Vec3f& normal) {
-  const Vec3f& os = tfs.getTranslation();
-  const Vec3f& ob = tfb.getTranslation();
-  const Matrix3f& Rb = tfb.getRotation();
+inline CoalScalar boxSphereDistance(const Box& b, const Transform3s& tfb,
+                                    const Sphere& s, const Transform3s& tfs,
+                                    Vec3s& pb, Vec3s& ps, Vec3s& normal) {
+  const Vec3s& os = tfs.getTranslation();
+  const Vec3s& ob = tfb.getTranslation();
+  const Matrix3s& Rb = tfb.getRotation();
 
   pb = ob;
 
   bool outside = false;
-  const Vec3f os_in_b_frame(Rb.transpose() * (os - ob));
+  const Vec3s os_in_b_frame(Rb.transpose() * (os - ob));
   int axis = -1;
-  FCL_REAL min_d = (std::numeric_limits<FCL_REAL>::max)();
+  CoalScalar min_d = (std::numeric_limits<CoalScalar>::max)();
   for (int i = 0; i < 3; ++i) {
-    FCL_REAL facedist;
+    CoalScalar facedist;
     if (os_in_b_frame(i) < -b.halfSide(i)) {  // outside
       pb.noalias() -= b.halfSide(i) * Rb.col(i);
       outside = true;
@@ -463,9 +466,9 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb,
     }
   }
   normal = pb - os;
-  FCL_REAL pdist = normal.norm();
-  FCL_REAL dist;  // distance between sphere and box
-  if (outside) {  // pb is on the box
+  CoalScalar pdist = normal.norm();
+  CoalScalar dist;  // distance between sphere and box
+  if (outside) {    // pb is on the box
     dist = pdist - s.radius;
     normal /= -pdist;
   } else {  // pb is inside the box
@@ -483,8 +486,8 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb,
   }
 
   // Take swept-sphere radius into account
-  const FCL_REAL ssrb = b.getSweptSphereRadius();
-  const FCL_REAL ssrs = s.getSweptSphereRadius();
+  const CoalScalar ssrb = b.getSweptSphereRadius();
+  const CoalScalar ssrs = s.getSweptSphereRadius();
   if (ssrb > 0 || ssrs > 0) {
     pb += ssrb * normal;
     ps -= ssrs * normal;
@@ -506,36 +509,36 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb,
 /// The points p1 and p2 are the same point and represent the origin of the
 /// intersection line between the objects. The normal is the direction of this
 /// line.
-inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1,
-                                           const Transform3f& tf1,
-                                           const Halfspace& s2,
-                                           const Transform3f& tf2, Vec3f& p1,
-                                           Vec3f& p2, Vec3f& normal) {
+inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1,
+                                             const Transform3s& tf1,
+                                             const Halfspace& s2,
+                                             const Transform3s& tf2, Vec3s& p1,
+                                             Vec3s& p2, Vec3s& normal) {
   Halfspace new_s1 = transform(s1, tf1);
   Halfspace new_s2 = transform(s2, tf2);
 
-  FCL_REAL distance;
-  Vec3f dir = (new_s1.n).cross(new_s2.n);
-  FCL_REAL dir_sq_norm = dir.squaredNorm();
+  CoalScalar distance;
+  Vec3s dir = (new_s1.n).cross(new_s2.n);
+  CoalScalar dir_sq_norm = dir.squaredNorm();
 
-  if (dir_sq_norm < std::numeric_limits<FCL_REAL>::epsilon())  // parallel
+  if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon())  // parallel
   {
     if (new_s1.n.dot(new_s2.n) > 0) {
       // If the two halfspaces have the same normal, one is inside the other
       // and they can't be separated. They have inifinte penetration depth.
-      distance = -(std::numeric_limits<FCL_REAL>::max)();
+      distance = -(std::numeric_limits<CoalScalar>::max)();
       if (new_s1.d <= new_s2.d) {
         normal = new_s1.n;
         p1 = normal * distance;
         p2 = new_s2.n * new_s2.d;
         assert(new_s2.distance(p2) <=
-               Eigen::NumTraits<FCL_REAL>::dummy_precision());
+               Eigen::NumTraits<CoalScalar>::dummy_precision());
       } else {
         normal = -new_s1.n;
         p1 << new_s1.n * new_s1.d;
         p2 = -(normal * distance);
         assert(new_s1.distance(p1) <=
-               Eigen::NumTraits<FCL_REAL>::dummy_precision());
+               Eigen::NumTraits<CoalScalar>::dummy_precision());
       }
     } else {
       distance = -(new_s1.d + new_s2.d);
@@ -547,7 +550,7 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1,
     // If the halfspaces are not parallel, they are in collision.
     // Their distance, in the sens of the norm of separation vector, is infinite
     // (it's impossible to find a translation which separates them)
-    distance = -(std::numeric_limits<FCL_REAL>::max)();
+    distance = -(std::numeric_limits<CoalScalar>::max)();
     // p1 and p2 are the same point, corresponding to a point on the
     // intersection line between the two objects. Normal is the direction of
     // that line.
@@ -559,8 +562,8 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1,
   }
 
   // Take swept-sphere radius into account
-  const FCL_REAL ssr1 = s1.getSweptSphereRadius();
-  const FCL_REAL ssr2 = s2.getSweptSphereRadius();
+  const CoalScalar ssr1 = s1.getSweptSphereRadius();
+  const CoalScalar ssr2 = s2.getSweptSphereRadius();
   if (ssr1 > 0 || ssr2 > 0) {
     p1 += ssr1 * normal;
     p2 -= ssr2 * normal;
@@ -582,18 +585,19 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1,
 /// The points p1 and p2 are the same point and represent the origin of the
 /// intersection line between the objects. The normal is the direction of this
 /// line.
-inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1,
-                                       const Transform3f& tf1, const Plane& s2,
-                                       const Transform3f& tf2, Vec3f& p1,
-                                       Vec3f& p2, Vec3f& normal) {
+inline CoalScalar halfspacePlaneDistance(const Halfspace& s1,
+                                         const Transform3s& tf1,
+                                         const Plane& s2,
+                                         const Transform3s& tf2, Vec3s& p1,
+                                         Vec3s& p2, Vec3s& normal) {
   Halfspace new_s1 = transform(s1, tf1);
   Plane new_s2 = transform(s2, tf2);
 
-  FCL_REAL distance;
-  Vec3f dir = (new_s1.n).cross(new_s2.n);
-  FCL_REAL dir_sq_norm = dir.squaredNorm();
+  CoalScalar distance;
+  Vec3s dir = (new_s1.n).cross(new_s2.n);
+  CoalScalar dir_sq_norm = dir.squaredNorm();
 
-  if (dir_sq_norm < std::numeric_limits<FCL_REAL>::epsilon())  // parallel
+  if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon())  // parallel
   {
     normal = new_s1.n;
     distance = new_s1.n.dot(new_s2.n) > 0 ? (new_s2.d - new_s1.d)
@@ -601,14 +605,14 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1,
     p1 = new_s1.n * new_s1.d;
     p2 = new_s2.n * new_s2.d;
     assert(new_s1.distance(p1) <=
-           Eigen::NumTraits<FCL_REAL>::dummy_precision());
+           Eigen::NumTraits<CoalScalar>::dummy_precision());
     assert(new_s2.distance(p2) <=
-           Eigen::NumTraits<FCL_REAL>::dummy_precision());
+           Eigen::NumTraits<CoalScalar>::dummy_precision());
   } else {
     // If the halfspace and plane are not parallel, they are in collision.
     // Their distance, in the sens of the norm of separation vector, is infinite
     // (it's impossible to find a translation which separates them)
-    distance = -(std::numeric_limits<FCL_REAL>::max)();
+    distance = -(std::numeric_limits<CoalScalar>::max)();
     // p1 and p2 are the same point, corresponding to a point on the
     // intersection line between the two objects. Normal is the direction of
     // that line.
@@ -620,8 +624,8 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1,
   }
 
   // Take swept-sphere radius into account
-  const FCL_REAL ssr1 = s1.getSweptSphereRadius();
-  const FCL_REAL ssr2 = s2.getSweptSphereRadius();
+  const CoalScalar ssr1 = s1.getSweptSphereRadius();
+  const CoalScalar ssr2 = s2.getSweptSphereRadius();
   if (ssr1 > 0 || ssr2 > 0) {
     p1 += ssr1 * normal;
     p2 -= ssr2 * normal;
@@ -643,27 +647,27 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1,
 /// The points p1 and p2 are the same point and represent the origin of the
 /// intersection line between the objects. The normal is the direction of this
 /// line.
-inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1,
-                                   const Plane& s2, const Transform3f& tf2,
-                                   Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3s& tf1,
+                                     const Plane& s2, const Transform3s& tf2,
+                                     Vec3s& p1, Vec3s& p2, Vec3s& normal) {
   Plane new_s1 = transform(s1, tf1);
   Plane new_s2 = transform(s2, tf2);
 
-  FCL_REAL distance;
-  Vec3f dir = (new_s1.n).cross(new_s2.n);
-  FCL_REAL dir_sq_norm = dir.squaredNorm();
+  CoalScalar distance;
+  Vec3s dir = (new_s1.n).cross(new_s2.n);
+  CoalScalar dir_sq_norm = dir.squaredNorm();
 
-  if (dir_sq_norm < std::numeric_limits<FCL_REAL>::epsilon())  // parallel
+  if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon())  // parallel
   {
     p1 = new_s1.n * new_s1.d;
     p2 = new_s2.n * new_s2.d;
     assert(new_s1.distance(p1) <=
-           Eigen::NumTraits<FCL_REAL>::dummy_precision());
+           Eigen::NumTraits<CoalScalar>::dummy_precision());
     assert(new_s2.distance(p2) <=
-           Eigen::NumTraits<FCL_REAL>::dummy_precision());
+           Eigen::NumTraits<CoalScalar>::dummy_precision());
     distance = (p1 - p2).norm();
 
-    if (distance > Eigen::NumTraits<FCL_REAL>::dummy_precision()) {
+    if (distance > Eigen::NumTraits<CoalScalar>::dummy_precision()) {
       normal = (p2 - p1).normalized();
     } else {
       normal = new_s1.n;
@@ -672,7 +676,7 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1,
     // If the planes are not parallel, they are in collision.
     // Their distance, in the sens of the norm of separation vector, is infinite
     // (it's impossible to find a translation which separates them)
-    distance = -(std::numeric_limits<FCL_REAL>::max)();
+    distance = -(std::numeric_limits<CoalScalar>::max)();
     // p1 and p2 are the same point, corresponding to a point on the
     // intersection line between the two objects. Normal is the direction of
     // that line.
@@ -684,8 +688,8 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1,
   }
 
   // Take swept-sphere radius into account
-  const FCL_REAL ssr1 = s1.getSweptSphereRadius();
-  const FCL_REAL ssr2 = s2.getSweptSphereRadius();
+  const CoalScalar ssr1 = s1.getSweptSphereRadius();
+  const CoalScalar ssr2 = s2.getSweptSphereRadius();
   if (ssr1 > 0 || ssr2 > 0) {
     p1 += ssr1 * normal;
     p2 -= ssr2 * normal;
@@ -696,15 +700,15 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1,
 }
 
 /// See the prototype below
-inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2,
-                                   const Vec3f& P3, const Vec3f& Q1,
-                                   const Vec3f& Q2, const Vec3f& Q3,
-                                   Vec3f& normal) {
-  Vec3f u((P2 - P1).cross(P3 - P1));
+inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2,
+                                     const Vec3s& P3, const Vec3s& Q1,
+                                     const Vec3s& Q2, const Vec3s& Q3,
+                                     Vec3s& normal) {
+  Vec3s u((P2 - P1).cross(P3 - P1));
   normal = u.normalized();
-  FCL_REAL depth1((P1 - Q1).dot(normal));
-  FCL_REAL depth2((P1 - Q2).dot(normal));
-  FCL_REAL depth3((P1 - Q3).dot(normal));
+  CoalScalar depth1((P1 - Q1).dot(normal));
+  CoalScalar depth2((P1 - Q2).dot(normal));
+  CoalScalar depth3((P1 - Q3).dot(normal));
   return std::max(depth1, std::max(depth2, depth3));
 }
 
@@ -715,23 +719,22 @@ inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2,
 //
 // Note that we compute here an upper bound of the penetration distance,
 // not the exact value.
-inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2,
-                                   const Vec3f& P3, const Vec3f& Q1,
-                                   const Vec3f& Q2, const Vec3f& Q3,
-                                   const Transform3f& tf1,
-                                   const Transform3f& tf2, Vec3f& normal) {
-  Vec3f globalP1(tf1.transform(P1));
-  Vec3f globalP2(tf1.transform(P2));
-  Vec3f globalP3(tf1.transform(P3));
-  Vec3f globalQ1(tf2.transform(Q1));
-  Vec3f globalQ2(tf2.transform(Q2));
-  Vec3f globalQ3(tf2.transform(Q3));
+inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2,
+                                     const Vec3s& P3, const Vec3s& Q1,
+                                     const Vec3s& Q2, const Vec3s& Q3,
+                                     const Transform3s& tf1,
+                                     const Transform3s& tf2, Vec3s& normal) {
+  Vec3s globalP1(tf1.transform(P1));
+  Vec3s globalP2(tf1.transform(P2));
+  Vec3s globalP3(tf1.transform(P3));
+  Vec3s globalQ1(tf2.transform(Q1));
+  Vec3s globalQ2(tf2.transform(Q2));
+  Vec3s globalQ3(tf2.transform(Q3));
   return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2,
                             globalQ3, normal);
 }
 
 }  // namespace details
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
-#endif  // HPP_FCL_SRC_NARROWPHASE_DETAILS_H
+#endif  // COAL_SRC_NARROWPHASE_DETAILS_H
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 3b03a9ff80027ed6904f0d5326b887d9b7f78fbd..0e81fce23c2a8f8387cd089e7492c3c0ca520fb5 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -36,31 +36,30 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/narrowphase/gjk.h>
-#include <hpp/fcl/internal/intersect.h>
-#include <hpp/fcl/internal/tools.h>
-#include <hpp/fcl/shape/geometric_shapes_traits.h>
-#include <hpp/fcl/narrowphase/narrowphase_defaults.h>
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/gjk.h"
+#include "coal/internal/intersect.h"
+#include "coal/internal/tools.h"
+#include "coal/shape/geometric_shapes_traits.h"
+#include "coal/narrowphase/narrowphase_defaults.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace details {
 
 void GJK::initialize() {
-  distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
+  distance_upper_bound = (std::numeric_limits<CoalScalar>::max)();
   gjk_variant = GJKVariant::DefaultGJK;
   convergence_criterion = GJKConvergenceCriterion::Default;
   convergence_criterion_type = GJKConvergenceCriterionType::Relative;
   reset(max_iterations, tolerance);
 }
 
-void GJK::reset(size_t max_iterations_, FCL_REAL tolerance_) {
+void GJK::reset(size_t max_iterations_, CoalScalar tolerance_) {
   max_iterations = max_iterations_;
   tolerance = tolerance_;
-  HPP_FCL_ASSERT(tolerance_ > 0, "Tolerance must be positive.",
-                 std::invalid_argument);
+  COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.",
+              std::invalid_argument);
   status = DidNotRun;
   nfree = 0;
   simplex = nullptr;
@@ -68,7 +67,7 @@ void GJK::reset(size_t max_iterations_, FCL_REAL tolerance_) {
   iterations_momentum_stop = 0;
 }
 
-Vec3f GJK::getGuessFromSimplex() const { return ray; }
+Vec3s GJK::getGuessFromSimplex() const { return ray; }
 
 namespace details {
 
@@ -91,7 +90,7 @@ namespace details {
 //   w0 = alpha * w[0].w0 + (1 - alpha) * w[1].w0
 //   w1 = alpha * w[0].w1 + (1 - alpha) * w[1].w1
 // clang-format on
-void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
+void getClosestPoints(const GJK::Simplex& simplex, Vec3s& w0, Vec3s& w1) {
   GJK::SimplexV* const* vs = simplex.vertex;
 
   for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) {
@@ -105,10 +104,10 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
       w1 = vs[0]->w1;
       return;
     case 2: {
-      const Vec3f &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w,
+      const Vec3s &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w,
                   b0 = vs[1]->w0, b1 = vs[1]->w1;
-      FCL_REAL la, lb;
-      Vec3f N(b - a);
+      CoalScalar la, lb;
+      Vec3s N(b - a);
       la = N.dot(-a);
       if (la <= 0) {
         assert(false);
@@ -138,8 +137,8 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
                                                     vs[2]->w, vs[3]->w);
       break;
     default:
-      HPP_FCL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]",
-                           std::logic_error);
+      COAL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]",
+                        std::logic_error);
   }
   w0.setZero();
   w1.setZero();
@@ -153,18 +152,18 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
 /// Inflate the points along a normal.
 /// The normal is typically the normal of the separating plane found by GJK
 /// or the normal found by EPA.
-/// The normal should follow hpp-fcl convention: it points from shape0 to
+/// The normal should follow coal convention: it points from shape0 to
 /// shape1.
 template <bool Separated>
-void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0,
-             Vec3f& w1) {
+void inflate(const MinkowskiDiff& shape, const Vec3s& normal, Vec3s& w0,
+             Vec3s& w1) {
 #ifndef NDEBUG
-  const FCL_REAL dummy_precision =
-      Eigen::NumTraits<FCL_REAL>::dummy_precision();
+  const CoalScalar dummy_precision =
+      Eigen::NumTraits<CoalScalar>::dummy_precision();
   assert((normal.norm() - 1) < dummy_precision);
 #endif
 
-  const Eigen::Array<FCL_REAL, 1, 2>& I(shape.swept_sphere_radius);
+  const Eigen::Array<CoalScalar, 1, 2>& I(shape.swept_sphere_radius);
   Eigen::Array<bool, 1, 2> inflate(I > 0);
   if (!inflate.any()) return;
 
@@ -174,10 +173,10 @@ void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0,
 
 }  // namespace details
 
-void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
-                                    Vec3f& w1, Vec3f& normal) const {
+void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+                                    Vec3s& w1, Vec3s& normal) const {
   details::getClosestPoints(*simplex, w0, w1);
-  if ((w1 - w0).norm() > Eigen::NumTraits<FCL_REAL>::dummy_precision()) {
+  if ((w1 - w0).norm() > Eigen::NumTraits<CoalScalar>::dummy_precision()) {
     normal = (w1 - w0).normalized();
   } else {
     normal = -this->ray.normalized();
@@ -185,12 +184,12 @@ void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
   details::inflate<true>(shape, normal, w0, w1);
 }
 
-GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
+GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3s& guess,
                           const support_func_guess_t& supportHint) {
-  FCL_REAL alpha = 0;
+  CoalScalar alpha = 0;
   iterations = 0;
-  const FCL_REAL swept_sphere_radius = shape_.swept_sphere_radius.sum();
-  const FCL_REAL upper_bound = distance_upper_bound + swept_sphere_radius;
+  const CoalScalar swept_sphere_radius = shape_.swept_sphere_radius.sum();
+  const CoalScalar upper_bound = distance_upper_bound + swept_sphere_radius;
 
   free_v[0] = &store_v[0];
   free_v[1] = &store_v[1];
@@ -205,19 +204,19 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
   simplices[current].rank = 0;
   support_hint = supportHint;
 
-  FCL_REAL rl = guess.norm();
+  CoalScalar rl = guess.norm();
   if (rl < tolerance) {
-    ray = Vec3f(-1, 0, 0);
+    ray = Vec3s(-1, 0, 0);
     rl = 1;
   } else
     ray = guess;
 
   // Momentum
   GJKVariant current_gjk_variant = gjk_variant;
-  Vec3f w = ray;
-  Vec3f dir = ray;
-  Vec3f y;
-  FCL_REAL momentum;
+  Vec3s w = ray;
+  Vec3s dir = ray;
+  Vec3s y;
+  CoalScalar momentum;
   bool normalize_support_direction = shape->normalize_support_direction;
   do {
     vertex_id_t next = (vertex_id_t)(1 - current);
@@ -252,9 +251,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
         // Normalize heuristic for collision pairs involving convex but not
         // strictly-convex shapes This corresponds to most use cases.
         if (normalize_support_direction) {
-          momentum = (FCL_REAL(iterations) + 2) / (FCL_REAL(iterations) + 3);
+          momentum =
+              (CoalScalar(iterations) + 2) / (CoalScalar(iterations) + 3);
           y = momentum * ray + (1 - momentum) * w;
-          FCL_REAL y_norm = y.norm();
+          CoalScalar y_norm = y.norm();
           // ray is the point of the Minkowski difference which currently the
           // closest to the origin. Therefore, y.norm() > ray.norm() Hence, if
           // check A above has not stopped the algorithm, we necessarily have
@@ -262,19 +262,20 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
           assert(y_norm > tolerance);
           dir = momentum * dir / dir.norm() + (1 - momentum) * y / y_norm;
         } else {
-          momentum = (FCL_REAL(iterations) + 1) / (FCL_REAL(iterations) + 3);
+          momentum =
+              (CoalScalar(iterations) + 1) / (CoalScalar(iterations) + 3);
           y = momentum * ray + (1 - momentum) * w;
           dir = momentum * dir + (1 - momentum) * y;
         }
         break;
 
       case PolyakAcceleration:
-        momentum = 1 / (FCL_REAL(iterations) + 1);
+        momentum = 1 / (CoalScalar(iterations) + 1);
         dir = momentum * dir + (1 - momentum) * ray;
         break;
 
       default:
-        HPP_FCL_THROW_PRETTY("Invalid momentum variant.", std::logic_error);
+        COAL_THROW_PRETTY("Invalid momentum variant.", std::logic_error);
     }
 
     // see below, ray points away from origin
@@ -285,7 +286,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
     w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
 
     // check B: no collision if omega > 0
-    FCL_REAL omega = dir.dot(w) / dir.norm();
+    CoalScalar omega = dir.dot(w) / dir.norm();
     if (omega > upper_bound) {
       distance = omega - swept_sphere_radius;
       status = NoCollisionEarlyStopped;
@@ -294,7 +295,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
 
     // Check to remove acceleration
     if (current_gjk_variant != DefaultGJK) {
-      FCL_REAL frank_wolfe_duality_gap = 2 * ray.dot(ray - w);
+      CoalScalar frank_wolfe_duality_gap = 2 * ray.dot(ray - w);
       if (frank_wolfe_duality_gap - tolerance <= 0) {
         removeVertex(simplices[current]);
         current_gjk_variant = DefaultGJK;  // move back to classic GJK
@@ -346,7 +347,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
         inside = projectTetrahedraOrigin(curr_simplex, next_simplex);
         break;
       default:
-        HPP_FCL_THROW_PRETTY("Invalid simplex rank", std::logic_error);
+        COAL_THROW_PRETTY("Invalid simplex rank", std::logic_error);
     }
     assert(nfree + next_simplex.rank == 4);
     current = next;
@@ -369,8 +370,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
   return status;
 }
 
-bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
-                           const FCL_REAL& omega) const {
+bool GJK::checkConvergence(const Vec3s& w, const CoalScalar& rl,
+                           CoalScalar& alpha, const CoalScalar& omega) const {
   // x^* is the optimal solution (projection of origin onto the Minkowski
   // difference).
   //  x^k is the current iterate (x^k = `ray` in the code).
@@ -381,13 +382,13 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
       // alpha is the distance to the best separating hyperplane found so far
       alpha = std::max(alpha, omega);
       // ||x^*|| - ||x^k|| <= diff
-      const FCL_REAL diff = rl - alpha;
+      const CoalScalar diff = rl - alpha;
       return ((diff - (tolerance + tolerance * rl)) <= 0);
     } break;
 
     case DualityGap: {
       // ||x^* - x^k||^2 <= diff
-      const FCL_REAL diff = 2 * ray.dot(ray - w);
+      const CoalScalar diff = 2 * ray.dot(ray - w);
       switch (convergence_criterion_type) {
         case Absolute:
           return ((diff - tolerance) <= 0);
@@ -396,8 +397,8 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
           return (((diff / tolerance * rl) - tolerance * rl) <= 0);
           break;
         default:
-          HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.",
-                               std::logic_error);
+          COAL_THROW_PRETTY("Invalid convergence criterion type.",
+                            std::logic_error);
       }
     } break;
 
@@ -405,7 +406,7 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
       // alpha is the distance to the best separating hyperplane found so far
       alpha = std::max(alpha, omega);
       // ||x^* - x^k||^2 <= diff
-      const FCL_REAL diff = rl * rl - alpha * alpha;
+      const CoalScalar diff = rl * rl - alpha * alpha;
       switch (convergence_criterion_type) {
         case Absolute:
           return ((diff - tolerance) <= 0);
@@ -414,13 +415,13 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
           return (((diff / tolerance * rl) - tolerance * rl) <= 0);
           break;
         default:
-          HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.",
-                               std::logic_error);
+          COAL_THROW_PRETTY("Invalid convergence criterion type.",
+                            std::logic_error);
       }
     } break;
 
     default:
-      HPP_FCL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error);
+      COAL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error);
   }
 }
 
@@ -428,14 +429,14 @@ inline void GJK::removeVertex(Simplex& simplex) {
   free_v[nfree++] = simplex.vertex[--simplex.rank];
 }
 
-inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v,
+inline void GJK::appendVertex(Simplex& simplex, const Vec3s& v,
                               support_func_guess_t& hint) {
   simplex.vertex[simplex.rank] = free_v[--nfree];  // set the memory
   getSupport(v, *simplex.vertex[simplex.rank++], hint);
 }
 
 bool GJK::encloseOrigin() {
-  Vec3f axis(Vec3f::Zero());
+  Vec3s axis(Vec3s::Zero());
   support_func_guess_t hint = support_func_guess_t::Zero();
   switch (simplex->rank) {
     case 1:
@@ -452,10 +453,10 @@ bool GJK::encloseOrigin() {
       }
       break;
     case 2: {
-      Vec3f d = simplex->vertex[1]->w - simplex->vertex[0]->w;
+      Vec3s d = simplex->vertex[1]->w - simplex->vertex[0]->w;
       for (int i = 0; i < 3; ++i) {
         axis[i] = 1;
-        Vec3f p = d.cross(axis);
+        Vec3s p = d.cross(axis);
         if (!p.isZero()) {
           appendVertex(*simplex, p, hint);
           if (encloseOrigin()) return true;
@@ -492,7 +493,7 @@ bool GJK::encloseOrigin() {
 }
 
 inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a,
-                          const Vec3f& A, GJK::Simplex& next, Vec3f& ray) {
+                          const Vec3s& A, GJK::Simplex& next, Vec3s& ray) {
   // A is the closest to the origin
   ray = A;
   next.vertex[0] = current.vertex[a];
@@ -500,9 +501,9 @@ inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a,
 }
 
 inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a,
-                            GJK::vertex_id_t b, const Vec3f& A, const Vec3f& B,
-                            const Vec3f& AB, const FCL_REAL& ABdotAO,
-                            GJK::Simplex& next, Vec3f& ray) {
+                            GJK::vertex_id_t b, const Vec3s& A, const Vec3s& B,
+                            const Vec3s& AB, const CoalScalar& ABdotAO,
+                            GJK::Simplex& next, Vec3s& ray) {
   // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
   ray = AB.dot(B) * A + ABdotAO * B;
 
@@ -516,8 +517,8 @@ inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a,
 
 inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a,
                              GJK::vertex_id_t b, GJK::vertex_id_t c,
-                             const Vec3f& ABC, const FCL_REAL& ABCdotAO,
-                             GJK::Simplex& next, Vec3f& ray) {
+                             const Vec3s& ABC, const CoalScalar& ABCdotAO,
+                             GJK::Simplex& next, Vec3s& ray) {
   next.rank = 3;
   next.vertex[2] = current.vertex[a];
 
@@ -543,11 +544,11 @@ inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a,
 bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) {
   const vertex_id_t a = 1, b = 0;
   // A is the last point we added.
-  const Vec3f& A = current.vertex[a]->w;
-  const Vec3f& B = current.vertex[b]->w;
+  const Vec3s& A = current.vertex[a]->w;
+  const Vec3s& B = current.vertex[b]->w;
 
-  const Vec3f AB = B - A;
-  const FCL_REAL d = AB.dot(-A);
+  const Vec3s AB = B - A;
+  const CoalScalar d = AB.dot(-A);
   assert(d <= AB.squaredNorm());
 
   if (d == 0) {
@@ -571,19 +572,19 @@ bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) {
 bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) {
   const vertex_id_t a = 2, b = 1, c = 0;
   // A is the last point we added.
-  const Vec3f &A = current.vertex[a]->w, B = current.vertex[b]->w,
+  const Vec3s &A = current.vertex[a]->w, B = current.vertex[b]->w,
               C = current.vertex[c]->w;
 
-  const Vec3f AB = B - A, AC = C - A, ABC = AB.cross(AC);
+  const Vec3s AB = B - A, AC = C - A, ABC = AB.cross(AC);
 
-  FCL_REAL edgeAC2o = ABC.cross(AC).dot(-A);
+  CoalScalar edgeAC2o = ABC.cross(AC).dot(-A);
   if (edgeAC2o >= 0) {
-    FCL_REAL towardsC = AC.dot(-A);
+    CoalScalar towardsC = AC.dot(-A);
     if (towardsC >= 0) {  // Region 1
       originToSegment(current, a, c, A, C, AC, towardsC, next, ray);
       free_v[nfree++] = current.vertex[b];
     } else {  // Region 4 or 5
-      FCL_REAL towardsB = AB.dot(-A);
+      CoalScalar towardsB = AB.dot(-A);
       if (towardsB < 0) {  // Region 5
         // A is the closest to the origin
         originToPoint(current, a, A, next, ray);
@@ -593,9 +594,9 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) {
       free_v[nfree++] = current.vertex[c];
     }
   } else {
-    FCL_REAL edgeAB2o = AB.cross(ABC).dot(-A);
+    CoalScalar edgeAB2o = AB.cross(ABC).dot(-A);
     if (edgeAB2o >= 0) {  // Region 4 or 5
-      FCL_REAL towardsB = AB.dot(-A);
+      CoalScalar towardsB = AB.dot(-A);
       if (towardsB < 0) {  // Region 5
         // A is the closest to the origin
         originToPoint(current, a, A, next, ray);
@@ -613,35 +614,35 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) {
 bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
   // The code of this function was generated using doc/gjk.py
   const vertex_id_t a = 3, b = 2, c = 1, d = 0;
-  const Vec3f& A(current.vertex[a]->w);
-  const Vec3f& B(current.vertex[b]->w);
-  const Vec3f& C(current.vertex[c]->w);
-  const Vec3f& D(current.vertex[d]->w);
-  const FCL_REAL aa = A.squaredNorm();
-  const FCL_REAL da = D.dot(A);
-  const FCL_REAL db = D.dot(B);
-  const FCL_REAL dc = D.dot(C);
-  const FCL_REAL dd = D.dot(D);
-  const FCL_REAL da_aa = da - aa;
-  const FCL_REAL ca = C.dot(A);
-  const FCL_REAL cb = C.dot(B);
-  const FCL_REAL cc = C.dot(C);
-  const FCL_REAL& cd = dc;
-  const FCL_REAL ca_aa = ca - aa;
-  const FCL_REAL ba = B.dot(A);
-  const FCL_REAL bb = B.dot(B);
-  const FCL_REAL& bc = cb;
-  const FCL_REAL& bd = db;
-  const FCL_REAL ba_aa = ba - aa;
-  const FCL_REAL ba_ca = ba - ca;
-  const FCL_REAL ca_da = ca - da;
-  const FCL_REAL da_ba = da - ba;
-  const Vec3f a_cross_b = A.cross(B);
-  const Vec3f a_cross_c = A.cross(C);
-
-  const FCL_REAL dummy_precision(
-      3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
-  HPP_FCL_UNUSED_VARIABLE(dummy_precision);
+  const Vec3s& A(current.vertex[a]->w);
+  const Vec3s& B(current.vertex[b]->w);
+  const Vec3s& C(current.vertex[c]->w);
+  const Vec3s& D(current.vertex[d]->w);
+  const CoalScalar aa = A.squaredNorm();
+  const CoalScalar da = D.dot(A);
+  const CoalScalar db = D.dot(B);
+  const CoalScalar dc = D.dot(C);
+  const CoalScalar dd = D.dot(D);
+  const CoalScalar da_aa = da - aa;
+  const CoalScalar ca = C.dot(A);
+  const CoalScalar cb = C.dot(B);
+  const CoalScalar cc = C.dot(C);
+  const CoalScalar& cd = dc;
+  const CoalScalar ca_aa = ca - aa;
+  const CoalScalar ba = B.dot(A);
+  const CoalScalar bb = B.dot(B);
+  const CoalScalar& bc = cb;
+  const CoalScalar& bd = db;
+  const CoalScalar ba_aa = ba - aa;
+  const CoalScalar ba_ca = ba - ca;
+  const CoalScalar ca_da = ca - da;
+  const CoalScalar da_ba = da - ba;
+  const Vec3s a_cross_b = A.cross(B);
+  const Vec3s a_cross_c = A.cross(C);
+
+  const CoalScalar dummy_precision(
+      3 * std::sqrt(std::numeric_limits<CoalScalar>::epsilon()));
+  COAL_UNUSED_VARIABLE(dummy_precision);
 
 #define REGION_INSIDE()               \
   ray.setZero();                      \
@@ -1011,7 +1012,7 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
 
 void EPA::initialize() { reset(max_iterations, tolerance); }
 
-void EPA::reset(size_t max_iterations_, FCL_REAL tolerance_) {
+void EPA::reset(size_t max_iterations_, CoalScalar tolerance_) {
   max_iterations = max_iterations_;
   tolerance = tolerance_;
   // EPA creates only 2 faces and 1 vertex per iteration.
@@ -1037,18 +1038,18 @@ void EPA::reset(size_t max_iterations_, FCL_REAL tolerance_) {
 }
 
 bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a,
-                      const SimplexVertex& b, FCL_REAL& dist) {
-  Vec3f ab = b.w - a.w;
-  Vec3f n_ab = ab.cross(face->n);
-  FCL_REAL a_dot_nab = a.w.dot(n_ab);
+                      const SimplexVertex& b, CoalScalar& dist) {
+  Vec3s ab = b.w - a.w;
+  Vec3s n_ab = ab.cross(face->n);
+  CoalScalar a_dot_nab = a.w.dot(n_ab);
 
   if (a_dot_nab < 0)  // the origin is on the outside part of ab
   {
     // following is similar to projectOrigin for two points
     // however, as we dont need to compute the parameterization, dont need to
     // compute 0 or 1
-    FCL_REAL a_dot_ab = a.w.dot(ab);
-    FCL_REAL b_dot_ab = b.w.dot(ab);
+    CoalScalar a_dot_ab = a.w.dot(ab);
+    CoalScalar b_dot_ab = b.w.dot(ab);
 
     if (a_dot_ab > 0)
       dist = a.w.norm();
@@ -1080,15 +1081,15 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c,
     const SimplexVertex& c = sv_store[id_c];
     face->n = (b.w - a.w).cross(c.w - a.w);
 
-    if (face->n.norm() > Eigen::NumTraits<FCL_REAL>::epsilon()) {
+    if (face->n.norm() > Eigen::NumTraits<CoalScalar>::epsilon()) {
       face->n.normalize();
 
       // If the origin projects outside the face, skip it in the
       // `findClosestFace` method.
       // The origin always projects inside the closest face.
-      FCL_REAL a_dot_nab = a.w.dot((b.w - a.w).cross(face->n));
-      FCL_REAL b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n));
-      FCL_REAL c_dot_nca = c.w.dot((a.w - c.w).cross(face->n));
+      CoalScalar a_dot_nab = a.w.dot((b.w - a.w).cross(face->n));
+      CoalScalar b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n));
+      CoalScalar c_dot_nca = c.w.dot((a.w - c.w).cross(face->n));
       if (a_dot_nab >= -tolerance &&  //
           b_dot_nbc >= -tolerance &&  //
           c_dot_nca >= -tolerance) {
@@ -1097,7 +1098,7 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c,
       } else {
         // We will never check this face, so we don't care about
         // its true distance to the origin.
-        face->d = std::numeric_limits<FCL_REAL>::max();
+        face->d = std::numeric_limits<CoalScalar>::max();
         face->ignore = true;
       }
 
@@ -1140,10 +1141,10 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c,
 /** @brief Find the best polytope face to split */
 EPA::SimplexFace* EPA::findClosestFace() {
   SimplexFace* minf = hull.root;
-  FCL_REAL mind = std::numeric_limits<FCL_REAL>::max();
+  CoalScalar mind = std::numeric_limits<CoalScalar>::max();
   for (SimplexFace* f = minf; f; f = f->next_face) {
     if (f->ignore) continue;
-    FCL_REAL sqd = f->d * f->d;
+    CoalScalar sqd = f->d * f->d;
     if (sqd < mind) {
       minf = f;
       mind = sqd;
@@ -1153,7 +1154,7 @@ EPA::SimplexFace* EPA::findClosestFace() {
   return minf;
 }
 
-EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
+EPA::Status EPA::evaluate(GJK& gjk, const Vec3s& guess) {
   GJK::Simplex& simplex = *gjk.getSimplex();
   support_hint = gjk.support_hint;
 
@@ -1246,8 +1247,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
         const SimplexVertex& vf1 = sv_store[closest_face->vertex_id[0]];
         const SimplexVertex& vf2 = sv_store[closest_face->vertex_id[1]];
         const SimplexVertex& vf3 = sv_store[closest_face->vertex_id[2]];
-        FCL_REAL fdist = closest_face->n.dot(w.w - vf1.w);
-        FCL_REAL wnorm = w.w.norm();
+        CoalScalar fdist = closest_face->n.dot(w.w - vf1.w);
+        CoalScalar wnorm = w.w.norm();
         // TODO(louis): we might want to use tol_abs and tol_rel; this might
         // obfuscate the code for the user though.
         if (fdist <= tolerance + tolerance * wnorm) {
@@ -1304,11 +1305,11 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
   // TODO: define a better normal
   assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance()));
   normal = -guess;
-  FCL_REAL nl = normal.norm();
+  CoalScalar nl = normal.norm();
   if (nl > 0)
     normal /= nl;
   else
-    normal = Vec3f(1, 0, 0);
+    normal = Vec3s(1, 0, 0);
   depth = 0;
   result.rank = 1;
   result.vertex[0] = simplex.vertex[0];
@@ -1407,8 +1408,8 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
   // recursive nature of `expand`, it is safer to go through the first case.
   // This is because `expand` can potentially loop indefinitly if the
   // Minkowski difference is very flat (hence the check above).
-  const FCL_REAL dummy_precision(
-      3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+  const CoalScalar dummy_precision(
+      3 * std::sqrt(std::numeric_limits<CoalScalar>::epsilon()));
   const SimplexVertex& vf = sv_store[f->vertex_id[e]];
   if (f->n.dot(w.w - vf.w) < dummy_precision) {
     // case 1: the support point is "below" `f`.
@@ -1448,10 +1449,10 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
   return false;
 }
 
-void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
-                                    Vec3f& w1, Vec3f& normal) const {
+void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+                                    Vec3s& w1, Vec3s& normal) const {
   details::getClosestPoints(result, w0, w1);
-  if ((w0 - w1).norm() > Eigen::NumTraits<FCL_REAL>::dummy_precision()) {
+  if ((w0 - w1).norm() > Eigen::NumTraits<CoalScalar>::dummy_precision()) {
     if (this->depth >= 0) {
       // The shapes are in collision.
       normal = (w0 - w1).normalized();
@@ -1476,24 +1477,24 @@ void ConvexBase::buildSupportWarmStart() {
   this->support_warm_starts.indices.reserve(
       ConvexBase::num_support_warm_starts);
 
-  Vec3f axiis(0, 0, 0);
+  Vec3s axiis(0, 0, 0);
   details::ShapeSupportData support_data;
   int support_hint = 0;
   for (int i = 0; i < 3; ++i) {
     axiis(i) = 1;
     {
-      Vec3f support;
-      hpp::fcl::details::getShapeSupport<false>(this, axiis, support,
-                                                support_hint, support_data);
+      Vec3s support;
+      coal::details::getShapeSupport<false>(this, axiis, support, support_hint,
+                                            support_data);
       this->support_warm_starts.points.emplace_back(support);
       this->support_warm_starts.indices.emplace_back(support_hint);
     }
 
     axiis(i) = -1;
     {
-      Vec3f support;
-      hpp::fcl::details::getShapeSupport<false>(this, axiis, support,
-                                                support_hint, support_data);
+      Vec3s support;
+      coal::details::getShapeSupport<false>(this, axiis, support, support_hint,
+                                            support_data);
       this->support_warm_starts.points.emplace_back(support);
       this->support_warm_starts.indices.emplace_back(support_hint);
     }
@@ -1501,24 +1502,24 @@ void ConvexBase::buildSupportWarmStart() {
     axiis(i) = 0;
   }
 
-  std::array<Vec3f, 4> eis = {Vec3f(1, 1, 1),    //
-                              Vec3f(-1, 1, 1),   //
-                              Vec3f(-1, -1, 1),  //
-                              Vec3f(1, -1, 1)};
+  std::array<Vec3s, 4> eis = {Vec3s(1, 1, 1),    //
+                              Vec3s(-1, 1, 1),   //
+                              Vec3s(-1, -1, 1),  //
+                              Vec3s(1, -1, 1)};
 
   for (size_t ei_index = 0; ei_index < 4; ++ei_index) {
     {
-      Vec3f support;
-      hpp::fcl::details::getShapeSupport<false>(this, eis[ei_index], support,
-                                                support_hint, support_data);
+      Vec3s support;
+      coal::details::getShapeSupport<false>(this, eis[ei_index], support,
+                                            support_hint, support_data);
       this->support_warm_starts.points.emplace_back(support);
       this->support_warm_starts.indices.emplace_back(support_hint);
     }
 
     {
-      Vec3f support;
-      hpp::fcl::details::getShapeSupport<false>(this, -eis[ei_index], support,
-                                                support_hint, support_data);
+      Vec3s support;
+      coal::details::getShapeSupport<false>(this, -eis[ei_index], support,
+                                            support_hint, support_data);
       this->support_warm_starts.points.emplace_back(support);
       this->support_warm_starts.indices.emplace_back(support_hint);
     }
@@ -1528,11 +1529,9 @@ void ConvexBase::buildSupportWarmStart() {
           ConvexBase::num_support_warm_starts ||
       this->support_warm_starts.indices.size() !=
           ConvexBase::num_support_warm_starts) {
-    HPP_FCL_THROW_PRETTY("Wrong number of support warm starts.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Wrong number of support warm starts.",
+                      std::runtime_error);
   }
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp
index 4db64acd1a73bd2d0bb7b2efa515b80e7c2fb20a..97c90bb57054a602c9af05370a22ec62e0ae0b86 100644
--- a/src/narrowphase/minkowski_difference.cpp
+++ b/src/narrowphase/minkowski_difference.cpp
@@ -36,21 +36,20 @@
 
 /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
 
-#include "hpp/fcl/narrowphase/minkowski_difference.h"
-#include "hpp/fcl/shape/geometric_shapes_traits.h"
+#include "coal/narrowphase/minkowski_difference.h"
+#include "coal/shape/geometric_shapes_traits.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace details {
 
 // ============================================================================
 template <typename Shape0, typename Shape1, bool TransformIsIdentity,
           int _SupportOptions>
-void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1,
-                   const Vec3f& ot1, const Vec3f& dir, Vec3f& support0,
-                   Vec3f& support1, support_func_guess_t& hint,
+void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3s& oR1,
+                   const Vec3s& ot1, const Vec3s& dir, Vec3s& support0,
+                   Vec3s& support1, support_func_guess_t& hint,
                    ShapeSupportData data[2]) {
-  assert(dir.norm() > Eigen::NumTraits<FCL_REAL>::epsilon());
+  assert(dir.norm() > Eigen::NumTraits<CoalScalar>::epsilon());
   getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]);
 
   if (TransformIsIdentity) {
@@ -65,8 +64,8 @@ void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1,
 // ============================================================================
 template <typename Shape0, typename Shape1, bool TransformIsIdentity,
           int _SupportOptions>
-void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir,
-                       Vec3f& support0, Vec3f& support1,
+void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3s& dir,
+                       Vec3s& support0, Vec3s& support1,
                        support_func_guess_t& hint, ShapeSupportData data[2]) {
   getSupportTpl<Shape0, Shape1, TransformIsIdentity, _SupportOptions>(
       static_cast<const Shape0*>(md.shapes[0]),
@@ -78,7 +77,7 @@ void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir,
 template <typename Shape0, int _SupportOptions>
 MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
     const ShapeBase* s1, bool identity,
-    Eigen::Array<FCL_REAL, 1, 2>& swept_sphere_radius,
+    Eigen::Array<CoalScalar, 1, 2>& swept_sphere_radius,
     ShapeSupportData data[2]) {
   if (_SupportOptions == SupportOptions::WithSweptSphere) {
     // No need to store the information of swept sphere radius
@@ -151,7 +150,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
       }
     }
     default:
-      HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
+      COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
   }
 }
 
@@ -159,7 +158,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
 template <int _SupportOptions>
 MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
     const ShapeBase* s0, const ShapeBase* s1, bool identity,
-    Eigen::Array<FCL_REAL, 1, 2>& swept_sphere_radius,
+    Eigen::Array<CoalScalar, 1, 2>& swept_sphere_radius,
     ShapeSupportData data[2]) {
   if (_SupportOptions == SupportOptions::WithSweptSphere) {
     // No need to store the information of swept sphere radius
@@ -221,7 +220,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
       break;
     }
     default:
-      HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
+      COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
   }
 }
 
@@ -253,7 +252,7 @@ bool getNormalizeSupportDirection(const ShapeBase* shape) {
       return (bool)shape_traits<ConvexBase>::NeedNesterovNormalizeHeuristic;
       break;
     default:
-      HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
+      COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
   }
 }
 
@@ -268,7 +267,7 @@ void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0,
 // ============================================================================
 template <int _SupportOptions>
 void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
-                        const Transform3f& tf0, const Transform3f& tf1) {
+                        const Transform3s& tf0, const Transform3s& tf1) {
   shapes[0] = shape0;
   shapes[1] = shape1;
   getNormalizeSupportDirectionFromShapes(shape0, shape1,
@@ -284,9 +283,9 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
       shape0, shape1, identity, swept_sphere_radius, data);
 }
 // clang-format off
-template void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*);
+template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*);
 
-template void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*);
+template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*);
 // clang-format on
 
 // ============================================================================
@@ -304,22 +303,21 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) {
       shape0, shape1, true, swept_sphere_radius, data);
 }
 // clang-format off
-template void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&);
+template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&);
 
-template void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&);
+template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&);
 // clang-format on
 
 // ============================================================================
 // clang-format off
-template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3f&, int&) const;
+template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const;
 
-template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3f&, int&) const;
+template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const;
 
-template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3f&, int&) const;
+template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const;
 
-template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3f&, int&) const;
+template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const;
 // clang-format on
 
 }  // namespace details
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp
index 3b632abb98f5cd8d27697e405b6ef6107d35e038..a900999d39c911fcc4735e3da62d785f25ce9143 100644
--- a/src/narrowphase/support_functions.cpp
+++ b/src/narrowphase/support_functions.cpp
@@ -36,12 +36,11 @@
 
 /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
 
-#include "hpp/fcl/narrowphase/support_functions.h"
+#include "coal/narrowphase/support_functions.h"
 
 #include <algorithm>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace details {
 
 // ============================================================================
@@ -49,8 +48,8 @@ namespace details {
   getShapeSupport<_SupportOptions>(static_cast<const ShapeType*>(shape), dir, \
                                    support, hint, support_data)
 template <int _SupportOptions>
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) {
-  Vec3f support;
+Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) {
+  Vec3s support;
   ShapeSupportData support_data;
   switch (shape->getNodeType()) {
     case GEOM_TRIANGLE:
@@ -90,30 +89,29 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) {
 
 // Explicit instantiation
 // clang-format off
-template HPP_FCL_DLLAPI Vec3f getSupport<SupportOptions::NoSweptSphere>(const ShapeBase*, const Vec3f&, int&);
+template COAL_DLLAPI Vec3s getSupport<SupportOptions::NoSweptSphere>(const ShapeBase*, const Vec3s&, int&);
 
-template HPP_FCL_DLLAPI Vec3f getSupport<SupportOptions::WithSweptSphere>(const ShapeBase*, const Vec3f&, int&);
+template COAL_DLLAPI Vec3s getSupport<SupportOptions::WithSweptSphere>(const ShapeBase*, const Vec3s&, int&);
 // clang-format on
 
 // ============================================================================
-#define getShapeSupportTplInstantiation(ShapeType)                             \
-  template HPP_FCL_DLLAPI void getShapeSupport<SupportOptions::NoSweptSphere>( \
-      const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint,    \
-      ShapeSupportData& support_data);                                         \
-                                                                               \
-  template HPP_FCL_DLLAPI void                                                 \
-  getShapeSupport<SupportOptions::WithSweptSphere>(                            \
-      const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint,    \
+#define getShapeSupportTplInstantiation(ShapeType)                            \
+  template COAL_DLLAPI void getShapeSupport<SupportOptions::NoSweptSphere>(   \
+      const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint,   \
+      ShapeSupportData& support_data);                                        \
+                                                                              \
+  template COAL_DLLAPI void getShapeSupport<SupportOptions::WithSweptSphere>( \
+      const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint,   \
       ShapeSupportData& support_data);
 
 // ============================================================================
 template <int _SupportOptions>
-void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
-                     Vec3f& support, int& /*unused*/,
+void getShapeSupport(const TriangleP* triangle, const Vec3s& dir,
+                     Vec3s& support, int& /*unused*/,
                      ShapeSupportData& /*unused*/) {
-  FCL_REAL dota = dir.dot(triangle->a);
-  FCL_REAL dotb = dir.dot(triangle->b);
-  FCL_REAL dotc = dir.dot(triangle->c);
+  CoalScalar dota = dir.dot(triangle->a);
+  CoalScalar dotb = dir.dot(triangle->b);
+  CoalScalar dotc = dir.dot(triangle->c);
   if (dota > dotb) {
     if (dotc > dota) {
       support = triangle->c;
@@ -132,22 +130,19 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
     support += triangle->getSweptSphereRadius() * dir.normalized();
   }
 }
-// clang-format off
-getShapeSupportTplInstantiation(TriangleP)
-    // clang-format on
-
-    // ============================================================================
-    template <int _SupportOptions>
-    inline void getShapeSupport(const Box* box, const Vec3f& dir,
-                                Vec3f& support, int& /*unused*/,
-                                ShapeSupportData& /*unused*/) {
+getShapeSupportTplInstantiation(TriangleP);
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support,
+                            int& /*unused*/, ShapeSupportData& /*unused*/) {
   // The inflate value is simply to make the specialized functions with box
   // have a preferred side for edge cases.
-  static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.;
-  static const FCL_REAL dummy_precision =
-      Eigen::NumTraits<FCL_REAL>::dummy_precision();
-  Vec3f support1 = (dir.array() > dummy_precision).select(box->halfSide, 0);
-  Vec3f support2 =
+  static const CoalScalar inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.;
+  static const CoalScalar dummy_precision =
+      Eigen::NumTraits<CoalScalar>::dummy_precision();
+  Vec3s support1 = (dir.array() > dummy_precision).select(box->halfSide, 0);
+  Vec3s support2 =
       (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0);
   support.noalias() = support1 + support2;
 
@@ -155,15 +150,13 @@ getShapeSupportTplInstantiation(TriangleP)
     support += box->getSweptSphereRadius() * dir.normalized();
   }
 }
-// clang-format off
-getShapeSupportTplInstantiation(Box)
-    // clang-format on
-
-    // ============================================================================
-    template <int _SupportOptions>
-    inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir,
-                                Vec3f& support, int& /*unused*/,
-                                ShapeSupportData& /*unused*/) {
+getShapeSupportTplInstantiation(Box);
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Sphere* sphere, const Vec3s& dir,
+                            Vec3s& support, int& /*unused*/,
+                            ShapeSupportData& /*unused*/) {
   if (_SupportOptions == SupportOptions::WithSweptSphere) {
     support.noalias() =
         (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized();
@@ -171,25 +164,23 @@ getShapeSupportTplInstantiation(Box)
     support.setZero();
   }
 
-  HPP_FCL_UNUSED_VARIABLE(sphere);
-  HPP_FCL_UNUSED_VARIABLE(dir);
+  COAL_UNUSED_VARIABLE(sphere);
+  COAL_UNUSED_VARIABLE(dir);
 }
-// clang-format off
-getShapeSupportTplInstantiation(Sphere)
-    // clang-format on
+getShapeSupportTplInstantiation(Sphere);
 
-    // ============================================================================
-    template <int _SupportOptions>
-    inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir,
-                                Vec3f& support, int& /*unused*/,
-                                ShapeSupportData& /*unused*/) {
-  FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
-  FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
-  FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir,
+                            Vec3s& support, int& /*unused*/,
+                            ShapeSupportData& /*unused*/) {
+  CoalScalar a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
+  CoalScalar b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
+  CoalScalar c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
 
-  Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
+  Vec3s v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
 
-  FCL_REAL d = std::sqrt(v.dot(dir));
+  CoalScalar d = std::sqrt(v.dot(dir));
 
   support = v / d;
 
@@ -197,17 +188,15 @@ getShapeSupportTplInstantiation(Sphere)
     support += ellipsoid->getSweptSphereRadius() * dir.normalized();
   }
 }
-// clang-format off
-getShapeSupportTplInstantiation(Ellipsoid)
-    // clang-format on
-
-    // ============================================================================
-    template <int _SupportOptions>
-    inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir,
-                                Vec3f& support, int& /*unused*/,
-                                ShapeSupportData& /*unused*/) {
-  static const FCL_REAL dummy_precision =
-      Eigen::NumTraits<FCL_REAL>::dummy_precision();
+getShapeSupportTplInstantiation(Ellipsoid);
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Capsule* capsule, const Vec3s& dir,
+                            Vec3s& support, int& /*unused*/,
+                            ShapeSupportData& /*unused*/) {
+  static const CoalScalar dummy_precision =
+      Eigen::NumTraits<CoalScalar>::dummy_precision();
   support.setZero();
   if (dir[2] > dummy_precision) {
     support[2] = capsule->halfLength;
@@ -220,23 +209,21 @@ getShapeSupportTplInstantiation(Ellipsoid)
         (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized();
   }
 }
-// clang-format off
-getShapeSupportTplInstantiation(Capsule)
-    // clang-format on
+getShapeSupportTplInstantiation(Capsule);
 
-    // ============================================================================
-    template <int _SupportOptions>
-    void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
-                         int& /*unused*/, ShapeSupportData& /*unused*/) {
-  static const FCL_REAL dummy_precision =
-      Eigen::NumTraits<FCL_REAL>::dummy_precision();
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/) {
+  static const CoalScalar dummy_precision =
+      Eigen::NumTraits<CoalScalar>::dummy_precision();
 
   // The cone radius is, for -h < z < h, (h - z) * r / (2*h)
   // The inflate value is simply to make the specialized functions with cone
   // have a preferred side for edge cases.
-  static const FCL_REAL inflate = 1 + 1e-10;
-  FCL_REAL h = cone->halfLength;
-  FCL_REAL r = cone->radius;
+  static const CoalScalar inflate = 1 + 1e-10;
+  CoalScalar h = cone->halfLength;
+  CoalScalar r = cone->radius;
 
   if (dir.head<2>().isZero(dummy_precision)) {
     support.head<2>().setZero();
@@ -246,22 +233,22 @@ getShapeSupportTplInstantiation(Capsule)
       support[2] = -inflate * h;
     }
   } else {
-    FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
-    FCL_REAL len = zdist + dir[2] * dir[2];
+    CoalScalar zdist = dir[0] * dir[0] + dir[1] * dir[1];
+    CoalScalar len = zdist + dir[2] * dir[2];
     zdist = std::sqrt(zdist);
 
     if (dir[2] <= 0) {
-      FCL_REAL rad = r / zdist;
+      CoalScalar rad = r / zdist;
       support.head<2>() = rad * dir.head<2>();
       support[2] = -h;
     } else {
       len = std::sqrt(len);
-      FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h);
+      CoalScalar sin_a = r / std::sqrt(r * r + 4 * h * h);
 
       if (dir[2] > len * sin_a)
         support << 0, 0, h;
       else {
-        FCL_REAL rad = r / zdist;
+        CoalScalar rad = r / zdist;
         support.head<2>() = rad * dir.head<2>();
         support[2] = -h;
       }
@@ -272,23 +259,20 @@ getShapeSupportTplInstantiation(Capsule)
     support += cone->getSweptSphereRadius() * dir.normalized();
   }
 }
-// clang-format off
-getShapeSupportTplInstantiation(Cone)
-    // clang-format on
+getShapeSupportTplInstantiation(Cone);
 
-    // ============================================================================
-    template <int _SupportOptions>
-    void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir,
-                         Vec3f& support, int& /*unused*/,
-                         ShapeSupportData& /*unused*/) {
-  static const FCL_REAL dummy_precision =
-      Eigen::NumTraits<FCL_REAL>::dummy_precision();
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/) {
+  static const CoalScalar dummy_precision =
+      Eigen::NumTraits<CoalScalar>::dummy_precision();
 
   // The inflate value is simply to make the specialized functions with cylinder
   // have a preferred side for edge cases.
-  static const FCL_REAL inflate = 1 + 1e-10;
-  FCL_REAL half_h = cylinder->halfLength;
-  FCL_REAL r = cylinder->radius;
+  static const CoalScalar inflate = 1 + 1e-10;
+  CoalScalar half_h = cylinder->halfLength;
+  CoalScalar r = cylinder->radius;
 
   const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision);
   if (dir_is_aligned_with_z) half_h *= inflate;
@@ -309,35 +293,33 @@ getShapeSupportTplInstantiation(Cone)
   }
 
   assert(fabs(support[0] * dir[1] - support[1] * dir[0]) <
-         sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+         sqrt(std::numeric_limits<CoalScalar>::epsilon()));
 
   if (_SupportOptions == SupportOptions::WithSweptSphere) {
     support += cylinder->getSweptSphereRadius() * dir.normalized();
   }
 }
-// clang-format off
-getShapeSupportTplInstantiation(Cylinder)
-    // clang-format on
+getShapeSupportTplInstantiation(Cylinder);
 
-    // ============================================================================
-    template <int _SupportOptions>
-    void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
-                            Vec3f& support, int& hint,
-                            ShapeSupportData& support_data) {
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportLog(const ConvexBase* convex, const Vec3s& dir,
+                        Vec3s& support, int& hint,
+                        ShapeSupportData& support_data) {
   assert(convex->neighbors != nullptr && "Convex has no neighbors.");
 
   // Use warm start if current support direction is distant from last support
   // direction.
   const double use_warm_start_threshold = 0.9;
-  Vec3f dir_normalized = dir.normalized();
+  Vec3s dir_normalized = dir.normalized();
   if (!support_data.last_dir.isZero() &&
       !convex->support_warm_starts.points.empty() &&
       support_data.last_dir.dot(dir_normalized) < use_warm_start_threshold) {
     // Change hint if last dir is too far from current dir.
-    FCL_REAL maxdot = convex->support_warm_starts.points[0].dot(dir);
+    CoalScalar maxdot = convex->support_warm_starts.points[0].dot(dir);
     hint = convex->support_warm_starts.indices[0];
     for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) {
-      FCL_REAL dot = convex->support_warm_starts.points[i].dot(dir);
+      CoalScalar dot = convex->support_warm_starts.points[i].dot(dir);
       if (dot > maxdot) {
         maxdot = dot;
         hint = convex->support_warm_starts.indices[i];
@@ -346,13 +328,13 @@ getShapeSupportTplInstantiation(Cylinder)
   }
   support_data.last_dir = dir_normalized;
 
-  const std::vector<Vec3f>& pts = *(convex->points);
+  const std::vector<Vec3s>& pts = *(convex->points);
   const std::vector<ConvexBase::Neighbors>& nn = *(convex->neighbors);
 
   if (hint < 0 || hint >= (int)convex->num_points) {
     hint = 0;
   }
-  FCL_REAL maxdot = pts[static_cast<size_t>(hint)].dot(dir);
+  CoalScalar maxdot = pts[static_cast<size_t>(hint)].dot(dir);
   std::vector<int8_t>& visited = support_data.visited;
   if (support_data.visited.size() == convex->num_points) {
     std::fill(visited.begin(), visited.end(), false);
@@ -374,7 +356,7 @@ getShapeSupportTplInstantiation(Cylinder)
       const unsigned int ip = n[in];
       if (visited[ip]) continue;
       visited[ip] = true;
-      const FCL_REAL dot = pts[ip].dot(dir);
+      const CoalScalar dot = pts[ip].dot(dir);
       bool better = false;
       if (dot > maxdot) {
         better = true;
@@ -398,15 +380,15 @@ getShapeSupportTplInstantiation(Cylinder)
 
 // ============================================================================
 template <int _SupportOptions>
-void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir,
-                           Vec3f& support, int& hint,
+void getShapeSupportLinear(const ConvexBase* convex, const Vec3s& dir,
+                           Vec3s& support, int& hint,
                            ShapeSupportData& /*unused*/) {
-  const std::vector<Vec3f>& pts = *(convex->points);
+  const std::vector<Vec3s>& pts = *(convex->points);
 
   hint = 0;
-  FCL_REAL maxdot = pts[0].dot(dir);
+  CoalScalar maxdot = pts[0].dot(dir);
   for (int i = 1; i < (int)convex->num_points; ++i) {
-    FCL_REAL dot = pts[static_cast<size_t>(i)].dot(dir);
+    CoalScalar dot = pts[static_cast<size_t>(i)].dot(dir);
     if (dot > maxdot) {
       maxdot = dot;
       hint = i;
@@ -422,7 +404,7 @@ void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir,
 
 // ============================================================================
 template <int _SupportOptions>
-void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support,
                      int& hint, ShapeSupportData& support_data) {
   // TODO add benchmark to set a proper value for switching between linear and
   // logarithmic.
@@ -435,44 +417,38 @@ void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
                                            support_data);
   }
 }
-// clang-format off
-getShapeSupportTplInstantiation(ConvexBase)
-    // clang-format on
-
-    // ============================================================================
-    template <int _SupportOptions>
-    inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir,
-                                Vec3f& support, int& hint,
-                                ShapeSupportData& support_data) {
+getShapeSupportTplInstantiation(ConvexBase);
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const SmallConvex* convex, const Vec3s& dir,
+                            Vec3s& support, int& hint,
+                            ShapeSupportData& support_data) {
   getShapeSupportLinear<_SupportOptions>(
       reinterpret_cast<const ConvexBase*>(convex), dir, support, hint,
       support_data);
 }
-// clang-format off
-getShapeSupportTplInstantiation(SmallConvex)
-    // clang-format on
-
-    // ============================================================================
-    template <int _SupportOptions>
-    inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
-                                Vec3f& support, int& hint,
-                                ShapeSupportData& support_data) {
+getShapeSupportTplInstantiation(SmallConvex);
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const LargeConvex* convex, const Vec3s& dir,
+                            Vec3s& support, int& hint,
+                            ShapeSupportData& support_data) {
   getShapeSupportLog<_SupportOptions>(
       reinterpret_cast<const ConvexBase*>(convex), dir, support, hint,
       support_data);
 }
-// clang-format off
-getShapeSupportTplInstantiation(LargeConvex)
-// clang-format on
+getShapeSupportTplInstantiation(LargeConvex);
 
 // ============================================================================
 #define CALL_GET_SHAPE_SUPPORT_SET(ShapeType)                               \
   getShapeSupportSet<_SupportOptions>(static_cast<const ShapeType*>(shape), \
                                       support_set, hint, support_data,      \
                                       max_num_supports, tol)
-    template <int _SupportOptions>
-    void getSupportSet(const ShapeBase* shape, SupportSet& support_set,
-                       int& hint, size_t max_num_supports, FCL_REAL tol) {
+template <int _SupportOptions>
+void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint,
+                   size_t max_num_supports, CoalScalar tol) {
   ShapeSupportData support_data;
   switch (shape->getNodeType()) {
     case GEOM_TRIANGLE:
@@ -508,39 +484,38 @@ getShapeSupportTplInstantiation(LargeConvex)
 
 // Explicit instantiation
 // clang-format off
-template HPP_FCL_DLLAPI void getSupportSet<SupportOptions::NoSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL);
+template COAL_DLLAPI void getSupportSet<SupportOptions::NoSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, CoalScalar);
 
-template HPP_FCL_DLLAPI void getSupportSet<SupportOptions::WithSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL);
+template COAL_DLLAPI void getSupportSet<SupportOptions::WithSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, CoalScalar);
 // clang-format on
 
 // ============================================================================
-#define getShapeSupportSetTplInstantiation(ShapeType)                     \
-  template HPP_FCL_DLLAPI void                                            \
-  getShapeSupportSet<SupportOptions::NoSweptSphere>(                      \
-      const ShapeType* shape_, SupportSet& support_set, int& hint,        \
-      ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); \
-                                                                          \
-  template HPP_FCL_DLLAPI void                                            \
-  getShapeSupportSet<SupportOptions::WithSweptSphere>(                    \
-      const ShapeType* shape_, SupportSet& support_set, int& hint,        \
-      ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol);
+#define getShapeSupportSetTplInstantiation(ShapeType)                          \
+  template COAL_DLLAPI void getShapeSupportSet<SupportOptions::NoSweptSphere>( \
+      const ShapeType* shape_, SupportSet& support_set, int& hint,             \
+      ShapeSupportData& data, size_t num_sampled_supports, CoalScalar tol);    \
+                                                                               \
+  template COAL_DLLAPI void                                                    \
+  getShapeSupportSet<SupportOptions::WithSweptSphere>(                         \
+      const ShapeType* shape_, SupportSet& support_set, int& hint,             \
+      ShapeSupportData& data, size_t num_sampled_supports, CoalScalar tol);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set,
                         int& hint /*unused*/,
                         ShapeSupportData& support_data /*unused*/,
-                        size_t /*unused*/, FCL_REAL tol) {
+                        size_t /*unused*/, CoalScalar tol) {
   assert(tol > 0);
   support_set.clear();
 
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   // We simply want to compute the support value, no need to take the
   // swept-sphere radius into account.
   getShapeSupport<SupportOptions::NoSweptSphere>(triangle, support_dir, support,
                                                  hint, support_data);
-  const FCL_REAL support_value = support.dot(support_dir);
+  const CoalScalar support_value = support.dot(support_dir);
 
   if (support_value - support_dir.dot(triangle->a) < tol) {
     // Note: at the moment, it's useless to take into account the
@@ -570,115 +545,108 @@ void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set,
     }
   }
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(TriangleP)
+getShapeSupportSetTplInstantiation(TriangleP);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const Box* box, SupportSet& support_set,
                         int& hint /*unused*/, ShapeSupportData& support_data,
-                        size_t /*unused*/, FCL_REAL tol) {
-  // clang-format on
+                        size_t /*unused*/, CoalScalar tol) {
   assert(tol > 0);
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   getShapeSupport<SupportOptions::NoSweptSphere>(box, support_dir, support,
                                                  hint, support_data);
-  const FCL_REAL support_value = support.dot(support_dir);
-
-  const FCL_REAL x = box->halfSide[0];
-  const FCL_REAL y = box->halfSide[1];
-  const FCL_REAL z = box->halfSide[2];
-  const std::array<Vec3f, 8> corners = {
-      Vec3f(x, y, z),  Vec3f(-x, y, z),  Vec3f(-x, -y, z),  Vec3f(x, -y, z),
-      Vec3f(x, y, -z), Vec3f(-x, y, -z), Vec3f(-x, -y, -z), Vec3f(x, -y, -z),
+  const CoalScalar support_value = support.dot(support_dir);
+
+  const CoalScalar x = box->halfSide[0];
+  const CoalScalar y = box->halfSide[1];
+  const CoalScalar z = box->halfSide[2];
+  const std::array<Vec3s, 8> corners = {
+      Vec3s(x, y, z),  Vec3s(-x, y, z),  Vec3s(-x, -y, z),  Vec3s(x, -y, z),
+      Vec3s(x, y, -z), Vec3s(-x, y, -z), Vec3s(-x, -y, -z), Vec3s(x, -y, -z),
   };
 
   SupportSet::Polygon& polygon = support_data.polygon;
   polygon.clear();
-  const Transform3f& tf = support_set.tf;
-  for (const Vec3f& corner : corners) {
-    const FCL_REAL val = corner.dot(support_dir);
+  const Transform3s& tf = support_set.tf;
+  for (const Vec3s& corner : corners) {
+    const CoalScalar val = corner.dot(support_dir);
     if (support_value - val < tol) {
       if (_SupportOptions == SupportOptions::WithSweptSphere) {
-        const Vec2f p =
+        const Vec2s p =
             tf.inverseTransform(corner +
                                 box->getSweptSphereRadius() * support_dir)
                 .template head<2>();
         polygon.emplace_back(p);
       } else {
-        const Vec2f p = tf.inverseTransform(corner).template head<2>();
+        const Vec2s p = tf.inverseTransform(corner).template head<2>();
         polygon.emplace_back(p);
       }
     }
   }
   computeSupportSetConvexHull(polygon, support_set.points());
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(Box)
+getShapeSupportSetTplInstantiation(Box);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set,
                         int& hint /*unused*/,
                         ShapeSupportData& support_data /*unused*/,
-                        size_t /*unused*/, FCL_REAL /*unused*/) {
-  // clang-format on
+                        size_t /*unused*/, CoalScalar /*unused*/) {
   support_set.points().clear();
 
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   getShapeSupport<_SupportOptions>(sphere, support_dir, support, hint,
                                    support_data);
   support_set.addPoint(support);
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(Sphere)
+getShapeSupportSetTplInstantiation(Sphere);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set,
                         int& hint, ShapeSupportData& support_data /*unused*/,
-                        size_t /*unused*/, FCL_REAL /*unused*/) {
-  // clang-format on
+                        size_t /*unused*/, CoalScalar /*unused*/) {
   support_set.points().clear();
 
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   getShapeSupport<_SupportOptions>(ellipsoid, support_dir, support, hint,
                                    support_data);
   support_set.addPoint(support);
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(Ellipsoid)
+getShapeSupportSetTplInstantiation(Ellipsoid);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
                         int& hint /*unused*/,
                         ShapeSupportData& support_data /*unused*/,
-                        size_t /*unused*/, FCL_REAL tol) {
+                        size_t /*unused*/, CoalScalar tol) {
   // clang-format on
   assert(tol > 0);
   support_set.points().clear();
 
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   getShapeSupport<SupportOptions::NoSweptSphere>(capsule, support_dir, support,
                                                  hint, support_data);
-  const FCL_REAL support_value =
+  const CoalScalar support_value =
       support_dir.dot(support + capsule->radius * support_dir);
   // The support set of a capsule has either 2 points or 1 point.
   // The two candidate points lie at the frontier between the cylinder and
   // sphere parts of the capsule.
-  const FCL_REAL h = capsule->halfLength;
-  const FCL_REAL r = capsule->radius;
-  const Vec3f p1(r * support_dir[0], r * support_dir[1], h);
-  const Vec3f p2(r * support_dir[0], r * support_dir[1], -h);
+  const CoalScalar h = capsule->halfLength;
+  const CoalScalar r = capsule->radius;
+  const Vec3s p1(r * support_dir[0], r * support_dir[1], h);
+  const Vec3s p2(r * support_dir[0], r * support_dir[1], -h);
   if ((support_value - support_dir.dot(p1) <= tol) &&
       (support_value - support_dir.dot(p2) <= tol)) {
     if (_SupportOptions == SupportOptions::WithSweptSphere) {
-      const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius();
+      const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius();
       support_set.addPoint(p1 + ssr_vec);
       support_set.addPoint(p2 + ssr_vec);
     } else {
@@ -687,45 +655,43 @@ void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
     }
   } else {
     if (_SupportOptions == SupportOptions::WithSweptSphere) {
-      const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius();
+      const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius();
       support_set.addPoint(support + ssr_vec);
     } else {
       support_set.addPoint(support);
     }
   }
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(Capsule)
+getShapeSupportSetTplInstantiation(Capsule);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
                         int& hint /*unused*/,
                         ShapeSupportData& support_data /*unused*/,
-                        size_t num_sampled_supports, FCL_REAL tol) {
-  // clang-format on
+                        size_t num_sampled_supports, CoalScalar tol) {
   assert(tol > 0);
   support_set.points().clear();
 
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   getShapeSupport<SupportOptions::NoSweptSphere>(cone, support_dir, support,
                                                  hint, support_data);
-  const FCL_REAL support_value = support.dot(support_dir);
+  const CoalScalar support_value = support.dot(support_dir);
 
   // If the support direction is perpendicular to the cone's basis, there is an
   // infinite amount of support points; otherwise there are up to two support
   // points (two if direction is perpendicular to the side of the cone and one
   // otherwise).
   //
-  // To check this condition, we look at two points on the cone's basis; these
-  // two points are symmetrical w.r.t the center of the circle. If both these
-  // points are tol away from the support plane, then all the points of the
-  // circle are tol away from the support plane.
-  const FCL_REAL r = cone->radius;
-  const FCL_REAL z = -cone->halfLength;
-  const Vec3f p1(r * support_dir[0], r * support_dir[1], z);
-  const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z);
+  // To check this condition, we look at two points on the cone's basis;
+  // these two points are symmetrical w.r.t the center of the circle. If
+  // both these points are tol away from the support plane, then all the
+  // points of the circle are tol away from the support plane.
+  const CoalScalar r = cone->radius;
+  const CoalScalar z = -cone->halfLength;
+  const Vec3s p1(r * support_dir[0], r * support_dir[1], z);
+  const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z);
 
   if ((support_value - support_dir.dot(p1) <= tol) &&
       (support_value - support_dir.dot(p2) <= tol)) {
@@ -733,11 +699,11 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
     // the basis of the cone. We sample `num_sampled_supports` points on the
     // base of the cone. We are guaranteed that these points like at a distance
     // tol of the support plane.
-    const FCL_REAL angle_increment =
-        2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports));
+    const CoalScalar angle_increment =
+        2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(num_sampled_supports));
     for (size_t i = 0; i < num_sampled_supports; ++i) {
-      const FCL_REAL theta = (FCL_REAL)(i)*angle_increment;
-      Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
+      const CoalScalar theta = (CoalScalar)(i)*angle_increment;
+      Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
       assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol);
       if (_SupportOptions == SupportOptions::WithSweptSphere) {
         point_on_cone_base += cone->getSweptSphereRadius() * support_dir;
@@ -748,7 +714,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
     // There are two potential supports to add: the tip of the cone and a point
     // on the basis of the cone. We compare each of these points to the support
     // value.
-    Vec3f cone_tip(0, 0, cone->halfLength);
+    Vec3s cone_tip(0, 0, cone->halfLength);
     if (support_value - support_dir.dot(cone_tip) <= tol) {
       if (_SupportOptions == SupportOptions::WithSweptSphere) {
         cone_tip += cone->getSweptSphereRadius() * support_dir;
@@ -756,7 +722,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
       support_set.addPoint(cone_tip);
     }
 
-    Vec3f point_on_cone_base = Vec3f(cone->radius * support_dir[0],  //
+    Vec3s point_on_cone_base = Vec3s(cone->radius * support_dir[0],  //
                                      cone->radius * support_dir[1],  //
                                      z);
     if (support_value - support_dir.dot(point_on_cone_base) <= tol) {
@@ -767,40 +733,38 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
     }
   }
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(Cone)
+getShapeSupportSetTplInstantiation(Cone);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
                         int& hint /*unused*/,
                         ShapeSupportData& support_data /*unused*/,
-                        size_t num_sampled_supports, FCL_REAL tol) {
-  // clang-format on
+                        size_t num_sampled_supports, CoalScalar tol) {
   assert(tol > 0);
   support_set.points().clear();
 
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   getShapeSupport<SupportOptions::NoSweptSphere>(cylinder, support_dir, support,
                                                  hint, support_data);
-  const FCL_REAL support_value = support.dot(support_dir);
+  const CoalScalar support_value = support.dot(support_dir);
 
   // The following is very similar to what is done for Cone's support set
   // computation.
-  const FCL_REAL r = cylinder->radius;
-  const FCL_REAL z =
+  const CoalScalar r = cylinder->radius;
+  const CoalScalar z =
       support_dir[2] <= 0 ? -cylinder->halfLength : cylinder->halfLength;
-  const Vec3f p1(r * support_dir[0], r * support_dir[1], z);
-  const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z);
+  const Vec3s p1(r * support_dir[0], r * support_dir[1], z);
+  const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z);
 
   if ((support_value - support_dir.dot(p1) <= tol) &&
       (support_value - support_dir.dot(p2) <= tol)) {
-    const FCL_REAL angle_increment =
-        2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports));
+    const CoalScalar angle_increment =
+        2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(num_sampled_supports));
     for (size_t i = 0; i < num_sampled_supports; ++i) {
-      const FCL_REAL theta = (FCL_REAL)(i)*angle_increment;
-      Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
+      const CoalScalar theta = (CoalScalar)(i)*angle_increment;
+      Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
       assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol);
       if (_SupportOptions == SupportOptions::WithSweptSphere) {
         point_on_cone_base += cylinder->getSweptSphereRadius() * support_dir;
@@ -810,7 +774,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
   } else {
     // There are two potential supports to add: one on each circle bases of the
     // cylinder.
-    Vec3f point_on_lower_circle = Vec3f(cylinder->radius * support_dir[0],  //
+    Vec3s point_on_lower_circle = Vec3s(cylinder->radius * support_dir[0],  //
                                         cylinder->radius * support_dir[1],  //
                                         -cylinder->halfLength);
     if (support_value - support_dir.dot(point_on_lower_circle) <= tol) {
@@ -820,7 +784,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
       support_set.addPoint(point_on_lower_circle);
     }
 
-    Vec3f point_on_upper_circle = Vec3f(cylinder->radius * support_dir[0],  //
+    Vec3s point_on_upper_circle = Vec3s(cylinder->radius * support_dir[0],  //
                                         cylinder->radius * support_dir[1],  //
                                         cylinder->halfLength);
     if (support_value - support_dir.dot(point_on_upper_circle) <= tol) {
@@ -831,38 +795,36 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
     }
   }
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(Cylinder)
+getShapeSupportSetTplInstantiation(Cylinder);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set,
                               int& hint /*unused*/,
                               ShapeSupportData& support_data, size_t /*unused*/,
-                              FCL_REAL tol) {
-  // clang-format on
+                              CoalScalar tol) {
   assert(tol > 0);
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   getShapeSupport<SupportOptions::NoSweptSphere>(convex, support_dir, support,
                                                  hint, support_data);
-  const FCL_REAL support_value = support_dir.dot(support);
+  const CoalScalar support_value = support_dir.dot(support);
 
-  const std::vector<Vec3f>& points = *(convex->points);
+  const std::vector<Vec3s>& points = *(convex->points);
   SupportSet::Polygon& polygon = support_data.polygon;
   polygon.clear();
-  const Transform3f& tf = support_set.tf;
-  for (const Vec3f& point : points) {
-    const FCL_REAL dot = support_dir.dot(point);
+  const Transform3s& tf = support_set.tf;
+  for (const Vec3s& point : points) {
+    const CoalScalar dot = support_dir.dot(point);
     if (support_value - dot <= tol) {
       if (_SupportOptions == SupportOptions::WithSweptSphere) {
-        const Vec2f p =
+        const Vec2s p =
             tf.inverseTransform(point +
                                 convex->getSweptSphereRadius() * support_dir)
                 .template head<2>();
         polygon.emplace_back(p);
       } else {
-        const Vec2f p = tf.inverseTransform(point).template head<2>();
+        const Vec2s p = tf.inverseTransform(point).template head<2>();
         polygon.emplace_back(p);
       }
     }
@@ -874,30 +836,30 @@ void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set,
 // ============================================================================
 template <int _SupportOptions>
 void convexSupportSetRecurse(
-    const std::vector<Vec3f>& points,
+    const std::vector<Vec3s>& points,
     const std::vector<ConvexBase::Neighbors>& neighbors,
-    const FCL_REAL swept_sphere_radius, const size_t vertex_idx,
-    const Vec3f& support_dir, const FCL_REAL support_value,
-    const Transform3f& tf, std::vector<int8_t>& visited,
-    SupportSet::Polygon& polygon, FCL_REAL tol) {
-  HPP_FCL_UNUSED_VARIABLE(swept_sphere_radius);
+    const CoalScalar swept_sphere_radius, const size_t vertex_idx,
+    const Vec3s& support_dir, const CoalScalar support_value,
+    const Transform3s& tf, std::vector<int8_t>& visited,
+    SupportSet::Polygon& polygon, CoalScalar tol) {
+  COAL_UNUSED_VARIABLE(swept_sphere_radius);
 
   if (visited[vertex_idx]) {
     return;
   }
 
   visited[vertex_idx] = true;
-  const Vec3f& point = points[vertex_idx];
-  const FCL_REAL val = point.dot(support_dir);
+  const Vec3s& point = points[vertex_idx];
+  const CoalScalar val = point.dot(support_dir);
   if (support_value - val <= tol) {
     if (_SupportOptions == SupportOptions::WithSweptSphere) {
-      const Vec2f p =
+      const Vec2s p =
           tf.inverseTransform(point + swept_sphere_radius * support_dir)
               .template head<2>();
       polygon.emplace_back(p);
 
     } else {
-      const Vec2f p = tf.inverseTransform(point).template head<2>();
+      const Vec2s p = tf.inverseTransform(point).template head<2>();
       polygon.emplace_back(p);
     }
 
@@ -915,17 +877,17 @@ void convexSupportSetRecurse(
 template <int _SupportOptions>
 void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set,
                            int& hint, ShapeSupportData& support_data,
-                           size_t /*unused*/, FCL_REAL tol) {
+                           size_t /*unused*/, CoalScalar tol) {
   assert(tol > 0);
-  Vec3f support;
-  const Vec3f& support_dir = support_set.getNormal();
+  Vec3s support;
+  const Vec3s& support_dir = support_set.getNormal();
   getShapeSupportLog<SupportOptions::NoSweptSphere>(
       convex, support_dir, support, hint, support_data);
-  const FCL_REAL support_value = support.dot(support_dir);
+  const CoalScalar support_value = support.dot(support_dir);
 
-  const std::vector<Vec3f>& points = *(convex->points);
+  const std::vector<Vec3s>& points = *(convex->points);
   const std::vector<ConvexBase::Neighbors>& neighbors = *(convex->neighbors);
-  const FCL_REAL swept_sphere_radius = convex->getSweptSphereRadius();
+  const CoalScalar swept_sphere_radius = convex->getSweptSphereRadius();
   std::vector<int8_t>& visited = support_data.visited;
   // `visited` is guaranteed to be of right size due to previous call to convex
   // log support function.
@@ -933,7 +895,7 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set,
 
   SupportSet::Polygon& polygon = support_data.polygon;
   polygon.clear();
-  const Transform3f& tf = support_set.tf;
+  const Transform3s& tf = support_set.tf;
 
   const size_t vertex_idx = (size_t)(hint);
   convexSupportSetRecurse<_SupportOptions>(
@@ -947,7 +909,8 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set,
 template <int _SupportOptions>
 void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set,
                         int& hint, ShapeSupportData& support_data,
-                        size_t num_sampled_supports /*unused*/, FCL_REAL tol) {
+                        size_t num_sampled_supports /*unused*/,
+                        CoalScalar tol) {
   if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold &&
       convex->neighbors != nullptr) {
     getShapeSupportSetLog<_SupportOptions>(
@@ -957,46 +920,42 @@ void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set,
         convex, support_set, hint, support_data, num_sampled_supports, tol);
   }
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(ConvexBase)
+getShapeSupportSetTplInstantiation(ConvexBase);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set,
                         int& hint /*unused*/,
                         ShapeSupportData& support_data /*unused*/,
-                        size_t num_sampled_supports /*unused*/, FCL_REAL tol) {
-  // clang-format on
+                        size_t num_sampled_supports /*unused*/,
+                        CoalScalar tol) {
   getShapeSupportSetLinear<_SupportOptions>(
       reinterpret_cast<const ConvexBase*>(convex), support_set, hint,
       support_data, num_sampled_supports, tol);
 }
-// clang-format off
-getShapeSupportSetTplInstantiation(SmallConvex)
+getShapeSupportSetTplInstantiation(SmallConvex);
 
 // ============================================================================
 template <int _SupportOptions>
 void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set,
                         int& hint, ShapeSupportData& support_data,
-                        size_t num_sampled_supports /*unused*/, FCL_REAL tol) {
-  // clang-format on
+                        size_t num_sampled_supports /*unused*/,
+                        CoalScalar tol) {
   getShapeSupportSetLog<_SupportOptions>(
       reinterpret_cast<const ConvexBase*>(convex), support_set, hint,
       support_data, num_sampled_supports, tol);
 }
-//clang-format off
-getShapeSupportSetTplInstantiation(LargeConvex)
-    // clang-format on
-
-    // ============================================================================
-    HPP_FCL_DLLAPI
-    void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
-                                     SupportSet::Polygon& cvx_hull) {
+getShapeSupportSetTplInstantiation(LargeConvex);
+
+// ============================================================================
+COAL_DLLAPI
+void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
+                                 SupportSet::Polygon& cvx_hull) {
   cvx_hull.clear();
 
   if (cloud.size() <= 2) {
     // Point or segment, nothing to do.
-    for (const Vec2f& point : cloud) {
+    for (const Vec2s& point : cloud) {
       cvx_hull.emplace_back(point);
     }
     return;
@@ -1013,16 +972,16 @@ getShapeSupportSetTplInstantiation(LargeConvex)
     if (cloud[0](1) > cloud[2](1)) {
       std::swap(cloud[0], cloud[2]);
     }
-    const Vec2f& a = cloud[0];
-    const Vec2f& b = cloud[1];
-    const Vec2f& c = cloud[2];
-    const FCL_REAL det =
+    const Vec2s& a = cloud[0];
+    const Vec2s& b = cloud[1];
+    const Vec2s& c = cloud[2];
+    const CoalScalar det =
         (b(0) - a(0)) * (c(1) - a(1)) - (b(1) - a(1)) * (c(0) - a(0));
     if (det < 0) {
       std::swap(cloud[1], cloud[2]);
     }
 
-    for (const Vec2f& point : cloud) {
+    for (const Vec2s& point : cloud) {
       cvx_hull.emplace_back(point);
     }
     return;
@@ -1036,9 +995,9 @@ getShapeSupportSetTplInstantiation(LargeConvex)
   // in the direction (0, -1) (take the element of the set which has the lowest
   // y coordinate).
   size_t support_idx = 0;
-  FCL_REAL support_val = cloud[0](1);
+  CoalScalar support_val = cloud[0](1);
   for (size_t i = 1; i < cloud.size(); ++i) {
-    const FCL_REAL val = cloud[i](1);
+    const CoalScalar val = cloud[i](1);
     if (val < support_val) {
       support_val = val;
       support_idx = i;
@@ -1047,17 +1006,17 @@ getShapeSupportSetTplInstantiation(LargeConvex)
   std::swap(cloud[0], cloud[support_idx]);
   cvx_hull.clear();
   cvx_hull.emplace_back(cloud[0]);
-  const Vec2f& v = cvx_hull[0];
+  const Vec2s& v = cvx_hull[0];
 
   // Step 2 - Sort the rest of the point cloud according to the angle made with
   // v. Note: we use stable_sort instead of sort because sort can fail if two
   // values are identical.
   std::stable_sort(
-      cloud.begin() + 1, cloud.end(), [&v](const Vec2f& p1, const Vec2f& p2) {
+      cloud.begin() + 1, cloud.end(), [&v](const Vec2s& p1, const Vec2s& p2) {
         // p1 is "smaller" than p2 if det(p1 - v, p2 - v) >= 0
-        const FCL_REAL det =
+        const CoalScalar det =
             (p1(0) - v(0)) * (p2(1) - v(1)) - (p1(1) - v(1)) * (p2(0) - v(0));
-        if (std::abs(det) <= Eigen::NumTraits<FCL_REAL>::dummy_precision()) {
+        if (std::abs(det) <= Eigen::NumTraits<CoalScalar>::dummy_precision()) {
           // If two points are identical or (v, p1, p2) are colinear, p1 is
           // "smaller" if it is closer to v.
           return ((p1 - v).squaredNorm() <= (p2 - v).squaredNorm());
@@ -1069,14 +1028,14 @@ getShapeSupportSetTplInstantiation(LargeConvex)
   // to the cvx-hull if they successively form "left turns" only. A left turn
   // is: considering the last three points of the cvx-hull, if they form a
   // right-hand basis (determinant > 0) then they make a left turn.
-  auto isRightSided = [](const Vec2f& p1, const Vec2f& p2, const Vec2f& p3) {
+  auto isRightSided = [](const Vec2s& p1, const Vec2s& p2, const Vec2s& p3) {
     // Checks if (p2 - p1, p3 - p1) forms a right-sided base based on
     // det(p2 - p1, p3 - p1)
-    const FCL_REAL det =
+    const CoalScalar det =
         (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(1)) * (p3(0) - p1(0));
     // Note: we set a dummy precision threshold so that identical points or
     // colinear pionts are not added to the cvx-hull.
-    return det > Eigen::NumTraits<FCL_REAL>::dummy_precision();
+    return det > Eigen::NumTraits<CoalScalar>::dummy_precision();
   };
 
   // We initialize the cvx-hull algo by adding the first three
@@ -1085,9 +1044,9 @@ getShapeSupportSetTplInstantiation(LargeConvex)
   // to form a right sided basis, hence to form a left turn.
   size_t cloud_beginning_idx = 1;
   while (cvx_hull.size() < 3) {
-    const Vec2f& vec = cloud[cloud_beginning_idx];
+    const Vec2s& vec = cloud[cloud_beginning_idx];
     if ((cvx_hull.back() - vec).squaredNorm() >
-        Eigen::NumTraits<FCL_REAL>::epsilon()) {
+        Eigen::NumTraits<CoalScalar>::epsilon()) {
       cvx_hull.emplace_back(vec);
     }
     ++cloud_beginning_idx;
@@ -1098,7 +1057,7 @@ getShapeSupportSetTplInstantiation(LargeConvex)
   // When we do a turn in the correct direction, we add a point to the
   // convex-hull.
   for (size_t i = cloud_beginning_idx; i < cloud.size(); ++i) {
-    const Vec2f& vec = cloud[i];
+    const Vec2s& vec = cloud[i];
     while (cvx_hull.size() > 1 &&
            !isRightSided(cvx_hull[cvx_hull.size() - 2],
                          cvx_hull[cvx_hull.size() - 1], vec)) {
@@ -1109,5 +1068,4 @@ getShapeSupportSetTplInstantiation(LargeConvex)
 }
 
 }  // namespace details
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/octree.cpp b/src/octree.cpp
index 7e01605965be4e3df605a279c89df4c4f124f1f2..186ad51a963298d20b7aa9d2762a5cabf38022da 100644
--- a/src/octree.cpp
+++ b/src/octree.cpp
@@ -32,11 +32,11 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <hpp/fcl/octree.h>
+#include "coal/octree.h"
+
 #include <array>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 namespace internal {
 struct Neighbors {
   char value;
@@ -55,29 +55,29 @@ struct Neighbors {
   void hasNeighboordPlusZ() { value |= 0x20; }
 };  // struct neighbors
 
-void computeNeighbors(const std::vector<Vec6f>& boxes,
+void computeNeighbors(const std::vector<Vec6s>& boxes,
                       std::vector<Neighbors>& neighbors) {
-  typedef std::vector<Vec6f> VectorVec6f;
-  FCL_REAL fixedSize = -1;
-  FCL_REAL e(1e-8);
+  typedef std::vector<Vec6s> VectorVec6s;
+  CoalScalar fixedSize = -1;
+  CoalScalar e(1e-8);
   for (std::size_t i = 0; i < boxes.size(); ++i) {
-    const Vec6f& box(boxes[i]);
+    const Vec6s& box(boxes[i]);
     Neighbors& n(neighbors[i]);
-    FCL_REAL x(box[0]);
-    FCL_REAL y(box[1]);
-    FCL_REAL z(box[2]);
-    FCL_REAL s(box[3]);
+    CoalScalar x(box[0]);
+    CoalScalar y(box[1]);
+    CoalScalar z(box[2]);
+    CoalScalar s(box[3]);
     if (fixedSize == -1)
       fixedSize = s;
     else
       assert(s == fixedSize);
 
-    for (VectorVec6f::const_iterator it = boxes.begin(); it != boxes.end();
+    for (VectorVec6s::const_iterator it = boxes.begin(); it != boxes.end();
          ++it) {
-      const Vec6f& otherBox = *it;
-      FCL_REAL xo(otherBox[0]);
-      FCL_REAL yo(otherBox[1]);
-      FCL_REAL zo(otherBox[2]);
+      const Vec6s& otherBox = *it;
+      CoalScalar xo(otherBox[0]);
+      CoalScalar yo(otherBox[1]);
+      CoalScalar zo(otherBox[2]);
       // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){
       //   continue;
       // }
@@ -106,35 +106,35 @@ void computeNeighbors(const std::vector<Vec6f>& boxes,
 }  // namespace internal
 
 void OcTree::exportAsObjFile(const std::string& filename) const {
-  std::vector<Vec6f> boxes(this->toBoxes());
+  std::vector<Vec6s> boxes(this->toBoxes());
   std::vector<internal::Neighbors> neighbors(boxes.size());
   internal::computeNeighbors(boxes, neighbors);
   // compute list of vertices and faces
 
-  typedef std::vector<Vec3f> VectorVec3f;
-  std::vector<Vec3f> vertices;
+  typedef std::vector<Vec3s> VectorVec3s;
+  std::vector<Vec3s> vertices;
 
   typedef std::array<std::size_t, 4> Array4;
   typedef std::vector<Array4> VectorArray4;
   std::vector<Array4> faces;
 
   for (std::size_t i = 0; i < boxes.size(); ++i) {
-    const Vec6f& box(boxes[i]);
+    const Vec6s& box(boxes[i]);
     internal::Neighbors& n(neighbors[i]);
 
-    FCL_REAL x(box[0]);
-    FCL_REAL y(box[1]);
-    FCL_REAL z(box[2]);
-    FCL_REAL size(box[3]);
+    CoalScalar x(box[0]);
+    CoalScalar y(box[1]);
+    CoalScalar z(box[2]);
+    CoalScalar size(box[3]);
 
-    vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z - .5 * size));
-    vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z - .5 * size));
-    vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z - .5 * size));
-    vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z - .5 * size));
-    vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z + .5 * size));
-    vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z + .5 * size));
-    vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z + .5 * size));
-    vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z + .5 * size));
+    vertices.push_back(Vec3s(x - .5 * size, y - .5 * size, z - .5 * size));
+    vertices.push_back(Vec3s(x + .5 * size, y - .5 * size, z - .5 * size));
+    vertices.push_back(Vec3s(x - .5 * size, y + .5 * size, z - .5 * size));
+    vertices.push_back(Vec3s(x + .5 * size, y + .5 * size, z - .5 * size));
+    vertices.push_back(Vec3s(x - .5 * size, y - .5 * size, z + .5 * size));
+    vertices.push_back(Vec3s(x + .5 * size, y - .5 * size, z + .5 * size));
+    vertices.push_back(Vec3s(x - .5 * size, y + .5 * size, z + .5 * size));
+    vertices.push_back(Vec3s(x + .5 * size, y + .5 * size, z + .5 * size));
 
     // Add face only if box has no neighbor with the same face
     if (!n.minusX()) {
@@ -166,15 +166,15 @@ void OcTree::exportAsObjFile(const std::string& filename) const {
   std::ofstream os;
   os.open(filename);
   if (!os.is_open())
-    HPP_FCL_THROW_PRETTY(
+    COAL_THROW_PRETTY(
         (std::string("failed to open file \"") + filename + std::string("\""))
             .c_str(),
         std::runtime_error);
   // write vertices
   os << "# list of vertices\n";
-  for (VectorVec3f::const_iterator it = vertices.begin(); it != vertices.end();
+  for (VectorVec3s::const_iterator it = vertices.begin(); it != vertices.end();
        ++it) {
-    const Vec3f& v = *it;
+    const Vec3s& v = *it;
     os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n';
   }
   os << "\n# list of faces\n";
@@ -186,9 +186,9 @@ void OcTree::exportAsObjFile(const std::string& filename) const {
 }
 
 OcTreePtr_t makeOctree(
-    const Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3>& point_cloud,
-    const FCL_REAL resolution) {
-  typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3> InputType;
+    const Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3>& point_cloud,
+    const CoalScalar resolution) {
+  typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3> InputType;
   typedef InputType::ConstRowXpr RowType;
 
   shared_ptr<octomap::OcTree> octree(new octomap::OcTree(resolution));
@@ -200,5 +200,4 @@ OcTreePtr_t makeOctree(
 
   return OcTreePtr_t(new OcTree(octree));
 }
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/serialization/serialization.cpp b/src/serialization/serialization.cpp
index af6763b31c97feda385dbe170b782baa1be789c5..692e79ddf15d593f5e1e898f8b10e5d8e65a8e65 100644
--- a/src/serialization/serialization.cpp
+++ b/src/serialization/serialization.cpp
@@ -34,51 +34,51 @@
 
 /** \author Justin Carpentier */
 
-#include "hpp/fcl/serialization/fwd.h"
+#include "coal/serialization/fwd.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
-#include "hpp/fcl/serialization/transform.h"
-#include "hpp/fcl/serialization/collision_data.h"
-#include "hpp/fcl/serialization/geometric_shapes.h"
-#include "hpp/fcl/serialization/convex.h"
-#include "hpp/fcl/serialization/hfield.h"
-#include "hpp/fcl/serialization/BVH_model.h"
-#ifdef HPP_FCL_HAS_OCTOMAP
-#include "hpp/fcl/serialization/octree.h"
+#include "coal/serialization/transform.h"
+#include "coal/serialization/collision_data.h"
+#include "coal/serialization/geometric_shapes.h"
+#include "coal/serialization/convex.h"
+#include "coal/serialization/hfield.h"
+#include "coal/serialization/BVH_model.h"
+#ifdef COAL_HAS_OCTOMAP
+#include "coal/serialization/octree.h"
 #endif
 
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionRequest)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionResult)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(DistanceRequest)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(DistanceResult)
+COAL_SERIALIZATION_DEFINE_EXPORT(CollisionRequest)
+COAL_SERIALIZATION_DEFINE_EXPORT(CollisionResult)
+COAL_SERIALIZATION_DEFINE_EXPORT(DistanceRequest)
+COAL_SERIALIZATION_DEFINE_EXPORT(DistanceResult)
 
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(ShapeBase)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionGeometry)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(TriangleP)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Box)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Sphere)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Ellipsoid)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Capsule)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Cone)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Cylinder)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Halfspace)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Plane)
+COAL_SERIALIZATION_DEFINE_EXPORT(ShapeBase)
+COAL_SERIALIZATION_DEFINE_EXPORT(CollisionGeometry)
+COAL_SERIALIZATION_DEFINE_EXPORT(TriangleP)
+COAL_SERIALIZATION_DEFINE_EXPORT(Box)
+COAL_SERIALIZATION_DEFINE_EXPORT(Sphere)
+COAL_SERIALIZATION_DEFINE_EXPORT(Ellipsoid)
+COAL_SERIALIZATION_DEFINE_EXPORT(Capsule)
+COAL_SERIALIZATION_DEFINE_EXPORT(Cone)
+COAL_SERIALIZATION_DEFINE_EXPORT(Cylinder)
+COAL_SERIALIZATION_DEFINE_EXPORT(Halfspace)
+COAL_SERIALIZATION_DEFINE_EXPORT(Plane)
 
-#define EXPORT_AND_CAST(Derived, Base)               \
-  HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base) \
-  HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Derived)       \
+#define EXPORT_AND_CAST(Derived, Base)            \
+  COAL_SERIALIZATION_CAST_REGISTER(Derived, Base) \
+  COAL_SERIALIZATION_DEFINE_EXPORT(Derived)       \
   /**/
 
-HPP_FCL_SERIALIZATION_CAST_REGISTER(ConvexBase, CollisionGeometry)
+COAL_SERIALIZATION_CAST_REGISTER(ConvexBase, CollisionGeometry)
 EXPORT_AND_CAST(Convex<Triangle>, ConvexBase)
 EXPORT_AND_CAST(Convex<Quadrilateral>, ConvexBase)
 
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField<AABB>)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField<OBB>)
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField<OBBRSS>)
+COAL_SERIALIZATION_DEFINE_EXPORT(HeightField<AABB>)
+COAL_SERIALIZATION_DEFINE_EXPORT(HeightField<OBB>)
+COAL_SERIALIZATION_DEFINE_EXPORT(HeightField<OBBRSS>)
 
-HPP_FCL_SERIALIZATION_CAST_REGISTER(BVHModelBase, CollisionGeometry)
+COAL_SERIALIZATION_CAST_REGISTER(BVHModelBase, CollisionGeometry)
 
 EXPORT_AND_CAST(BVHModel<AABB>, BVHModelBase)
 EXPORT_AND_CAST(BVHModel<OBB>, BVHModelBase)
@@ -89,6 +89,6 @@ EXPORT_AND_CAST(BVHModel<KDOP<16>>, BVHModelBase)
 EXPORT_AND_CAST(BVHModel<KDOP<18>>, BVHModelBase)
 EXPORT_AND_CAST(BVHModel<KDOP<24>>, BVHModelBase)
 
-#ifdef HPP_FCL_HAS_OCTOMAP
-HPP_FCL_SERIALIZATION_DEFINE_EXPORT(OcTree)
+#ifdef COAL_HAS_OCTOMAP
+COAL_SERIALIZATION_DEFINE_EXPORT(OcTree)
 #endif
diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp
index 74fb1d6aaf2e7e03021e085b642af1dcd3f753cc..e32677a038199eeedb844b4d85227ef2bb13bd5f 100644
--- a/src/shape/convex.cpp
+++ b/src/shape/convex.cpp
@@ -1,6 +1,6 @@
-#include <hpp/fcl/shape/convex.h>
+#include "coal/shape/convex.h"
 
-#ifdef HPP_FCL_HAS_QHULL
+#ifdef COAL_HAS_QHULL
 #include <libqhullcpp/QhullError.h>
 #include <libqhullcpp/QhullFacet.h>
 #include <libqhullcpp/QhullLinkedList.h>
@@ -17,22 +17,21 @@ using orgQhull::QhullVertexList;
 using orgQhull::QhullVertexSet;
 #endif
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 // Reorders `tri` such that the dot product between the normal of triangle and
 // the vector `triangle barycentre - convex_tri.center` is positive.
 void reorderTriangle(const Convex<Triangle>* convex_tri, Triangle& tri) {
-  Vec3f p0, p1, p2;
+  Vec3s p0, p1, p2;
   p0 = (*(convex_tri->points))[tri[0]];
   p1 = (*(convex_tri->points))[tri[1]];
   p2 = (*(convex_tri->points))[tri[2]];
 
-  Vec3f barycentre_tri, center_barycenter;
+  Vec3s barycentre_tri, center_barycenter;
   barycentre_tri = (p0 + p1 + p2) / 3;
   center_barycenter = barycentre_tri - convex_tri->center;
 
-  Vec3f edge_tri1, edge_tri2, n_tri;
+  Vec3s edge_tri1, edge_tri2, n_tri;
   edge_tri1 = p1 - p0;
   edge_tri2 = p2 - p1;
   n_tri = edge_tri1.cross(edge_tri2);
@@ -42,22 +41,22 @@ void reorderTriangle(const Convex<Triangle>* convex_tri, Triangle& tri) {
   }
 }
 
-ConvexBase* ConvexBase::convexHull(std::shared_ptr<std::vector<Vec3f>>& pts,
+ConvexBase* ConvexBase::convexHull(std::shared_ptr<std::vector<Vec3s>>& pts,
                                    unsigned int num_points, bool keepTriangles,
                                    const char* qhullCommand) {
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   return ConvexBase::convexHull(pts->data(), num_points, keepTriangles,
                                 qhullCommand);
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
+  COAL_COMPILER_DIAGNOSTIC_POP
 }
 
-ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
+ConvexBase* ConvexBase::convexHull(const Vec3s* pts, unsigned int num_points,
                                    bool keepTriangles,
                                    const char* qhullCommand) {
-#ifdef HPP_FCL_HAS_QHULL
+#ifdef COAL_HAS_QHULL
   if (num_points <= 3) {
-    HPP_FCL_THROW_PRETTY(
+    COAL_THROW_PRETTY(
         "You shouldn't use this function with less than"
         " 4 points.",
         std::invalid_argument);
@@ -71,7 +70,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
 
   if (qh.qhullStatus() != qh_ERRnone) {
     if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl;
-    HPP_FCL_THROW_PRETTY("Qhull failed", std::logic_error);
+    COAL_THROW_PRETTY("Qhull failed", std::logic_error);
   }
 
   typedef std::size_t index_type;
@@ -82,15 +81,15 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
 
   // Initialize the vertices
   size_t nvertex = static_cast<size_t>(qh.vertexCount());
-  std::shared_ptr<std::vector<Vec3f>> vertices(
-      new std::vector<Vec3f>(size_t(nvertex)));
+  std::shared_ptr<std::vector<Vec3s>> vertices(
+      new std::vector<Vec3s>(size_t(nvertex)));
   QhullVertexList vertexList(qh.vertexList());
   size_t i_vertex = 0;
   for (QhullVertexList::const_iterator v = vertexList.begin();
        v != vertexList.end(); ++v) {
     QhullPoint pt((*v).point());
     pts_to_vertices[(size_t)pt.id()] = (int)i_vertex;
-    (*vertices)[i_vertex] = Vec3f(pt[0], pt[1], pt[2]);
+    (*vertices)[i_vertex] = Vec3s(pt[0], pt[1], pt[2]);
     ++i_vertex;
   }
   assert(i_vertex == nvertex);
@@ -144,7 +143,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
       }
     } else {
       if (keepTriangles) {  // TODO I think there is a memory leak here.
-        HPP_FCL_THROW_PRETTY(
+        COAL_THROW_PRETTY(
             "You requested to keep triangles so you "
             "must pass option \"Qt\" to qhull via the qhull command argument.",
             std::invalid_argument);
@@ -184,7 +183,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
   for (size_t i = 0; i < static_cast<size_t>(nvertex); ++i) {
     Neighbors& n = neighbors_[i];
     if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
-      HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error);
+      COAL_THROW_PRETTY("Too many neighbors.", std::logic_error);
     n.count_ = (unsigned char)nneighbors[i].size();
     n.n_ = p_nneighbors;
     p_nneighbors =
@@ -197,20 +196,20 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
   convex->buildSupportWarmStart();
   return convex;
 #else
-  HPP_FCL_THROW_PRETTY(
+  COAL_THROW_PRETTY(
       "Library built without qhull. Cannot build object of this type.",
       std::logic_error);
-  HPP_FCL_UNUSED_VARIABLE(pts);
-  HPP_FCL_UNUSED_VARIABLE(num_points);
-  HPP_FCL_UNUSED_VARIABLE(keepTriangles);
-  HPP_FCL_UNUSED_VARIABLE(qhullCommand);
+  COAL_UNUSED_VARIABLE(pts);
+  COAL_UNUSED_VARIABLE(num_points);
+  COAL_UNUSED_VARIABLE(keepTriangles);
+  COAL_UNUSED_VARIABLE(qhullCommand);
 #endif
 }
 
-#ifdef HPP_FCL_HAS_QHULL
+#ifdef COAL_HAS_QHULL
 void ConvexBase::buildDoubleDescription() {
   if (num_points <= 3) {
-    HPP_FCL_THROW_PRETTY(
+    COAL_THROW_PRETTY(
         "You shouldn't use this function with a convex less than"
         " 4 points.",
         std::invalid_argument);
@@ -223,7 +222,7 @@ void ConvexBase::buildDoubleDescription() {
 
   if (qh.qhullStatus() != qh_ERRnone) {
     if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl;
-    HPP_FCL_THROW_PRETTY("Qhull failed", std::logic_error);
+    COAL_THROW_PRETTY("Qhull failed", std::logic_error);
   }
 
   buildDoubleDescriptionFromQHullResult(qh);
@@ -231,15 +230,15 @@ void ConvexBase::buildDoubleDescription() {
 
 void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) {
   num_normals_and_offsets = static_cast<unsigned int>(qh.facetCount());
-  normals.reset(new std::vector<Vec3f>(num_normals_and_offsets));
-  std::vector<Vec3f>& normals_ = *normals;
+  normals.reset(new std::vector<Vec3s>(num_normals_and_offsets));
+  std::vector<Vec3s>& normals_ = *normals;
   offsets.reset(new std::vector<double>(num_normals_and_offsets));
   std::vector<double>& offsets_ = *offsets;
   unsigned int i_normal = 0;
   for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet();
        facet = facet.next()) {
     const orgQhull::QhullHyperplane& plane = facet.hyperplane();
-    normals_[i_normal] = Vec3f(plane.coordinates()[0], plane.coordinates()[1],
+    normals_[i_normal] = Vec3s(plane.coordinates()[0], plane.coordinates()[1],
                                plane.coordinates()[2]);
     offsets_[i_normal] = plane.offset();
     i_normal++;
@@ -248,5 +247,4 @@ void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) {
 }
 #endif
 
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index 4c8af2a1fbbdab5aa645591a16374da7cad5612c..cea609b3fc727d49738ecc84a4d2893ff901aebc 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/src/shape/geometric_shapes.cpp
@@ -35,27 +35,26 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
+#include "coal/shape/geometric_shapes.h"
+#include "coal/shape/geometric_shapes_utility.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
-void ConvexBase::initialize(std::shared_ptr<std::vector<Vec3f>> points_,
+void ConvexBase::initialize(std::shared_ptr<std::vector<Vec3s>> points_,
                             unsigned int num_points_) {
   this->points = points_;
   this->num_points = num_points_;
-  HPP_FCL_ASSERT(this->points->size() == this->num_points,
-                 "The number of points is not consistent with the size of the "
-                 "points vector",
-                 std::logic_error);
+  COAL_ASSERT(this->points->size() == this->num_points,
+              "The number of points is not consistent with the size of the "
+              "points vector",
+              std::logic_error);
   this->num_normals_and_offsets = 0;
   this->normals.reset();
   this->offsets.reset();
   this->computeCenter();
 }
 
-void ConvexBase::set(std::shared_ptr<std::vector<Vec3f>> points_,
+void ConvexBase::set(std::shared_ptr<std::vector<Vec3s>> points_,
                      unsigned int num_points_) {
   initialize(points_, num_points_);
 }
@@ -67,7 +66,7 @@ ConvexBase::ConvexBase(const ConvexBase& other)
       center(other.center) {
   if (other.points.get() && other.points->size() > 0) {
     // Deep copy of other points
-    points.reset(new std::vector<Vec3f>(*other.points));
+    points.reset(new std::vector<Vec3s>(*other.points));
   } else
     points.reset();
 
@@ -94,7 +93,7 @@ ConvexBase::ConvexBase(const ConvexBase& other)
     nneighbors_.reset();
 
   if (other.normals.get() && other.normals->size() > 0) {
-    normals.reset(new std::vector<Vec3f>(*(other.normals)));
+    normals.reset(new std::vector<Vec3s>(*(other.normals)));
   } else
     normals.reset();
 
@@ -112,16 +111,16 @@ ConvexBase::~ConvexBase() {}
 
 void ConvexBase::computeCenter() {
   center.setZero();
-  const std::vector<Vec3f>& points_ = *points;
+  const std::vector<Vec3s>& points_ = *points;
   for (std::size_t i = 0; i < num_points; ++i)
     center += points_[i];  // TODO(jcarpent): vectorization
   center /= num_points;
 }
 
 void Halfspace::unitNormalTest() {
-  FCL_REAL l = n.norm();
+  CoalScalar l = n.norm();
   if (l > 0) {
-    FCL_REAL inv_l = 1.0 / l;
+    CoalScalar inv_l = 1.0 / l;
     n *= inv_l;
     d *= inv_l;
   } else {
@@ -131,9 +130,9 @@ void Halfspace::unitNormalTest() {
 }
 
 void Plane::unitNormalTest() {
-  FCL_REAL l = n.norm();
+  CoalScalar l = n.norm();
   if (l > 0) {
-    FCL_REAL inv_l = 1.0 / l;
+    CoalScalar inv_l = 1.0 / l;
     n *= inv_l;
     d *= inv_l;
   } else {
@@ -143,115 +142,113 @@ void Plane::unitNormalTest() {
 }
 
 void Box::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
 void Sphere::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = radius;
 }
 
 void Ellipsoid::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
 void Capsule::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
 void Cone::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
 void Cylinder::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
 void ConvexBase::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
 void Halfspace::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
 void Plane::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
 void TriangleP::computeLocalAABB() {
-  computeBV<AABB>(*this, Transform3f(), aabb_local);
-  const FCL_REAL ssr = this->getSweptSphereRadius();
+  computeBV<AABB>(*this, Transform3s(), aabb_local);
+  const CoalScalar ssr = this->getSweptSphereRadius();
   if (ssr > 0) {
-    aabb_local.min_ -= Vec3f::Constant(ssr);
-    aabb_local.max_ += Vec3f::Constant(ssr);
+    aabb_local.min_ -= Vec3s::Constant(ssr);
+    aabb_local.max_ += Vec3s::Constant(ssr);
   }
   aabb_center = aabb_local.center();
   aabb_radius = (aabb_local.min_ - aabb_center).norm();
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index d8cd7fc08e55b9e778e526a2f7a3a3384052d459..a687a8e30f32a0b02a8f7ad97c0ddf05fb28b095 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -35,198 +35,197 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/internal/BV_fitter.h>
-#include <hpp/fcl/internal/tools.h>
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/BV_fitter.h"
+#include "coal/internal/tools.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 namespace details {
 
-std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf) {
-  std::vector<Vec3f> result(8);
-  FCL_REAL a = box.halfSide[0];
-  FCL_REAL b = box.halfSide[1];
-  FCL_REAL c = box.halfSide[2];
-  result[0] = tf.transform(Vec3f(a, b, c));
-  result[1] = tf.transform(Vec3f(a, b, -c));
-  result[2] = tf.transform(Vec3f(a, -b, c));
-  result[3] = tf.transform(Vec3f(a, -b, -c));
-  result[4] = tf.transform(Vec3f(-a, b, c));
-  result[5] = tf.transform(Vec3f(-a, b, -c));
-  result[6] = tf.transform(Vec3f(-a, -b, c));
-  result[7] = tf.transform(Vec3f(-a, -b, -c));
+std::vector<Vec3s> getBoundVertices(const Box& box, const Transform3s& tf) {
+  std::vector<Vec3s> result(8);
+  CoalScalar a = box.halfSide[0];
+  CoalScalar b = box.halfSide[1];
+  CoalScalar c = box.halfSide[2];
+  result[0] = tf.transform(Vec3s(a, b, c));
+  result[1] = tf.transform(Vec3s(a, b, -c));
+  result[2] = tf.transform(Vec3s(a, -b, c));
+  result[3] = tf.transform(Vec3s(a, -b, -c));
+  result[4] = tf.transform(Vec3s(-a, b, c));
+  result[5] = tf.transform(Vec3s(-a, b, -c));
+  result[6] = tf.transform(Vec3s(-a, -b, c));
+  result[7] = tf.transform(Vec3s(-a, -b, -c));
 
   return result;
 }
 
 // we use icosahedron to bound the sphere
-std::vector<Vec3f> getBoundVertices(const Sphere& sphere,
-                                    const Transform3f& tf) {
-  std::vector<Vec3f> result(12);
-  const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
-  FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
-
-  FCL_REAL a = edge_size;
-  FCL_REAL b = m * edge_size;
-  result[0] = tf.transform(Vec3f(0, a, b));
-  result[1] = tf.transform(Vec3f(0, -a, b));
-  result[2] = tf.transform(Vec3f(0, a, -b));
-  result[3] = tf.transform(Vec3f(0, -a, -b));
-  result[4] = tf.transform(Vec3f(a, b, 0));
-  result[5] = tf.transform(Vec3f(-a, b, 0));
-  result[6] = tf.transform(Vec3f(a, -b, 0));
-  result[7] = tf.transform(Vec3f(-a, -b, 0));
-  result[8] = tf.transform(Vec3f(b, 0, a));
-  result[9] = tf.transform(Vec3f(b, 0, -a));
-  result[10] = tf.transform(Vec3f(-b, 0, a));
-  result[11] = tf.transform(Vec3f(-b, 0, -a));
+std::vector<Vec3s> getBoundVertices(const Sphere& sphere,
+                                    const Transform3s& tf) {
+  std::vector<Vec3s> result(12);
+  const CoalScalar m = (1 + sqrt(5.0)) / 2.0;
+  CoalScalar edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
+
+  CoalScalar a = edge_size;
+  CoalScalar b = m * edge_size;
+  result[0] = tf.transform(Vec3s(0, a, b));
+  result[1] = tf.transform(Vec3s(0, -a, b));
+  result[2] = tf.transform(Vec3s(0, a, -b));
+  result[3] = tf.transform(Vec3s(0, -a, -b));
+  result[4] = tf.transform(Vec3s(a, b, 0));
+  result[5] = tf.transform(Vec3s(-a, b, 0));
+  result[6] = tf.transform(Vec3s(a, -b, 0));
+  result[7] = tf.transform(Vec3s(-a, -b, 0));
+  result[8] = tf.transform(Vec3s(b, 0, a));
+  result[9] = tf.transform(Vec3s(b, 0, -a));
+  result[10] = tf.transform(Vec3s(-b, 0, a));
+  result[11] = tf.transform(Vec3s(-b, 0, -a));
 
   return result;
 }
 
 // we use scaled icosahedron to bound the ellipsoid
-std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
-                                    const Transform3f& tf) {
-  std::vector<Vec3f> result(12);
-  const FCL_REAL phi = (1 + sqrt(5.0)) / 2.0;
-
-  const FCL_REAL a = sqrt(3.0) / (phi * phi);
-  const FCL_REAL b = phi * a;
-
-  const FCL_REAL& A = ellipsoid.radii[0];
-  const FCL_REAL& B = ellipsoid.radii[1];
-  const FCL_REAL& C = ellipsoid.radii[2];
-
-  FCL_REAL Aa = A * a;
-  FCL_REAL Ab = A * b;
-  FCL_REAL Ba = B * a;
-  FCL_REAL Bb = B * b;
-  FCL_REAL Ca = C * a;
-  FCL_REAL Cb = C * b;
-  result[0] = tf.transform(Vec3f(0, Ba, Cb));
-  result[1] = tf.transform(Vec3f(0, -Ba, Cb));
-  result[2] = tf.transform(Vec3f(0, Ba, -Cb));
-  result[3] = tf.transform(Vec3f(0, -Ba, -Cb));
-  result[4] = tf.transform(Vec3f(Aa, Bb, 0));
-  result[5] = tf.transform(Vec3f(-Aa, Bb, 0));
-  result[6] = tf.transform(Vec3f(Aa, -Bb, 0));
-  result[7] = tf.transform(Vec3f(-Aa, -Bb, 0));
-  result[8] = tf.transform(Vec3f(Ab, 0, Ca));
-  result[9] = tf.transform(Vec3f(Ab, 0, -Ca));
-  result[10] = tf.transform(Vec3f(-Ab, 0, Ca));
-  result[11] = tf.transform(Vec3f(-Ab, 0, -Ca));
+std::vector<Vec3s> getBoundVertices(const Ellipsoid& ellipsoid,
+                                    const Transform3s& tf) {
+  std::vector<Vec3s> result(12);
+  const CoalScalar phi = (1 + sqrt(5.0)) / 2.0;
+
+  const CoalScalar a = sqrt(3.0) / (phi * phi);
+  const CoalScalar b = phi * a;
+
+  const CoalScalar& A = ellipsoid.radii[0];
+  const CoalScalar& B = ellipsoid.radii[1];
+  const CoalScalar& C = ellipsoid.radii[2];
+
+  CoalScalar Aa = A * a;
+  CoalScalar Ab = A * b;
+  CoalScalar Ba = B * a;
+  CoalScalar Bb = B * b;
+  CoalScalar Ca = C * a;
+  CoalScalar Cb = C * b;
+  result[0] = tf.transform(Vec3s(0, Ba, Cb));
+  result[1] = tf.transform(Vec3s(0, -Ba, Cb));
+  result[2] = tf.transform(Vec3s(0, Ba, -Cb));
+  result[3] = tf.transform(Vec3s(0, -Ba, -Cb));
+  result[4] = tf.transform(Vec3s(Aa, Bb, 0));
+  result[5] = tf.transform(Vec3s(-Aa, Bb, 0));
+  result[6] = tf.transform(Vec3s(Aa, -Bb, 0));
+  result[7] = tf.transform(Vec3s(-Aa, -Bb, 0));
+  result[8] = tf.transform(Vec3s(Ab, 0, Ca));
+  result[9] = tf.transform(Vec3s(Ab, 0, -Ca));
+  result[10] = tf.transform(Vec3s(-Ab, 0, Ca));
+  result[11] = tf.transform(Vec3s(-Ab, 0, -Ca));
 
   return result;
 }
 
-std::vector<Vec3f> getBoundVertices(const Capsule& capsule,
-                                    const Transform3f& tf) {
-  std::vector<Vec3f> result(36);
-  const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
-
-  FCL_REAL hl = capsule.halfLength;
-  FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
-  FCL_REAL a = edge_size;
-  FCL_REAL b = m * edge_size;
-  FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0);
-
-  result[0] = tf.transform(Vec3f(0, a, b + hl));
-  result[1] = tf.transform(Vec3f(0, -a, b + hl));
-  result[2] = tf.transform(Vec3f(0, a, -b + hl));
-  result[3] = tf.transform(Vec3f(0, -a, -b + hl));
-  result[4] = tf.transform(Vec3f(a, b, hl));
-  result[5] = tf.transform(Vec3f(-a, b, hl));
-  result[6] = tf.transform(Vec3f(a, -b, hl));
-  result[7] = tf.transform(Vec3f(-a, -b, hl));
-  result[8] = tf.transform(Vec3f(b, 0, a + hl));
-  result[9] = tf.transform(Vec3f(b, 0, -a + hl));
-  result[10] = tf.transform(Vec3f(-b, 0, a + hl));
-  result[11] = tf.transform(Vec3f(-b, 0, -a + hl));
-
-  result[12] = tf.transform(Vec3f(0, a, b - hl));
-  result[13] = tf.transform(Vec3f(0, -a, b - hl));
-  result[14] = tf.transform(Vec3f(0, a, -b - hl));
-  result[15] = tf.transform(Vec3f(0, -a, -b - hl));
-  result[16] = tf.transform(Vec3f(a, b, -hl));
-  result[17] = tf.transform(Vec3f(-a, b, -hl));
-  result[18] = tf.transform(Vec3f(a, -b, -hl));
-  result[19] = tf.transform(Vec3f(-a, -b, -hl));
-  result[20] = tf.transform(Vec3f(b, 0, a - hl));
-  result[21] = tf.transform(Vec3f(b, 0, -a - hl));
-  result[22] = tf.transform(Vec3f(-b, 0, a - hl));
-  result[23] = tf.transform(Vec3f(-b, 0, -a - hl));
-
-  FCL_REAL c = 0.5 * r2;
-  FCL_REAL d = capsule.radius;
-  result[24] = tf.transform(Vec3f(r2, 0, hl));
-  result[25] = tf.transform(Vec3f(c, d, hl));
-  result[26] = tf.transform(Vec3f(-c, d, hl));
-  result[27] = tf.transform(Vec3f(-r2, 0, hl));
-  result[28] = tf.transform(Vec3f(-c, -d, hl));
-  result[29] = tf.transform(Vec3f(c, -d, hl));
-
-  result[30] = tf.transform(Vec3f(r2, 0, -hl));
-  result[31] = tf.transform(Vec3f(c, d, -hl));
-  result[32] = tf.transform(Vec3f(-c, d, -hl));
-  result[33] = tf.transform(Vec3f(-r2, 0, -hl));
-  result[34] = tf.transform(Vec3f(-c, -d, -hl));
-  result[35] = tf.transform(Vec3f(c, -d, -hl));
+std::vector<Vec3s> getBoundVertices(const Capsule& capsule,
+                                    const Transform3s& tf) {
+  std::vector<Vec3s> result(36);
+  const CoalScalar m = (1 + sqrt(5.0)) / 2.0;
+
+  CoalScalar hl = capsule.halfLength;
+  CoalScalar edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
+  CoalScalar a = edge_size;
+  CoalScalar b = m * edge_size;
+  CoalScalar r2 = capsule.radius * 2 / sqrt(3.0);
+
+  result[0] = tf.transform(Vec3s(0, a, b + hl));
+  result[1] = tf.transform(Vec3s(0, -a, b + hl));
+  result[2] = tf.transform(Vec3s(0, a, -b + hl));
+  result[3] = tf.transform(Vec3s(0, -a, -b + hl));
+  result[4] = tf.transform(Vec3s(a, b, hl));
+  result[5] = tf.transform(Vec3s(-a, b, hl));
+  result[6] = tf.transform(Vec3s(a, -b, hl));
+  result[7] = tf.transform(Vec3s(-a, -b, hl));
+  result[8] = tf.transform(Vec3s(b, 0, a + hl));
+  result[9] = tf.transform(Vec3s(b, 0, -a + hl));
+  result[10] = tf.transform(Vec3s(-b, 0, a + hl));
+  result[11] = tf.transform(Vec3s(-b, 0, -a + hl));
+
+  result[12] = tf.transform(Vec3s(0, a, b - hl));
+  result[13] = tf.transform(Vec3s(0, -a, b - hl));
+  result[14] = tf.transform(Vec3s(0, a, -b - hl));
+  result[15] = tf.transform(Vec3s(0, -a, -b - hl));
+  result[16] = tf.transform(Vec3s(a, b, -hl));
+  result[17] = tf.transform(Vec3s(-a, b, -hl));
+  result[18] = tf.transform(Vec3s(a, -b, -hl));
+  result[19] = tf.transform(Vec3s(-a, -b, -hl));
+  result[20] = tf.transform(Vec3s(b, 0, a - hl));
+  result[21] = tf.transform(Vec3s(b, 0, -a - hl));
+  result[22] = tf.transform(Vec3s(-b, 0, a - hl));
+  result[23] = tf.transform(Vec3s(-b, 0, -a - hl));
+
+  CoalScalar c = 0.5 * r2;
+  CoalScalar d = capsule.radius;
+  result[24] = tf.transform(Vec3s(r2, 0, hl));
+  result[25] = tf.transform(Vec3s(c, d, hl));
+  result[26] = tf.transform(Vec3s(-c, d, hl));
+  result[27] = tf.transform(Vec3s(-r2, 0, hl));
+  result[28] = tf.transform(Vec3s(-c, -d, hl));
+  result[29] = tf.transform(Vec3s(c, -d, hl));
+
+  result[30] = tf.transform(Vec3s(r2, 0, -hl));
+  result[31] = tf.transform(Vec3s(c, d, -hl));
+  result[32] = tf.transform(Vec3s(-c, d, -hl));
+  result[33] = tf.transform(Vec3s(-r2, 0, -hl));
+  result[34] = tf.transform(Vec3s(-c, -d, -hl));
+  result[35] = tf.transform(Vec3s(c, -d, -hl));
 
   return result;
 }
 
-std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf) {
-  std::vector<Vec3f> result(7);
+std::vector<Vec3s> getBoundVertices(const Cone& cone, const Transform3s& tf) {
+  std::vector<Vec3s> result(7);
 
-  FCL_REAL hl = cone.halfLength;
-  FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
-  FCL_REAL a = 0.5 * r2;
-  FCL_REAL b = cone.radius;
+  CoalScalar hl = cone.halfLength;
+  CoalScalar r2 = cone.radius * 2 / sqrt(3.0);
+  CoalScalar a = 0.5 * r2;
+  CoalScalar b = cone.radius;
 
-  result[0] = tf.transform(Vec3f(r2, 0, -hl));
-  result[1] = tf.transform(Vec3f(a, b, -hl));
-  result[2] = tf.transform(Vec3f(-a, b, -hl));
-  result[3] = tf.transform(Vec3f(-r2, 0, -hl));
-  result[4] = tf.transform(Vec3f(-a, -b, -hl));
-  result[5] = tf.transform(Vec3f(a, -b, -hl));
+  result[0] = tf.transform(Vec3s(r2, 0, -hl));
+  result[1] = tf.transform(Vec3s(a, b, -hl));
+  result[2] = tf.transform(Vec3s(-a, b, -hl));
+  result[3] = tf.transform(Vec3s(-r2, 0, -hl));
+  result[4] = tf.transform(Vec3s(-a, -b, -hl));
+  result[5] = tf.transform(Vec3s(a, -b, -hl));
 
-  result[6] = tf.transform(Vec3f(0, 0, hl));
+  result[6] = tf.transform(Vec3s(0, 0, hl));
 
   return result;
 }
 
-std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder,
-                                    const Transform3f& tf) {
-  std::vector<Vec3f> result(12);
-
-  FCL_REAL hl = cylinder.halfLength;
-  FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
-  FCL_REAL a = 0.5 * r2;
-  FCL_REAL b = cylinder.radius;
-
-  result[0] = tf.transform(Vec3f(r2, 0, -hl));
-  result[1] = tf.transform(Vec3f(a, b, -hl));
-  result[2] = tf.transform(Vec3f(-a, b, -hl));
-  result[3] = tf.transform(Vec3f(-r2, 0, -hl));
-  result[4] = tf.transform(Vec3f(-a, -b, -hl));
-  result[5] = tf.transform(Vec3f(a, -b, -hl));
-
-  result[6] = tf.transform(Vec3f(r2, 0, hl));
-  result[7] = tf.transform(Vec3f(a, b, hl));
-  result[8] = tf.transform(Vec3f(-a, b, hl));
-  result[9] = tf.transform(Vec3f(-r2, 0, hl));
-  result[10] = tf.transform(Vec3f(-a, -b, hl));
-  result[11] = tf.transform(Vec3f(a, -b, hl));
+std::vector<Vec3s> getBoundVertices(const Cylinder& cylinder,
+                                    const Transform3s& tf) {
+  std::vector<Vec3s> result(12);
+
+  CoalScalar hl = cylinder.halfLength;
+  CoalScalar r2 = cylinder.radius * 2 / sqrt(3.0);
+  CoalScalar a = 0.5 * r2;
+  CoalScalar b = cylinder.radius;
+
+  result[0] = tf.transform(Vec3s(r2, 0, -hl));
+  result[1] = tf.transform(Vec3s(a, b, -hl));
+  result[2] = tf.transform(Vec3s(-a, b, -hl));
+  result[3] = tf.transform(Vec3s(-r2, 0, -hl));
+  result[4] = tf.transform(Vec3s(-a, -b, -hl));
+  result[5] = tf.transform(Vec3s(a, -b, -hl));
+
+  result[6] = tf.transform(Vec3s(r2, 0, hl));
+  result[7] = tf.transform(Vec3s(a, b, hl));
+  result[8] = tf.transform(Vec3s(-a, b, hl));
+  result[9] = tf.transform(Vec3s(-r2, 0, hl));
+  result[10] = tf.transform(Vec3s(-a, -b, hl));
+  result[11] = tf.transform(Vec3s(a, -b, hl));
 
   return result;
 }
 
-std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
-                                    const Transform3f& tf) {
-  std::vector<Vec3f> result(convex.num_points);
-  const std::vector<Vec3f>& points_ = *(convex.points);
+std::vector<Vec3s> getBoundVertices(const ConvexBase& convex,
+                                    const Transform3s& tf) {
+  std::vector<Vec3s> result(convex.num_points);
+  const std::vector<Vec3s>& points_ = *(convex.points);
   for (std::size_t i = 0; i < convex.num_points; ++i) {
     result[i] = tf.transform(points_[i]);
   }
@@ -234,9 +233,9 @@ std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
   return result;
 }
 
-std::vector<Vec3f> getBoundVertices(const TriangleP& triangle,
-                                    const Transform3f& tf) {
-  std::vector<Vec3f> result(3);
+std::vector<Vec3s> getBoundVertices(const TriangleP& triangle,
+                                    const Transform3s& tf) {
+  std::vector<Vec3s> result(3);
   result[0] = tf.transform(triangle.a);
   result[1] = tf.transform(triangle.b);
   result[2] = tf.transform(triangle.c);
@@ -246,30 +245,30 @@ std::vector<Vec3f> getBoundVertices(const TriangleP& triangle,
 
 }  // namespace details
 
-Halfspace transform(const Halfspace& a, const Transform3f& tf) {
+Halfspace transform(const Halfspace& a, const Transform3s& tf) {
   /// suppose the initial halfspace is n * x <= d
   /// after transform (R, T), x --> x' = R x + T
   /// and the new half space becomes n' * x' <= d'
   /// where n' = R * n
   ///   and d' = d + n' * T
 
-  Vec3f n = tf.getRotation() * a.n;
-  FCL_REAL d = a.d + n.dot(tf.getTranslation());
+  Vec3s n = tf.getRotation() * a.n;
+  CoalScalar d = a.d + n.dot(tf.getTranslation());
   Halfspace result(n, d);
   result.setSweptSphereRadius(a.getSweptSphereRadius());
 
   return result;
 }
 
-Plane transform(const Plane& a, const Transform3f& tf) {
+Plane transform(const Plane& a, const Transform3s& tf) {
   /// suppose the initial halfspace is n * x <= d
   /// after transform (R, T), x --> x' = R x + T
   /// and the new half space becomes n' * x' <= d'
   /// where n' = R * n
   ///   and d' = d + n' * T
 
-  Vec3f n = tf.getRotation() * a.n;
-  FCL_REAL d = a.d + n.dot(tf.getTranslation());
+  Vec3s n = tf.getRotation() * a.n;
+  CoalScalar d = a.d + n.dot(tf.getTranslation());
   Plane result(n, d);
   result.setSweptSphereRadius(a.getSweptSphereRadius());
 
@@ -277,11 +276,11 @@ Plane transform(const Plane& a, const Transform3f& tf) {
 }
 
 std::array<Halfspace, 2> transformToHalfspaces(const Plane& a,
-                                               const Transform3f& tf) {
+                                               const Transform3s& tf) {
   // A plane can be represented by two halfspaces
 
-  Vec3f n = tf.getRotation() * a.n;
-  FCL_REAL d = a.d + n.dot(tf.getTranslation());
+  Vec3s n = tf.getRotation() * a.n;
+  CoalScalar d = a.d + n.dot(tf.getTranslation());
   std::array<Halfspace, 2> result = {Halfspace(n, d), Halfspace(-n, -d)};
   result[0].setSweptSphereRadius(a.getSweptSphereRadius());
   result[1].setSweptSphereRadius(a.getSweptSphereRadius());
@@ -290,91 +289,91 @@ std::array<Halfspace, 2> transformToHalfspaces(const Plane& a,
 }
 
 template <>
-void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv) {
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+void computeBV<AABB, Box>(const Box& s, const Transform3s& tf, AABB& bv) {
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
-  Vec3f v_delta(R.cwiseAbs() * s.halfSide);
+  Vec3s v_delta(R.cwiseAbs() * s.halfSide);
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }
 
 template <>
-void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv) {
-  const Vec3f& T = tf.getTranslation();
+void computeBV<AABB, Sphere>(const Sphere& s, const Transform3s& tf, AABB& bv) {
+  const Vec3s& T = tf.getTranslation();
 
-  Vec3f v_delta(Vec3f::Constant(s.radius));
+  Vec3s v_delta(Vec3s::Constant(s.radius));
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }
 
 template <>
-void computeBV<AABB, Ellipsoid>(const Ellipsoid& e, const Transform3f& tf,
+void computeBV<AABB, Ellipsoid>(const Ellipsoid& e, const Transform3s& tf,
                                 AABB& bv) {
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
-  Vec3f v_delta = R * e.radii;
+  Vec3s v_delta = R * e.radii;
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }
 
 template <>
-void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf,
+void computeBV<AABB, Capsule>(const Capsule& s, const Transform3s& tf,
                               AABB& bv) {
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
-  Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius));
+  Vec3s v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3s::Constant(s.radius));
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }
 
 template <>
-void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv) {
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
-
-  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
-                     fabs(R(0, 2) * s.halfLength);
-  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
-                     fabs(R(1, 2) * s.halfLength);
-  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
-                     fabs(R(2, 2) * s.halfLength);
-
-  Vec3f v_delta(x_range, y_range, z_range);
+void computeBV<AABB, Cone>(const Cone& s, const Transform3s& tf, AABB& bv) {
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
+
+  CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
+                       fabs(R(0, 2) * s.halfLength);
+  CoalScalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
+                       fabs(R(1, 2) * s.halfLength);
+  CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
+                       fabs(R(2, 2) * s.halfLength);
+
+  Vec3s v_delta(x_range, y_range, z_range);
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }
 
 template <>
-void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf,
+void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3s& tf,
                                AABB& bv) {
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
-  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
-                     fabs(R(0, 2) * s.halfLength);
-  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
-                     fabs(R(1, 2) * s.halfLength);
-  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
-                     fabs(R(2, 2) * s.halfLength);
+  CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
+                       fabs(R(0, 2) * s.halfLength);
+  CoalScalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
+                       fabs(R(1, 2) * s.halfLength);
+  CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
+                       fabs(R(2, 2) * s.halfLength);
 
-  Vec3f v_delta(x_range, y_range, z_range);
+  Vec3s v_delta(x_range, y_range, z_range);
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }
 
 template <>
-void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
+void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3s& tf,
                                  AABB& bv) {
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
   AABB bv_;
-  const std::vector<Vec3f>& points_ = *(s.points);
+  const std::vector<Vec3s>& points_ = *(s.points);
   for (std::size_t i = 0; i < s.num_points; ++i) {
-    Vec3f new_p = R * points_[i] + T;
+    Vec3s new_p = R * points_[i] + T;
     bv_ += new_p;
   }
 
@@ -382,34 +381,34 @@ void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
 }
 
 template <>
-void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf,
+void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3s& tf,
                                 AABB& bv) {
   bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
 }
 
 template <>
-void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf,
+void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3s& tf,
                                 AABB& bv) {
   Halfspace new_s = transform(s, tf);
-  const Vec3f& n = new_s.n;
-  const FCL_REAL& d = new_s.d;
+  const Vec3s& n = new_s.n;
+  const CoalScalar& d = new_s.d;
 
   AABB bv_;
-  bv_.min_ = Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)());
-  bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)());
-  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  bv_.min_ = Vec3s::Constant(-(std::numeric_limits<CoalScalar>::max)());
+  bv_.max_ = Vec3s::Constant((std::numeric_limits<CoalScalar>::max)());
+  if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     // normal aligned with x axis
     if (n[0] < 0)
       bv_.min_[0] = -d;
     else if (n[0] > 0)
       bv_.max_[0] = d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     // normal aligned with y axis
     if (n[1] < 0)
       bv_.min_[1] = -d;
     else if (n[1] > 0)
       bv_.max_[1] = d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) {
     // normal aligned with z axis
     if (n[2] < 0)
       bv_.min_[2] = -d;
@@ -421,29 +420,29 @@ void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf,
 }
 
 template <>
-void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv) {
+void computeBV<AABB, Plane>(const Plane& s, const Transform3s& tf, AABB& bv) {
   Plane new_s = transform(s, tf);
-  const Vec3f& n = new_s.n;
-  const FCL_REAL& d = new_s.d;
+  const Vec3s& n = new_s.n;
+  const CoalScalar& d = new_s.d;
 
   AABB bv_;
-  bv_.min_ = Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)());
-  bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)());
-  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  bv_.min_ = Vec3s::Constant(-(std::numeric_limits<CoalScalar>::max)());
+  bv_.max_ = Vec3s::Constant((std::numeric_limits<CoalScalar>::max)());
+  if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     // normal aligned with x axis
     if (n[0] < 0) {
       bv_.min_[0] = bv_.max_[0] = -d;
     } else if (n[0] > 0) {
       bv_.min_[0] = bv_.max_[0] = d;
     }
-  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     // normal aligned with y axis
     if (n[1] < 0) {
       bv_.min_[1] = bv_.max_[1] = -d;
     } else if (n[1] > 0) {
       bv_.min_[1] = bv_.max_[1] = d;
     }
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) {
     // normal aligned with z axis
     if (n[2] < 0) {
       bv_.min_[2] = bv_.max_[2] = -d;
@@ -456,13 +455,13 @@ void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv) {
 }
 
 template <>
-void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv) {
+void computeBV<OBB, Box>(const Box& s, const Transform3s& tf, OBB& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
   bv.To = T;
   bv.axes = R;
@@ -470,12 +469,12 @@ void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv) {
 }
 
 template <>
-void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv) {
+void computeBV<OBB, Sphere>(const Sphere& s, const Transform3s& tf, OBB& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
-  const Vec3f& T = tf.getTranslation();
+  const Vec3s& T = tf.getTranslation();
 
   bv.To.noalias() = T;
   bv.axes.setIdentity();
@@ -483,13 +482,13 @@ void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv) {
 }
 
 template <>
-void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv) {
+void computeBV<OBB, Capsule>(const Capsule& s, const Transform3s& tf, OBB& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
   bv.To.noalias() = T;
   bv.axes.noalias() = R;
@@ -497,13 +496,13 @@ void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv) {
 }
 
 template <>
-void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv) {
+void computeBV<OBB, Cone>(const Cone& s, const Transform3s& tf, OBB& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
   bv.To.noalias() = T;
   bv.axes.noalias() = R;
@@ -511,14 +510,14 @@ void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv) {
 }
 
 template <>
-void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf,
+void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3s& tf,
                               OBB& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
   bv.To.noalias() = T;
   bv.axes.noalias() = R;
@@ -526,14 +525,14 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf,
 }
 
 template <>
-void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
+void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3s& tf,
                                 OBB& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
-  const Matrix3f& R = tf.getRotation();
-  const Vec3f& T = tf.getTranslation();
+  const Matrix3s& R = tf.getRotation();
+  const Vec3s& T = tf.getTranslation();
 
   fit(s.points->data(), s.num_points, bv);
 
@@ -543,109 +542,109 @@ void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
 }
 
 template <>
-void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f&,
+void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3s&,
                                OBB& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   /// Half space can only have very rough OBB
   bv.axes.setIdentity();
   bv.To.setZero();
-  bv.extent.setConstant(((std::numeric_limits<FCL_REAL>::max)()));
+  bv.extent.setConstant(((std::numeric_limits<CoalScalar>::max)()));
 }
 
 template <>
-void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f&,
+void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3s&,
                                RSS& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   /// Half space can only have very rough RSS
   bv.axes.setIdentity();
   bv.Tr.setZero();
   bv.length[0] = bv.length[1] = bv.radius =
-      (std::numeric_limits<FCL_REAL>::max)();
+      (std::numeric_limits<CoalScalar>::max)();
 }
 
 template <>
-void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf,
+void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3s& tf,
                                   OBBRSS& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   computeBV<OBB, Halfspace>(s, tf, bv.obb);
   computeBV<RSS, Halfspace>(s, tf, bv.rss);
 }
 
 template <>
-void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf,
+void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3s& tf,
                                 kIOS& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   bv.num_spheres = 1;
   computeBV<OBB, Halfspace>(s, tf, bv.obb);
-  bv.spheres[0].o = Vec3f();
-  bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)();
+  bv.spheres[0].o = Vec3s();
+  bv.spheres[0].r = (std::numeric_limits<CoalScalar>::max)();
 }
 
 template <>
-void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf,
+void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3s& tf,
                                     KDOP<16>& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   Halfspace new_s = transform(s, tf);
-  const Vec3f& n = new_s.n;
-  const FCL_REAL& d = new_s.d;
+  const Vec3s& n = new_s.n;
+  const CoalScalar& d = new_s.d;
 
   const short D = 8;
   for (short i = 0; i < D; ++i)
-    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
   for (short i = D; i < 2 * D; ++i)
-    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = (std::numeric_limits<CoalScalar>::max)();
 
-  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D) = d;
     else
       bv.dist(0) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(D + 1) = d;
     else
       bv.dist(1) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) {
     if (n[2] > 0)
       bv.dist(D + 2) = d;
     else
       bv.dist(2) = -d;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) {
     if (n[0] > 0)
       bv.dist(D + 3) = n[0] * d * 2;
     else
       bv.dist(3) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) {
     if (n[1] > 0)
       bv.dist(D + 4) = n[0] * d * 2;
     else
       bv.dist(4) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) {
     if (n[1] > 0)
       bv.dist(D + 5) = n[1] * d * 2;
     else
       bv.dist(5) = n[1] * d * 2;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D + 6) = n[0] * d * 2;
     else
       bv.dist(6) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D + 7) = n[0] * d * 2;
     else
@@ -654,64 +653,64 @@ void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf,
 }
 
 template <>
-void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf,
+void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3s& tf,
                                     KDOP<18>& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   Halfspace new_s = transform(s, tf);
-  const Vec3f& n = new_s.n;
-  const FCL_REAL& d = new_s.d;
+  const Vec3s& n = new_s.n;
+  const CoalScalar& d = new_s.d;
 
   const short D = 9;
 
   for (short i = 0; i < D; ++i)
-    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
   for (short i = D; i < 2 * D; ++i)
-    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = (std::numeric_limits<CoalScalar>::max)();
 
-  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D) = d;
     else
       bv.dist(0) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(D + 1) = d;
     else
       bv.dist(1) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) {
     if (n[2] > 0)
       bv.dist(D + 2) = d;
     else
       bv.dist(2) = -d;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) {
     if (n[0] > 0)
       bv.dist(D + 3) = n[0] * d * 2;
     else
       bv.dist(3) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) {
     if (n[1] > 0)
       bv.dist(D + 4) = n[0] * d * 2;
     else
       bv.dist(4) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) {
     if (n[1] > 0)
       bv.dist(D + 5) = n[1] * d * 2;
     else
       bv.dist(5) = n[1] * d * 2;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D + 6) = n[0] * d * 2;
     else
       bv.dist(6) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D + 7) = n[0] * d * 2;
     else
       bv.dist(7) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(D + 8) = n[1] * d * 2;
     else
@@ -720,79 +719,79 @@ void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf,
 }
 
 template <>
-void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf,
+void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3s& tf,
                                     KDOP<24>& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   Halfspace new_s = transform(s, tf);
-  const Vec3f& n = new_s.n;
-  const FCL_REAL& d = new_s.d;
+  const Vec3s& n = new_s.n;
+  const CoalScalar& d = new_s.d;
 
   const short D = 12;
 
   for (short i = 0; i < D; ++i)
-    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
   for (short i = D; i < 2 * D; ++i)
-    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = (std::numeric_limits<CoalScalar>::max)();
 
-  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D) = d;
     else
       bv.dist(0) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(D + 1) = d;
     else
       bv.dist(1) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) {
     if (n[2] > 0)
       bv.dist(D + 2) = d;
     else
       bv.dist(2) = -d;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) {
     if (n[0] > 0)
       bv.dist(D + 3) = n[0] * d * 2;
     else
       bv.dist(3) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) {
     if (n[1] > 0)
       bv.dist(D + 4) = n[0] * d * 2;
     else
       bv.dist(4) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) {
     if (n[1] > 0)
       bv.dist(D + 5) = n[1] * d * 2;
     else
       bv.dist(5) = n[1] * d * 2;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D + 6) = n[0] * d * 2;
     else
       bv.dist(6) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D + 7) = n[0] * d * 2;
     else
       bv.dist(7) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(D + 8) = n[1] * d * 2;
     else
       bv.dist(8) = n[1] * d * 2;
-  } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] + n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D + 9) = n[0] * d * 3;
     else
       bv.dist(9) = n[0] * d * 3;
-  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] + n[1] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(D + 10) = n[0] * d * 3;
     else
       bv.dist(10) = n[0] * d * 3;
-  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] + n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(D + 11) = n[1] * d * 3;
     else
@@ -801,302 +800,300 @@ void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf,
 }
 
 template <>
-void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv) {
+void computeBV<OBB, Plane>(const Plane& s, const Transform3s& tf, OBB& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
-  Vec3f n = tf.getRotation() * s.n;
+  Vec3s n = tf.getRotation() * s.n;
   generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
   bv.axes.col(0).noalias() = n;
 
-  bv.extent << 0, (std::numeric_limits<FCL_REAL>::max)(),
-      (std::numeric_limits<FCL_REAL>::max)();
+  bv.extent << 0, (std::numeric_limits<CoalScalar>::max)(),
+      (std::numeric_limits<CoalScalar>::max)();
 
-  Vec3f p = s.n * s.d;
+  Vec3s p = s.n * s.d;
   bv.To =
       tf.transform(p);  /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T
 }
 
 template <>
-void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv) {
+void computeBV<RSS, Plane>(const Plane& s, const Transform3s& tf, RSS& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
-  Vec3f n = tf.getRotation() * s.n;
+  Vec3s n = tf.getRotation() * s.n;
 
   generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
   bv.axes.col(0).noalias() = n;
 
-  bv.length[0] = (std::numeric_limits<FCL_REAL>::max)();
-  bv.length[1] = (std::numeric_limits<FCL_REAL>::max)();
+  bv.length[0] = (std::numeric_limits<CoalScalar>::max)();
+  bv.length[1] = (std::numeric_limits<CoalScalar>::max)();
 
   bv.radius = 0;
 
-  Vec3f p = s.n * s.d;
+  Vec3s p = s.n * s.d;
   bv.Tr = tf.transform(p);
 }
 
 template <>
-void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf,
+void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3s& tf,
                               OBBRSS& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   computeBV<OBB, Plane>(s, tf, bv.obb);
   computeBV<RSS, Plane>(s, tf, bv.rss);
 }
 
 template <>
-void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv) {
+void computeBV<kIOS, Plane>(const Plane& s, const Transform3s& tf, kIOS& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   bv.num_spheres = 1;
   computeBV<OBB, Plane>(s, tf, bv.obb);
-  bv.spheres[0].o = Vec3f();
-  bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)();
+  bv.spheres[0].o = Vec3s();
+  bv.spheres[0].r = (std::numeric_limits<CoalScalar>::max)();
 }
 
 template <>
-void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf,
+void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3s& tf,
                                 KDOP<16>& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   Plane new_s = transform(s, tf);
-  const Vec3f& n = new_s.n;
-  const FCL_REAL& d = new_s.d;
+  const Vec3s& n = new_s.n;
+  const CoalScalar& d = new_s.d;
 
   const short D = 8;
 
   for (short i = 0; i < D; ++i)
-    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
   for (short i = D; i < 2 * D; ++i)
-    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = (std::numeric_limits<CoalScalar>::max)();
 
-  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(0) = bv.dist(D) = d;
     else
       bv.dist(0) = bv.dist(D) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(1) = bv.dist(D + 1) = d;
     else
       bv.dist(1) = bv.dist(D + 1) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) {
     if (n[2] > 0)
       bv.dist(2) = bv.dist(D + 2) = d;
     else
       bv.dist(2) = bv.dist(D + 2) = -d;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) {
     bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) {
     bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) {
     bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) {
     bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) {
     bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
   }
 }
 
 template <>
-void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf,
+void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3s& tf,
                                 KDOP<18>& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   Plane new_s = transform(s, tf);
-  const Vec3f& n = new_s.n;
-  const FCL_REAL& d = new_s.d;
+  const Vec3s& n = new_s.n;
+  const CoalScalar& d = new_s.d;
 
   const short D = 9;
 
   for (short i = 0; i < D; ++i)
-    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
   for (short i = D; i < 2 * D; ++i)
-    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = (std::numeric_limits<CoalScalar>::max)();
 
-  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(0) = bv.dist(D) = d;
     else
       bv.dist(0) = bv.dist(D) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(1) = bv.dist(D + 1) = d;
     else
       bv.dist(1) = bv.dist(D + 1) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) {
     if (n[2] > 0)
       bv.dist(2) = bv.dist(D + 2) = d;
     else
       bv.dist(2) = bv.dist(D + 2) = -d;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) {
     bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) {
     bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) {
     bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) {
     bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) {
     bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) {
     bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
   }
 }
 
 template <>
-void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf,
+void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3s& tf,
                                 KDOP<24>& bv) {
   if (s.getSweptSphereRadius() > 0) {
-    HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.",
-                         std::runtime_error);
+    COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
+                      std::runtime_error);
   }
   Plane new_s = transform(s, tf);
-  const Vec3f& n = new_s.n;
-  const FCL_REAL& d = new_s.d;
+  const Vec3s& n = new_s.n;
+  const CoalScalar& d = new_s.d;
 
   const short D = 12;
 
   for (short i = 0; i < D; ++i)
-    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
   for (short i = D; i < 2 * D; ++i)
-    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+    bv.dist(i) = (std::numeric_limits<CoalScalar>::max)();
 
-  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[0] > 0)
       bv.dist(0) = bv.dist(D) = d;
     else
       bv.dist(0) = bv.dist(D) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) {
     if (n[1] > 0)
       bv.dist(1) = bv.dist(D + 1) = d;
     else
       bv.dist(1) = bv.dist(D + 1) = -d;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) {
     if (n[2] > 0)
       bv.dist(2) = bv.dist(D + 2) = d;
     else
       bv.dist(2) = bv.dist(D + 2) = -d;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) {
     bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) {
     bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) {
     bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
-  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
+  } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) {
     bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
-  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) {
     bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
-  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) {
     bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
-  } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
+  } else if (n[0] + n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) {
     bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
-  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] + n[1] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) {
     bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
-  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
+  } else if (n[0] + n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) {
     bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
   }
 }
 
-void constructBox(const AABB& bv, Box& box, Transform3f& tf) {
+void constructBox(const AABB& bv, Box& box, Transform3s& tf) {
   box = Box(bv.max_ - bv.min_);
-  tf = Transform3f(bv.center());
+  tf = Transform3s(bv.center());
 }
 
-void constructBox(const OBB& bv, Box& box, Transform3f& tf) {
+void constructBox(const OBB& bv, Box& box, Transform3s& tf) {
   box = Box(bv.extent * 2);
-  tf = Transform3f(bv.axes, bv.To);
+  tf = Transform3s(bv.axes, bv.To);
 }
 
-void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) {
+void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf) {
   box = Box(bv.obb.extent * 2);
-  tf = Transform3f(bv.obb.axes, bv.obb.To);
+  tf = Transform3s(bv.obb.axes, bv.obb.To);
 }
 
-void constructBox(const kIOS& bv, Box& box, Transform3f& tf) {
+void constructBox(const kIOS& bv, Box& box, Transform3s& tf) {
   box = Box(bv.obb.extent * 2);
-  tf = Transform3f(bv.obb.axes, bv.obb.To);
+  tf = Transform3s(bv.obb.axes, bv.obb.To);
 }
 
-void constructBox(const RSS& bv, Box& box, Transform3f& tf) {
+void constructBox(const RSS& bv, Box& box, Transform3s& tf) {
   box = Box(bv.width(), bv.height(), bv.depth());
-  tf = Transform3f(bv.axes, bv.Tr);
+  tf = Transform3s(bv.axes, bv.Tr);
 }
 
-void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) {
+void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf) {
   box = Box(bv.width(), bv.height(), bv.depth());
-  tf = Transform3f(bv.center());
+  tf = Transform3s(bv.center());
 }
 
-void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) {
+void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf) {
   box = Box(bv.width(), bv.height(), bv.depth());
-  tf = Transform3f(bv.center());
+  tf = Transform3s(bv.center());
 }
 
-void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) {
+void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf) {
   box = Box(bv.width(), bv.height(), bv.depth());
-  tf = Transform3f(bv.center());
+  tf = Transform3s(bv.center());
 }
 
-void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box,
-                  Transform3f& tf) {
+void constructBox(const AABB& bv, const Transform3s& tf_bv, Box& box,
+                  Transform3s& tf) {
   box = Box(bv.max_ - bv.min_);
-  tf = tf_bv * Transform3f(bv.center());
+  tf = tf_bv * Transform3s(bv.center());
 }
 
-void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box,
-                  Transform3f& tf) {
+void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box,
+                  Transform3s& tf) {
   box = Box(bv.extent * 2);
-  tf = tf_bv * Transform3f(bv.axes, bv.To);
+  tf = tf_bv * Transform3s(bv.axes, bv.To);
 }
 
-void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box,
-                  Transform3f& tf) {
+void constructBox(const OBBRSS& bv, const Transform3s& tf_bv, Box& box,
+                  Transform3s& tf) {
   box = Box(bv.obb.extent * 2);
-  tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
+  tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To);
 }
 
-void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box,
-                  Transform3f& tf) {
+void constructBox(const kIOS& bv, const Transform3s& tf_bv, Box& box,
+                  Transform3s& tf) {
   box = Box(bv.obb.extent * 2);
-  tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
+  tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To);
 }
 
-void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box,
-                  Transform3f& tf) {
+void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box,
+                  Transform3s& tf) {
   box = Box(bv.width(), bv.height(), bv.depth());
-  tf = tf_bv * Transform3f(bv.axes, bv.Tr);
+  tf = tf_bv * Transform3s(bv.axes, bv.Tr);
 }
 
-void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box,
-                  Transform3f& tf) {
+void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv, Box& box,
+                  Transform3s& tf) {
   box = Box(bv.width(), bv.height(), bv.depth());
-  tf = tf_bv * Transform3f(bv.center());
+  tf = tf_bv * Transform3s(bv.center());
 }
 
-void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box,
-                  Transform3f& tf) {
+void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv, Box& box,
+                  Transform3s& tf) {
   box = Box(bv.width(), bv.height(), bv.depth());
-  tf = tf_bv * Transform3f(bv.center());
+  tf = tf_bv * Transform3s(bv.center());
 }
 
-void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box,
-                  Transform3f& tf) {
+void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv, Box& box,
+                  Transform3s& tf) {
   box = Box(bv.width(), bv.height(), bv.depth());
-  tf = tf_bv * Transform3f(bv.center());
+  tf = tf_bv * Transform3s(bv.center());
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/traits_traversal.h b/src/traits_traversal.h
index 73282d36da2b809e726eb8d5e5a3c8dea5a8bc9f..5667a69c67a6db3fa8ea185e3f24434d38339eb6 100644
--- a/src/traits_traversal.h
+++ b/src/traits_traversal.h
@@ -7,54 +7,53 @@
 
 /** \author Lucile Remigy, Joseph Mirabel */
 
-#include <hpp/fcl/collision_func_matrix.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
+#include "coal/collision_func_matrix.h"
+#include "coal/narrowphase/narrowphase.h"
 #include <../src/collision_node.h>
-#include <hpp/fcl/internal/traversal_node_setup.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/traversal_node_setup.h"
+#include "coal/internal/shape_shape_func.h"
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 // TraversalTraitsCollision for collision_func_matrix.cpp
 
 template <typename TypeA, typename TypeB>
-struct HPP_FCL_LOCAL TraversalTraitsCollision{};
+struct COAL_LOCAL TraversalTraitsCollision{};
 
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
 
 template <typename T_SH>
-struct HPP_FCL_LOCAL TraversalTraitsCollision<T_SH, OcTree> {
+struct COAL_LOCAL TraversalTraitsCollision<T_SH, OcTree> {
   typedef ShapeOcTreeCollisionTraversalNode<T_SH> CollisionTraversal_t;
 };
 
 template <typename T_SH>
-struct HPP_FCL_LOCAL TraversalTraitsCollision<OcTree, T_SH> {
+struct COAL_LOCAL TraversalTraitsCollision<OcTree, T_SH> {
   typedef OcTreeShapeCollisionTraversalNode<T_SH> CollisionTraversal_t;
 };
 
 template <>
-struct HPP_FCL_LOCAL TraversalTraitsCollision<OcTree, OcTree> {
+struct COAL_LOCAL TraversalTraitsCollision<OcTree, OcTree> {
   typedef OcTreeCollisionTraversalNode CollisionTraversal_t;
 };
 
 template <typename T_BVH>
-struct HPP_FCL_LOCAL TraversalTraitsCollision<OcTree, BVHModel<T_BVH> > {
+struct COAL_LOCAL TraversalTraitsCollision<OcTree, BVHModel<T_BVH> > {
   typedef OcTreeMeshCollisionTraversalNode<T_BVH> CollisionTraversal_t;
 };
 
 template <typename T_BVH>
-struct HPP_FCL_LOCAL TraversalTraitsCollision<BVHModel<T_BVH>, OcTree> {
+struct COAL_LOCAL TraversalTraitsCollision<BVHModel<T_BVH>, OcTree> {
   typedef MeshOcTreeCollisionTraversalNode<T_BVH> CollisionTraversal_t;
 };
 
 template <typename T_HF>
-struct HPP_FCL_LOCAL TraversalTraitsCollision<OcTree, HeightField<T_HF> > {
+struct COAL_LOCAL TraversalTraitsCollision<OcTree, HeightField<T_HF> > {
   typedef OcTreeHeightFieldCollisionTraversalNode<T_HF> CollisionTraversal_t;
 };
 
 template <typename T_HF>
-struct HPP_FCL_LOCAL TraversalTraitsCollision<HeightField<T_HF>, OcTree> {
+struct COAL_LOCAL TraversalTraitsCollision<HeightField<T_HF>, OcTree> {
   typedef HeightFieldOcTreeCollisionTraversalNode<T_HF> CollisionTraversal_t;
 };
 
@@ -63,37 +62,35 @@ struct HPP_FCL_LOCAL TraversalTraitsCollision<HeightField<T_HF>, OcTree> {
 // TraversalTraitsDistance for distance_func_matrix.cpp
 
 template <typename TypeA, typename TypeB>
-struct HPP_FCL_LOCAL TraversalTraitsDistance{};
+struct COAL_LOCAL TraversalTraitsDistance{};
 
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
 
 template <typename T_SH>
-struct HPP_FCL_LOCAL TraversalTraitsDistance<T_SH, OcTree> {
+struct COAL_LOCAL TraversalTraitsDistance<T_SH, OcTree> {
   typedef ShapeOcTreeDistanceTraversalNode<T_SH> CollisionTraversal_t;
 };
 
 template <typename T_SH>
-struct HPP_FCL_LOCAL TraversalTraitsDistance<OcTree, T_SH> {
+struct COAL_LOCAL TraversalTraitsDistance<OcTree, T_SH> {
   typedef OcTreeShapeDistanceTraversalNode<T_SH> CollisionTraversal_t;
 };
 
 template <>
-struct HPP_FCL_LOCAL TraversalTraitsDistance<OcTree, OcTree> {
+struct COAL_LOCAL TraversalTraitsDistance<OcTree, OcTree> {
   typedef OcTreeDistanceTraversalNode CollisionTraversal_t;
 };
 
 template <typename T_BVH>
-struct HPP_FCL_LOCAL TraversalTraitsDistance<OcTree, BVHModel<T_BVH> > {
+struct COAL_LOCAL TraversalTraitsDistance<OcTree, BVHModel<T_BVH> > {
   typedef OcTreeMeshDistanceTraversalNode<T_BVH> CollisionTraversal_t;
 };
 
 template <typename T_BVH>
-struct HPP_FCL_LOCAL TraversalTraitsDistance<BVHModel<T_BVH>, OcTree> {
+struct COAL_LOCAL TraversalTraitsDistance<BVHModel<T_BVH>, OcTree> {
   typedef MeshOcTreeDistanceTraversalNode<T_BVH> CollisionTraversal_t;
 };
 
 #endif
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp
index 082b6c31ab783983d58625250253d75e4f5056d7..2b133c5a6487d1fa7bfda8cbaf3936f0ddf0cb33 100644
--- a/src/traversal/traversal_recurse.cpp
+++ b/src/traversal/traversal_recurse.cpp
@@ -35,16 +35,15 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/internal/traversal_recurse.h>
+#include "coal/internal/traversal_recurse.h"
 
 #include <vector>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1,
                       unsigned int b2, BVHFrontList* front_list,
-                      FCL_REAL& sqrDistLowerBound) {
-  FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
+                      CoalScalar& sqrDistLowerBound) {
+  CoalScalar sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
   bool l1 = node->isFirstNodeLeaf(b1);
   bool l2 = node->isSecondNodeLeaf(b2);
   if (l1 && l2) {
@@ -86,15 +85,15 @@ void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1,
 
 void collisionNonRecurse(CollisionTraversalNodeBase* node,
                          BVHFrontList* front_list,
-                         FCL_REAL& sqrDistLowerBound) {
+                         CoalScalar& sqrDistLowerBound) {
   typedef std::pair<unsigned int, unsigned int> BVPair_t;
   // typedef std::stack<BVPair_t, std::vector<BVPair_t> > Stack_t;
   typedef std::vector<BVPair_t> Stack_t;
 
   Stack_t pairs;
   pairs.reserve(1000);
-  sqrDistLowerBound = std::numeric_limits<FCL_REAL>::infinity();
-  FCL_REAL sdlb = std::numeric_limits<FCL_REAL>::infinity();
+  sqrDistLowerBound = std::numeric_limits<CoalScalar>::infinity();
+  CoalScalar sdlb = std::numeric_limits<CoalScalar>::infinity();
 
   pairs.push_back(BVPair_t(0, 0));
 
@@ -176,8 +175,8 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
     c2 = (unsigned int)node->getSecondRightChild(b2);
   }
 
-  FCL_REAL d1 = node->BVDistanceLowerBound(a1, a2);
-  FCL_REAL d2 = node->BVDistanceLowerBound(c1, c2);
+  CoalScalar d1 = node->BVDistanceLowerBound(a1, a2);
+  CoalScalar d2 = node->BVDistanceLowerBound(c1, c2);
 
   if (d2 < d1) {
     if (!node->canStop(d2))
@@ -203,21 +202,22 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
 }
 
 /** @brief Bounding volume test structure */
-struct HPP_FCL_LOCAL BVT {
+struct COAL_LOCAL BVT {
   /** @brief distance between bvs */
-  FCL_REAL d;
+  CoalScalar d;
 
   /** @brief bv indices for a pair of bvs in two models */
   unsigned int b1, b2;
 };
 
 /** @brief Comparer between two BVT */
-struct HPP_FCL_LOCAL BVT_Comparer{
-    bool operator()(const BVT& lhs, const BVT& rhs) const {return lhs.d > rhs.d;
-}  // namespace fcl
-};  // namespace hpp
+struct COAL_LOCAL BVT_Comparer{bool operator()(const BVT& lhs, const BVT& rhs)
+                                   const {return lhs.d > rhs.d;
+}  // namespace coal
+}
+;
 
-struct HPP_FCL_LOCAL BVTQ {
+struct COAL_LOCAL BVTQ {
   BVTQ() : qsize(2) {}
 
   bool empty() const { return pq.empty(); }
@@ -308,8 +308,8 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
                                            const CollisionRequest& /*request*/,
                                            CollisionResult& result,
                                            BVHFrontList* front_list) {
-  FCL_REAL sqrDistLowerBound = -1, sqrDistLowerBound1 = 0,
-           sqrDistLowerBound2 = 0;
+  CoalScalar sqrDistLowerBound = -1, sqrDistLowerBound1 = 0,
+             sqrDistLowerBound2 = 0;
   BVHFrontList::iterator front_iter;
   BVHFrontList append;
   for (front_iter = front_list->begin(); front_iter != front_list->end();
@@ -359,6 +359,4 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
   }
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2891ad7893d67ef12098e5dc0921b49354b8c563..c85077bb0135f2c43ffb40a9cbf2268733181f12 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -3,80 +3,83 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework filesystem)
 
 config_files(fcl_resources/config.h)
 
-macro(add_fcl_test test_name source)
-  ADD_UNIT_TEST(${test_name} ${source})
-  target_link_libraries(${test_name}
+function(add_coal_test test_name source)
+  set(target_name ${PROJECT_NAME}-${test_name})
+  ADD_UNIT_TEST(${target_name} ${source})
+  target_link_libraries(${target_name}
     PUBLIC
-    hpp-fcl
+    ${LIBRARY_NAME}
     Boost::filesystem
-    utility
+    ${utility_target}
     )
   IF(NOT WIN32)
-    target_compile_options(${test_name} PRIVATE "-Wno-c99-extensions")
-  ENDIF(NOT WIN32)
-  if(HPP_FCL_HAS_QHULL)
-    target_compile_options(${test_name} PRIVATE -DHPP_FCL_HAS_QHULL)
+    target_compile_options(${target_name} PRIVATE "-Wno-c99-extensions")
+  ENDIF()
+  if(COAL_HAS_QHULL)
+    target_compile_options(${target_name} PRIVATE -DCOAL_HAS_QHULL)
   endif()
-endmacro(add_fcl_test)
+endfunction()
 
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
 
-add_library(utility STATIC utility.cpp)
-target_link_libraries(utility PUBLIC ${PROJECT_NAME})
+set(utility_target ${PROJECT_NAME}-utility)
+add_library(${utility_target} STATIC utility.cpp)
+target_link_libraries(${utility_target} PUBLIC ${PROJECT_NAME})
 
-add_fcl_test(math math.cpp)
+add_coal_test(math math.cpp)
 
-add_fcl_test(collision collision.cpp)
-add_fcl_test(contact_patch contact_patch.cpp)
-add_fcl_test(distance distance.cpp)
-add_fcl_test(swept_sphere_radius swept_sphere_radius.cpp)
-add_fcl_test(normal_and_nearest_points normal_and_nearest_points.cpp)
-add_fcl_test(distance_lower_bound distance_lower_bound.cpp)
-add_fcl_test(security_margin security_margin.cpp)
-add_fcl_test(geometric_shapes geometric_shapes.cpp)
-add_fcl_test(shape_inflation shape_inflation.cpp)
-#add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp)
-add_fcl_test(gjk_asserts gjk_asserts.cpp)
-add_fcl_test(frontlist frontlist.cpp)
-SET_TESTS_PROPERTIES(frontlist PROPERTIES TIMEOUT 7200)
+add_coal_test(collision collision.cpp)
+add_coal_test(contact_patch contact_patch.cpp)
+add_coal_test(distance distance.cpp)
+add_coal_test(swept_sphere_radius swept_sphere_radius.cpp)
+add_coal_test(normal_and_nearest_points normal_and_nearest_points.cpp)
+add_coal_test(distance_lower_bound distance_lower_bound.cpp)
+add_coal_test(security_margin security_margin.cpp)
+add_coal_test(geometric_shapes geometric_shapes.cpp)
+add_coal_test(shape_inflation shape_inflation.cpp)
+#add_coal_test(shape_mesh_consistency shape_mesh_consistency.cpp)
+add_coal_test(gjk_asserts gjk_asserts.cpp)
+add_coal_test(frontlist frontlist.cpp)
+SET_TESTS_PROPERTIES(${PROJECT_NAME}-frontlist PROPERTIES TIMEOUT 7200)
 
-# add_fcl_test(sphere_capsule sphere_capsule.cpp)
-add_fcl_test(capsule_capsule capsule_capsule.cpp)
-add_fcl_test(box_box_distance box_box_distance.cpp)
-add_fcl_test(box_box_collision box_box_collision.cpp)
-add_fcl_test(simple simple.cpp)
-add_fcl_test(capsule_box_1 capsule_box_1.cpp)
-add_fcl_test(capsule_box_2 capsule_box_2.cpp)
-add_fcl_test(obb obb.cpp)
-add_fcl_test(convex convex.cpp)
+# add_coal_test(sphere_capsule sphere_capsule.cpp)
+add_coal_test(capsule_capsule capsule_capsule.cpp)
+add_coal_test(box_box_distance box_box_distance.cpp)
+add_coal_test(box_box_collision box_box_collision.cpp)
+add_coal_test(simple simple.cpp)
+add_coal_test(capsule_box_1 capsule_box_1.cpp)
+add_coal_test(capsule_box_2 capsule_box_2.cpp)
+add_coal_test(obb obb.cpp)
+add_coal_test(convex convex.cpp)
 
-add_fcl_test(bvh_models bvh_models.cpp)
-add_fcl_test(collision_node_asserts collision_node_asserts.cpp)
-add_fcl_test(hfields hfields.cpp)
+add_coal_test(bvh_models bvh_models.cpp)
+add_coal_test(collision_node_asserts collision_node_asserts.cpp)
+add_coal_test(hfields hfields.cpp)
 
-add_fcl_test(profiling profiling.cpp)
+add_coal_test(profiling profiling.cpp)
 
-add_fcl_test(gjk gjk.cpp)
-add_fcl_test(accelerated_gjk accelerated_gjk.cpp)
-add_fcl_test(gjk_convergence_criterion gjk_convergence_criterion.cpp)
-if(HPP_FCL_HAS_OCTOMAP)
-  add_fcl_test(octree octree.cpp)
-endif(HPP_FCL_HAS_OCTOMAP)
+add_coal_test(gjk gjk.cpp)
+add_coal_test(accelerated_gjk accelerated_gjk.cpp)
+add_coal_test(gjk_convergence_criterion gjk_convergence_criterion.cpp)
+if(COAL_HAS_OCTOMAP)
+  add_coal_test(octree octree.cpp)
+endif(COAL_HAS_OCTOMAP)
 
-add_fcl_test(serialization serialization.cpp)
+add_coal_test(serialization serialization.cpp)
 
 # Broadphase
-add_fcl_test(broadphase broadphase.cpp)
-set_tests_properties(broadphase PROPERTIES WILL_FAIL TRUE)
-add_fcl_test(broadphase_dynamic_AABB_tree broadphase_dynamic_AABB_tree.cpp)
-add_fcl_test(broadphase_collision_1 broadphase_collision_1.cpp)
-add_fcl_test(broadphase_collision_2 broadphase_collision_2.cpp)
+add_coal_test(broadphase broadphase.cpp)
+set_tests_properties(${PROJECT_NAME}-broadphase PROPERTIES WILL_FAIL TRUE)
+add_coal_test(broadphase_dynamic_AABB_tree broadphase_dynamic_AABB_tree.cpp)
+add_coal_test(broadphase_collision_1 broadphase_collision_1.cpp)
+add_coal_test(broadphase_collision_2 broadphase_collision_2.cpp)
 
 ## Benchmark
-add_executable(test-benchmark benchmark.cpp)
-target_link_libraries(test-benchmark
+set(test_benchmark_target ${PROJECT_NAME}-test-benchmark)
+add_executable(${test_benchmark_target} benchmark.cpp)
+target_link_libraries(${test_benchmark_target}
   PUBLIC
-  utility
+  ${utility_target}
   Boost::filesystem
   ${PROJECT_NAME}
   )
diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp
index 67d430d2f0c059dfd1d7d7cd9ba4a47bee4e551a..367b49aa44aff82bf56a59fa114d27d088e2284c 100644
--- a/test/accelerated_gjk.cpp
+++ b/test/accelerated_gjk.cpp
@@ -34,32 +34,32 @@
 
 /** \author Louis Montaut */
 
-#define BOOST_TEST_MODULE FCL_NESTEROV_GJK
+#define BOOST_TEST_MODULE COAL_NESTEROV_GJK
 #include <boost/test/included/unit_test.hpp>
 
 #include <Eigen/Geometry>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/internal/tools.h>
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/internal/tools.h"
 
 #include "utility.h"
 
-using hpp::fcl::Box;
-using hpp::fcl::Capsule;
-using hpp::fcl::constructPolytopeFromEllipsoid;
-using hpp::fcl::Convex;
-using hpp::fcl::Ellipsoid;
-using hpp::fcl::FCL_REAL;
-using hpp::fcl::GJKSolver;
-using hpp::fcl::GJKVariant;
-using hpp::fcl::ShapeBase;
-using hpp::fcl::support_func_guess_t;
-using hpp::fcl::Transform3f;
-using hpp::fcl::Triangle;
-using hpp::fcl::Vec3f;
-using hpp::fcl::details::GJK;
-using hpp::fcl::details::MinkowskiDiff;
-using hpp::fcl::details::SupportOptions;
+using coal::Box;
+using coal::Capsule;
+using coal::CoalScalar;
+using coal::constructPolytopeFromEllipsoid;
+using coal::Convex;
+using coal::Ellipsoid;
+using coal::GJKSolver;
+using coal::GJKVariant;
+using coal::ShapeBase;
+using coal::support_func_guess_t;
+using coal::Transform3s;
+using coal::Triangle;
+using coal::Vec3s;
+using coal::details::GJK;
+using coal::details::MinkowskiDiff;
+using coal::details::SupportOptions;
 using std::size_t;
 
 BOOST_AUTO_TEST_CASE(set_gjk_variant) {
@@ -107,7 +107,7 @@ BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) {
 void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
   // Solvers
   unsigned int max_iterations = 128;
-  FCL_REAL tolerance = 1e-6;
+  CoalScalar tolerance = 1e-6;
   GJK gjk(max_iterations, tolerance);
   GJK gjk_nesterov(max_iterations, tolerance);
   gjk_nesterov.gjk_variant = GJKVariant::NesterovAcceleration;
@@ -119,13 +119,13 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
 
   // Generate random transforms
   size_t n = 1000;
-  FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.};
-  std::vector<Transform3f> transforms;
+  CoalScalar extents[] = {-3., -3., 0, 3., 3., 3.};
+  std::vector<Transform3s> transforms;
   generateRandomTransforms(extents, transforms, n);
-  Transform3f identity = Transform3f::Identity();
+  Transform3s identity = Transform3s::Identity();
 
   // Same init for both solvers
-  Vec3f init_guess = Vec3f(1, 0, 0);
+  Vec3s init_guess = Vec3s(1, 0, 0);
   support_func_guess_t init_support_guess;
   init_support_guess.setZero();
 
@@ -139,7 +139,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
     // Evaluate both solvers twice, make sure they give the same solution
     GJK::Status res_gjk_1 =
         gjk.evaluate(mink_diff, init_guess, init_support_guess);
-    Vec3f ray_gjk = gjk.ray;
+    Vec3s ray_gjk = gjk.ray;
     GJK::Status res_gjk_2 =
         gjk.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(res_gjk_1 == res_gjk_2);
@@ -150,7 +150,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
     // --------------
     GJK::Status res_nesterov_gjk_1 =
         gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess);
-    Vec3f ray_nesterov = gjk_nesterov.ray;
+    Vec3s ray_nesterov = gjk_nesterov.ray;
     GJK::Status res_nesterov_gjk_2 =
         gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(res_nesterov_gjk_1 == res_nesterov_gjk_2);
@@ -171,7 +171,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
     // ------------
     GJK::Status res_polyak_gjk_1 =
         gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess);
-    Vec3f ray_polyak = gjk_polyak.ray;
+    Vec3s ray_polyak = gjk_polyak.ray;
     GJK::Status res_polyak_gjk_2 =
         gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(res_polyak_gjk_1 == res_polyak_gjk_2);
diff --git a/test/benchmark.cpp b/test/benchmark.cpp
index 54a22e5b3b2bdef282d63a4df755e439ef60f78c..fd1e973c4b69db6969a8715c647b416e24e784f1 100644
--- a/test/benchmark.cpp
+++ b/test/benchmark.cpp
@@ -1,25 +1,25 @@
 // Copyright (c) 2016, Joseph Mirabel
 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
 //
-// This file is part of hpp-fcl.
-// hpp-fcl is free software: you can redistribute it
+// This file is part of Coal.
+// Coal is free software: you can redistribute it
 // and/or modify it under the terms of the GNU Lesser General Public
 // License as published by the Free Software Foundation, either version
 // 3 of the License, or (at your option) any later version.
 //
-// hpp-fcl is distributed in the hope that it will be
+// Coal is distributed in the hope that it will be
 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 // General Lesser Public License for more details.  You should have
 // received a copy of the GNU Lesser General Public License along with
-// hpp-fcl. If not, see <http://www.gnu.org/licenses/>.
+// Coal. If not, see <http://www.gnu.org/licenses/>.
 
 #include <boost/filesystem.hpp>
 
-#include <hpp/fcl/internal/traversal_node_setup.h>
-#include <hpp/fcl/internal/traversal_node_bvhs.h>
+#include "coal/internal/traversal_node_setup.h"
+#include "coal/internal/traversal_node_bvhs.h"
 #include "../src/collision_node.h"
-#include <hpp/fcl/internal/BV_splitter.h>
+#include "coal/internal/BV_splitter.h"
 
 #include "utility.h"
 #include "fcl_resources/config.h"
@@ -27,26 +27,26 @@
 #define RUN_CASE(BV, tf, models, split) \
   run<BV>(tf, models, split, #BV " - " #split ":\t")
 
-using namespace hpp::fcl;
+using namespace coal;
 
 bool verbose = false;
-FCL_REAL DELTA = 0.001;
+CoalScalar DELTA = 0.001;
 
 template <typename BV>
-void makeModel(const std::vector<Vec3f>& vertices,
+void makeModel(const std::vector<Vec3s>& vertices,
                const std::vector<Triangle>& triangles,
                SplitMethodType split_method, BVHModel<BV>& model);
 
 template <typename BV, typename TraversalNode>
-double distance(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
+double distance(const std::vector<Transform3s>& tf, const BVHModel<BV>& m1,
                 const BVHModel<BV>& m2, bool verbose);
 
 template <typename BV, typename TraversalNode>
-double collide(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
+double collide(const std::vector<Transform3s>& tf, const BVHModel<BV>& m1,
                const BVHModel<BV>& m2, bool verbose);
 
 template <typename BV>
-double run(const std::vector<Transform3f>& tf,
+double run(const std::vector<Transform3s>& tf,
            const BVHModel<BV> (&models)[2][3], int split_method,
            const char* sm_name);
 
@@ -78,7 +78,7 @@ struct traits<OBBRSS> {
 };
 
 template <typename BV>
-void makeModel(const std::vector<Vec3f>& vertices,
+void makeModel(const std::vector<Vec3s>& vertices,
                const std::vector<Triangle>& triangles,
                SplitMethodType split_method, BVHModel<BV>& model) {
   model.bv_splitter.reset(new BVSplitter<BV>(split_method));
@@ -90,9 +90,9 @@ void makeModel(const std::vector<Vec3f>& vertices,
 }
 
 template <typename BV, typename TraversalNode>
-double distance(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
+double distance(const std::vector<Transform3s>& tf, const BVHModel<BV>& m1,
                 const BVHModel<BV>& m2, bool verbose) {
-  Transform3f pose2;
+  Transform3s pose2;
 
   DistanceResult local_result;
   DistanceRequest request(true);
@@ -114,9 +114,9 @@ double distance(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
 }
 
 template <typename BV, typename TraversalNode>
-double collide(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
+double collide(const std::vector<Transform3s>& tf, const BVHModel<BV>& m1,
                const BVHModel<BV>& m2, bool verbose) {
-  Transform3f pose2;
+  Transform3s pose2;
 
   CollisionResult local_result;
   CollisionRequest request;
@@ -141,7 +141,7 @@ double collide(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
 }
 
 template <typename BV>
-double run(const std::vector<Transform3f>& tf,
+double run(const std::vector<Transform3s>& tf,
            const BVHModel<BV> (&models)[2][3], int split_method,
            const char* prefix) {
   double col = collide<BV, typename traits<BV>::CollisionTraversalNode>(
@@ -154,7 +154,7 @@ double run(const std::vector<Transform3f>& tf,
 }
 
 template <>
-double run<OBB>(const std::vector<Transform3f>& tf,
+double run<OBB>(const std::vector<Transform3s>& tf,
                 const BVHModel<OBB> (&models)[2][3], int split_method,
                 const char* prefix) {
   double col = collide<OBB, traits<OBB>::CollisionTraversalNode>(
@@ -166,7 +166,7 @@ double run<OBB>(const std::vector<Transform3f>& tf,
 }
 
 int main(int, char*[]) {
-  std::vector<Vec3f> p1, p2;
+  std::vector<Vec3s> p1, p2;
   std::vector<Triangle> t1, t2;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
@@ -207,8 +207,8 @@ int main(int, char*[]) {
             ms_obbrss[1][SPLIT_METHOD_BV_CENTER]);
   makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]);
 
-  std::vector<Transform3f> transforms;  // t0
-  FCL_REAL extents[] = {-3000, -3000, -3000, 3000, 3000, 3000};
+  std::vector<Transform3s> transforms;  // t0
+  CoalScalar extents[] = {-3000, -3000, -3000, 3000, 3000, 3000};
   std::size_t n = 10000;
 
   generateRandomTransforms(extents, transforms, n);
diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp
index b3276a18dc0b89f5324c4791bb0f12fd6ac97b0f..63c1f4fcf5a824cdf85bf31b383fca6563dd57a4 100644
--- a/test/box_box_collision.cpp
+++ b/test/box_box_collision.cpp
@@ -1,21 +1,21 @@
-#define BOOST_TEST_MODULE BOX_BOX_COLLISION
+#define BOOST_TEST_MODULE COAL_BOX_BOX_COLLISION
 #include <boost/test/included/unit_test.hpp>
 
 #include <Eigen/Geometry>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/internal/tools.h>
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/internal/tools.h"
 
 #include "utility.h"
 
-using hpp::fcl::Box;
-using hpp::fcl::collide;
-using hpp::fcl::CollisionRequest;
-using hpp::fcl::CollisionResult;
-using hpp::fcl::ComputeCollision;
-using hpp::fcl::FCL_REAL;
-using hpp::fcl::Transform3f;
-using hpp::fcl::Vec3f;
+using coal::Box;
+using coal::CoalScalar;
+using coal::collide;
+using coal::CollisionRequest;
+using coal::CollisionResult;
+using coal::ComputeCollision;
+using coal::Transform3s;
+using coal::Vec3s;
 
 BOOST_AUTO_TEST_CASE(box_box_collision) {
   // Define boxes
@@ -23,8 +23,8 @@ BOOST_AUTO_TEST_CASE(box_box_collision) {
   Box shape2(1, 1, 1);
 
   // Define transforms
-  Transform3f T1 = Transform3f::Identity();
-  Transform3f T2 = Transform3f::Identity();
+  Transform3s T1 = Transform3s::Identity();
+  Transform3s T2 = Transform3s::Identity();
 
   // Compute collision
   CollisionRequest req;
@@ -33,13 +33,13 @@ BOOST_AUTO_TEST_CASE(box_box_collision) {
   CollisionResult res;
   ComputeCollision collide_functor(&shape1, &shape2);
 
-  T1.setTranslation(Vec3f(0, 0, 0));
+  T1.setTranslation(Vec3s(0, 0, 0));
   res.clear();
   BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == true);
   res.clear();
   BOOST_CHECK(collide_functor(T1, T2, req, res) == true);
 
-  T1.setTranslation(Vec3f(2, 0, 0));
+  T1.setTranslation(Vec3s(2, 0, 0));
   res.clear();
   BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == false);
   res.clear();
diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp
index d0c272cd4a72929930568b277e97340d63444d3b..82ff5bfeeb66352a570d5566f5c308feadcdae45 100644
--- a/test/box_box_distance.cpp
+++ b/test/box_box_distance.cpp
@@ -37,34 +37,34 @@
 #define _USE_MATH_DEFINES
 #include <cmath>
 
-#define BOOST_TEST_MODULE FCL_BOX_BOX
+#define BOOST_TEST_MODULE COAL_BOX_BOX
 #include <boost/test/included/unit_test.hpp>
 
 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
 
 #include <cmath>
 #include <iostream>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/distance.h"
+#include "coal/math/transform.h"
+#include "coal/collision.h"
+#include "coal/collision_object.h"
+#include "coal/shape/geometric_shapes.h"
 
 #include "utility.h"
 
-using hpp::fcl::CollisionGeometryPtr_t;
-using hpp::fcl::CollisionObject;
-using hpp::fcl::DistanceRequest;
-using hpp::fcl::DistanceResult;
-using hpp::fcl::Transform3f;
-using hpp::fcl::Vec3f;
+using coal::CollisionGeometryPtr_t;
+using coal::CollisionObject;
+using coal::DistanceRequest;
+using coal::DistanceResult;
+using coal::Transform3s;
+using coal::Vec3s;
 
 BOOST_AUTO_TEST_CASE(distance_box_box_1) {
-  CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2));
-  CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2));
+  CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2));
+  CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2));
 
-  Transform3f tf1;
-  Transform3f tf2(Vec3f(25, 20, 5));
+  Transform3s tf1;
+  Transform3s tf2(Vec3s(25, 20, 5));
 
   CollisionObject o1(s1, tf1);
   CollisionObject o2(s2, tf2);
@@ -73,7 +73,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) {
   DistanceRequest distanceRequest(true, true, 0, 0);
   DistanceResult distanceResult;
 
-  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);
+  coal::distance(&o1, &o2, distanceRequest, distanceResult);
 
   std::cerr << "Applied transformations on two boxes" << std::endl;
   std::cerr << " T1 = " << tf1.getTranslation() << std::endl
@@ -87,8 +87,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) {
   double dy = 20 - 5 - 1;
   double dz = 5 - 1 - 1;
 
-  const Vec3f& p1 = distanceResult.nearest_points[0];
-  const Vec3f& p2 = distanceResult.nearest_points[1];
+  const Vec3s& p1 = distanceResult.nearest_points[0];
+  const Vec3s& p2 = distanceResult.nearest_points[1];
   BOOST_CHECK_CLOSE(distanceResult.min_distance,
                     sqrt(dx * dx + dy * dy + dz * dz), 1e-4);
 
@@ -101,14 +101,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) {
 }
 
 BOOST_AUTO_TEST_CASE(distance_box_box_2) {
-  CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2));
-  CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2));
+  CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2));
+  CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2));
   static double pi = M_PI;
-  Transform3f tf1;
-  Transform3f tf2(
-      hpp::fcl::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3),
-                         sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)),
-      Vec3f(0, 0, 10));
+  Transform3s tf1;
+  Transform3s tf2(coal::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3),
+                                 sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)),
+                  Vec3s(0, 0, 10));
 
   CollisionObject o1(s1, tf1);
   CollisionObject o2(s2, tf2);
@@ -117,7 +116,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) {
   DistanceRequest distanceRequest(true, true, 0, 0);
   DistanceResult distanceResult;
 
-  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);
+  coal::distance(&o1, &o2, distanceRequest, distanceResult);
 
   std::cerr << "Applied transformations on two boxes" << std::endl;
   std::cerr << " T1 = " << tf1.getTranslation() << std::endl
@@ -128,8 +127,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) {
             << ", p2 = " << distanceResult.nearest_points[1]
             << ", distance = " << distanceResult.min_distance << std::endl;
 
-  const Vec3f& p1 = distanceResult.nearest_points[0];
-  const Vec3f& p2 = distanceResult.nearest_points[1];
+  const Vec3s& p1 = distanceResult.nearest_points[0];
+  const Vec3s& p2 = distanceResult.nearest_points[1];
   double distance = -1.62123444 + 10 - 1;
   BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
 
@@ -142,13 +141,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) {
 }
 
 BOOST_AUTO_TEST_CASE(distance_box_box_3) {
-  CollisionGeometryPtr_t s1(new hpp::fcl::Box(1, 1, 1));
-  CollisionGeometryPtr_t s2(new hpp::fcl::Box(1, 1, 1));
+  CollisionGeometryPtr_t s1(new coal::Box(1, 1, 1));
+  CollisionGeometryPtr_t s2(new coal::Box(1, 1, 1));
   static double pi = M_PI;
-  Transform3f tf1(hpp::fcl::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)),
-                  Vec3f(-2, 1, .5));
-  Transform3f tf2(hpp::fcl::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0),
-                  Vec3f(2, .5, .5));
+  Transform3s tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)),
+                  Vec3s(-2, 1, .5));
+  Transform3s tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0),
+                  Vec3s(2, .5, .5));
 
   CollisionObject o1(s1, tf1);
   CollisionObject o2(s2, tf2);
@@ -157,7 +156,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
   DistanceRequest distanceRequest(true, 0, 0);
   DistanceResult distanceResult;
 
-  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);
+  coal::distance(&o1, &o2, distanceRequest, distanceResult);
 
   std::cerr << "Applied transformations on two boxes" << std::endl;
   std::cerr << " T1 = " << tf1.getTranslation() << std::endl
@@ -168,13 +167,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
             << ", p2 = " << distanceResult.nearest_points[1]
             << ", distance = " << distanceResult.min_distance << std::endl;
 
-  const Vec3f& p1 = distanceResult.nearest_points[0];
-  const Vec3f& p2 = distanceResult.nearest_points[1];
+  const Vec3s& p1 = distanceResult.nearest_points[0];
+  const Vec3s& p2 = distanceResult.nearest_points[1];
   double distance = 4 - sqrt(2);
   BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
 
-  const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5);
-  const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5);
+  const Vec3s p1Ref(sqrt(2) / 2 - 2, 1, .5);
+  const Vec3s p2Ref(2 - sqrt(2) / 2, 1, .5);
   BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4);
   BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4);
   BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4);
@@ -183,16 +182,16 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
   BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4);
 
   // Apply the same global transform to both objects and recompute
-  Transform3f tf3(hpp::fcl::makeQuat(0.435952844074, -0.718287018243,
-                                     0.310622451066, 0.444435113443),
-                  Vec3f(4, 5, 6));
+  Transform3s tf3(coal::makeQuat(0.435952844074, -0.718287018243,
+                                 0.310622451066, 0.444435113443),
+                  Vec3s(4, 5, 6));
   tf1 = tf3 * tf1;
   tf2 = tf3 * tf2;
   o1 = CollisionObject(s1, tf1);
   o2 = CollisionObject(s2, tf2);
 
   distanceResult.clear();
-  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);
+  coal::distance(&o1, &o2, distanceRequest, distanceResult);
 
   std::cerr << "Applied transformations on two boxes" << std::endl;
   std::cerr << " T1 = " << tf1.getTranslation() << std::endl
@@ -205,8 +204,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
 
   BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
 
-  const Vec3f p1Moved = tf3.transform(p1Ref);
-  const Vec3f p2Moved = tf3.transform(p2Ref);
+  const Vec3s p1Moved = tf3.transform(p1Ref);
+  const Vec3s p2Moved = tf3.transform(p2Ref);
   BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4);
   BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4);
   BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4);
@@ -216,38 +215,38 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
 }
 
 BOOST_AUTO_TEST_CASE(distance_box_box_4) {
-  hpp::fcl::Box s1(1, 1, 1);
-  hpp::fcl::Box s2(1, 1, 1);
+  coal::Box s1(1, 1, 1);
+  coal::Box s2(1, 1, 1);
 
   // Enable computation of nearest points
   DistanceRequest distanceRequest(true, true, 0, 0);
   DistanceResult distanceResult;
   double distance;
 
-  Transform3f tf1(Vec3f(2, 0, 0));
-  Transform3f tf2;
-  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
+  Transform3s tf1(Vec3s(2, 0, 0));
+  Transform3s tf2;
+  coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
 
   distance = 1.;
   BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
 
-  tf1.setTranslation(Vec3f(1.01, 0, 0));
+  tf1.setTranslation(Vec3s(1.01, 0, 0));
   distanceResult.clear();
-  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
+  coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
 
   distance = 0.01;
   BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
 
-  tf1.setTranslation(Vec3f(0.99, 0, 0));
+  tf1.setTranslation(Vec3s(0.99, 0, 0));
   distanceResult.clear();
-  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
+  coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
 
   distance = -0.01;
   BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
 
-  tf1.setTranslation(Vec3f(0, 0, 0));
+  tf1.setTranslation(Vec3s(0, 0, 0));
   distanceResult.clear();
-  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
+  coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
 
   distance = -1;
   BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
diff --git a/test/broadphase.cpp b/test/broadphase.cpp
index 65b96cff6f1a67eae27b65d6fecdbc84b8828a84..a28b85636ce85e4aab55aef788faa8549d773edd 100644
--- a/test/broadphase.cpp
+++ b/test/broadphase.cpp
@@ -35,13 +35,13 @@
 
 /** \author Jia Pan */
 
-#define BOOST_TEST_MODULE FCL_BROADPHASE
+#define BOOST_TEST_MODULE COAL_BROADPHASE
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/config.hh>
-#include <hpp/fcl/broadphase/broadphase.h>
-#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
-#include <hpp/fcl/math/transform.h>
+#include "coal/config.hh"
+#include "coal/broadphase/broadphase.h"
+#include "coal/shape/geometric_shape_to_BVH_model.h"
+#include "coal/math/transform.h"
 #include "utility.h"
 
 #if USE_GOOGLEHASH
@@ -54,8 +54,8 @@
 #include <iostream>
 #include <iomanip>
 
-using namespace hpp::fcl;
-using namespace hpp::fcl::detail;
+using namespace coal;
+using namespace coal::detail;
 
 /// @brief Generate environment with 3 * n objects for self distance, so we try
 /// to make sure none of them collide with each other.
@@ -75,7 +75,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size,
 void broad_phase_self_distance_test(double env_scale, std::size_t env_size,
                                     bool use_mesh = false);
 
-FCL_REAL DELTA = 0.01;
+CoalScalar DELTA = 0.01;
 
 #if USE_GOOGLEHASH
 template <typename U, typename V>
@@ -146,9 +146,9 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
                                       double env_scale, std::size_t n) {
   int n_edge = static_cast<int>(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;
+  CoalScalar step_size = env_scale * 2 / n_edge;
+  CoalScalar delta_size = step_size * 0.05;
+  CoalScalar single_size = step_size - 2 * delta_size;
 
   int i = 0;
   for (; i < n_edge * n_edge * n_edge / 4; ++i) {
@@ -159,7 +159,7 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
     Box* box = new Box(single_size, single_size, single_size);
     env.push_back(new CollisionObject(
         shared_ptr<CollisionGeometry>(box),
-        Transform3f(Vec3f(
+        Transform3s(Vec3s(
             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))));
@@ -174,7 +174,7 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
     Sphere* sphere = new Sphere(single_size / 2);
     env.push_back(new CollisionObject(
         shared_ptr<CollisionGeometry>(sphere),
-        Transform3f(Vec3f(
+        Transform3s(Vec3s(
             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))));
@@ -189,7 +189,7 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
     Cylinder* cylinder = new Cylinder(single_size / 2, single_size);
     env.push_back(new CollisionObject(
         shared_ptr<CollisionGeometry>(cylinder),
-        Transform3f(Vec3f(
+        Transform3s(Vec3s(
             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))));
@@ -204,7 +204,7 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
     Cone* cone = new Cone(single_size / 2, single_size);
     env.push_back(new CollisionObject(
         shared_ptr<CollisionGeometry>(cone),
-        Transform3f(Vec3f(
+        Transform3s(Vec3s(
             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))));
@@ -216,9 +216,9 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
                                           double env_scale, std::size_t n) {
   int n_edge = static_cast<int>(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;
+  CoalScalar step_size = env_scale * 2 / n_edge;
+  CoalScalar delta_size = step_size * 0.05;
+  CoalScalar single_size = step_size - 2 * delta_size;
 
   int i = 0;
   for (; i < n_edge * n_edge * n_edge / 4; ++i) {
@@ -228,10 +228,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
 
     Box box(single_size, single_size, single_size);
     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
-    generateBVHModel(*model, box, Transform3f());
+    generateBVHModel(*model, box, Transform3s());
     env.push_back(new CollisionObject(
         shared_ptr<CollisionGeometry>(model),
-        Transform3f(Vec3f(
+        Transform3s(Vec3s(
             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))));
@@ -245,10 +245,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
 
     Sphere sphere(single_size / 2);
     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
-    generateBVHModel(*model, sphere, Transform3f(), 16, 16);
+    generateBVHModel(*model, sphere, Transform3s(), 16, 16);
     env.push_back(new CollisionObject(
         shared_ptr<CollisionGeometry>(model),
-        Transform3f(Vec3f(
+        Transform3s(Vec3s(
             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))));
@@ -262,10 +262,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
 
     Cylinder cylinder(single_size / 2, single_size);
     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
-    generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
+    generateBVHModel(*model, cylinder, Transform3s(), 16, 16);
     env.push_back(new CollisionObject(
         shared_ptr<CollisionGeometry>(model),
-        Transform3f(Vec3f(
+        Transform3s(Vec3s(
             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))));
@@ -279,10 +279,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
 
     Cone cone(single_size / 2, single_size);
     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
-    generateBVHModel(*model, cone, Transform3f(), 16, 16);
+    generateBVHModel(*model, cone, Transform3s(), 16, 16);
     env.push_back(new CollisionObject(
         shared_ptr<CollisionGeometry>(model),
-        Transform3f(Vec3f(
+        Transform3s(Vec3s(
             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))));
@@ -308,11 +308,12 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size,
   managers.push_back(new SaPCollisionManager());
   managers.push_back(new IntervalTreeCollisionManager());
 
-  Vec3f lower_limit, upper_limit;
+  Vec3s 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);
+  CoalScalar 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<
@@ -457,9 +458,9 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size,
   managers.push_back(new SaPCollisionManager());
   managers.push_back(new IntervalTreeCollisionManager());
 
-  Vec3f lower_limit, upper_limit;
+  Vec3s lower_limit, upper_limit;
   SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
-  FCL_REAL cell_size =
+  CoalScalar 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);
@@ -564,7 +565,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size,
 
   std::cout << "distance time" << std::endl;
   for (size_t i = 0; i < ts.size(); ++i) {
-    FCL_REAL tmp = 0;
+    CoalScalar tmp = 0;
     for (size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
     std::cout << std::setw(w) << tmp << " ";
   }
diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp
index 4798e66d0f2e07e3d4dbc2b676b4eee1ae25684e..4afeda0c5f8d61ce6d4eb09f8201c6be8b50ce46 100644
--- a/test/broadphase_collision_1.cpp
+++ b/test/broadphase_collision_1.cpp
@@ -35,19 +35,19 @@
  */
 /** @author Jia Pan */
 
-#define BOOST_TEST_MODULE BROADPHASE_COLLISION_1
+#define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_1
 #include <boost/test/included/unit_test.hpp>
 
-#include "hpp/fcl/broadphase/broadphase_bruteforce.h"
-#include "hpp/fcl/broadphase/broadphase_spatialhash.h"
-#include "hpp/fcl/broadphase/broadphase_SaP.h"
-#include "hpp/fcl/broadphase/broadphase_SSaP.h"
-#include "hpp/fcl/broadphase/broadphase_interval_tree.h"
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
-#include "hpp/fcl/broadphase/default_broadphase_callbacks.h"
-#include "hpp/fcl/broadphase/detail/sparse_hash_table.h"
-#include "hpp/fcl/broadphase/detail/spatial_hash.h"
+#include "coal/broadphase/broadphase_bruteforce.h"
+#include "coal/broadphase/broadphase_spatialhash.h"
+#include "coal/broadphase/broadphase_SaP.h"
+#include "coal/broadphase/broadphase_SSaP.h"
+#include "coal/broadphase/broadphase_interval_tree.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
+#include "coal/broadphase/default_broadphase_callbacks.h"
+#include "coal/broadphase/detail/sparse_hash_table.h"
+#include "coal/broadphase/detail/spatial_hash.h"
 #include "utility.h"
 
 #include <boost/math/constants/constants.hpp>
@@ -61,15 +61,17 @@
 #include <iostream>
 #include <iomanip>
 
-using namespace hpp::fcl;
+using namespace coal;
 
 /// @brief make sure if broadphase algorithms doesn't check twice for the same
 /// collision object pair
-void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size,
+void broad_phase_duplicate_check_test(CoalScalar env_scale,
+                                      std::size_t env_size,
                                       bool verbose = false);
 
 /// @brief test for broad phase update
-void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size,
+void broad_phase_update_collision_test(CoalScalar env_scale,
+                                       std::size_t env_size,
                                        std::size_t query_size,
                                        std::size_t num_max_contacts = 1,
                                        bool exhaustive = false,
@@ -197,8 +199,8 @@ struct CollisionFunctionForUniquenessChecking : CollisionCallBackBase {
 };
 
 //==============================================================================
-void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size,
-                                      bool verbose) {
+void broad_phase_duplicate_check_test(CoalScalar env_scale,
+                                      std::size_t env_size, bool verbose) {
   std::vector<TStruct> ts;
   std::vector<BenchTimer> timers;
 
@@ -210,9 +212,9 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size,
   managers.push_back(new SSaPCollisionManager());
   managers.push_back(new SaPCollisionManager());
   managers.push_back(new IntervalTreeCollisionManager());
-  Vec3f lower_limit, upper_limit;
+  Vec3s lower_limit, upper_limit;
   SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
-  FCL_REAL cell_size =
+  CoalScalar 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);
@@ -264,30 +266,30 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size,
   }
 
   // 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;
+  CoalScalar delta_angle_max =
+      10 / 360.0 * 2 * boost::math::constants::pi<CoalScalar>();
+  CoalScalar 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;
-
-    Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) *
-                Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) *
-                Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ()));
-    Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
-
-    Matrix3f R = env[i]->getRotation();
-    Vec3f T = env[i]->getTranslation();
+    CoalScalar rand_angle_x =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max;
+    CoalScalar rand_trans_x =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max;
+    CoalScalar rand_angle_y =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max;
+    CoalScalar rand_trans_y =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max;
+    CoalScalar rand_angle_z =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max;
+    CoalScalar rand_trans_z =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max;
+
+    Matrix3s dR(Eigen::AngleAxisd(rand_angle_x, Vec3s::UnitX()) *
+                Eigen::AngleAxisd(rand_angle_y, Vec3s::UnitY()) *
+                Eigen::AngleAxisd(rand_angle_z, Vec3s::UnitZ()));
+    Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z);
+
+    Matrix3s R = env[i]->getRotation();
+    Vec3s T = env[i]->getTranslation();
     env[i]->setTransform(dR * R, dR * T + dT);
     env[i]->computeAABB();
   }
@@ -340,7 +342,7 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size,
 
   std::cout << "collision time" << std::endl;
   for (size_t i = 0; i < ts.size(); ++i) {
-    FCL_REAL tmp = 0;
+    CoalScalar tmp = 0;
     for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
     std::cout << std::setw(w) << tmp << " ";
   }
@@ -353,7 +355,8 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size,
   std::cout << std::endl;
 }
 
-void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size,
+void broad_phase_update_collision_test(CoalScalar env_scale,
+                                       std::size_t env_size,
                                        std::size_t query_size,
                                        std::size_t num_max_contacts,
                                        bool exhaustive, bool use_mesh) {
@@ -380,9 +383,9 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size,
   managers.push_back(new SaPCollisionManager());
   managers.push_back(new IntervalTreeCollisionManager());
 
-  Vec3f lower_limit, upper_limit;
+  Vec3s lower_limit, upper_limit;
   SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
-  FCL_REAL cell_size =
+  CoalScalar 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);
@@ -436,30 +439,30 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size,
   }
 
   // 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;
+  CoalScalar delta_angle_max =
+      10 / 360.0 * 2 * boost::math::constants::pi<CoalScalar>();
+  CoalScalar 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;
-
-    Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) *
-                Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) *
-                Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ()));
-    Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
-
-    Matrix3f R = env[i]->getRotation();
-    Vec3f T = env[i]->getTranslation();
+    CoalScalar rand_angle_x =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max;
+    CoalScalar rand_trans_x =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max;
+    CoalScalar rand_angle_y =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max;
+    CoalScalar rand_trans_y =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max;
+    CoalScalar rand_angle_z =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max;
+    CoalScalar rand_trans_z =
+        2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max;
+
+    Matrix3s dR(Eigen::AngleAxisd(rand_angle_x, Vec3s::UnitX()) *
+                Eigen::AngleAxisd(rand_angle_y, Vec3s::UnitY()) *
+                Eigen::AngleAxisd(rand_angle_z, Vec3s::UnitZ()));
+    Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z);
+
+    Matrix3s R = env[i]->getRotation();
+    Vec3s T = env[i]->getTranslation();
     env[i]->setTransform(dR * R, dR * T + dT);
     env[i]->computeAABB();
   }
@@ -578,7 +581,7 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size,
 
   std::cout << "collision time" << std::endl;
   for (size_t i = 0; i < ts.size(); ++i) {
-    FCL_REAL tmp = 0;
+    CoalScalar tmp = 0;
     for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
     std::cout << std::setw(w) << tmp << " ";
   }
diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp
index a6a66746593f6ade0fc24b90f6d4b3353e44541e..fe5465829bdf2dd9ebf906a3f802dba1d898b8bc 100644
--- a/test/broadphase_collision_2.cpp
+++ b/test/broadphase_collision_2.cpp
@@ -35,19 +35,19 @@
 
 /** @author Jia Pan */
 
-#define BOOST_TEST_MODULE BROADPHASE_COLLISION_2
+#define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_2
 #include <boost/test/included/unit_test.hpp>
 
-#include "hpp/fcl/broadphase/broadphase_bruteforce.h"
-#include "hpp/fcl/broadphase/broadphase_spatialhash.h"
-#include "hpp/fcl/broadphase/broadphase_SaP.h"
-#include "hpp/fcl/broadphase/broadphase_SSaP.h"
-#include "hpp/fcl/broadphase/broadphase_interval_tree.h"
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
-#include "hpp/fcl/broadphase/default_broadphase_callbacks.h"
-#include "hpp/fcl/broadphase/detail/sparse_hash_table.h"
-#include "hpp/fcl/broadphase/detail/spatial_hash.h"
+#include "coal/broadphase/broadphase_bruteforce.h"
+#include "coal/broadphase/broadphase_spatialhash.h"
+#include "coal/broadphase/broadphase_SaP.h"
+#include "coal/broadphase/broadphase_SSaP.h"
+#include "coal/broadphase/broadphase_interval_tree.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
+#include "coal/broadphase/default_broadphase_callbacks.h"
+#include "coal/broadphase/detail/sparse_hash_table.h"
+#include "coal/broadphase/detail/spatial_hash.h"
 #include "utility.h"
 
 #if USE_GOOGLEHASH
@@ -59,10 +59,10 @@
 #include <iostream>
 #include <iomanip>
 
-using namespace hpp::fcl;
+using namespace coal;
 
 /// @brief test for broad phase collision and self collision
-void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size,
+void broad_phase_collision_test(CoalScalar 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);
@@ -182,7 +182,7 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) {
 #endif
 }
 
-void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size,
+void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size,
                                 std::size_t query_size,
                                 std::size_t num_max_contacts, bool exhaustive,
                                 bool use_mesh) {
@@ -208,11 +208,11 @@ void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size,
   managers.push_back(new SaPCollisionManager());
   managers.push_back(new IntervalTreeCollisionManager());
 
-  Vec3f lower_limit, upper_limit;
+  Vec3s lower_limit, upper_limit;
   SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
-  // FCL_REAL ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0);
-  FCL_REAL ncell_per_axis = 20;
-  FCL_REAL cell_size =
+  // CoalScalar ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0);
+  CoalScalar ncell_per_axis = 20;
+  CoalScalar 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);
@@ -365,7 +365,7 @@ void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size,
 
   std::cout << "collision time" << std::endl;
   for (size_t i = 0; i < ts.size(); ++i) {
-    FCL_REAL tmp = 0;
+    CoalScalar tmp = 0;
     for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
     std::cout << std::setw(w) << tmp << " ";
   }
diff --git a/test/broadphase_dynamic_AABB_tree.cpp b/test/broadphase_dynamic_AABB_tree.cpp
index c16b09d15bd92eb3290e22c1d34be7ba13ddb4d1..f13c73e80e620222e583795bb980212c80fcb8f7 100644
--- a/test/broadphase_dynamic_AABB_tree.cpp
+++ b/test/broadphase_dynamic_AABB_tree.cpp
@@ -36,17 +36,17 @@
 
 /** Tests the dynamic axis-aligned bounding box tree.*/
 
-#include <iostream>
-#include <memory>
-
-#define BOOST_TEST_MODULE BROADPHASE_DYNAMIC_AABB_TREE
+#define BOOST_TEST_MODULE COAL_BROADPHASE_DYNAMIC_AABB_TREE
 #include <boost/test/included/unit_test.hpp>
 
-// #include "hpp/fcl/data_types.h"
-#include "hpp/fcl/shape/geometric_shapes.h"
-#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
+// #include "coal/data_types.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
+
+#include <iostream>
+#include <memory>
 
-using namespace hpp::fcl;
+using namespace coal;
 
 // Pack the data for callback function.
 struct CallBackData {
@@ -58,16 +58,16 @@ struct CallBackData {
 // the dynamic tree against the `data`. We assume that the first two
 // parameters are always objects[0] and objects[1] in two possible orders,
 // so we can safely ignore the second parameter. We do not use the last
-// FCL_REAL& parameter, which specifies the distance beyond which the
+// CoalScalar& parameter, which specifies the distance beyond which the
 // pair of objects will be skipped.
 
 struct DistanceCallBackDerived : DistanceCallBackBase {
-  bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist) {
+  bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist) {
     return distance_callback(o1, o2, &data, dist);
   }
 
   bool distance_callback(CollisionObject* a, CollisionObject*,
-                         void* callback_data, FCL_REAL&) {
+                         void* callback_data, CoalScalar&) {
     // Unpack the data.
     CallBackData* data = static_cast<CallBackData*>(callback_data);
     const std::vector<CollisionObject*>& objects = *(data->objects);
diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp
index 10c54a39ace52694a1754dacfd4b7d893b7305a3..74a946621e7b9ee6cbf2a26d2cc2f11e55e02901 100644
--- a/test/bvh_models.cpp
+++ b/test/bvh_models.cpp
@@ -35,32 +35,32 @@
 
 /** \author Jeongseok Lee */
 
-#define BOOST_TEST_MODULE FCL_BVH_MODELS
+#define BOOST_TEST_MODULE COAL_BVH_MODELS
 #include <boost/test/included/unit_test.hpp>
 #include <boost/filesystem.hpp>
 
 #include "fcl_resources/config.h"
 
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/BVH/BVH_utility.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
-#include <hpp/fcl/mesh_loader/assimp.h>
-#include <hpp/fcl/mesh_loader/loader.h>
+#include "coal/collision.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/BVH/BVH_utility.h"
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/shape/geometric_shape_to_BVH_model.h"
+#include "coal/mesh_loader/assimp.h"
+#include "coal/mesh_loader/loader.h"
 #include "utility.h"
 #include <iostream>
 
-using namespace hpp::fcl;
+using namespace coal;
 
 template <typename BV>
 void testBVHModelPointCloud() {
-  Box box(Vec3f::Ones());
+  Box box(Vec3s::Ones());
   double a = box.halfSide[0];
   double b = box.halfSide[1];
   double c = box.halfSide[2];
-  std::vector<Vec3f> points(8);
+  std::vector<Vec3s> points(8);
   points[0] << a, -b, c;
   points[1] << a, b, c;
   points[2] << -a, b, c;
@@ -114,7 +114,7 @@ void testBVHModelPointCloud() {
       return;
     }
 
-    Matrixx3f all_points((Eigen::DenseIndex)points.size(), 3);
+    MatrixX3s all_points((Eigen::DenseIndex)points.size(), 3);
     for (size_t k = 0; k < points.size(); ++k)
       all_points.row((Eigen::DenseIndex)k) = points[k].transpose();
 
@@ -139,13 +139,13 @@ void testBVHModelPointCloud() {
 template <typename BV>
 void testBVHModelTriangles() {
   shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-  Box box(Vec3f::Ones());
-  AABB aabb(Vec3f(-1, 0, -1), Vec3f(1, 1, 1));
+  Box box(Vec3s::Ones());
+  AABB aabb(Vec3s(-1, 0, -1), Vec3s(1, 1, 1));
 
   double a = box.halfSide[0];
   double b = box.halfSide[1];
   double c = box.halfSide[2];
-  std::vector<Vec3f> points(8);
+  std::vector<Vec3s> points(8);
   std::vector<Triangle> tri_indices(12);
   points[0] << a, -b, c;
   points[1] << a, b, c;
@@ -190,22 +190,22 @@ void testBVHModelTriangles() {
   BOOST_CHECK_EQUAL(model->num_tris, 12);
   BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
 
-  Transform3f pose;
+  Transform3s pose;
   shared_ptr<BVHModel<BV> > cropped(BVHExtract(*model, pose, aabb));
   BOOST_REQUIRE(cropped);
   BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
   BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
   BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
 
-  pose.setTranslation(Vec3f(0, 1, 0));
+  pose.setTranslation(Vec3s(0, 1, 0));
   cropped.reset(BVHExtract(*model, pose, aabb));
   BOOST_REQUIRE(cropped);
   BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
   BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
   BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
 
-  pose.setTranslation(Vec3f(0, 0, 0));
-  FCL_REAL sqrt2_2 = std::sqrt(2) / 2;
+  pose.setTranslation(Vec3s(0, 0, 0));
+  CoalScalar sqrt2_2 = std::sqrt(2) / 2;
   pose.setQuatRotation(Quatf(sqrt2_2, sqrt2_2, 0, 0));
   cropped.reset(BVHExtract(*model, pose, aabb));
   BOOST_REQUIRE(cropped);
@@ -213,13 +213,13 @@ void testBVHModelTriangles() {
   BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
   BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
 
-  pose.setTranslation(-Vec3f(1, 1, 1));
+  pose.setTranslation(-Vec3s(1, 1, 1));
   pose.setQuatRotation(Quatf::Identity());
   cropped.reset(BVHExtract(*model, pose, aabb));
   BOOST_CHECK(!cropped);
 
-  aabb = AABB(Vec3f(-0.1, -0.1, -0.1), Vec3f(0.1, 0.1, 0.1));
-  pose.setTranslation(Vec3f(-0.5, -0.5, 0));
+  aabb = AABB(Vec3s(-0.1, -0.1, -0.1), Vec3s(0.1, 0.1, 0.1));
+  pose.setTranslation(Vec3s(-0.5, -0.5, 0));
   cropped.reset(BVHExtract(*model, pose, aabb));
   BOOST_REQUIRE(cropped);
   BOOST_CHECK_EQUAL(cropped->num_tris, 2);
@@ -229,12 +229,12 @@ void testBVHModelTriangles() {
 template <typename BV>
 void testBVHModelSubModel() {
   shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-  Box box(Vec3f::Ones());
+  Box box(Vec3s::Ones());
 
   double a = box.halfSide[0];
   double b = box.halfSide[1];
   double c = box.halfSide[2];
-  std::vector<Vec3f> points(8);
+  std::vector<Vec3s> points(8);
   std::vector<Triangle> tri_indices(12);
   points[0] << a, -b, c;
   points[1] << a, b, c;
@@ -304,7 +304,7 @@ void testLoadPolyhedron() {
   typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
   PolyhedronPtr_t P1(new Polyhedron_t), P2;
 
-  Vec3f scale;
+  Vec3s scale;
   scale.setConstant(1);
   loadPolyhedronFromResource(env, scale, P1);
 
@@ -331,11 +331,11 @@ void testLoadGerardBauzil() {
   typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
   PolyhedronPtr_t P1(new Polyhedron_t), P2;
 
-  Vec3f scale;
+  Vec3s scale;
   scale.setConstant(1);
   loadPolyhedronFromResource(env, scale, P1);
   CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27));
-  Transform3f pos(Vec3f(-1.33, 1.36, .14));
+  Transform3s pos(Vec3s(-1.33, 1.36, .14));
   CollisionObject obj(cylinder, pos);
   CollisionObject stairs(P1);
 
@@ -373,10 +373,10 @@ BOOST_AUTO_TEST_CASE(load_illformated_mesh) {
 }
 
 BOOST_AUTO_TEST_CASE(test_convex) {
-  Box* box_ptr = new hpp::fcl::Box(1, 1, 1);
+  Box* box_ptr = new coal::Box(1, 1, 1);
   CollisionGeometryPtr_t b1(box_ptr);
   BVHModel<OBBRSS> box_bvh_model = BVHModel<OBBRSS>();
-  generateBVHModel(box_bvh_model, *box_ptr, Transform3f());
+  generateBVHModel(box_bvh_model, *box_ptr, Transform3s());
   box_bvh_model.buildConvexRepresentation(false);
 
   box_bvh_model.convex->computeLocalAABB();
diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp
index e86e5328ffc94572e4b8bb9b7ad5711e38dc6528..30b882b027b598b548f4fd17c2db627fe8fc8d6f 100644
--- a/test/capsule_box_1.cpp
+++ b/test/capsule_box_1.cpp
@@ -34,42 +34,42 @@
 
 /** \author Florent Lamiraux */
 
-#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES
+#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES
 #include <boost/test/included/unit_test.hpp>
 
 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
 
 #include <cmath>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/distance.h"
+#include "coal/math/transform.h"
+#include "coal/collision.h"
+#include "coal/collision_object.h"
+#include "coal/shape/geometric_shapes.h"
 
 #include "utility.h"
 
 BOOST_AUTO_TEST_CASE(distance_capsule_box) {
-  using hpp::fcl::CollisionGeometryPtr_t;
+  using coal::CollisionGeometryPtr_t;
   // Capsule of radius 2 and of height 4
-  CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.));
+  CollisionGeometryPtr_t capsuleGeometry(new coal::Capsule(2., 4.));
   // Box of size 1 by 2 by 4
-  CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.));
+  CollisionGeometryPtr_t boxGeometry(new coal::Box(1., 2., 4.));
 
   // Enable computation of nearest points
-  hpp::fcl::DistanceRequest distanceRequest(true, 0, 0);
-  hpp::fcl::DistanceResult distanceResult;
+  coal::DistanceRequest distanceRequest(true, 0, 0);
+  coal::DistanceResult distanceResult;
 
-  hpp::fcl::Transform3f tf1(hpp::fcl::Vec3f(3., 0, 0));
-  hpp::fcl::Transform3f tf2;
-  hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1);
-  hpp::fcl::CollisionObject box(boxGeometry, tf2);
+  coal::Transform3s tf1(coal::Vec3s(3., 0, 0));
+  coal::Transform3s tf2;
+  coal::CollisionObject capsule(capsuleGeometry, tf1);
+  coal::CollisionObject box(boxGeometry, tf2);
 
   // test distance
-  hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult);
+  coal::distance(&capsule, &box, distanceRequest, distanceResult);
   // Nearest point on capsule
-  hpp::fcl::Vec3f o1(distanceResult.nearest_points[0]);
+  coal::Vec3s o1(distanceResult.nearest_points[0]);
   // Nearest point on box
-  hpp::fcl::Vec3f o2(distanceResult.nearest_points[1]);
+  coal::Vec3s o2(distanceResult.nearest_points[1]);
   BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.5, 1e-1);
   BOOST_CHECK_CLOSE(o1[0], 1.0, 1e-1);
   CHECK_CLOSE_TO_0(o1[1], 1e-1);
@@ -77,12 +77,12 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) {
   CHECK_CLOSE_TO_0(o2[1], 1e-1);
 
   // Move capsule above box
-  tf1 = hpp::fcl::Transform3f(hpp::fcl::Vec3f(0., 0., 8.));
+  tf1 = coal::Transform3s(coal::Vec3s(0., 0., 8.));
   capsule.setTransform(tf1);
 
   // test distance
   distanceResult.clear();
-  hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult);
+  coal::distance(&capsule, &box, distanceRequest, distanceResult);
   o1 = distanceResult.nearest_points[0];
   o2 = distanceResult.nearest_points[1];
 
@@ -96,13 +96,13 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) {
   BOOST_CHECK_CLOSE(o2[2], 2.0, 1e-1);
 
   // Rotate capsule around y axis by pi/2 and move it behind box
-  tf1.setTranslation(hpp::fcl::Vec3f(-10., 0., 0.));
-  tf1.setQuatRotation(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0));
+  tf1.setTranslation(coal::Vec3s(-10., 0., 0.));
+  tf1.setQuatRotation(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0));
   capsule.setTransform(tf1);
 
   // test distance
   distanceResult.clear();
-  hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult);
+  coal::distance(&capsule, &box, distanceRequest, distanceResult);
   o1 = distanceResult.nearest_points[0];
   o2 = distanceResult.nearest_points[1];
 
diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp
index add71efd614d7a16f5e4c2dbe4c9608cdde7801b..bdf84d5922dd68718f2d3d578cb9a4eaa7797aa1 100644
--- a/test/capsule_box_2.cpp
+++ b/test/capsule_box_2.cpp
@@ -34,7 +34,7 @@
 
 /** \author Florent Lamiraux */
 
-#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES
+#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES
 #include <boost/test/included/unit_test.hpp>
 
 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
@@ -42,36 +42,35 @@
 #include "utility.h"
 
 #include <cmath>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/distance.h"
+#include "coal/math/transform.h"
+#include "coal/collision.h"
+#include "coal/collision_object.h"
+#include "coal/shape/geometric_shapes.h"
 
 BOOST_AUTO_TEST_CASE(distance_capsule_box) {
-  typedef hpp::fcl::shared_ptr<hpp::fcl::CollisionGeometry>
-      CollisionGeometryPtr_t;
+  typedef coal::shared_ptr<coal::CollisionGeometry> CollisionGeometryPtr_t;
   // Capsule of radius 2 and of height 4
-  CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.));
+  CollisionGeometryPtr_t capsuleGeometry(new coal::Capsule(2., 4.));
   // Box of size 1 by 2 by 4
-  CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.));
+  CollisionGeometryPtr_t boxGeometry(new coal::Box(1., 2., 4.));
 
   // Enable computation of nearest points
-  hpp::fcl::DistanceRequest distanceRequest(true, 0, 0);
-  hpp::fcl::DistanceResult distanceResult;
+  coal::DistanceRequest distanceRequest(true, 0, 0);
+  coal::DistanceResult distanceResult;
 
   // Rotate capsule around y axis by pi/2 and move it behind box
-  hpp::fcl::Transform3f tf1(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0),
-                            hpp::fcl::Vec3f(-10., 0.8, 1.5));
-  hpp::fcl::Transform3f tf2;
-  hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1);
-  hpp::fcl::CollisionObject box(boxGeometry, tf2);
+  coal::Transform3s tf1(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0),
+                        coal::Vec3s(-10., 0.8, 1.5));
+  coal::Transform3s tf2;
+  coal::CollisionObject capsule(capsuleGeometry, tf1);
+  coal::CollisionObject box(boxGeometry, tf2);
 
   // test distance
   distanceResult.clear();
-  hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult);
-  hpp::fcl::Vec3f o1 = distanceResult.nearest_points[0];
-  hpp::fcl::Vec3f o2 = distanceResult.nearest_points[1];
+  coal::distance(&capsule, &box, distanceRequest, distanceResult);
+  coal::Vec3s o1 = distanceResult.nearest_points[0];
+  coal::Vec3s o2 = distanceResult.nearest_points[1];
 
   BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-2);
   BOOST_CHECK_CLOSE(o1[0], -6, 1e-2);
diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp
index 25d95455d5e340e1bd5d4d1a7e3cd15501841cda..b3f3e53047c92ec7be3da87e8d6751cfafa43d81 100644
--- a/test/capsule_capsule.cpp
+++ b/test/capsule_capsule.cpp
@@ -35,23 +35,23 @@
 
 /** \author Karsten Knese <Karsten.Knese@googlemail.com> */
 
-#define BOOST_TEST_MODULE FCL_CAPSULE_CAPSULE
+#define BOOST_TEST_MODULE COAL_CAPSULE_CAPSULE
 #include <boost/test/included/unit_test.hpp>
 
 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
 
 #include <cmath>
 #include <iostream>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
+#include "coal/distance.h"
+#include "coal/collision.h"
+#include "coal/math/transform.h"
+#include "coal/collision.h"
+#include "coal/collision_object.h"
+#include "coal/shape/geometric_shapes.h"
 
 #include "utility.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
 BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) {
   const double radius = 1.;
@@ -68,8 +68,8 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) {
   int num_tests = 1e6;
 #endif
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
   for (int i = 0; i < num_tests; ++i) {
     Eigen::Vector3d p1 = Eigen::Vector3d::Random() * (2. * radius);
@@ -121,8 +121,8 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) {
   int num_tests = 1e6;
 #endif
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
   Eigen::Vector3d p1 = Eigen::Vector3d::Zero();
   Eigen::Vector3d p2_no_collision =
@@ -180,9 +180,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) {
 
   p2_no_collision = Eigen::Vector3d(0., 0., 2 * (length / 2. + radius) + 1e-3);
 
-  Transform3f geom1_placement(Eigen::Matrix3d::Identity(),
+  Transform3s geom1_placement(Eigen::Matrix3d::Identity(),
                               Eigen::Vector3d::Zero());
-  Transform3f geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision);
+  Transform3s geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision);
 
   for (int i = 0; i < num_tests; ++i) {
     Eigen::Matrix3d rot =
@@ -190,9 +190,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) {
             .toRotationMatrix();
     Eigen::Vector3d trans = Eigen::Vector3d::Random();
 
-    Transform3f displacement(rot, trans);
-    Transform3f tf1 = displacement * geom1_placement;
-    Transform3f tf2 = displacement * geom2_placement;
+    Transform3s displacement(rot, trans);
+    Transform3s tf1 = displacement * geom1_placement;
+    Transform3s tf2 = displacement * geom2_placement;
 
     CollisionObject capsule_o1(c1, tf1);
     CollisionObject capsule_o2(c2, tf2);
@@ -218,9 +218,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) {
             .toRotationMatrix();
     Eigen::Vector3d trans = Eigen::Vector3d::Random();
 
-    Transform3f displacement(rot, trans);
-    Transform3f tf1 = displacement * geom1_placement;
-    Transform3f tf2 = displacement * geom2_placement;
+    Transform3s displacement(rot, trans);
+    Transform3s tf1 = displacement * geom1_placement;
+    Transform3s tf2 = displacement * geom2_placement;
 
     CollisionObject capsule_o1(c1, tf1);
     CollisionObject capsule_o2(c2, tf2);
@@ -240,8 +240,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) {
   CollisionGeometryPtr_t s1(new Capsule(5, 10));
   CollisionGeometryPtr_t s2(new Capsule(5, 10));
 
-  Transform3f tf1;
-  Transform3f tf2(Vec3f(20.1, 0, 0));
+  Transform3s tf1;
+  Transform3s tf2(Vec3s(20.1, 0, 0));
 
   CollisionObject o1(s1, tf1);
   CollisionObject o2(s2, tf2);
@@ -266,8 +266,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) {
   CollisionGeometryPtr_t s1(new Capsule(5, 10));
   CollisionGeometryPtr_t s2(new Capsule(5, 10));
 
-  Transform3f tf1;
-  Transform3f tf2(Vec3f(20, 20, 0));
+  Transform3s tf1;
+  Transform3s tf2(Vec3s(20, 20, 0));
 
   CollisionObject o1(s1, tf1);
   CollisionObject o2(s2, tf2);
@@ -285,7 +285,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) {
             << ", p2 = " << distanceResult.nearest_points[1]
             << ", distance = " << distanceResult.min_distance << std::endl;
 
-  FCL_REAL expected = sqrt(800) - 10;
+  CoalScalar expected = sqrt(800) - 10;
   BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6);
 }
 
@@ -293,8 +293,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) {
   CollisionGeometryPtr_t s1(new Capsule(5, 10));
   CollisionGeometryPtr_t s2(new Capsule(5, 10));
 
-  Transform3f tf1;
-  Transform3f tf2(Vec3f(0, 0, 20.1));
+  Transform3s tf1;
+  Transform3s tf2(Vec3s(0, 0, 20.1));
 
   CollisionObject o1(s1, tf1);
   CollisionObject o2(s2, tf2);
@@ -319,8 +319,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) {
   CollisionGeometryPtr_t s1(new Capsule(5, 10));
   CollisionGeometryPtr_t s2(new Capsule(5, 10));
 
-  Transform3f tf1;
-  Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3f(0, 0, 25.1));
+  Transform3s tf1;
+  Transform3s tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3s(0, 0, 25.1));
 
   CollisionObject o1(s1, tf1);
   CollisionObject o2(s2, tf2);
@@ -343,8 +343,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) {
             << std::endl
             << "distance = " << distanceResult.min_distance << std::endl;
 
-  const Vec3f& p1 = distanceResult.nearest_points[0];
-  const Vec3f& p2 = distanceResult.nearest_points[1];
+  const Vec3s& p1 = distanceResult.nearest_points[0];
+  const Vec3s& p2 = distanceResult.nearest_points[1];
 
   BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
   CHECK_CLOSE_TO_0(p1[0], 1e-4);
diff --git a/test/collision.cpp b/test/collision.cpp
index 5b70aae4b17c4f9a73a5b106c261dcf7132d18d9..70c8d2810fcbad5e6e7c2131433b980eb727cbca 100644
--- a/test/collision.cpp
+++ b/test/collision.cpp
@@ -37,60 +37,60 @@
 
 #include <boost/mpl/vector.hpp>
 
-#define BOOST_TEST_MODULE FCL_COLLISION
+#define BOOST_TEST_MODULE COAL_COLLISION
 #include <boost/test/included/unit_test.hpp>
 
 #include <fstream>
 #include <boost/assign/list_of.hpp>
 
-#include <hpp/fcl/fwd.hh>
+#include "coal/fwd.hh"
 
-HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+COAL_COMPILER_DIAGNOSTIC_PUSH
+COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 
-#include <hpp/fcl/collision.h>
+#include "coal/collision.h"
 
-HPP_FCL_COMPILER_DIAGNOSTIC_POP
+COAL_COMPILER_DIAGNOSTIC_POP
 
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/mesh_loader/assimp.h>
+#include "coal/BV/BV.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/mesh_loader/assimp.h"
 
-#include <hpp/fcl/internal/traversal_node_bvhs.h>
-#include <hpp/fcl/internal/traversal_node_setup.h>
+#include "coal/internal/traversal_node_bvhs.h"
+#include "coal/internal/traversal_node_setup.h"
 #include "../src/collision_node.h"
-#include <hpp/fcl/internal/BV_splitter.h>
+#include "coal/internal/BV_splitter.h"
 
-#include <hpp/fcl/timings.h>
+#include "coal/timings.h"
 
 #include "utility.h"
 #include "fcl_resources/config.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 namespace utf = boost::unit_test::framework;
 
 int num_max_contacts = (std::numeric_limits<int>::max)();
 
 BOOST_AUTO_TEST_CASE(OBB_Box_test) {
-  FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
-  std::vector<Transform3f> rotate_transform;
+  CoalScalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
+  std::vector<Transform3s> rotate_transform;
   generateRandomTransforms(r_extents, rotate_transform, 1);
 
   AABB aabb1;
-  aabb1.min_ = Vec3f(-600, -600, -600);
-  aabb1.max_ = Vec3f(600, 600, 600);
+  aabb1.min_ = Vec3s(-600, -600, -600);
+  aabb1.max_ = Vec3s(600, 600, 600);
 
   OBB obb1;
   convertBV(aabb1, rotate_transform[0], obb1);
   Box box1;
-  Transform3f box1_tf;
+  Transform3s box1_tf;
   constructBox(aabb1, rotate_transform[0], box1, box1_tf);
 
-  FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
+  CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
   std::size_t n = 1000;
 
-  std::vector<Transform3f> transforms;
+  std::vector<Transform3s> transforms;
   generateRandomTransforms(extents, transforms, n);
 
   for (std::size_t i = 0; i < transforms.size(); ++i) {
@@ -102,7 +102,7 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) {
     convertBV(aabb, transforms[i], obb2);
 
     Box box2;
-    Transform3f box2_tf;
+    Transform3s box2_tf;
     constructBox(aabb, transforms[i], box2, box2_tf);
 
     GJKSolver solver;
@@ -123,28 +123,28 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) {
 }
 
 BOOST_AUTO_TEST_CASE(OBB_shape_test) {
-  FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
-  std::vector<Transform3f> rotate_transform;
+  CoalScalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
+  std::vector<Transform3s> rotate_transform;
   generateRandomTransforms(r_extents, rotate_transform, 1);
 
   AABB aabb1;
-  aabb1.min_ = Vec3f(-600, -600, -600);
-  aabb1.max_ = Vec3f(600, 600, 600);
+  aabb1.min_ = Vec3s(-600, -600, -600);
+  aabb1.max_ = Vec3s(600, 600, 600);
 
   OBB obb1;
   convertBV(aabb1, rotate_transform[0], obb1);
   Box box1;
-  Transform3f box1_tf;
+  Transform3s box1_tf;
   constructBox(aabb1, rotate_transform[0], box1, box1_tf);
 
-  FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
+  CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
   std::size_t n = 1000;
 
-  std::vector<Transform3f> transforms;
+  std::vector<Transform3s> transforms;
   generateRandomTransforms(extents, transforms, n);
 
   for (std::size_t i = 0; i < transforms.size(); ++i) {
-    FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5;
+    CoalScalar len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5;
     OBB obb2;
     GJKSolver solver;
 
@@ -206,18 +206,18 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) {
 }
 
 BOOST_AUTO_TEST_CASE(OBB_AABB_test) {
-  FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
+  CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
   std::size_t n = 1000;
 
-  std::vector<Transform3f> transforms;
+  std::vector<Transform3s> transforms;
   generateRandomTransforms(extents, transforms, n);
 
   AABB aabb1;
-  aabb1.min_ = Vec3f(-600, -600, -600);
-  aabb1.max_ = Vec3f(600, 600, 600);
+  aabb1.min_ = Vec3s(-600, -600, -600);
+  aabb1.max_ = Vec3s(600, 600, 600);
 
   OBB obb1;
-  convertBV(aabb1, Transform3f(), obb1);
+  convertBV(aabb1, Transform3s(), obb1);
 
   for (std::size_t i = 0; i < transforms.size(); ++i) {
     AABB aabb;
@@ -227,7 +227,7 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) {
     AABB aabb2 = translate(aabb, transforms[i].getTranslation());
 
     OBB obb2;
-    convertBV(aabb, Transform3f(transforms[i].getTranslation()), obb2);
+    convertBV(aabb, Transform3s(transforms[i].getTranslation()), obb2);
 
     bool overlap_aabb = aabb1.overlap(aabb2);
     bool overlap_obb = obb1.overlap(obb2);
@@ -308,11 +308,11 @@ struct traits<KDOP<N>, Oriented, recursive> : base_traits {
   enum { IS_IMPLEMENTED = false };
 };
 
-HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+COAL_COMPILER_DIAGNOSTIC_PUSH
+COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 
 struct mesh_mesh_run_test {
-  mesh_mesh_run_test(const std::vector<Transform3f>& _transforms,
+  mesh_mesh_run_test(const std::vector<Transform3s>& _transforms,
                      const CollisionRequest _request)
       : transforms(_transforms),
         request(_request),
@@ -321,7 +321,7 @@ struct mesh_mesh_run_test {
         isInit(false),
         indent(0) {}
 
-  const std::vector<Transform3f>& transforms;
+  const std::vector<Transform3s>& transforms;
   const CollisionRequest request;
   bool enable_statistics, benchmark;
   std::vector<Contacts_t> contacts;
@@ -330,7 +330,7 @@ struct mesh_mesh_run_test {
 
   int indent;
 
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
+  COAL_COMPILER_DIAGNOSTIC_POP
 
   const char* getindent() {
     assert(indent < 9);
@@ -347,7 +347,7 @@ struct mesh_mesh_run_test {
   }
 
   template <typename BV>
-  void query(const std::vector<Transform3f>& transforms,
+  void query(const std::vector<Transform3s>& transforms,
              SplitMethodType splitMethod, const CollisionRequest request,
              std::vector<Contacts_t>& contacts) {
     BENCHMARK_HEADER("BV");
@@ -369,13 +369,13 @@ struct mesh_mesh_run_test {
     model1->bv_splitter.reset(new BVSplitter<BV>(splitMethod));
     model2->bv_splitter.reset(new BVSplitter<BV>(splitMethod));
 
-    loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3f::Ones(),
+    loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3s::Ones(),
                                model1);
-    loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3f::Ones(),
+    loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3s::Ones(),
                                model2);
 
     Timer timer(false);
-    const Transform3f tf2;
+    const Transform3s tf2;
     const std::size_t N = transforms.size();
 
     contacts.resize(3 * N);
@@ -385,7 +385,7 @@ struct mesh_mesh_run_test {
       ++indent;
 
       for (std::size_t i = 0; i < transforms.size(); ++i) {
-        const Transform3f& tf1 = transforms[i];
+        const Transform3s& tf1 = transforms[i];
         timer.start();
 
         CollisionResult local_result;
@@ -429,7 +429,7 @@ struct mesh_mesh_run_test {
       ++indent;
 
       for (std::size_t i = 0; i < transforms.size(); ++i) {
-        const Transform3f tf1 = transforms[i];
+        const Transform3s tf1 = transforms[i];
 
         timer.start();
         CollisionResult local_result;
@@ -438,9 +438,9 @@ struct mesh_mesh_run_test {
         node.enable_statistics = enable_statistics;
 
         BVH_t* model1_tmp = new BVH_t(*model1);
-        Transform3f tf1_tmp = tf1;
+        Transform3s tf1_tmp = tf1;
         BVH_t* model2_tmp = new BVH_t(*model2);
-        Transform3f tf2_tmp = tf2;
+        Transform3s tf2_tmp = tf2;
 
         bool success = initialize(node, *model1_tmp, tf1_tmp, *model2_tmp,
                                   tf2_tmp, local_result, true, true);
@@ -482,7 +482,7 @@ struct mesh_mesh_run_test {
 
       for (std::size_t i = 0; i < transforms.size(); ++i) {
         timer.start();
-        const Transform3f tf1 = transforms[i];
+        const Transform3s tf1 = transforms[i];
 
         CollisionResult local_result;
         MeshCollisionTraversalNode<BV, 0> node(request);
@@ -623,8 +623,8 @@ struct mesh_mesh_run_test {
 //      calls function collide with identity for both object poses,
 //
 BOOST_AUTO_TEST_CASE(mesh_mesh) {
-  std::vector<Transform3f> transforms;
-  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+  std::vector<Transform3s> transforms;
+  CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
 #ifndef NDEBUG  // if debug mode
   std::size_t n = 1;
 #else
@@ -641,8 +641,8 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) {
             << transforms[i].getQuatRotation().coeffs().format(f));
   }
 
-  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 
   // Request all contacts and check that all methods give the same result.
   mesh_mesh_run_test runner(
@@ -650,12 +650,12 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) {
   runner.enable_statistics = true;
   boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner);
 
-  HPP_FCL_COMPILER_DIAGNOSTIC_POP
+  COAL_COMPILER_DIAGNOSTIC_POP
 }
 
 BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) {
-  std::vector<Transform3f> transforms;
-  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+  std::vector<Transform3s> transforms;
+  CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
 #ifndef NDEBUG
   std::size_t n = 0;
 #else
diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp
index f5b6de08b829eac5d2e5ca7173332a3b835f3e6b..698d1d5b46e89963560a1d54b230e5837482726a 100644
--- a/test/collision_node_asserts.cpp
+++ b/test/collision_node_asserts.cpp
@@ -1,24 +1,24 @@
-#define BOOST_TEST_MODULE FCL_COLLISION_NODE_ASSERT
+#define BOOST_TEST_MODULE COAL_COLLISION_NODE_ASSERT
 
 #include <boost/test/included/unit_test.hpp>
 #include <boost/math/constants/constants.hpp>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/collision.h>
+#include "coal/BVH/BVH_model.h"
+#include "coal/collision.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
-constexpr FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
+constexpr CoalScalar pi = boost::math::constants::pi<CoalScalar>();
 
 double DegToRad(const double& deg) {
   static double degToRad = pi / 180.;
   return deg * degToRad;
 }
-std::vector<Vec3f> dirs{Vec3f::UnitZ(),  -Vec3f::UnitZ(), Vec3f::UnitY(),
-                        -Vec3f::UnitY(), Vec3f::UnitX(),  -Vec3f::UnitX()};
+std::vector<Vec3s> dirs{Vec3s::UnitZ(),  -Vec3s::UnitZ(), Vec3s::UnitY(),
+                        -Vec3s::UnitY(), Vec3s::UnitX(),  -Vec3s::UnitX()};
 
 BOOST_AUTO_TEST_CASE(TestTriangles) {
-  std::vector<Vec3f> triVertices{Vec3f(1, 0, 0), Vec3f(1, 1, 0),
-                                 Vec3f(0, 1, 0)};
+  std::vector<Vec3s> triVertices{Vec3s(1, 0, 0), Vec3s(1, 1, 0),
+                                 Vec3s(0, 1, 0)};
   std::vector<Triangle> triangle{{0, 1, 2}};
 
   BVHModel<OBBRSS> tri1{};
@@ -36,19 +36,19 @@ BOOST_AUTO_TEST_CASE(TestTriangles) {
 
   ComputeCollision compute(&tri1, &tri2);
 
-  Transform3f tri1Tf{};
-  Transform3f tri2Tf{};
+  Transform3s tri1Tf{};
+  Transform3s tri2Tf{};
 
   /// check some angles for two triangles
   for (int i = 0; i < 360; i += 30) {
     for (int j = 0; j < 180; j += 30) {
       for (int k = 0; k < 180; k += 30) {
         tri1Tf.setQuatRotation(
-            Eigen::AngleAxis<double>(0., Vec3f::UnitZ()) *
-            Eigen::AngleAxis<double>(DegToRad(k), Vec3f::UnitY()));
+            Eigen::AngleAxis<double>(0., Vec3s::UnitZ()) *
+            Eigen::AngleAxis<double>(DegToRad(k), Vec3s::UnitY()));
         tri2Tf.setQuatRotation(
-            Eigen::AngleAxis<double>(DegToRad(i), Vec3f::UnitZ()) *
-            Eigen::AngleAxis<double>(DegToRad(j), Vec3f::UnitY()));
+            Eigen::AngleAxis<double>(DegToRad(i), Vec3s::UnitZ()) *
+            Eigen::AngleAxis<double>(DegToRad(j), Vec3s::UnitY()));
         CollisionResult result;
 
         /// assertion: src/collision_node.cpp:58
diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp
index 4bebaaea9a1718955b2f58ca10a3e41dd06cc9b3..a78b84f446cf8f821aecdc108e12a19dc95ee038 100644
--- a/test/contact_patch.cpp
+++ b/test/contact_patch.cpp
@@ -34,72 +34,72 @@
 
 /** \author Louis Montaut */
 
-#define BOOST_TEST_MODULE FCL_CONTACT_PATCH
+#define BOOST_TEST_MODULE COAL_CONTACT_PATCH
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/contact_patch.h>
+#include "coal/contact_patch.h"
 
 #include "utility.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
 BOOST_AUTO_TEST_CASE(box_box_no_collision) {
-  const FCL_REAL halfside = 0.5;
+  const CoalScalar halfside = 0.5;
   const Box box1(2 * halfside, 2 * halfside, 2 * halfside);
   const Box box2(2 * halfside, 2 * halfside, 2 * halfside);
 
-  const Transform3f tf1;
-  Transform3f tf2;
+  const Transform3s tf1;
+  Transform3s tf2;
   // set translation to separate the shapes
-  const FCL_REAL offset = 0.001;
-  tf2.setTranslation(Vec3f(0, 0, 2 * halfside + offset));
+  const CoalScalar offset = 0.001;
+  tf2.setTranslation(Vec3s(0, 0, 2 * halfside + offset));
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                  num_max_contact);
   CollisionResult col_res;
 
-  hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res);
+  coal::collide(&box1, tf1, &box2, tf2, col_req, col_res);
 
   BOOST_CHECK(!col_res.isCollision());
 
   const ContactPatchRequest patch_req;
   ContactPatchResult patch_res(patch_req);
-  hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
-                                patch_res);
+  coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 0);
 }
 
 BOOST_AUTO_TEST_CASE(box_sphere) {
-  const FCL_REAL halfside = 0.5;
+  const CoalScalar halfside = 0.5;
   const Box box(2 * halfside, 2 * halfside, 2 * halfside);
   const Sphere sphere(halfside);
 
-  const Transform3f tf1;
-  Transform3f tf2;
+  const Transform3s tf1;
+  Transform3s tf2;
   // set translation to have a collision
-  const FCL_REAL offset = 0.001;
-  tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset));
+  const CoalScalar offset = 0.001;
+  tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset));
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                  num_max_contact);
   CollisionResult col_res;
 
-  hpp::fcl::collide(&box, tf1, &sphere, tf2, col_req, col_res);
+  coal::collide(&box, tf1, &sphere, tf2, col_req, col_res);
 
   BOOST_CHECK(col_res.isCollision());
 
   const ContactPatchRequest patch_req;
   ContactPatchResult patch_res(patch_req);
-  hpp::fcl::computeContactPatch(&box, tf1, &sphere, tf2, col_res, patch_req,
-                                patch_res);
+  coal::computeContactPatch(&box, tf1, &sphere, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 1);
   if (patch_res.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
     const ContactPatch& contact_patch = patch_res.getContactPatch(0);
     BOOST_CHECK(contact_patch.size() == 1);
-    const FCL_REAL tol = 1e-8;
+    const CoalScalar tol = 1e-8;
     EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol);
     EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol);
     EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol);
@@ -109,40 +109,40 @@ BOOST_AUTO_TEST_CASE(box_sphere) {
 }
 
 BOOST_AUTO_TEST_CASE(box_box) {
-  const FCL_REAL halfside = 0.5;
+  const CoalScalar halfside = 0.5;
   const Box box1(2 * halfside, 2 * halfside, 2 * halfside);
   const Box box2(2 * halfside, 2 * halfside, 2 * halfside);
 
-  const Transform3f tf1;
-  Transform3f tf2;
+  const Transform3s tf1;
+  Transform3s tf2;
   // set translation to have a collision
-  const FCL_REAL offset = 0.001;
-  tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset));
+  const CoalScalar offset = 0.001;
+  tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset));
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                  num_max_contact);
   CollisionResult col_res;
 
-  hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res);
+  coal::collide(&box1, tf1, &box2, tf2, col_req, col_res);
 
   BOOST_CHECK(col_res.isCollision());
 
   const ContactPatchRequest patch_req;
   ContactPatchResult patch_res1(patch_req);
   ContactPatchResult patch_res2(patch_req);
-  hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
-                                patch_res1);
-  hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
-                                patch_res2);
+  coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
+                            patch_res1);
+  coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
+                            patch_res2);
   BOOST_CHECK(patch_res1.numContactPatches() == 1);
   BOOST_CHECK(patch_res2.numContactPatches() == 1);
 
   if (patch_res1.numContactPatches() > 0 &&
       patch_res2.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
-    const FCL_REAL tol = 1e-6;
-    EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol);
+    const CoalScalar tol = 1e-6;
+    EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol);
 
     const size_t expected_size = 4;
     ContactPatch expected(expected_size);
@@ -150,11 +150,11 @@ BOOST_AUTO_TEST_CASE(box_box) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    const std::array<Vec3f, 4> corners = {
-        Vec3f(halfside, halfside, halfside),
-        Vec3f(halfside, -halfside, halfside),
-        Vec3f(-halfside, -halfside, halfside),
-        Vec3f(-halfside, halfside, halfside),
+    const std::array<Vec3s, 4> corners = {
+        Vec3s(halfside, halfside, halfside),
+        Vec3s(halfside, -halfside, halfside),
+        Vec3s(-halfside, -halfside, halfside),
+        Vec3s(-halfside, halfside, halfside),
     };
     for (size_t i = 0; i < expected_size; ++i) {
       expected.addPoint(corners[i] +
@@ -168,40 +168,40 @@ BOOST_AUTO_TEST_CASE(box_box) {
 
 BOOST_AUTO_TEST_CASE(halfspace_box) {
   const Halfspace hspace(0, 0, 1, 0);
-  const FCL_REAL halfside = 0.5;
+  const CoalScalar halfside = 0.5;
   const Box box(2 * halfside, 2 * halfside, 2 * halfside);
 
-  const Transform3f tf1;
-  Transform3f tf2;
+  const Transform3s tf1;
+  Transform3s tf2;
   // set translation to have a collision
-  const FCL_REAL offset = 0.001;
-  tf2.setTranslation(Vec3f(0, 0, halfside - offset));
+  const CoalScalar offset = 0.001;
+  tf2.setTranslation(Vec3s(0, 0, halfside - offset));
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                  num_max_contact);
   CollisionResult col_res;
 
-  hpp::fcl::collide(&hspace, tf1, &box, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &box, tf2, col_req, col_res);
 
   BOOST_CHECK(col_res.isCollision());
 
   const ContactPatchRequest patch_req;
   ContactPatchResult patch_res1(patch_req);
   ContactPatchResult patch_res2(patch_req);
-  hpp::fcl::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req,
-                                patch_res1);
-  hpp::fcl::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req,
-                                patch_res2);
+  coal::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req,
+                            patch_res1);
+  coal::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req,
+                            patch_res2);
   BOOST_CHECK(patch_res1.numContactPatches() == 1);
   BOOST_CHECK(patch_res2.numContactPatches() == 1);
 
   if (patch_res1.numContactPatches() > 0 &&
       patch_res2.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
-    const FCL_REAL tol = 1e-6;
+    const CoalScalar tol = 1e-6;
     EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol);
-    EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3f(0, 0, 1), tol);
+    EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3s(0, 0, 1), tol);
 
     const size_t expected_size = 4;
     ContactPatch expected(expected_size);
@@ -209,11 +209,11 @@ BOOST_AUTO_TEST_CASE(halfspace_box) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    const std::array<Vec3f, 4> corners = {
-        tf2.transform(Vec3f(halfside, halfside, -halfside)),
-        tf2.transform(Vec3f(halfside, -halfside, -halfside)),
-        tf2.transform(Vec3f(-halfside, -halfside, -halfside)),
-        tf2.transform(Vec3f(-halfside, halfside, -halfside)),
+    const std::array<Vec3s, 4> corners = {
+        tf2.transform(Vec3s(halfside, halfside, -halfside)),
+        tf2.transform(Vec3s(halfside, -halfside, -halfside)),
+        tf2.transform(Vec3s(-halfside, -halfside, -halfside)),
+        tf2.transform(Vec3s(-halfside, halfside, -halfside)),
     };
     for (size_t i = 0; i < expected_size; ++i) {
       expected.addPoint(corners[i] -
@@ -227,34 +227,34 @@ BOOST_AUTO_TEST_CASE(halfspace_box) {
 
 BOOST_AUTO_TEST_CASE(halfspace_capsule) {
   const Halfspace hspace(0, 0, 1, 0);
-  const FCL_REAL radius = 0.25;
-  const FCL_REAL height = 1.;
+  const CoalScalar radius = 0.25;
+  const CoalScalar height = 1.;
   const Capsule capsule(radius, height);
 
-  const Transform3f tf1;
-  Transform3f tf2;
+  const Transform3s tf1;
+  Transform3s tf2;
   // set translation to have a collision
-  const FCL_REAL offset = 0.001;
-  tf2.setTranslation(Vec3f(0, 0, height / 2 - offset));
+  const CoalScalar offset = 0.001;
+  tf2.setTranslation(Vec3s(0, 0, height / 2 - offset));
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                  num_max_contact);
   CollisionResult col_res;
-  hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res);
   BOOST_CHECK(col_res.isCollision());
 
   const ContactPatchRequest patch_req;
   BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() ==
               ContactPatch::default_preallocated_size);
   ContactPatchResult patch_res(patch_req);
-  hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req,
-                                patch_res);
+  coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 1);
 
   if (patch_res.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
-    const FCL_REAL tol = 1e-6;
+    const CoalScalar tol = 1e-6;
     EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol);
 
     const size_t expected_size = 1;
@@ -263,7 +263,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    const Vec3f capsule_end(0, 0, -capsule.halfLength);
+    const Vec3s capsule_end(0, 0, -capsule.halfLength);
     expected.addPoint(tf2.transform(capsule_end));
 
     const ContactPatch& contact_patch = patch_res.getContactPatch(0);
@@ -277,15 +277,15 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
   tf2.rotation().col(1) << 0, 1, 0;
   tf2.rotation().col(2) << 0, 0, -1;
   col_res.clear();
-  hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res);
   BOOST_CHECK(col_res.isCollision());
   patch_res.clear();
-  hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req,
-                                patch_res);
+  coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 1);
   if (patch_res.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
-    const FCL_REAL tol = 1e-6;
+    const CoalScalar tol = 1e-6;
     EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol);
 
     const size_t expected_size = 1;
@@ -294,7 +294,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    const Vec3f capsule_end(0, 0, capsule.halfLength);
+    const Vec3s capsule_end(0, 0, capsule.halfLength);
     expected.addPoint(tf2.transform(capsule_end));
 
     const ContactPatch& contact_patch = patch_res.getContactPatch(0);
@@ -309,15 +309,15 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
   tf2.rotation().col(2) << -1, 0, 0;
   tf2.translation() << 0, 0, capsule.radius - offset;
   col_res.clear();
-  hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res);
   BOOST_CHECK(col_res.isCollision());
   patch_res.clear();
-  hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req,
-                                patch_res);
+  coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 1);
   if (patch_res.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
-    const FCL_REAL tol = 1e-6;
+    const CoalScalar tol = 1e-6;
     EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol);
 
     const size_t expected_size = 2;
@@ -326,8 +326,8 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    const Vec3f p1(-capsule.radius, 0, capsule.halfLength);
-    const Vec3f p2(-capsule.radius, 0, -capsule.halfLength);
+    const Vec3s p1(-capsule.radius, 0, capsule.halfLength);
+    const Vec3s p2(-capsule.radius, 0, -capsule.halfLength);
     expected.addPoint(tf2.transform(p1));
     expected.addPoint(tf2.transform(p2));
 
@@ -339,34 +339,34 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
 
 BOOST_AUTO_TEST_CASE(halfspace_cone) {
   const Halfspace hspace(0, 0, 1, 0);
-  const FCL_REAL radius = 0.25;
-  const FCL_REAL height = 1.;
+  const CoalScalar radius = 0.25;
+  const CoalScalar height = 1.;
   const Cone cone(radius, height);
 
-  const Transform3f tf1;
-  Transform3f tf2;
+  const Transform3s tf1;
+  Transform3s tf2;
   // set translation to have a collision
-  const FCL_REAL offset = 0.001;
-  tf2.setTranslation(Vec3f(0, 0, height / 2 - offset));
+  const CoalScalar offset = 0.001;
+  tf2.setTranslation(Vec3s(0, 0, height / 2 - offset));
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                  num_max_contact);
   CollisionResult col_res;
-  hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res);
   BOOST_CHECK(col_res.isCollision());
 
   const ContactPatchRequest patch_req;
   BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() ==
               ContactPatch::default_preallocated_size);
   ContactPatchResult patch_res(patch_req);
-  hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req,
-                                patch_res);
+  coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 1);
 
   if (patch_res.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
-    const FCL_REAL tol = 1e-6;
+    const CoalScalar tol = 1e-6;
     EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol);
 
     const size_t expected_size = ContactPatch::default_preallocated_size;
@@ -375,12 +375,12 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    std::array<Vec3f, ContactPatch::default_preallocated_size> points;
-    const FCL_REAL angle_increment =
-        2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(6));
+    std::array<Vec3s, ContactPatch::default_preallocated_size> points;
+    const CoalScalar angle_increment =
+        2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6));
     for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) {
-      const FCL_REAL theta = (FCL_REAL)(i)*angle_increment;
-      Vec3f point_on_cone_base(std::cos(theta) * cone.radius,
+      const CoalScalar theta = (CoalScalar)(i)*angle_increment;
+      Vec3s point_on_cone_base(std::cos(theta) * cone.radius,
                                std::sin(theta) * cone.radius, -cone.halfLength);
       expected.addPoint(tf2.transform(point_on_cone_base));
     }
@@ -396,17 +396,17 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
   tf2.rotation().col(1) << 0, 1, 0;
   tf2.rotation().col(2) << 0, 0, -1;
   col_res.clear();
-  hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res);
   BOOST_CHECK(col_res.isCollision());
   patch_res.clear();
-  hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req,
-                                patch_res);
+  coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 1);
   if (patch_res.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
     const ContactPatch& contact_patch = patch_res.getContactPatch(0);
     BOOST_CHECK(contact_patch.size() == 1);
-    const FCL_REAL tol = 1e-8;
+    const CoalScalar tol = 1e-8;
     EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol);
     EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol);
     EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol);
@@ -419,7 +419,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    const Vec3f cone_tip(0, 0, cone.halfLength);
+    const Vec3s cone_tip(0, 0, cone.halfLength);
     expected.addPoint(tf2.transform(cone_tip));
 
     BOOST_CHECK(contact_patch.isSame(expected, tol));
@@ -432,17 +432,17 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
   tf2.rotation().col(2) << -1, 0, 0;
   tf2.translation() << 0, 0, cone.radius - offset;
   col_res.clear();
-  hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res);
   BOOST_CHECK(col_res.isCollision());
   patch_res.clear();
-  hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req,
-                                patch_res);
+  coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 1);
   if (patch_res.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
     const ContactPatch& contact_patch = patch_res.getContactPatch(0);
     BOOST_CHECK(contact_patch.size() == 1);
-    const FCL_REAL tol = 1e-8;
+    const CoalScalar tol = 1e-8;
     EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol);
     EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol);
     EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol);
@@ -455,7 +455,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    const Vec3f point_on_circle_basis(-cone.radius, 0, -cone.halfLength);
+    const Vec3s point_on_circle_basis(-cone.radius, 0, -cone.halfLength);
     expected.addPoint(tf2.transform(point_on_circle_basis));
 
     BOOST_CHECK(contact_patch.isSame(expected, tol));
@@ -464,38 +464,38 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
 
 BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
   const Halfspace hspace(0, 0, 1, 0);
-  const FCL_REAL radius = 0.25;
-  const FCL_REAL height = 1.;
+  const CoalScalar radius = 0.25;
+  const CoalScalar height = 1.;
   const Cylinder cylinder(radius, height);
 
-  const Transform3f tf1;
-  Transform3f tf2;
+  const Transform3s tf1;
+  Transform3s tf2;
   // set translation to have a collision
-  const FCL_REAL offset = 0.001;
-  tf2.setTranslation(Vec3f(0, 0, height / 2 - offset));
+  const CoalScalar offset = 0.001;
+  tf2.setTranslation(Vec3s(0, 0, height / 2 - offset));
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                  num_max_contact);
   CollisionResult col_res;
-  hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res);
   BOOST_CHECK(col_res.isCollision());
 
   if (col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
     const size_t expected_size = ContactPatch::default_preallocated_size;
-    const FCL_REAL tol = 1e-6;
+    const CoalScalar tol = 1e-6;
     ContactPatch expected(expected_size);
     expected.tf.rotation() =
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    std::array<Vec3f, ContactPatch::default_preallocated_size> points;
-    const FCL_REAL angle_increment =
-        2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(6));
+    std::array<Vec3s, ContactPatch::default_preallocated_size> points;
+    const CoalScalar angle_increment =
+        2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6));
     for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) {
-      const FCL_REAL theta = (FCL_REAL)(i)*angle_increment;
-      Vec3f point_on_cone_base(std::cos(theta) * cylinder.radius,
+      const CoalScalar theta = (CoalScalar)(i)*angle_increment;
+      Vec3s point_on_cone_base(std::cos(theta) * cylinder.radius,
                                std::sin(theta) * cylinder.radius,
                                -cylinder.halfLength);
       expected.addPoint(tf2.transform(point_on_cone_base));
@@ -505,8 +505,8 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
     BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() ==
                 ContactPatch::default_preallocated_size);
     ContactPatchResult patch_res(patch_req);
-    hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
 
     if (patch_res.numContactPatches() > 0) {
@@ -522,11 +522,11 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
     tf2.rotation().col(1) << 0, 1, 0;
     tf2.rotation().col(2) << 0, 0, -1;
     col_res.clear();
-    hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res);
+    coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     patch_res.clear();
-    hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
     if (patch_res.numContactPatches() > 0 && col_res.isCollision()) {
       EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol);
@@ -544,17 +544,17 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
   tf2.translation() << 0, 0, cylinder.radius - offset;
 
   col_res.clear();
-  hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res);
+  coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res);
   BOOST_CHECK(col_res.isCollision());
 
   const ContactPatchRequest patch_req;
   ContactPatchResult patch_res(patch_req);
-  hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res,
-                                patch_req, patch_res);
+  coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req,
+                            patch_res);
   BOOST_CHECK(patch_res.numContactPatches() == 1);
   if (col_res.isCollision() && patch_res.numContactPatches() > 0) {
     const Contact& contact = col_res.getContact(0);
-    const FCL_REAL tol = 1e-6;
+    const CoalScalar tol = 1e-6;
 
     const size_t expected_size = 2;
     ContactPatch expected(expected_size);
@@ -563,9 +563,9 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
     expected.addPoint(
-        tf2.transform(Vec3f(cylinder.radius, 0, cylinder.halfLength)));
+        tf2.transform(Vec3s(cylinder.radius, 0, cylinder.halfLength)));
     expected.addPoint(
-        tf2.transform(Vec3f(cylinder.radius, 0, -cylinder.halfLength)));
+        tf2.transform(Vec3s(cylinder.radius, 0, -cylinder.halfLength)));
 
     const ContactPatch& contact_patch = patch_res.getContactPatch(0);
     BOOST_CHECK(expected.isSame(contact_patch, tol));
@@ -573,40 +573,40 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
 }
 
 BOOST_AUTO_TEST_CASE(convex_convex) {
-  const FCL_REAL halfside = 0.5;
+  const CoalScalar halfside = 0.5;
   const Convex<Quadrilateral> box1(buildBox(halfside, halfside, halfside));
   const Convex<Quadrilateral> box2(buildBox(halfside, halfside, halfside));
 
-  const Transform3f tf1;
-  Transform3f tf2;
+  const Transform3s tf1;
+  Transform3s tf2;
   // set translation to have a collision
-  const FCL_REAL offset = 0.001;
-  tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset));
+  const CoalScalar offset = 0.001;
+  tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset));
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                  num_max_contact);
   CollisionResult col_res;
 
-  hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res);
+  coal::collide(&box1, tf1, &box2, tf2, col_req, col_res);
 
   BOOST_CHECK(col_res.isCollision());
 
   const ContactPatchRequest patch_req;
   ContactPatchResult patch_res1(patch_req);
   ContactPatchResult patch_res2(patch_req);
-  hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
-                                patch_res1);
-  hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
-                                patch_res2);
+  coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
+                            patch_res1);
+  coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req,
+                            patch_res2);
   BOOST_CHECK(patch_res1.numContactPatches() == 1);
   BOOST_CHECK(patch_res2.numContactPatches() == 1);
 
   if (patch_res1.numContactPatches() > 0 &&
       patch_res2.numContactPatches() > 0 && col_res.isCollision()) {
     const Contact& contact = col_res.getContact(0);
-    const FCL_REAL tol = 1e-6;
-    EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol);
+    const CoalScalar tol = 1e-6;
+    EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol);
 
     const size_t expected_size = 4;
     ContactPatch expected(expected_size);
@@ -614,11 +614,11 @@ BOOST_AUTO_TEST_CASE(convex_convex) {
         constructOrthonormalBasisFromVector(contact.normal);
     expected.tf.translation() = contact.pos;
     expected.penetration_depth = contact.penetration_depth;
-    const std::array<Vec3f, 4> corners = {
-        Vec3f(halfside, halfside, halfside),
-        Vec3f(halfside, -halfside, halfside),
-        Vec3f(-halfside, -halfside, halfside),
-        Vec3f(-halfside, halfside, halfside),
+    const std::array<Vec3s, 4> corners = {
+        Vec3s(halfside, halfside, halfside),
+        Vec3s(halfside, -halfside, halfside),
+        Vec3s(-halfside, -halfside, halfside),
+        Vec3s(-halfside, halfside, halfside),
     };
     for (size_t i = 0; i < expected_size; ++i) {
       expected.addPoint(corners[i] +
@@ -635,11 +635,11 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
   // Two tetrahedrons make contact on one of their edge.
 
   const size_t expected_size = 2;
-  const Vec3f expected_cp1(0, 0.5, 0);
-  const Vec3f expected_cp2(0, 1, 0);
+  const Vec3s expected_cp1(0, 0.5, 0);
+  const Vec3s expected_cp2(0, 1, 0);
 
-  const Transform3f tf1;  // identity
-  const Transform3f tf2;  // identity
+  const Transform3s tf1;  // identity
+  const Transform3s tf2;  // identity
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -650,22 +650,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
 
   {
     // Case 1 - Face-Face contact
-    std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
-        Vec3f(-1, 0, 0),
-        Vec3f(0, 0, 0),
-        Vec3f(0, 1, 0),
-        Vec3f(-1, -1, -1),
+    std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+        Vec3s(-1, 0, 0),
+        Vec3s(0, 0, 0),
+        Vec3s(0, 1, 0),
+        Vec3s(-1, -1, -1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris1(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
                                    Triangle(0, 3, 1), Triangle(2, 1, 3)}));
     Convex<Triangle> tetra1(pts1, 4, tris1, 4);
 
-    std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
-        Vec3f(0, 0.5, 0),
-        Vec3f(0, 1.5, 0),
-        Vec3f(1, 0.5, 0),
-        Vec3f(1, 1, 1),
+    std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+        Vec3s(0, 0.5, 0),
+        Vec3s(0, 1.5, 0),
+        Vec3s(1, 0.5, 0),
+        Vec3s(1, 1, 1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris2(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -673,16 +673,16 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
     Convex<Triangle> tetra2(pts2, 4, tris2, 4);
 
     col_res.clear();
-    hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
+    coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     patch_res.clear();
-    hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
 
     if (patch_res.numContactPatches() > 0) {
       const Contact& contact = col_res.getContact(0);
-      const FCL_REAL tol = 1e-6;
+      const CoalScalar tol = 1e-6;
 
       ContactPatch expected(expected_size);
       // GJK/EPA can return any normal which is in the dual cone
@@ -701,22 +701,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
 
   {
     // Case 2 - Face-Segment contact
-    std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
-        Vec3f(-1, 0, -0.2),
-        Vec3f(0, 0, 0),
-        Vec3f(0, 1, 0),
-        Vec3f(-1, -1, -1),
+    std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+        Vec3s(-1, 0, -0.2),
+        Vec3s(0, 0, 0),
+        Vec3s(0, 1, 0),
+        Vec3s(-1, -1, -1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris1(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
                                    Triangle(0, 3, 1), Triangle(2, 1, 3)}));
     Convex<Triangle> tetra1(pts1, 4, tris1, 4);
 
-    std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
-        Vec3f(0, 0.5, 0),
-        Vec3f(0, 1.5, 0),
-        Vec3f(1, 0.5, 0),
-        Vec3f(1, 1, 1),
+    std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+        Vec3s(0, 0.5, 0),
+        Vec3s(0, 1.5, 0),
+        Vec3s(1, 0.5, 0),
+        Vec3s(1, 1, 1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris2(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -724,16 +724,16 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
     Convex<Triangle> tetra2(pts2, 4, tris2, 4);
 
     col_res.clear();
-    hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
+    coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     patch_res.clear();
-    hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
 
     if (patch_res.numContactPatches() > 0) {
       const Contact& contact = col_res.getContact(0);
-      const FCL_REAL tol = 1e-6;
+      const CoalScalar tol = 1e-6;
 
       ContactPatch expected(expected_size);
       expected.tf.rotation() =
@@ -750,22 +750,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
 
   {
     // Case 3 - Segment-Segment contact
-    std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
-        Vec3f(-1, 0, -0.2),
-        Vec3f(0, 0, 0),
-        Vec3f(0, 1, 0),
-        Vec3f(-1, -1, -1),
+    std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+        Vec3s(-1, 0, -0.2),
+        Vec3s(0, 0, 0),
+        Vec3s(0, 1, 0),
+        Vec3s(-1, -1, -1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris1(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
                                    Triangle(0, 3, 1), Triangle(2, 1, 3)}));
     Convex<Triangle> tetra1(pts1, 4, tris1, 4);
 
-    std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
-        Vec3f(0, 0.5, 0),
-        Vec3f(0, 1.5, 0),
-        Vec3f(1, 0.5, 0.5),
-        Vec3f(1, 1, 1),
+    std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+        Vec3s(0, 0.5, 0),
+        Vec3s(0, 1.5, 0),
+        Vec3s(1, 0.5, 0.5),
+        Vec3s(1, 1, 1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris2(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -773,16 +773,16 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
     Convex<Triangle> tetra2(pts2, 4, tris2, 4);
 
     col_res.clear();
-    hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
+    coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     patch_res.clear();
-    hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
 
     if (patch_res.numContactPatches() > 0) {
       const Contact& contact = col_res.getContact(0);
-      const FCL_REAL tol = 1e-6;
+      const CoalScalar tol = 1e-6;
 
       ContactPatch expected(expected_size);
       expected.tf.rotation() =
@@ -802,10 +802,10 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
   // This case covers the vertex-vertex edge case of contact patches.
   // Two tetrahedrons make contact on one of their vertex.
   const size_t expected_size = 1;
-  const Vec3f expected_cp(0, 0, 0);
+  const Vec3s expected_cp(0, 0, 0);
 
-  const Transform3f tf1;  // identity
-  const Transform3f tf2;  // identity
+  const Transform3s tf1;  // identity
+  const Transform3s tf2;  // identity
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -816,22 +816,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
 
   {
     // Case 1 - Face-Face contact
-    std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
-        Vec3f(-1, 0, 0),
-        Vec3f(0, 0, 0),
-        Vec3f(0, 1, 0),
-        Vec3f(-1, -1, -1),
+    std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+        Vec3s(-1, 0, 0),
+        Vec3s(0, 0, 0),
+        Vec3s(0, 1, 0),
+        Vec3s(-1, -1, -1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris1(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
                                    Triangle(0, 3, 1), Triangle(2, 1, 3)}));
     Convex<Triangle> tetra1(pts1, 4, tris1, 4);
 
-    std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
-        Vec3f(1, 0, 0),
-        Vec3f(0, 0, 0),
-        Vec3f(0, -1, 0),
-        Vec3f(1, 1, 1),
+    std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+        Vec3s(1, 0, 0),
+        Vec3s(0, 0, 0),
+        Vec3s(0, -1, 0),
+        Vec3s(1, 1, 1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris2(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -839,16 +839,16 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
     Convex<Triangle> tetra2(pts2, 4, tris2, 4);
 
     col_res.clear();
-    hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
+    coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     patch_res.clear();
-    hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
 
     if (patch_res.numContactPatches() > 0) {
       const Contact& contact = col_res.getContact(0);
-      const FCL_REAL tol = 1e-6;
+      const CoalScalar tol = 1e-6;
 
       ContactPatch expected(expected_size);
       expected.tf.rotation() =
@@ -864,22 +864,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
 
   {
     // Case 2 - Segment-Face contact
-    std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
-        Vec3f(-1, 0, -0.5),
-        Vec3f(0, 0, 0),
-        Vec3f(0, 1, 0),
-        Vec3f(-1, -1, -1),
+    std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+        Vec3s(-1, 0, -0.5),
+        Vec3s(0, 0, 0),
+        Vec3s(0, 1, 0),
+        Vec3s(-1, -1, -1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris1(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
                                    Triangle(0, 3, 1), Triangle(2, 1, 3)}));
     Convex<Triangle> tetra1(pts1, 4, tris1, 4);
 
-    std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
-        Vec3f(1, 0, 0),
-        Vec3f(0, 0, 0),
-        Vec3f(0, -1, 0),
-        Vec3f(1, 1, 1),
+    std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+        Vec3s(1, 0, 0),
+        Vec3s(0, 0, 0),
+        Vec3s(0, -1, 0),
+        Vec3s(1, 1, 1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris2(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -887,16 +887,16 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
     Convex<Triangle> tetra2(pts2, 4, tris2, 4);
 
     col_res.clear();
-    hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
+    coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     patch_res.clear();
-    hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
 
     if (patch_res.numContactPatches() > 0) {
       const Contact& contact = col_res.getContact(0);
-      const FCL_REAL tol = 1e-6;
+      const CoalScalar tol = 1e-6;
 
       ContactPatch expected(expected_size);
       expected.tf.rotation() =
@@ -912,22 +912,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
 
   {
     // Case 2 - Segment-Segment contact
-    std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
-        Vec3f(-1, 0, -0.2),
-        Vec3f(0, 0, 0),
-        Vec3f(0, 1, 0),
-        Vec3f(-1, -1, -1),
+    std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+        Vec3s(-1, 0, -0.2),
+        Vec3s(0, 0, 0),
+        Vec3s(0, 1, 0),
+        Vec3s(-1, -1, -1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris1(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
                                    Triangle(0, 3, 1), Triangle(2, 1, 3)}));
     Convex<Triangle> tetra1(pts1, 4, tris1, 4);
 
-    std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
-        Vec3f(1, 0, 0),
-        Vec3f(0, 0, 0),
-        Vec3f(0, -1, 0.5),
-        Vec3f(1, 1, 1),
+    std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+        Vec3s(1, 0, 0),
+        Vec3s(0, 0, 0),
+        Vec3s(0, -1, 0.5),
+        Vec3s(1, 1, 1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris2(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -935,16 +935,16 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
     Convex<Triangle> tetra2(pts2, 4, tris2, 4);
 
     col_res.clear();
-    hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
+    coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     patch_res.clear();
-    hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
 
     if (patch_res.numContactPatches() > 0) {
       const Contact& contact = col_res.getContact(0);
-      const FCL_REAL tol = 1e-6;
+      const CoalScalar tol = 1e-6;
 
       ContactPatch expected(expected_size);
       expected.tf.rotation() =
@@ -963,11 +963,11 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) {
   // This case covers the segment-face edge case of contact patches.
   // Two tetrahedrons make contact on one of their segment/face respectively.
   const size_t expected_size = 2;
-  const Vec3f expected_cp1(0, 0, 0);
-  const Vec3f expected_cp2(-0.5, 0.5, 0);
+  const Vec3s expected_cp1(0, 0, 0);
+  const Vec3s expected_cp2(-0.5, 0.5, 0);
 
-  const Transform3f tf1;  // identity
-  const Transform3f tf2;  // identity
+  const Transform3s tf1;  // identity
+  const Transform3s tf2;  // identity
 
   const size_t num_max_contact = 1;
   const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -977,22 +977,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) {
   ContactPatchResult patch_res(patch_req);
 
   {
-    std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
-        Vec3f(-1, 0, -0),
-        Vec3f(0, 0, 0),
-        Vec3f(0, 1, 0),
-        Vec3f(-1, -1, -1),
+    std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+        Vec3s(-1, 0, -0),
+        Vec3s(0, 0, 0),
+        Vec3s(0, 1, 0),
+        Vec3s(-1, -1, -1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris1(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
                                    Triangle(0, 3, 1), Triangle(2, 1, 3)}));
     Convex<Triangle> tetra1(pts1, 4, tris1, 4);
 
-    std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
-        Vec3f(-0.5, 0.5, 0),
-        Vec3f(0.5, -0.5, 0),
-        Vec3f(1, 0.5, 0.5),
-        Vec3f(1, 1, 1),
+    std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+        Vec3s(-0.5, 0.5, 0),
+        Vec3s(0.5, -0.5, 0),
+        Vec3s(1, 0.5, 0.5),
+        Vec3s(1, 1, 1),
     }));
     std::shared_ptr<std::vector<Triangle>> tris2(
         new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -1000,16 +1000,16 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) {
     Convex<Triangle> tetra2(pts2, 4, tris2, 4);
 
     col_res.clear();
-    hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
+    coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     patch_res.clear();
-    hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res,
-                                  patch_req, patch_res);
+    coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req,
+                              patch_res);
     BOOST_CHECK(patch_res.numContactPatches() == 1);
 
     if (patch_res.numContactPatches() > 0) {
       const Contact& contact = col_res.getContact(0);
-      const FCL_REAL tol = 1e-6;
+      const CoalScalar tol = 1e-6;
 
       ContactPatch expected(expected_size);
       expected.tf.rotation() =
diff --git a/test/convex.cpp b/test/convex.cpp
index 744902b975415595801716e41c238b8049c75512..52363b7d85505c0f0b3f799cdb08dc1b8796e05c 100644
--- a/test/convex.cpp
+++ b/test/convex.cpp
@@ -34,19 +34,19 @@
 
 /** \author Joseph Mirabel */
 
-#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES
+#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/shape/convex.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/distance.h>
+#include "coal/shape/convex.h"
+#include "coal/collision.h"
+#include "coal/distance.h"
 
 #include "utility.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
 BOOST_AUTO_TEST_CASE(convex) {
-  FCL_REAL l = 1, w = 1, d = 1;
+  CoalScalar l = 1, w = 1, d = 1;
   Convex<Quadrilateral> box(buildBox(l, w, d));
 
   // Check neighbors
@@ -88,8 +88,8 @@ BOOST_AUTO_TEST_CASE(convex) {
 
 template <typename Sa, typename Sb>
 void compareShapeIntersection(const Sa& sa, const Sb& sb,
-                              const Transform3f& tf1, const Transform3f& tf2,
-                              FCL_REAL tol = 1e-9) {
+                              const Transform3s& tf1, const Transform3s& tf2,
+                              CoalScalar tol = 1e-9) {
   CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1);
   CollisionResult resA, resB;
 
@@ -117,8 +117,8 @@ void compareShapeIntersection(const Sa& sa, const Sb& sb,
 }
 
 template <typename Sa, typename Sb>
-void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1,
-                          const Transform3f& tf2, FCL_REAL tol = 1e-9) {
+void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3s& tf1,
+                          const Transform3s& tf2, CoalScalar tol = 1e-9) {
   DistanceRequest request(true);
   DistanceResult resA, resB;
 
@@ -149,19 +149,19 @@ void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1,
 }
 
 BOOST_AUTO_TEST_CASE(compare_convex_box) {
-  FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
-  FCL_REAL l = 1, w = 1, d = 1, eps = 1e-4;
+  CoalScalar extents[6] = {0, 0, 0, 10, 10, 10};
+  CoalScalar l = 1, w = 1, d = 1, eps = 1e-4;
   Box box(l * 2, w * 2, d * 2);
   Convex<Quadrilateral> convex_box(buildBox(l, w, d));
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  tf2.setTranslation(Vec3f(3, 0, 0));
+  tf2.setTranslation(Vec3s(3, 0, 0));
   compareShapeIntersection(box, convex_box, tf1, tf2, eps);
   compareShapeDistance(box, convex_box, tf1, tf2, eps);
 
-  tf2.setTranslation(Vec3f(0, 0, 0));
+  tf2.setTranslation(Vec3s(0, 0, 0));
   compareShapeIntersection(box, convex_box, tf1, tf2, eps);
   compareShapeDistance(box, convex_box, tf1, tf2, eps);
 
@@ -172,10 +172,10 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) {
   }
 }
 
-#ifdef HPP_FCL_HAS_QHULL
+#ifdef COAL_HAS_QHULL
 BOOST_AUTO_TEST_CASE(convex_hull_throw) {
-  std::shared_ptr<std::vector<Vec3f>> points(
-      new std::vector<Vec3f>({Vec3f(1, 1, 1), Vec3f(0, 0, 0), Vec3f(1, 0, 0)}));
+  std::shared_ptr<std::vector<Vec3s>> points(
+      new std::vector<Vec3s>({Vec3s(1, 1, 1), Vec3s(0, 0, 0), Vec3s(1, 0, 0)}));
 
   BOOST_CHECK_THROW(ConvexBase::convexHull(points, 0, false, NULL),
                     std::invalid_argument);
@@ -188,11 +188,11 @@ BOOST_AUTO_TEST_CASE(convex_hull_throw) {
 }
 
 BOOST_AUTO_TEST_CASE(convex_hull_quad) {
-  std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
-      Vec3f(1, 1, 1),
-      Vec3f(0, 0, 0),
-      Vec3f(1, 0, 0),
-      Vec3f(0, 0, 1),
+  std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
+      Vec3s(1, 1, 1),
+      Vec3s(0, 0, 0),
+      Vec3s(1, 0, 0),
+      Vec3s(0, 0, 1),
   }));
 
   ConvexBase* convexHull = ConvexBase::convexHull(points, 4, false, NULL);
@@ -205,26 +205,26 @@ BOOST_AUTO_TEST_CASE(convex_hull_quad) {
 }
 
 BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
-  std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
-      Vec3f(1, 1, 1),
-      Vec3f(1, 1, -1),
-      Vec3f(1, -1, 1),
-      Vec3f(1, -1, -1),
-      Vec3f(-1, 1, 1),
-      Vec3f(-1, 1, -1),
-      Vec3f(-1, -1, 1),
-      Vec3f(-1, -1, -1),
-      Vec3f(0, 0, 0),
-      Vec3f(0, 0, 0.99),
+  std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
+      Vec3s(1, 1, 1),
+      Vec3s(1, 1, -1),
+      Vec3s(1, -1, 1),
+      Vec3s(1, -1, -1),
+      Vec3s(-1, 1, 1),
+      Vec3s(-1, 1, -1),
+      Vec3s(-1, -1, 1),
+      Vec3s(-1, -1, -1),
+      Vec3s(0, 0, 0),
+      Vec3s(0, 0, 0.99),
   }));
 
   ConvexBase* convexHull = ConvexBase::convexHull(points, 9, false, NULL);
 
   BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
   {
-    const std::vector<Vec3f>& cvxhull_points = *(convexHull->points);
+    const std::vector<Vec3s>& cvxhull_points = *(convexHull->points);
     for (size_t i = 0; i < 8; ++i) {
-      BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1));
+      BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1));
       BOOST_CHECK_EQUAL((*(convexHull->neighbors))[i].count(), 3);
     }
   }
@@ -236,9 +236,9 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
 
   BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
   {
-    const std::vector<Vec3f>& cvxhull_points = *(convexHull->points);
+    const std::vector<Vec3s>& cvxhull_points = *(convexHull->points);
     for (size_t i = 0; i < 8; ++i) {
-      BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1));
+      BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1));
       BOOST_CHECK((*(convexHull->neighbors))[i].count() >= 3);
       BOOST_CHECK((*(convexHull->neighbors))[i].count() <= 6);
     }
@@ -249,16 +249,16 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
 BOOST_AUTO_TEST_CASE(convex_copy_constructor) {
   Convex<Triangle>* convexHullTriCopy;
   {
-    std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
-        Vec3f(1, 1, 1),
-        Vec3f(1, 1, -1),
-        Vec3f(1, -1, 1),
-        Vec3f(1, -1, -1),
-        Vec3f(-1, 1, 1),
-        Vec3f(-1, 1, -1),
-        Vec3f(-1, -1, 1),
-        Vec3f(-1, -1, -1),
-        Vec3f(0, 0, 0),
+    std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
+        Vec3s(1, 1, 1),
+        Vec3s(1, 1, -1),
+        Vec3s(1, -1, 1),
+        Vec3s(1, -1, -1),
+        Vec3s(-1, 1, 1),
+        Vec3s(-1, 1, -1),
+        Vec3s(-1, -1, 1),
+        Vec3s(-1, -1, -1),
+        Vec3s(0, 0, 0),
     }));
 
     Convex<Triangle>* convexHullTri = dynamic_cast<Convex<Triangle>*>(
@@ -272,16 +272,16 @@ BOOST_AUTO_TEST_CASE(convex_copy_constructor) {
 }
 
 BOOST_AUTO_TEST_CASE(convex_clone) {
-  std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
-      Vec3f(1, 1, 1),
-      Vec3f(1, 1, -1),
-      Vec3f(1, -1, 1),
-      Vec3f(1, -1, -1),
-      Vec3f(-1, 1, 1),
-      Vec3f(-1, 1, -1),
-      Vec3f(-1, -1, 1),
-      Vec3f(-1, -1, -1),
-      Vec3f(0, 0, 0),
+  std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
+      Vec3s(1, 1, 1),
+      Vec3s(1, 1, -1),
+      Vec3s(1, -1, 1),
+      Vec3s(1, -1, -1),
+      Vec3s(-1, 1, 1),
+      Vec3s(-1, 1, -1),
+      Vec3s(-1, -1, 1),
+      Vec3s(-1, -1, -1),
+      Vec3s(0, 0, 0),
   }));
 
   Convex<Triangle>* convexHullTri = dynamic_cast<Convex<Triangle>*>(
diff --git a/test/distance.cpp b/test/distance.cpp
index ed365417ae1fe5b6a0b9d8c74444454d636413b7..acb89251386537e41b782fcaff7c2c68aba67036 100644
--- a/test/distance.cpp
+++ b/test/distance.cpp
@@ -35,51 +35,51 @@
 
 /** \author Jia Pan */
 
-#define BOOST_TEST_MODULE FCL_DISTANCE
+#define BOOST_TEST_MODULE COAL_DISTANCE
 #include <chrono>
 
 #include <boost/test/included/unit_test.hpp>
 #include <boost/filesystem.hpp>
 
-#include <hpp/fcl/internal/traversal_node_bvhs.h>
-#include <hpp/fcl/internal/traversal_node_setup.h>
+#include "coal/internal/traversal_node_bvhs.h"
+#include "coal/internal/traversal_node_setup.h"
 #include "../src/collision_node.h"
-#include <hpp/fcl/internal/BV_splitter.h>
+#include "coal/internal/BV_splitter.h"
 
 #include "utility.h"
 #include "fcl_resources/config.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 namespace utf = boost::unit_test::framework;
 
 bool verbose = false;
-FCL_REAL DELTA = 0.001;
+CoalScalar DELTA = 0.001;
 
 template <typename BV>
-void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
+void distance_Test(const Transform3s& tf, const std::vector<Vec3s>& vertices1,
                    const std::vector<Triangle>& triangles1,
-                   const std::vector<Vec3f>& vertices2,
+                   const std::vector<Vec3s>& vertices2,
                    const std::vector<Triangle>& triangles2,
                    SplitMethodType split_method, unsigned int qsize,
                    DistanceRes& distance_result, bool verbose = true);
 
-bool collide_Test_OBB(const Transform3f& tf,
-                      const std::vector<Vec3f>& vertices1,
+bool collide_Test_OBB(const Transform3s& tf,
+                      const std::vector<Vec3s>& vertices1,
                       const std::vector<Triangle>& triangles1,
-                      const std::vector<Vec3f>& vertices2,
+                      const std::vector<Vec3s>& vertices2,
                       const std::vector<Triangle>& triangles2,
                       SplitMethodType split_method, bool verbose);
 
 template <typename BV, typename TraversalNode>
-void distance_Test_Oriented(const Transform3f& tf,
-                            const std::vector<Vec3f>& vertices1,
+void distance_Test_Oriented(const Transform3s& tf,
+                            const std::vector<Vec3s>& vertices1,
                             const std::vector<Triangle>& triangles1,
-                            const std::vector<Vec3f>& vertices2,
+                            const std::vector<Vec3s>& vertices2,
                             const std::vector<Triangle>& triangles2,
                             SplitMethodType split_method, unsigned int qsize,
                             DistanceRes& distance_result, bool verbose = true);
 
-inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) {
+inline bool nearlyEqual(const Vec3s& a, const Vec3s& b) {
   if (fabs(a[0] - b[0]) > DELTA) return false;
   if (fabs(a[1] - b[1]) > DELTA) return false;
   if (fabs(a[2] - b[2]) > DELTA) return false;
@@ -87,14 +87,14 @@ inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) {
 }
 
 BOOST_AUTO_TEST_CASE(mesh_distance) {
-  std::vector<Vec3f> p1, p2;
+  std::vector<Vec3s> p1, p2;
   std::vector<Triangle> t1, t2;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
   loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
 
-  std::vector<Transform3f> transforms;  // t0
-  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+  std::vector<Transform3s> transforms;  // t0
+  CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
 #ifndef NDEBUG  // if debug mode
   std::size_t n = 1;
 #else
@@ -468,10 +468,10 @@ BOOST_AUTO_TEST_CASE(mesh_distance) {
 }
 
 template <typename BV, typename TraversalNode>
-void distance_Test_Oriented(const Transform3f& tf,
-                            const std::vector<Vec3f>& vertices1,
+void distance_Test_Oriented(const Transform3s& tf,
+                            const std::vector<Vec3s>& vertices1,
                             const std::vector<Triangle>& triangles1,
-                            const std::vector<Vec3f>& vertices2,
+                            const std::vector<Vec3s>& vertices2,
                             const std::vector<Triangle>& triangles2,
                             SplitMethodType split_method, unsigned int qsize,
                             DistanceRes& distance_result, bool verbose) {
@@ -491,7 +491,7 @@ void distance_Test_Oriented(const Transform3f& tf,
   DistanceResult local_result;
   TraversalNode node;
   if (!initialize(node, (const BVHModel<BV>&)m1, tf, (const BVHModel<BV>&)m2,
-                  Transform3f(), DistanceRequest(true), local_result))
+                  Transform3s(), DistanceRequest(true), local_result))
     std::cout << "initialize error" << std::endl;
 
   node.enable_statistics = verbose;
@@ -499,8 +499,8 @@ void distance_Test_Oriented(const Transform3f& tf,
   distance(&node, NULL, qsize);
 
   // points are in local coordinate, to global coordinate
-  Vec3f p1 = local_result.nearest_points[0];
-  Vec3f p2 = local_result.nearest_points[1];
+  Vec3s p1 = local_result.nearest_points[0];
+  Vec3s p2 = local_result.nearest_points[1];
 
   distance_result.distance = local_result.min_distance;
   distance_result.p1 = p1;
@@ -516,9 +516,9 @@ void distance_Test_Oriented(const Transform3f& tf,
 }
 
 template <typename BV>
-void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
+void distance_Test(const Transform3s& tf, const std::vector<Vec3s>& vertices1,
                    const std::vector<Triangle>& triangles1,
-                   const std::vector<Vec3f>& vertices2,
+                   const std::vector<Vec3s>& vertices2,
                    const std::vector<Triangle>& triangles2,
                    SplitMethodType split_method, unsigned int qsize,
                    DistanceRes& distance_result, bool verbose) {
@@ -535,7 +535,7 @@ void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Transform3f pose1(tf), pose2;
+  Transform3s pose1(tf), pose2;
 
   DistanceResult local_result;
   MeshDistanceTraversalNode<BV> node;
@@ -565,10 +565,10 @@ void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
   }
 }
 
-bool collide_Test_OBB(const Transform3f& tf,
-                      const std::vector<Vec3f>& vertices1,
+bool collide_Test_OBB(const Transform3s& tf,
+                      const std::vector<Vec3s>& vertices1,
                       const std::vector<Triangle>& triangles1,
-                      const std::vector<Vec3f>& vertices2,
+                      const std::vector<Vec3s>& vertices2,
                       const std::vector<Triangle>& triangles2,
                       SplitMethodType split_method, bool verbose) {
   BVHModel<OBB> m1;
@@ -588,7 +588,7 @@ bool collide_Test_OBB(const Transform3f& tf,
   CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1);
   MeshCollisionTraversalNodeOBB node(request);
   bool success(initialize(node, (const BVHModel<OBB>&)m1, tf,
-                          (const BVHModel<OBB>&)m2, Transform3f(),
+                          (const BVHModel<OBB>&)m2, Transform3s(),
                           local_result));
   BOOST_REQUIRE(success);
 
diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp
index d14825dd1fbe01f69afe90589dcf60c643467181..f4386597d78fc953bff332f69d11564fdd72337b 100644
--- a/test/distance_lower_bound.cpp
+++ b/test/distance_lower_bound.cpp
@@ -32,39 +32,39 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#define BOOST_TEST_MODULE FCL_DISTANCE_LOWER_BOUND
+#define BOOST_TEST_MODULE COAL_DISTANCE_LOWER_BOUND
 #include <boost/test/included/unit_test.hpp>
 #include <boost/filesystem.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/BV/OBBRSS.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/distance.h>
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/collision.h"
+#include "coal/distance.h"
 #include "utility.h"
 #include "fcl_resources/config.h"
 
-using hpp::fcl::BVHModel;
-using hpp::fcl::CollisionGeometryPtr_t;
-using hpp::fcl::CollisionObject;
-using hpp::fcl::CollisionRequest;
-using hpp::fcl::CollisionResult;
-using hpp::fcl::DistanceRequest;
-using hpp::fcl::DistanceResult;
-using hpp::fcl::FCL_REAL;
-using hpp::fcl::OBBRSS;
-using hpp::fcl::shared_ptr;
-using hpp::fcl::Transform3f;
-using hpp::fcl::Triangle;
-using hpp::fcl::Vec3f;
-
-bool testDistanceLowerBound(const Transform3f& tf,
+using coal::BVHModel;
+using coal::CoalScalar;
+using coal::CollisionGeometryPtr_t;
+using coal::CollisionObject;
+using coal::CollisionRequest;
+using coal::CollisionResult;
+using coal::DistanceRequest;
+using coal::DistanceResult;
+using coal::OBBRSS;
+using coal::shared_ptr;
+using coal::Transform3s;
+using coal::Triangle;
+using coal::Vec3s;
+
+bool testDistanceLowerBound(const Transform3s& tf,
                             const CollisionGeometryPtr_t& m1,
                             const CollisionGeometryPtr_t& m2,
-                            FCL_REAL& distance) {
-  Transform3f pose1(tf), pose2;
+                            CoalScalar& distance) {
+  Transform3s pose1(tf), pose2;
 
   CollisionRequest request;
 
@@ -72,37 +72,37 @@ bool testDistanceLowerBound(const Transform3f& tf,
   CollisionObject co1(m1, pose1);
   CollisionObject co2(m2, pose2);
 
-  hpp::fcl::collide(&co1, &co2, request, result);
+  coal::collide(&co1, &co2, request, result);
   distance = result.distance_lower_bound;
 
   return result.isCollision();
 }
 
-bool testCollide(const Transform3f& tf, const CollisionGeometryPtr_t& m1,
+bool testCollide(const Transform3s& tf, const CollisionGeometryPtr_t& m1,
                  const CollisionGeometryPtr_t& m2) {
-  Transform3f pose1(tf), pose2;
+  Transform3s pose1(tf), pose2;
 
-  CollisionRequest request(hpp::fcl::NO_REQUEST, 1);
+  CollisionRequest request(coal::NO_REQUEST, 1);
   request.enable_distance_lower_bound = false;
 
   CollisionResult result;
   CollisionObject co1(m1, pose1);
   CollisionObject co2(m2, pose2);
 
-  hpp::fcl::collide(&co1, &co2, request, result);
+  coal::collide(&co1, &co2, request, result);
   return result.isCollision();
 }
 
-bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1,
-                  const CollisionGeometryPtr_t& m2, FCL_REAL& distance) {
-  Transform3f pose1(tf), pose2;
+bool testDistance(const Transform3s& tf, const CollisionGeometryPtr_t& m1,
+                  const CollisionGeometryPtr_t& m2, CoalScalar& distance) {
+  Transform3s pose1(tf), pose2;
 
   DistanceRequest request;
   DistanceResult result;
   CollisionObject co1(m1, pose1);
   CollisionObject co2(m2, pose2);
 
-  hpp::fcl::distance(&co1, &co2, request, result);
+  coal::distance(&co1, &co2, request, result);
   distance = result.min_distance;
 
   if (result.min_distance <= 0) {
@@ -113,7 +113,7 @@ bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1,
 }
 
 BOOST_AUTO_TEST_CASE(mesh_mesh) {
-  std::vector<Vec3f> p1, p2;
+  std::vector<Vec3s> p1, p2;
   std::vector<Triangle> t1, t2;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
 
@@ -131,15 +131,15 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) {
   m2->addSubModel(p2, t2);
   m2->endModel();
 
-  std::vector<Transform3f> transforms;
-  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+  std::vector<Transform3s> transforms;
+  CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
   std::size_t n = 100;
 
   generateRandomTransforms(extents, transforms, n);
 
   // collision
   for (std::size_t i = 0; i < transforms.size(); ++i) {
-    FCL_REAL distanceLowerBound, distance;
+    CoalScalar distanceLowerBound, distance;
     bool col1, col2, col3;
     col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound);
     col2 = testDistance(transforms[i], m1, m2, distance);
@@ -158,21 +158,21 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) {
 }
 
 BOOST_AUTO_TEST_CASE(box_sphere) {
-  shared_ptr<hpp::fcl::Sphere> sphere(new hpp::fcl::Sphere(0.5));
-  shared_ptr<hpp::fcl::Box> box(new hpp::fcl::Box(1., 1., 1.));
+  shared_ptr<coal::Sphere> sphere(new coal::Sphere(0.5));
+  shared_ptr<coal::Box> box(new coal::Box(1., 1., 1.));
 
-  Transform3f M1;
+  Transform3s M1;
   M1.setIdentity();
-  Transform3f M2;
+  Transform3s M2;
   M2.setIdentity();
 
-  std::vector<Transform3f> transforms;
-  FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
+  std::vector<Transform3s> transforms;
+  CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.};
   const std::size_t n = 1000;
 
   generateRandomTransforms(extents, transforms, n);
 
-  FCL_REAL distanceLowerBound, distance;
+  CoalScalar distanceLowerBound, distance;
   bool col1, col2;
   col1 = testDistanceLowerBound(M1, sphere, box, distanceLowerBound);
   col2 = testDistance(M1, sphere, box, distance);
@@ -180,7 +180,7 @@ BOOST_AUTO_TEST_CASE(box_sphere) {
   BOOST_CHECK(distanceLowerBound <= distance);
 
   for (std::size_t i = 0; i < transforms.size(); ++i) {
-    FCL_REAL distanceLowerBound, distance;
+    CoalScalar distanceLowerBound, distance;
     bool col1, col2;
     col1 =
         testDistanceLowerBound(transforms[i], sphere, box, distanceLowerBound);
@@ -195,21 +195,21 @@ BOOST_AUTO_TEST_CASE(box_sphere) {
 }
 
 BOOST_AUTO_TEST_CASE(sphere_sphere) {
-  shared_ptr<hpp::fcl::Sphere> sphere1(new hpp::fcl::Sphere(0.5));
-  shared_ptr<hpp::fcl::Sphere> sphere2(new hpp::fcl::Sphere(1.));
+  shared_ptr<coal::Sphere> sphere1(new coal::Sphere(0.5));
+  shared_ptr<coal::Sphere> sphere2(new coal::Sphere(1.));
 
-  Transform3f M1;
+  Transform3s M1;
   M1.setIdentity();
-  Transform3f M2;
+  Transform3s M2;
   M2.setIdentity();
 
-  std::vector<Transform3f> transforms;
-  FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
+  std::vector<Transform3s> transforms;
+  CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.};
   const std::size_t n = 1000;
 
   generateRandomTransforms(extents, transforms, n);
 
-  FCL_REAL distanceLowerBound, distance;
+  CoalScalar distanceLowerBound, distance;
   bool col1, col2;
   col1 = testDistanceLowerBound(M1, sphere1, sphere2, distanceLowerBound);
   col2 = testDistance(M1, sphere1, sphere2, distance);
@@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
   BOOST_CHECK(distanceLowerBound <= distance);
 
   for (std::size_t i = 0; i < transforms.size(); ++i) {
-    FCL_REAL distanceLowerBound, distance;
+    CoalScalar distanceLowerBound, distance;
     bool col1, col2;
     col1 = testDistanceLowerBound(transforms[i], sphere1, sphere2,
                                   distanceLowerBound);
@@ -232,28 +232,28 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
 }
 
 BOOST_AUTO_TEST_CASE(box_mesh) {
-  std::vector<Vec3f> p1;
+  std::vector<Vec3s> p1;
   std::vector<Triangle> t1;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
 
   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
 
   shared_ptr<BVHModel<OBBRSS> > m1(new BVHModel<OBBRSS>);
-  shared_ptr<hpp::fcl::Box> m2(new hpp::fcl::Box(500, 200, 150));
+  shared_ptr<coal::Box> m2(new coal::Box(500, 200, 150));
 
   m1->beginModel();
   m1->addSubModel(p1, t1);
   m1->endModel();
 
-  std::vector<Transform3f> transforms;
-  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+  std::vector<Transform3s> transforms;
+  CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
   std::size_t n = 100;
 
   generateRandomTransforms(extents, transforms, n);
 
   // collision
   for (std::size_t i = 0; i < transforms.size(); ++i) {
-    FCL_REAL distanceLowerBound, distance;
+    CoalScalar distanceLowerBound, distance;
     bool col1, col2;
     col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound);
     col2 = testDistance(transforms[i], m1, m2, distance);
diff --git a/test/frontlist.cpp b/test/frontlist.cpp
index 724144faae58f56fb06f453226c95b1b39136a50..9e68a326ced5542fbd598c4b99125cea1e303205 100644
--- a/test/frontlist.cpp
+++ b/test/frontlist.cpp
@@ -35,59 +35,59 @@
 
 /** \author Jia Pan */
 
-#define BOOST_TEST_MODULE FCL_FRONT_LIST
+#define BOOST_TEST_MODULE COAL_FRONT_LIST
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/internal/traversal_node_bvhs.h>
-#include <hpp/fcl/internal/traversal_node_setup.h>
+#include "coal/internal/traversal_node_bvhs.h"
+#include "coal/internal/traversal_node_setup.h"
 #include <../src/collision_node.h>
-#include <hpp/fcl/internal/BV_splitter.h>
+#include "coal/internal/BV_splitter.h"
 #include "utility.h"
 
 #include "fcl_resources/config.h"
 #include <boost/filesystem.hpp>
 
-using namespace hpp::fcl;
+using namespace coal;
 namespace utf = boost::unit_test::framework;
 
 template <typename BV>
-bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
-                             const std::vector<Vec3f>& vertices1,
+bool collide_front_list_Test(const Transform3s& tf1, const Transform3s& tf2,
+                             const std::vector<Vec3s>& vertices1,
                              const std::vector<Triangle>& triangles1,
-                             const std::vector<Vec3f>& vertices2,
+                             const std::vector<Vec3s>& 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,
+bool collide_front_list_Test_Oriented(const Transform3s& tf1,
+                                      const Transform3s& tf2,
+                                      const std::vector<Vec3s>& vertices1,
                                       const std::vector<Triangle>& triangles1,
-                                      const std::vector<Vec3f>& vertices2,
+                                      const std::vector<Vec3s>& 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,
+bool collide_Test(const Transform3s& tf, const std::vector<Vec3s>& vertices1,
                   const std::vector<Triangle>& triangles1,
-                  const std::vector<Vec3f>& vertices2,
+                  const std::vector<Vec3s>& vertices2,
                   const std::vector<Triangle>& triangles2,
                   SplitMethodType split_method, bool verbose);
 
 // TODO: randomly still have some runtime error
 BOOST_AUTO_TEST_CASE(front_list) {
-  std::vector<Vec3f> p1, p2;
+  std::vector<Vec3s> p1, p2;
   std::vector<Triangle> t1, t2;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
   loadOBJFile((path / "rob.obj").string().c_str(), 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::vector<Transform3s> transforms;   // t0
+  std::vector<Transform3s> transforms2;  // t1
+  CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+  CoalScalar delta_trans[] = {1, 1, 1};
 #ifndef NDEBUG  // if debug mode
   std::size_t n = 2;
 #else
@@ -270,10 +270,10 @@ BOOST_AUTO_TEST_CASE(front_list) {
 }
 
 template <typename BV>
-bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
-                             const std::vector<Vec3f>& vertices1,
+bool collide_front_list_Test(const Transform3s& tf1, const Transform3s& tf2,
+                             const std::vector<Vec3s>& vertices1,
                              const std::vector<Triangle>& triangles1,
-                             const std::vector<Vec3f>& vertices2,
+                             const std::vector<Vec3s>& vertices2,
                              const std::vector<Triangle>& triangles2,
                              SplitMethodType split_method, bool refit_bottomup,
                              bool verbose) {
@@ -284,7 +284,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
 
   BVHFrontList front_list;
 
-  std::vector<Vec3f> vertices1_new(vertices1.size());
+  std::vector<Vec3s> vertices1_new(vertices1.size());
   for (std::size_t i = 0; i < vertices1_new.size(); ++i) {
     vertices1_new[i] = tf1.transform(vertices1[i]);
   }
@@ -297,7 +297,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Transform3f pose1, pose2;
+  Transform3s pose1, pose2;
 
   CollisionResult local_result;
   CollisionRequest request(NO_REQUEST, (std::numeric_limits<int>::max)());
@@ -336,11 +336,11 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
 }
 
 template <typename BV, typename TraversalNode>
-bool collide_front_list_Test_Oriented(const Transform3f& tf1,
-                                      const Transform3f& tf2,
-                                      const std::vector<Vec3f>& vertices1,
+bool collide_front_list_Test_Oriented(const Transform3s& tf1,
+                                      const Transform3s& tf2,
+                                      const std::vector<Vec3s>& vertices1,
                                       const std::vector<Triangle>& triangles1,
-                                      const std::vector<Vec3f>& vertices2,
+                                      const std::vector<Vec3s>& vertices2,
                                       const std::vector<Triangle>& triangles2,
                                       SplitMethodType split_method,
                                       bool verbose) {
@@ -359,7 +359,7 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Transform3f pose1(tf1), pose2;
+  Transform3s pose1(tf1), pose2;
 
   CollisionResult local_result;
   CollisionRequest request(NO_REQUEST, (std::numeric_limits<int>::max)());
@@ -392,9 +392,9 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1,
 }
 
 template <typename BV>
-bool collide_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
+bool collide_Test(const Transform3s& tf, const std::vector<Vec3s>& vertices1,
                   const std::vector<Triangle>& triangles1,
-                  const std::vector<Vec3f>& vertices2,
+                  const std::vector<Vec3s>& vertices2,
                   const std::vector<Triangle>& triangles2,
                   SplitMethodType split_method, bool verbose) {
   BVHModel<BV> m1;
@@ -410,7 +410,7 @@ bool collide_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
   m2.addSubModel(vertices2, triangles2);
   m2.endModel();
 
-  Transform3f pose1(tf), pose2;
+  Transform3s pose1(tf), pose2;
 
   CollisionResult local_result;
   CollisionRequest request(NO_REQUEST, (std::numeric_limits<int>::max)());
diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp
index ec7425248c10f2963a81462407273395e2af16d3..ec8e6b468d06bbea835ada9f1d54cb2420969d97 100644
--- a/test/geometric_shapes.cpp
+++ b/test/geometric_shapes.cpp
@@ -36,23 +36,23 @@
 
 /** \author Jia Pan */
 
-#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES
+#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/distance.h>
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/collision.h"
+#include "coal/distance.h"
 #include "utility.h"
 #include <iostream>
-#include <hpp/fcl/internal/tools.h>
-#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/tools.h"
+#include "coal/shape/geometric_shape_to_BVH_model.h"
+#include "coal/internal/shape_shape_func.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
-FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
+CoalScalar extents[6] = {0, 0, 0, 10, 10, 10};
 
-FCL_REAL tol_gjk = 0.01;
+CoalScalar tol_gjk = 0.01;
 GJKSolver solver1;
 GJKSolver solver2;
 
@@ -66,8 +66,7 @@ int line;
                                                << "].")
 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 std::ostream& operator<<(std::ostream& os, const ShapeBase&) {
   return os << "a_shape";
 }
@@ -75,16 +74,15 @@ std::ostream& operator<<(std::ostream& os, const ShapeBase&) {
 std::ostream& operator<<(std::ostream& os, const Box& b) {
   return os << "Box(" << 2 * b.halfSide.transpose() << ')';
 }
-}  // namespace fcl
-}  // namespace hpp
+}  // namespace coal
 
 template <typename S1, typename S2>
 void printComparisonError(const std::string& comparison_type, const S1& s1,
-                          const Transform3f& tf1, const S2& s2,
-                          const Transform3f& tf2,
-                          const Vec3f& contact_or_normal,
-                          const Vec3f& expected_contact_or_normal,
-                          bool check_opposite_normal, FCL_REAL tol) {
+                          const Transform3s& tf1, const S2& s2,
+                          const Transform3s& tf2,
+                          const Vec3s& contact_or_normal,
+                          const Vec3s& expected_contact_or_normal,
+                          bool check_opposite_normal, CoalScalar tol) {
   std::cout << "Disagreement between " << comparison_type << " and expected_"
             << comparison_type << " for " << getNodeTypeName(s1.getNodeType())
             << " and " << getNodeTypeName(s2.getNodeType()) << ".\n"
@@ -111,9 +109,9 @@ void printComparisonError(const std::string& comparison_type, const S1& s1,
 
 template <typename S1, typename S2>
 void printComparisonError(const std::string& comparison_type, const S1& s1,
-                          const Transform3f& tf1, const S2& s2,
-                          const Transform3f& tf2, FCL_REAL depth,
-                          FCL_REAL expected_depth, FCL_REAL tol) {
+                          const Transform3s& tf1, const S2& s2,
+                          const Transform3s& tf2, CoalScalar depth,
+                          CoalScalar expected_depth, CoalScalar tol) {
   std::cout << "Disagreement between " << comparison_type << " and expected_"
             << comparison_type << " for " << getNodeTypeName(s1.getNodeType())
             << " and " << getNodeTypeName(s2.getNodeType()) << ".\n"
@@ -128,12 +126,12 @@ void printComparisonError(const std::string& comparison_type, const S1& s1,
 }
 
 template <typename S1, typename S2>
-void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2,
-                    const Transform3f& tf2, const Vec3f& contact,
-                    Vec3f* expected_point, FCL_REAL depth,
-                    FCL_REAL* expected_depth, const Vec3f& normal,
-                    Vec3f* expected_normal, bool check_opposite_normal,
-                    FCL_REAL tol) {
+void compareContact(const S1& s1, const Transform3s& tf1, const S2& s2,
+                    const Transform3s& tf2, const Vec3s& contact,
+                    Vec3s* expected_point, CoalScalar depth,
+                    CoalScalar* expected_depth, const Vec3s& normal,
+                    Vec3s* expected_normal, bool check_opposite_normal,
+                    CoalScalar tol) {
   if (expected_point) {
     bool contact_equal = isEqual(contact, *expected_point, tol);
     FCL_CHECK(contact_equal);
@@ -164,17 +162,18 @@ void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2,
 }
 
 template <typename S1, typename S2>
-void testShapeCollide(const S1& s1, const Transform3f& tf1, const S2& s2,
-                      const Transform3f& tf2, bool expect_collision,
-                      Vec3f* expected_point = NULL,
-                      FCL_REAL* expected_depth = NULL,
-                      Vec3f* expected_normal = NULL,
-                      bool check_opposite_normal = false, FCL_REAL tol = 1e-9) {
+void testShapeCollide(const S1& s1, const Transform3s& tf1, const S2& s2,
+                      const Transform3s& tf2, bool expect_collision,
+                      Vec3s* expected_point = NULL,
+                      CoalScalar* expected_depth = NULL,
+                      Vec3s* expected_normal = NULL,
+                      bool check_opposite_normal = false,
+                      CoalScalar tol = 1e-9) {
   CollisionRequest request;
   CollisionResult result;
 
-  Vec3f contact;
-  Vec3f normal;  // normal direction should be from object 1 to object 2
+  Vec3s contact;
+  Vec3s normal;  // normal direction should be from object 1 to object 2
   bool collision;
   bool check_failed = false;
 
@@ -211,85 +210,85 @@ void testShapeCollide(const S1& s1, const Transform3f& tf1, const S2& s2,
 BOOST_AUTO_TEST_CASE(box_to_bvh) {
   Box shape(1, 1, 1);
   BVHModel<OBB> bvh;
-  generateBVHModel(bvh, shape, Transform3f());
+  generateBVHModel(bvh, shape, Transform3s());
 }
 
 BOOST_AUTO_TEST_CASE(sphere_to_bvh) {
   Sphere shape(1);
   BVHModel<OBB> bvh;
-  generateBVHModel(bvh, shape, Transform3f(), 10, 10);
-  generateBVHModel(bvh, shape, Transform3f(), 50);
+  generateBVHModel(bvh, shape, Transform3s(), 10, 10);
+  generateBVHModel(bvh, shape, Transform3s(), 50);
 }
 
 BOOST_AUTO_TEST_CASE(cylinder_to_bvh) {
   Cylinder shape(1, 1);
   BVHModel<OBB> bvh;
-  generateBVHModel(bvh, shape, Transform3f(), 10, 10);
-  generateBVHModel(bvh, shape, Transform3f(), 50);
+  generateBVHModel(bvh, shape, Transform3s(), 10, 10);
+  generateBVHModel(bvh, shape, Transform3s(), 50);
 }
 
 BOOST_AUTO_TEST_CASE(cone_to_bvh) {
   Cone shape(1, 1);
   BVHModel<OBB> bvh;
-  generateBVHModel(bvh, shape, Transform3f(), 10, 10);
-  generateBVHModel(bvh, shape, Transform3f(), 50);
+  generateBVHModel(bvh, shape, Transform3s(), 10, 10);
+  generateBVHModel(bvh, shape, Transform3s(), 50);
 }
 
 BOOST_AUTO_TEST_CASE(collide_spheresphere) {
   Sphere s1(20);
   Sphere s2(10);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  // Vec3f point;
-  // FCL_REAL depth;
-  Vec3f normal;
+  // Vec3s point;
+  // CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(40, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(40, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(40, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(30, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(30, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(30.01, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(30.01, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  tf2 = transform * Transform3s(Vec3s(30.01, 0, 0));
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(29.9, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(29.9, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  tf2 = transform * Transform3s(Vec3s(29.9, 0, 0));
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   normal << 1, 0, 0;  // If the centers of two sphere are at the same position,
                       // the normal is (1, 0, 0)
   SET_LINE;
@@ -302,46 +301,46 @@ BOOST_AUTO_TEST_CASE(collide_spheresphere) {
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-29.9, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-29.9, 0, 0));
   normal << -1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  tf2 = transform * Transform3s(Vec3s(-29.9, 0, 0));
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-30.0, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-30.0, 0, 0));
   normal << -1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-30.01, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-30.01, 0, 0));
   normal << -1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-30.01, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 }
 
-bool compareContactPoints(const Vec3f& c1, const Vec3f& c2) {
+bool compareContactPoints(const Vec3s& c1, const Vec3s& c2) {
   return c1[2] < c2[2];
 }  // Ascending order
 
-void testBoxBoxContactPoints(const Matrix3f& R) {
+void testBoxBoxContactPoints(const Matrix3s& R) {
   Box s1(100, 100, 100);
   Box s2(10, 20, 30);
 
   // Vertices of s2
-  std::vector<Vec3f> vertices(8);
+  std::vector<Vec3s> vertices(8);
   vertices[0] << 1, 1, 1;
   vertices[1] << 1, 1, -1;
   vertices[2] << 1, -1, 1;
@@ -355,17 +354,17 @@ void testBoxBoxContactPoints(const Matrix3f& R) {
     vertices[i].array() *= s2.halfSide.array();
   }
 
-  Transform3f tf1 = Transform3f(Vec3f(0, 0, -50));
-  Transform3f tf2 = Transform3f(R);
+  Transform3s tf1 = Transform3s(Vec3s(0, 0, -50));
+  Transform3s tf2 = Transform3s(R);
 
-  Vec3f normal;
-  Vec3f p1, p2;
+  Vec3s normal;
+  Vec3s p1, p2;
 
   // Make sure the two boxes are colliding
   solver1.gjk_tolerance = 1e-5;
   solver1.epa_tolerance = 1e-5;
   const bool compute_penetration = true;
-  FCL_REAL distance = solver1.shapeDistance(
+  CoalScalar distance = solver1.shapeDistance(
       s1, tf1, s2, tf2, compute_penetration, p1, p2, normal);
   FCL_CHECK(distance <= 0);
 
@@ -376,8 +375,8 @@ void testBoxBoxContactPoints(const Matrix3f& R) {
   std::sort(vertices.begin(), vertices.end(), compareContactPoints);
 
   // The lowest vertex along z-axis should be the contact point
-  FCL_CHECK(normal.isApprox(Vec3f(0, 0, 1), 1e-6));
-  Vec3f point = 0.5 * (p1 + p2);
+  FCL_CHECK(normal.isApprox(Vec3s(0, 0, 1), 1e-6));
+  Vec3s point = 0.5 * (p1 + p2);
   FCL_CHECK(vertices[0].head<2>().isApprox(point.head<2>(), 1e-6));
   FCL_CHECK(vertices[0][2] <= point[2] && point[2] < 0);
 }
@@ -386,21 +385,21 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) {
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  // Vec3f point;
-  // FCL_REAL depth;
-  Vec3f normal;
+  // Vec3s point;
+  // CoalScalar depth;
+  Vec3s normal;
 
   Quatf q;
-  q = AngleAxis((FCL_REAL)3.140 / 6, UnitZ);
+  q = AngleAxis((CoalScalar)3.140 / 6, UnitZ);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   // TODO: Need convention for normal when the centers of two objects are at
   // same position. The current result is (1, 0, 0).
   normal << 1, 0, 0;
@@ -411,41 +410,41 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) {
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at
   // same position. The current result is (1, 0, 0).
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(15, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(15, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-8);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(15.01, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(15.01, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(15.01, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(15.01, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(q);
+  tf1 = Transform3s();
+  tf2 = Transform3s(q);
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(q);
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  tf2 = transform * Transform3s(q);
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
 
   int numTests = 100;
   for (int i = 0; i < numTests; ++i) {
-    Transform3f tf;
+    Transform3s tf;
     generateRandomTransform(extents, tf);
     SET_LINE;
     testBoxBoxContactPoints(tf.getRotation());
@@ -456,18 +455,18 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) {
   Sphere s1(20);
   Box s2(5, 5, 5);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  // Vec3f point;
-  // FCL_REAL depth;
-  Vec3f normal;
+  // Vec3s point;
+  // CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   // TODO: Need convention for normal when the centers of two objects are at
   // same position. The current result is (-1, 0, 0).
   normal << -1, 0, 0;
@@ -481,68 +480,68 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) {
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(22.50001, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(22.50001, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(22.501, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(22.501, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(22.4, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(22.4, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  tf2 = transform * Transform3s(Vec3s(22.4, 0, 0));
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 }
 
 BOOST_AUTO_TEST_CASE(distance_spherebox) {
-  hpp::fcl::Matrix3f rotSphere;
+  coal::Matrix3s rotSphere;
   rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
-  hpp::fcl::Vec3f trSphere(0.0, 0.0, 0.0);
+  coal::Vec3s trSphere(0.0, 0.0, 0.0);
 
-  hpp::fcl::Matrix3f rotBox;
+  coal::Matrix3s rotBox;
   rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
-  hpp::fcl::Vec3f trBox(0.0, 5.0, 3.0);
+  coal::Vec3s trBox(0.0, 5.0, 3.0);
 
-  hpp::fcl::Sphere sphere(1);
-  hpp::fcl::Box box(10, 2, 10);
+  coal::Sphere sphere(1);
+  coal::Box box(10, 2, 10);
 
-  hpp::fcl::DistanceResult result;
-  distance(&sphere, Transform3f(rotSphere, trSphere), &box,
-           Transform3f(rotBox, trBox), DistanceRequest(true), result);
+  coal::DistanceResult result;
+  distance(&sphere, Transform3s(rotSphere, trSphere), &box,
+           Transform3s(rotBox, trBox), DistanceRequest(true), result);
 
-  FCL_REAL eps = Eigen::NumTraits<FCL_REAL>::epsilon();
+  CoalScalar eps = Eigen::NumTraits<CoalScalar>::epsilon();
   BOOST_CHECK_CLOSE(result.min_distance, 3., eps);
-  EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3f(0, 1, 0), eps);
-  EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3f(0, 4, 0), eps);
-  EIGEN_VECTOR_IS_APPROX(result.normal, Vec3f(0, 1, 0), eps);
+  EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3s(0, 1, 0), eps);
+  EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3s(0, 4, 0), eps);
+  EIGEN_VECTOR_IS_APPROX(result.normal, Vec3s(0, 1, 0), eps);
 }
 
 BOOST_AUTO_TEST_CASE(collide_spherecapsule) {
   Sphere s1(20);
   Capsule s2(5, 10);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  // Vec3f point;
-  // FCL_REAL depth;
-  Vec3f normal;
+  // Vec3s point;
+  // CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   // TODO: Need convention for normal when the centers of two objects are at
   // same position.
   SET_LINE;
@@ -555,39 +554,39 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) {
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(24.9, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(24.9, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  tf2 = transform * Transform3s(Vec3s(24.9, 0, 0));
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(25, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(25, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(24.999999, 0, 0));
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  tf2 = transform * Transform3s(Vec3s(24.999999, 0, 0));
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(25.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(25.1, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  tf2 = transform * Transform3s(Vec3s(25.1, 0, 0));
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 }
@@ -596,18 +595,18 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) {
   Cylinder s1(5, 15);
   Cylinder s2(5, 15);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  // Vec3f point;
-  // FCL_REAL depth;
-  Vec3f normal;
+  // Vec3s point;
+  // CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   // TODO: Need convention for normal when the centers of two objects are at
   // same position.
   SET_LINE;
@@ -620,37 +619,37 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) {
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(9.9, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 9.9, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 9.9, 0));
   normal << 0, 1, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(9.9, 0, 0));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  tf2 = transform * Transform3s(Vec3s(9.9, 0, 0));
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(10.01, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(10.01, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(10.01, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 }
@@ -659,18 +658,18 @@ BOOST_AUTO_TEST_CASE(collide_conecone) {
   Cone s1(5, 10);
   Cone s2(5, 10);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  // Vec3f point;
-  // FCL_REAL depth;
-  Vec3f normal;
+  // Vec3s point;
+  // CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   // TODO: Need convention for normal when the centers of two objects are at
   // same position.
   SET_LINE;
@@ -683,10 +682,10 @@ BOOST_AUTO_TEST_CASE(collide_conecone) {
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
 
-  tf1 = Transform3f();
+  tf1 = Transform3s();
   // z=0 is a singular points. Two normals could be returned.
-  tf2 = Transform3f(Vec3f(9.9, 0, 0.00001));
-  normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
+  tf2 = Transform3s(Vec3s(9.9, 0, 0.00001));
+  normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
                .normalized();
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
@@ -698,37 +697,37 @@ BOOST_AUTO_TEST_CASE(collide_conecone) {
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0.00001));
-  normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
+  tf2 = transform * Transform3s(Vec3s(9.9, 0, 0.00001));
+  normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
                .normalized();
   normal = transform.getRotation() * normal;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, true, tol_gjk);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(10.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(10.1, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(10.001, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(10.001, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(10.001, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(10.001, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 9.9));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 9.9));
   normal << 0, 0, 1;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
-  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  tf2 = transform * Transform3s(Vec3s(0, 0, 9.9));
+  normal = transform.getRotation() * Vec3s(0, 0, 1);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 }
@@ -737,18 +736,18 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) {
   Cylinder s1(5, 10);
   Cone s2(5, 10);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  // Vec3f point;
-  // FCL_REAL depth;
-  Vec3f normal;
+  // Vec3s point;
+  // CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   // TODO: Need convention for normal when the centers of two objects are at
   // same position.
   SET_LINE;
@@ -761,82 +760,82 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) {
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(9.9, 0, 0));
   normal =
-      Vec3f(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius))
+      Vec3s(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius))
           .normalized();
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(9.9, 0, 0));
   normal = transform.getRotation() * normal;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(9.9, 0, 0.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(9.9, 0, 0.1));
   normal << 1, 0, 0;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0.1));
+  tf2 = transform * Transform3s(Vec3s(9.9, 0, 0.1));
   normal = transform.getRotation() * normal;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(10.01, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(10.01, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(10.01, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(10, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(10, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(10, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(10, 0, 0));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 9.9));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 9.9));
   normal << 0, 0, 1;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
-  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  tf2 = transform * Transform3s(Vec3s(0, 0, 9.9));
+  normal = transform.getRotation() * Vec3s(0, 0, 1);
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 10.01));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 10.01));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 10.01));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 10.01));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 10));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 10));
   normal << 0, 0, 1;
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 10.1));
   SET_LINE;
   testShapeCollide(s1, tf1, s2, tf2, false);
 }
@@ -844,35 +843,35 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) {
 BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
   Sphere s(10);
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f normal;
+  Vec3s normal;
 
   //
   // Testing collision x, y, z
   //
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 20, 0, 0;
     t[1] << -20, 0, 0;
     t[2] << 0, 20, 0;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();  // identity
+    Transform3s tf_tri = Transform3s();  // identity
 
-    tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, 0.001));
     normal << 0, 0, 1;
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, -0.001));
     normal << 0, 0, -1;
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
@@ -880,28 +879,28 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 30, 0, 0;
     t[1] << 9.9, -20, 0;
     t[2] << 9.9, 20, 0;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();
+    Transform3s tf_tri = Transform3s();
 
-    tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, 0.001));
     normal << 9.9, 0, 0.001;
     normal.normalize();
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, -0.001));
     normal << 9.9, 0, -0.001;
     normal.normalize();
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
@@ -909,26 +908,26 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 30, 0, 0;
     t[1] << -20, 0, 0;
     t[2] << 0, 0, 20;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();
+    Transform3s tf_tri = Transform3s();
 
-    tf_tri.setTranslation(Vec3f(0, 0.001, 0));
+    tf_tri.setTranslation(Vec3s(0, 0.001, 0));
     normal << 0, 1, 0;
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, -0.001, 0));
+    tf_tri.setTranslation(Vec3s(0, -0.001, 0));
     normal << 0, -1, 0;
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
@@ -936,25 +935,25 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 0, 30, 0;
     t[1] << 0, -10, 0;
     t[2] << 0, 0, 20;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();
+    Transform3s tf_tri = Transform3s();
 
-    tf_tri.setTranslation(Vec3f(0.001, 0, 0));
+    tf_tri.setTranslation(Vec3s(0.001, 0, 0));
     normal << 1, 0, 0;
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(-0.001, 0, 0));
+    tf_tri.setTranslation(Vec3s(-0.001, 0, 0));
     normal << -1, 0, 0;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     SET_LINE;
     normal = transform.getRotation() * normal;
     SET_LINE;
@@ -966,307 +965,307 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
   // Testing no collision x, y, z
   //
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 20, 0, 0;
     t[1] << -20, 0, 0;
     t[2] << 0, 20, 0;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();
+    Transform3s tf_tri = Transform3s();
 
-    tf_tri.setTranslation(Vec3f(0, 0, 10.1));
+    tf_tri.setTranslation(Vec3s(0, 0, 10.1));
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(0, 0, -10.1));
+    tf_tri.setTranslation(Vec3s(0, 0, -10.1));
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, false);
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 20, 0, 0;
     t[1] << -20, 0, 0;
     t[2] << 0, 0, 20;
     TriangleP tri(t[0], t[1], t[2]);
 
-    Transform3f tf_tri = Transform3f();
-    tf_tri.setTranslation(Vec3f(0, 10.1, 0));
+    Transform3s tf_tri = Transform3s();
+    tf_tri.setTranslation(Vec3s(0, 10.1, 0));
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(0, -10.1, 0));
+    tf_tri.setTranslation(Vec3s(0, -10.1, 0));
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, false);
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 0, 20, 0;
     t[1] << 0, -20, 0;
     t[2] << 0, 0, 20;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();
+    Transform3s tf_tri = Transform3s();
 
-    tf_tri.setTranslation(Vec3f(10.1, 0, 0));
+    tf_tri.setTranslation(Vec3s(10.1, 0, 0));
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(-10.1, 0, 0));
+    tf_tri.setTranslation(Vec3s(-10.1, 0, 0));
     SET_LINE;
-    testShapeCollide(s, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(s, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(s, transform, tri, transform * tf_tri, false);
   }
 }
 
 BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) {
-  Halfspace hs(Vec3f(0, 0, 1), 0);
+  Halfspace hs(Vec3s(0, 0, 1), 0);
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f normal;
+  Vec3s normal;
   normal = hs.n;  // with halfspaces, it's simple: normal is always
                   // the normal of the halfspace.
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 20, 0, 0;
     t[1] << -20, 0, 0;
     t[2] << 0, 20, 0;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();  // identity
+    Transform3s tf_tri = Transform3s();  // identity
 
-    tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, -0.001));
     normal = hs.n;
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(1, 1, 0.001));
+    tf_tri.setTranslation(Vec3s(1, 1, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(-1, -1, 0.001));
+    tf_tri.setTranslation(Vec3s(-1, -1, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 30, 0, 0;
     t[1] << -20, 0, 0;
     t[2] << 0, 0, 20;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();
+    Transform3s tf_tri = Transform3s();
 
-    tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, -0.001));
     normal = hs.n;
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(1, 1, 0.001));
+    tf_tri.setTranslation(Vec3s(1, 1, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(-1, -1, 0.001));
+    tf_tri.setTranslation(Vec3s(-1, -1, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 0, 30, 0;
     t[1] << 0, -10, 0;
     t[2] << 0, 0, 20;
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();
+    Transform3s tf_tri = Transform3s();
 
-    tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, -0.001));
     normal = hs.n;
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+    tf_tri.setTranslation(Vec3s(0, 0, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(1, 1, 0.001));
+    tf_tri.setTranslation(Vec3s(1, 1, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(-1, -1, 0.001));
+    tf_tri.setTranslation(Vec3s(-1, -1, 0.001));
     SET_LINE;
-    testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(hs, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(hs, transform, tri, transform * tf_tri, false);
   }
 }
 
 BOOST_AUTO_TEST_CASE(collide_planetriangle) {
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f normal;
+  Vec3s normal;
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 20, 0, 0.05;
     t[1] << -20, 0, 0.05;
     t[2] << 0, 20, -0.1;
-    Plane pl(Vec3f(0, 0, 1), 0);
+    Plane pl(Vec3s(0, 0, 1), 0);
 
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();  // identity
+    Transform3s tf_tri = Transform3s();  // identity
     normal = -pl.n;
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, 0, 0.05));
+    tf_tri.setTranslation(Vec3s(0, 0, 0.05));
     normal = pl.n;
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, 0, -0.06));
+    tf_tri.setTranslation(Vec3s(0, 0, -0.06));
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(0, 0, 0.11));
+    tf_tri.setTranslation(Vec3s(0, 0, 0.11));
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, false);
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 30, 0.05, 0;
     t[1] << -20, 0.05, 0;
     t[2] << 0, -0.1, 20;
-    Plane pl(Vec3f(0, 1, 0), 0);
+    Plane pl(Vec3s(0, 1, 0), 0);
 
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();  // identity
+    Transform3s tf_tri = Transform3s();  // identity
     normal = -pl.n;
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, 0.05, 0));
+    tf_tri.setTranslation(Vec3s(0, 0.05, 0));
     normal = pl.n;
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0, -0.06, 0));
+    tf_tri.setTranslation(Vec3s(0, -0.06, 0));
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(0, 0.11, 0));
+    tf_tri.setTranslation(Vec3s(0, 0.11, 0));
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, false);
   }
 
   {
-    Vec3f t[3];
+    Vec3s t[3];
     t[0] << 0.05, 30, 0;
     t[1] << 0.05, -10, 0;
     t[2] << -0.1, 0, 20;
-    Plane pl(Vec3f(1, 0, 0), 0);
+    Plane pl(Vec3s(1, 0, 0), 0);
 
     TriangleP tri(t[0], t[1], t[2]);
-    Transform3f tf_tri = Transform3f();  // identity
+    Transform3s tf_tri = Transform3s();  // identity
     normal = -pl.n;
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(0.05, 0, 0));
+    tf_tri.setTranslation(Vec3s(0.05, 0, 0));
     normal = pl.n;
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal);
     normal = transform.getRotation() * normal;
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
                      &normal);
 
-    tf_tri.setTranslation(Vec3f(-0.06, 0, 0));
+    tf_tri.setTranslation(Vec3s(-0.06, 0, 0));
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, false);
 
-    tf_tri.setTranslation(Vec3f(0.11, 0, 0));
+    tf_tri.setTranslation(Vec3s(0.11, 0, 0));
     SET_LINE;
-    testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
+    testShapeCollide(pl, Transform3s(), tri, tf_tri, false);
     SET_LINE;
     testShapeCollide(pl, transform, tri, transform * tf_tri, false);
   }
@@ -1274,20 +1273,20 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
 
 BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
   Sphere s(10);
-  Halfspace hs(Vec3f(1, 0, 0), 0);
+  Halfspace hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << -5, 0, 0;
   depth = -10;
   normal << -1, 0, 0;
@@ -1296,14 +1295,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(-5, 0, 0));
+  contact = transform.transform(Vec3s(-5, 0, 0));
   depth = -10;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(5, 0, 0));
   contact << -2.5, 0, 0;
   depth = -15;
   normal << -1, 0, 0;
@@ -1311,15 +1310,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(5, 0, 0));
-  contact = transform.transform(Vec3f(-2.5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(5, 0, 0));
+  contact = transform.transform(Vec3s(-2.5, 0, 0));
   depth = -15;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-5, 0, 0));
   contact << -7.5, 0, 0;
   depth = -5;
   normal << -1, 0, 0;
@@ -1327,25 +1326,25 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
-  contact = transform.transform(Vec3f(-7.5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-5, 0, 0));
+  contact = transform.transform(Vec3s(-7.5, 0, 0));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-10.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-10.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-10.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(10.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(10.1, 0, 0));
   contact << 0.05, 0, 0;
   depth = -20.1;
   normal << -1, 0, 0;
@@ -1353,32 +1352,32 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
-  contact = transform.transform(Vec3f(0.05, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(10.1, 0, 0));
+  contact = transform.transform(Vec3s(0.05, 0, 0));
   depth = -20.1;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(collide_planesphere) {
   Sphere s(10);
-  Plane hs(Vec3f(1, 0, 0), 0);
+  Plane hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
-  Vec3f p1, p2;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
+  Vec3s p1, p2;
 
-  FCL_REAL eps = 1e-6;
-  tf1 = Transform3f(Vec3f(eps, 0, 0));
-  tf2 = Transform3f();
+  CoalScalar eps = 1e-6;
+  tf1 = Transform3s(Vec3s(eps, 0, 0));
+  tf2 = Transform3s();
   depth = -10 + eps;
   p1 << -10 + eps, 0, 0;
   p2 << 0, 0, 0;
@@ -1391,13 +1390,13 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   normal =
-      transform.getRotation() * Vec3f(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+      transform.getRotation() * Vec3s(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
   eps = -1e-6;
-  tf1 = Transform3f(Vec3f(eps, 0, 0));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(eps, 0, 0));
+  tf2 = Transform3s();
   depth = -10 - eps;
   p1 << 10 + eps, 0, 0;
   p2 << 0, 0, 0;
@@ -1409,12 +1408,12 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
   tf1 = transform * tf1;
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
-  normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(5, 0, 0));
   p1 << 10, 0, 0;
   p2 << 5, 0, 0;
   contact << (p1 + p2) / 2;
@@ -1424,15 +1423,15 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(5, 0, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -5;
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-5, 0, 0));
   p1 << -10, 0, 0;
   p2 << -5, 0, 0;
   contact << (p1 + p2) / 2;
@@ -1442,50 +1441,50 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-5, 0, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-10.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-10.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-10.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(10.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(10.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(10.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 }
 
 BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
   Box s(5, 10, 20);
-  Halfspace hs(Vec3f(1, 0, 0), 0);
+  Halfspace hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << -1.25, 0, 0;
   depth = -2.5;
   normal << -1, 0, 0;
@@ -1494,14 +1493,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(-1.25, 0, 0));
+  contact = transform.transform(Vec3s(-1.25, 0, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(1.25, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(1.25, 0, 0));
   contact << -0.625, 0, 0;
   depth = -3.75;
   normal << -1, 0, 0;
@@ -1509,15 +1508,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
-  contact = transform.transform(Vec3f(-0.625, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(1.25, 0, 0));
+  contact = transform.transform(Vec3s(-0.625, 0, 0));
   depth = -3.75;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-1.25, 0, 0));
   contact << -1.875, 0, 0;
   depth = -1.25;
   normal << -1, 0, 0;
@@ -1525,15 +1524,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
-  contact = transform.transform(Vec3f(-1.875, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-1.25, 0, 0));
+  contact = transform.transform(Vec3s(-1.875, 0, 0));
   depth = -1.25;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.51, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(2.51, 0, 0));
   contact << 0.005, 0, 0;
   depth = -5.01;
   normal << -1, 0, 0;
@@ -1541,47 +1540,47 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
-  contact = transform.transform(Vec3f(0.005, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(2.51, 0, 0));
+  contact = transform.transform(Vec3s(0.005, 0, 0));
   depth = -5.01;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-2.51, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-2.51, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f(transform.getRotation());
-  tf2 = Transform3f();
+  tf1 = Transform3s(transform.getRotation());
+  tf2 = Transform3s();
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true);
 }
 
 BOOST_AUTO_TEST_CASE(collide_planebox) {
   Box s(5, 10, 20);
-  Plane hs(Vec3f(1, 0, 0), 0);
+  Plane hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
-  Vec3f p1(2.5, 0, 0);
-  Vec3f p2(0, 0, 0);
+  tf1 = Transform3s();
+  tf2 = Transform3s();
+  Vec3s p1(2.5, 0, 0);
+  Vec3s p2(0, 0, 0);
   contact << (p1 + p2) / 2;
   depth = -2.5;
   normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
@@ -1592,12 +1591,12 @@ BOOST_AUTO_TEST_CASE(collide_planebox) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(1.25, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(1.25, 0, 0));
   p2 << 1.25, 0, 0;
   contact << (p1 + p2) / 2;
   depth = -1.25;
@@ -1606,15 +1605,15 @@ BOOST_AUTO_TEST_CASE(collide_planebox) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(1.25, 0, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -1.25;
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-1.25, 0, 0));
   p1 << -2.5, 0, 0;
   p2 << -1.25, 0, 0;
   contact << (p1 + p2) / 2;
@@ -1624,55 +1623,55 @@ BOOST_AUTO_TEST_CASE(collide_planebox) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-1.25, 0, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -1.25;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.51, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(2.51, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(2.51, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-2.51, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-2.51, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f(transform.getRotation());
-  tf2 = Transform3f();
+  tf1 = Transform3s(transform.getRotation());
+  tf2 = Transform3s();
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true);
 }
 
 BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   Capsule s(5, 10);
-  Halfspace hs(Vec3f(1, 0, 0), 0);
+  Halfspace hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << -2.5, 0, 0;
   depth = -5;
   normal << -1, 0, 0;
@@ -1681,14 +1680,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(-2.5, 0, 0));
+  contact = transform.transform(Vec3s(-2.5, 0, 0));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(2.5, 0, 0));
   contact << -1.25, 0, 0;
   depth = -7.5;
   normal << -1, 0, 0;
@@ -1696,15 +1695,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
-  contact = transform.transform(Vec3f(-1.25, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(2.5, 0, 0));
+  contact = transform.transform(Vec3s(-1.25, 0, 0));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-2.5, 0, 0));
   contact << -3.75, 0, 0;
   depth = -2.5;
   normal << -1, 0, 0;
@@ -1712,15 +1711,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
-  contact = transform.transform(Vec3f(-3.75, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0));
+  contact = transform.transform(Vec3s(-3.75, 0, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(5.1, 0, 0));
   contact << 0.05, 0, 0;
   depth = -10.1;
   normal << -1, 0, 0;
@@ -1728,27 +1727,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
-  contact = transform.transform(Vec3f(0.05, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(5.1, 0, 0));
+  contact = transform.transform(Vec3s(0.05, 0, 0));
   depth = -10.1;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Halfspace(Vec3f(0, 1, 0), 0);
+  hs = Halfspace(Vec3s(0, 1, 0), 0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, -2.5, 0;
   depth = -5;
   normal << 0, -1, 0;
@@ -1757,14 +1756,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, -2.5, 0));
+  contact = transform.transform(Vec3s(0, -2.5, 0));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 2.5, 0));
   contact << 0, -1.25, 0;
   depth = -7.5;
   normal << 0, -1, 0;
@@ -1772,15 +1771,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
-  contact = transform.transform(Vec3f(0, -1.25, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 2.5, 0));
+  contact = transform.transform(Vec3s(0, -1.25, 0));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -2.5, 0));
   contact << 0, -3.75, 0;
   depth = -2.5;
   normal << 0, -1, 0;
@@ -1788,15 +1787,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
-  contact = transform.transform(Vec3f(0, -3.75, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -2.5, 0));
+  contact = transform.transform(Vec3s(0, -3.75, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 5.1, 0));
   contact << 0, 0.05, 0;
   depth = -10.1;
   normal << 0, -1, 0;
@@ -1804,27 +1803,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
-  contact = transform.transform(Vec3f(0, 0.05, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 5.1, 0));
+  contact = transform.transform(Vec3s(0, 0.05, 0));
   depth = -10.1;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Halfspace(Vec3f(0, 0, 1), 0);
+  hs = Halfspace(Vec3s(0, 0, 1), 0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, 0, -5;
   depth = -10;
   normal << 0, 0, -1;
@@ -1833,14 +1832,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, 0, -5));
+  contact = transform.transform(Vec3s(0, 0, -5));
   depth = -10;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 2.5));
   contact << 0, 0, -3.75;
   depth = -12.5;
   normal << 0, 0, -1;
@@ -1848,15 +1847,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
-  contact = transform.transform(Vec3f(0, 0, -3.75));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 2.5));
+  contact = transform.transform(Vec3s(0, 0, -3.75));
   depth = -12.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -2.5));
   contact << 0, 0, -6.25;
   depth = -7.5;
   normal << 0, 0, -1;
@@ -1864,15 +1863,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
-  contact = transform.transform(Vec3f(0, 0, -6.25));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -2.5));
+  contact = transform.transform(Vec3s(0, 0, -6.25));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 10.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 10.1));
   contact << 0, 0, 0.05;
   depth = -20.1;
   normal << 0, 0, -1;
@@ -1880,40 +1879,40 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
-  contact = transform.transform(Vec3f(0, 0, 0.05));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 10.1));
+  contact = transform.transform(Vec3s(0, 0, 0.05));
   depth = -20.1;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -10.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 }
 
 BOOST_AUTO_TEST_CASE(collide_planecapsule) {
   Capsule s(5, 10);
-  Plane hs(Vec3f(1, 0, 0), 0);
+  Plane hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, 0, 0;
   depth = -5;
   normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
@@ -1922,14 +1921,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, 0, 0));
+  contact = transform.transform(Vec3s(0, 0, 0));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(2.5, 0, 0));
   contact << 2.5, 0, 0;
   depth = -2.5;
   normal << 1, 0, 0;
@@ -1937,15 +1936,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
-  contact = transform.transform(Vec3f(2.5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(2.5, 0, 0));
+  contact = transform.transform(Vec3s(2.5, 0, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-2.5, 0, 0));
   contact << -2.5, 0, 0;
   depth = -2.5;
   normal << -1, 0, 0;
@@ -1953,37 +1952,37 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
-  contact = transform.transform(Vec3f(-2.5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0));
+  contact = transform.transform(Vec3s(-2.5, 0, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Plane(Vec3f(0, 1, 0), 0);
+  hs = Plane(Vec3s(0, 1, 0), 0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, 0, 0;
   depth = -5;
   normal << 0, 1, 0;  // (0, 1, 0) or (0, -1, 0)
@@ -1992,14 +1991,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, 0, 0));
+  contact = transform.transform(Vec3s(0, 0, 0));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(0, 1, 0);  // (0, 1, 0) or (0, -1, 0)
+  normal = transform.getRotation() * Vec3s(0, 1, 0);  // (0, 1, 0) or (0, -1, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 2.5, 0));
   contact << 0, 2.5, 0;
   depth = -2.5;
   normal << 0, 1, 0;
@@ -2007,15 +2006,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
-  contact = transform.transform(Vec3f(0, 2.5, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 2.5, 0));
+  contact = transform.transform(Vec3s(0, 2.5, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 1, 0);
+  normal = transform.getRotation() * Vec3s(0, 1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -2.5, 0));
   contact << 0, -2.5, 0;
   depth = -2.5;
   normal << 0, -1, 0;
@@ -2023,37 +2022,37 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
-  contact = transform.transform(Vec3f(0, -2.5, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -2.5, 0));
+  contact = transform.transform(Vec3s(0, -2.5, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Plane(Vec3f(0, 0, 1), 0);
+  hs = Plane(Vec3s(0, 0, 1), 0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, 0, 0;
   depth = -10;
   normal << 0, 0, 1;  // (0, 0, 1) or (0, 0, -1)
@@ -2062,14 +2061,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, 0, 0));
+  contact = transform.transform(Vec3s(0, 0, 0));
   depth = -10;
-  normal = transform.getRotation() * Vec3f(0, 0, 1);  // (0, 0, 1) or (0, 0, -1)
+  normal = transform.getRotation() * Vec3s(0, 0, 1);  // (0, 0, 1) or (0, 0, -1)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 2.5));
   contact << 0, 0, 2.5;
   depth = -7.5;
   normal << 0, 0, 1;
@@ -2077,15 +2076,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
-  contact = transform.transform(Vec3f(0, 0, 2.5));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 2.5));
+  contact = transform.transform(Vec3s(0, 0, 2.5));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  normal = transform.getRotation() * Vec3s(0, 0, 1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -2.5));
   contact << 0, 0, -2.5;
   depth = -7.5;
   normal << 0, 0, -1;
@@ -2093,50 +2092,50 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
-  contact = transform.transform(Vec3f(0, 0, -2.5));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -2.5));
+  contact = transform.transform(Vec3s(0, 0, -2.5));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 10.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -10.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 }
 
 BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   Cylinder s(5, 10);
-  Halfspace hs(Vec3f(1, 0, 0), 0);
+  Halfspace hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << -2.5, 0, 0;
   depth = -5;
   normal << -1, 0, 0;
@@ -2145,14 +2144,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(-2.5, 0, 0));
+  contact = transform.transform(Vec3s(-2.5, 0, 0));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(2.5, 0, 0));
   contact << -1.25, 0, 0;
   depth = -7.5;
   normal << -1, 0, 0;
@@ -2160,15 +2159,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
-  contact = transform.transform(Vec3f(-1.25, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(2.5, 0, 0));
+  contact = transform.transform(Vec3s(-1.25, 0, 0));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-2.5, 0, 0));
   contact << -3.75, 0, 0;
   depth = -2.5;
   normal << -1, 0, 0;
@@ -2176,15 +2175,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
-  contact = transform.transform(Vec3f(-3.75, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0));
+  contact = transform.transform(Vec3s(-3.75, 0, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(5.1, 0, 0));
   contact << 0.05, 0, 0;
   depth = -10.1;
   normal << -1, 0, 0;
@@ -2192,27 +2191,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
-  contact = transform.transform(Vec3f(0.05, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(5.1, 0, 0));
+  contact = transform.transform(Vec3s(0.05, 0, 0));
   depth = -10.1;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Halfspace(Vec3f(0, 1, 0), 0);
+  hs = Halfspace(Vec3s(0, 1, 0), 0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, -2.5, 0;
   depth = -5;
   normal << 0, -1, 0;
@@ -2221,14 +2220,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, -2.5, 0));
+  contact = transform.transform(Vec3s(0, -2.5, 0));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 2.5, 0));
   contact << 0, -1.25, 0;
   depth = -7.5;
   normal << 0, -1, 0;
@@ -2236,15 +2235,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
-  contact = transform.transform(Vec3f(0, -1.25, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 2.5, 0));
+  contact = transform.transform(Vec3s(0, -1.25, 0));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -2.5, 0));
   contact << 0, -3.75, 0;
   depth = -2.5;
   normal << 0, -1, 0;
@@ -2252,15 +2251,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
-  contact = transform.transform(Vec3f(0, -3.75, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -2.5, 0));
+  contact = transform.transform(Vec3s(0, -3.75, 0));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 5.1, 0));
   contact << 0, 0.05, 0;
   depth = -10.1;
   normal << 0, -1, 0;
@@ -2268,27 +2267,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
-  contact = transform.transform(Vec3f(0, 0.05, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 5.1, 0));
+  contact = transform.transform(Vec3s(0, 0.05, 0));
   depth = -10.1;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Halfspace(Vec3f(0, 0, 1), 0);
+  hs = Halfspace(Vec3s(0, 0, 1), 0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, 0, -2.5;
   depth = -5;
   normal << 0, 0, -1;
@@ -2297,14 +2296,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, 0, -2.5));
+  contact = transform.transform(Vec3s(0, 0, -2.5));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 2.5));
   contact << 0, 0, -1.25;
   depth = -7.5;
   normal << 0, 0, -1;
@@ -2312,15 +2311,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
-  contact = transform.transform(Vec3f(0, 0, -1.25));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 2.5));
+  contact = transform.transform(Vec3s(0, 0, -1.25));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -2.5));
   contact << 0, 0, -3.75;
   depth = -2.5;
   normal << 0, 0, -1;
@@ -2328,15 +2327,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
-  contact = transform.transform(Vec3f(0, 0, -3.75));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -2.5));
+  contact = transform.transform(Vec3s(0, 0, -3.75));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 5.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 5.1));
   contact << 0, 0, 0.05;
   depth = -10.1;
   normal << 0, 0, -1;
@@ -2344,42 +2343,42 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
-  contact = transform.transform(Vec3f(0, 0, 0.05));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 5.1));
+  contact = transform.transform(Vec3s(0, 0, 0.05));
   depth = -10.1;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -5.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -5.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -5.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 }
 
 BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   Cylinder s(5, 10);
-  Plane hs(Vec3f(1, 0, 0), 0);
+  Plane hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
-  Vec3f p1, p2;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
+  Vec3s p1, p2;
 
-  FCL_REAL eps = 1e-6;
-  tf1 = Transform3f(Vec3f(eps, 0, 0));
-  tf2 = Transform3f();
+  CoalScalar eps = 1e-6;
+  tf1 = Transform3s(Vec3s(eps, 0, 0));
+  tf2 = Transform3s();
   p1 << -5 + eps, 0, 0;
   p2 << 0, 0, 0;
   contact << (p1 + p2) / 2;
@@ -2393,13 +2392,13 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 + eps;
   normal =
-      transform.getRotation() * Vec3f(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+      transform.getRotation() * Vec3s(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
   eps = -1e-6;
-  tf1 = Transform3f(Vec3f(eps, 0, 0));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(eps, 0, 0));
+  tf2 = Transform3s();
   p1 << 5 + eps, 0, 0;
   p2 << 0, 0, 0;
   contact << (p1 + p2) / 2;
@@ -2413,12 +2412,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 - eps;
   normal =
-      transform.getRotation() * Vec3f(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+      transform.getRotation() * Vec3s(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(2.5, 0, 0));
   p1 << 5, 0, 0;
   p2 << 2.5, 0, 0;
   contact << (p1 + p2) / 2;
@@ -2428,15 +2427,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(2.5, 0, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-2.5, 0, 0));
   p1 << -5, 0, 0;
   p2 << -2.5, 0, 0;
   contact << (p1 + p2) / 2;
@@ -2446,38 +2445,38 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Plane(Vec3f(0, 1, 0), 0);
+  hs = Plane(Vec3s(0, 1, 0), 0);
 
   eps = 1e-6;
-  tf1 = Transform3f(Vec3f(0, eps, 0));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(0, eps, 0));
+  tf2 = Transform3s();
   p1 << 0, -5 + eps, 0;
   p2 << 0, 0, 0;
   contact << (p1 + p2) / 2;
@@ -2490,13 +2489,13 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 + eps;
-  normal = transform.getRotation() * Vec3f(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
   eps = -1e-6;
-  tf1 = Transform3f(Vec3f(0, eps, 0));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(0, eps, 0));
+  tf2 = Transform3s();
   p1 << 0, 5 + eps, 0;
   p2 << 0, 0, 0;
   contact << (p1 + p2) / 2;
@@ -2509,12 +2508,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 - eps;
-  normal = transform.getRotation() * Vec3f(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 2.5, 0));
   p1 << 0, 5, 0;
   p2 << 0, 2.5, 0;
   contact << (p1 + p2) / 2;
@@ -2524,15 +2523,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 2.5, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 1, 0);
+  normal = transform.getRotation() * Vec3s(0, 1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -2.5, 0));
   p1 << 0, -5, 0;
   p2 << 0, -2.5, 0;
   contact << (p1 + p2) / 2;
@@ -2542,38 +2541,38 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -2.5, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Plane(Vec3f(0, 0, 1), 0);
+  hs = Plane(Vec3s(0, 0, 1), 0);
 
   eps = 1e-6;
-  tf1 = Transform3f(Vec3f(0, 0, eps));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(0, 0, eps));
+  tf2 = Transform3s();
   p1 << 0, 0, -5 + eps;
   p2 << 0, 0, 0;
   contact << (p1 + p2) / 2;
@@ -2586,13 +2585,13 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 + eps;
-  normal = transform.getRotation() * Vec3f(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
   eps = -1e-6;
-  tf1 = Transform3f(Vec3f(0, 0, eps));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(0, 0, eps));
+  tf2 = Transform3s();
   p1 << 0, 0, 5 + eps;
   p2 << 0, 0, 0;
   contact << (p1 + p2) / 2;
@@ -2605,12 +2604,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 - eps;
-  normal = transform.getRotation() * Vec3f(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 2.5));
   p1 << 0, 0, 5;
   p2 << 0, 0, 2.5;
   contact << (p1 + p2) / 2;
@@ -2620,15 +2619,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 2.5));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  normal = transform.getRotation() * Vec3s(0, 0, 1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -2.5));
   p1 << 0, 0, -5.;
   p2 << 0, 0, -2.5;
   contact << (p1 + p2) / 2;
@@ -2638,50 +2637,50 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -2.5));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 10.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -10.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 }
 
 BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   Cone s(5, 10);
-  Halfspace hs(Vec3f(1, 0, 0), 0);
+  Halfspace hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << -2.5, 0, -5;
   depth = -5;
   normal << -1, 0, 0;
@@ -2690,14 +2689,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(-2.5, 0, -5));
+  contact = transform.transform(Vec3s(-2.5, 0, -5));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(2.5, 0, 0));
   contact << -1.25, 0, -5;
   depth = -7.5;
   normal << -1, 0, 0;
@@ -2705,15 +2704,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
-  contact = transform.transform(Vec3f(-1.25, 0, -5));
+  tf2 = transform * Transform3s(Vec3s(2.5, 0, 0));
+  contact = transform.transform(Vec3s(-1.25, 0, -5));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-2.5, 0, 0));
   contact << -3.75, 0, -5;
   depth = -2.5;
   normal << -1, 0, 0;
@@ -2721,15 +2720,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
-  contact = transform.transform(Vec3f(-3.75, 0, -5));
+  tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0));
+  contact = transform.transform(Vec3s(-3.75, 0, -5));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(5.1, 0, 0));
   contact << 0.05, 0, -5;
   depth = -10.1;
   normal << -1, 0, 0;
@@ -2737,27 +2736,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
-  contact = transform.transform(Vec3f(0.05, 0, -5));
+  tf2 = transform * Transform3s(Vec3s(5.1, 0, 0));
+  contact = transform.transform(Vec3s(0.05, 0, -5));
   depth = -10.1;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Halfspace(Vec3f(0, 1, 0), 0);
+  hs = Halfspace(Vec3s(0, 1, 0), 0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, -2.5, -5;
   depth = -5;
   normal << 0, -1, 0;
@@ -2766,14 +2765,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, -2.5, -5));
+  contact = transform.transform(Vec3s(0, -2.5, -5));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 2.5, 0));
   contact << 0, -1.25, -5;
   depth = -7.5;
   normal << 0, -1, 0;
@@ -2781,15 +2780,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
-  contact = transform.transform(Vec3f(0, -1.25, -5));
+  tf2 = transform * Transform3s(Vec3s(0, 2.5, 0));
+  contact = transform.transform(Vec3s(0, -1.25, -5));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -2.5, 0));
   contact << 0, -3.75, -5;
   depth = -2.5;
   normal << 0, -1, 0;
@@ -2797,15 +2796,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
-  contact = transform.transform(Vec3f(0, -3.75, -5));
+  tf2 = transform * Transform3s(Vec3s(0, -2.5, 0));
+  contact = transform.transform(Vec3s(0, -3.75, -5));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 5.1, 0));
   contact << 0, 0.05, -5;
   depth = -10.1;
   normal << 0, -1, 0;
@@ -2813,27 +2812,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
-  contact = transform.transform(Vec3f(0, 0.05, -5));
+  tf2 = transform * Transform3s(Vec3s(0, 5.1, 0));
+  contact = transform.transform(Vec3s(0, 0.05, -5));
   depth = -10.1;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Halfspace(Vec3f(0, 0, 1), 0);
+  hs = Halfspace(Vec3s(0, 0, 1), 0);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f();
+  tf1 = Transform3s();
+  tf2 = Transform3s();
   contact << 0, 0, -2.5;
   depth = -5;
   normal << 0, 0, -1;
@@ -2842,14 +2841,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
 
   tf1 = transform;
   tf2 = transform;
-  contact = transform.transform(Vec3f(0, 0, -2.5));
+  contact = transform.transform(Vec3s(0, 0, -2.5));
   depth = -5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 2.5));
   contact << 0, 0, -1.25;
   depth = -7.5;
   normal << 0, 0, -1;
@@ -2857,15 +2856,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
-  contact = transform.transform(Vec3f(0, 0, -1.25));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 2.5));
+  contact = transform.transform(Vec3s(0, 0, -1.25));
   depth = -7.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -2.5));
   contact << 0, 0, -3.75;
   depth = -2.5;
   normal << 0, 0, -1;
@@ -2873,15 +2872,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
-  contact = transform.transform(Vec3f(0, 0, -3.75));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -2.5));
+  contact = transform.transform(Vec3s(0, 0, -3.75));
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 5.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 5.1));
   contact << 0, 0, 0.05;
   depth = -10.1;
   normal << 0, 0, -1;
@@ -2889,42 +2888,42 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
-  contact = transform.transform(Vec3f(0, 0, 0.05));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 5.1));
+  contact = transform.transform(Vec3s(0, 0, 0.05));
   depth = -10.1;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -5.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -5.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -5.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 }
 
 BOOST_AUTO_TEST_CASE(collide_planecone) {
   Cone s(5, 10);
-  Plane hs(Vec3f(1, 0, 0), 0);
+  Plane hs(Vec3s(1, 0, 0), 0);
 
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL depth;
-  Vec3f normal;
-  Vec3f p1, p2;
+  Vec3s contact;
+  CoalScalar depth;
+  Vec3s normal;
+  Vec3s p1, p2;
 
-  FCL_REAL eps = 1e-6;
-  tf1 = Transform3f(Vec3f(eps, 0, 0));
-  tf2 = Transform3f();
+  CoalScalar eps = 1e-6;
+  tf1 = Transform3s(Vec3s(eps, 0, 0));
+  tf2 = Transform3s();
   p1 << -5 + eps, 0, -5;
   p2 << 0, 0, -5;
   contact << (p1 + p2) / 2;
@@ -2938,13 +2937,13 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 + eps;
   normal =
-      transform.getRotation() * Vec3f(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+      transform.getRotation() * Vec3s(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
   eps = -1e-6;
-  tf1 = Transform3f(Vec3f(eps, 0, 0));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(eps, 0, 0));
+  tf2 = Transform3s();
   p1 << 5 + eps, 0, -5;
   p2 << 0, 0, -5;
   contact << (p1 + p2) / 2;
@@ -2958,12 +2957,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 - eps;
   normal =
-      transform.getRotation() * Vec3f(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+      transform.getRotation() * Vec3s(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(2.5, 0, 0));
   p1 << 5, 0, -5;
   p2 << 2.5, 0, -5;
   contact << (p1 + p2) / 2;
@@ -2973,15 +2972,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(2.5, 0, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  normal = transform.getRotation() * Vec3s(1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-2.5, 0, 0));
   p1 << -5, 0, -5;
   p2 << -2.5, 0, -5;
   contact << (p1 + p2) / 2;
@@ -2991,38 +2990,38 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  normal = transform.getRotation() * Vec3s(-1, 0, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Plane(Vec3f(0, 1, 0), 0);
+  hs = Plane(Vec3s(0, 1, 0), 0);
 
   eps = 1e-6;
-  tf1 = Transform3f(Vec3f(0, eps, 0));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(0, eps, 0));
+  tf2 = Transform3s();
   p1 << 0, -5 + eps, -5;
   p2 << 0, 0, -5;
   contact << (p1 + p2) / 2;
@@ -3035,13 +3034,13 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 + eps;
-  normal = transform.getRotation() * Vec3f(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
   eps = -1e-6;
-  tf1 = Transform3f(Vec3f(0, eps, 0));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(0, eps, 0));
+  tf2 = Transform3s();
   p1 << 0, 5 + eps, -5;
   p2 << 0, 0, -5;
   contact << (p1 + p2) / 2;
@@ -3054,12 +3053,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 - eps;
-  normal = transform.getRotation() * Vec3f(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 2.5, 0));
   p1 << 0, 5, -5;
   p2 << 0, 2.5, -5;
   contact << (p1 + p2) / 2;
@@ -3069,15 +3068,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 2.5, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 1, 0);
+  normal = transform.getRotation() * Vec3s(0, 1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -2.5, 0));
   p1 << 0, -5, -5;
   p2 << 0, -2.5, -5;
   contact << (p1 + p2) / 2;
@@ -3087,38 +3086,38 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -2.5, 0));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  normal = transform.getRotation() * Vec3s(0, -1, 0);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, 5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  tf2 = transform * Transform3s(Vec3s(0, -5.1, 0));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  hs = Plane(Vec3f(0, 0, 1), 0);
+  hs = Plane(Vec3s(0, 0, 1), 0);
 
   eps = 1e-6;
-  tf1 = Transform3f(Vec3f(0, 0, eps));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(0, 0, eps));
+  tf2 = Transform3s();
   p1 << 0, 0, -5 + eps;
   p2 << 0, 0, 0;
   contact << (p1 + p2) / 2;
@@ -3131,13 +3130,13 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 + eps;
-  normal = transform.getRotation() * Vec3f(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
   eps = -1e-6;
-  tf1 = Transform3f(Vec3f(0, 0, eps));
-  tf2 = Transform3f();
+  tf1 = Transform3s(Vec3s(0, 0, eps));
+  tf2 = Transform3s();
   p1 << 0, 0, 5 + eps;
   p2 << 0, 0, 0;
   contact << (p1 + p2) / 2;
@@ -3150,12 +3149,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   tf2 = transform;
   contact = transform.transform((p1 + p2) / 2);
   depth = -5 - eps;
-  normal = transform.getRotation() * Vec3f(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
+  normal = transform.getRotation() * Vec3s(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 2.5));
   p1 << 0, 0, 5;
   p2 << 0, 0, 2.5;
   contact << (p1 + p2) / 2;
@@ -3165,15 +3164,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 2.5));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  normal = transform.getRotation() * Vec3s(0, 0, 1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -2.5));
   p1 << 0, 0, -5;
   p2 << 0, 0, -2.5;
   contact << (p1 + p2) / 2;
@@ -3183,48 +3182,48 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -2.5));
   contact = transform.transform((p1 + p2) / 2);
   depth = -2.5;
-  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  normal = transform.getRotation() * Vec3s(0, 0, -1);
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, 10.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, 10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, 10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
-  tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(0, 0, -10.1));
+  tf1 = Transform3s();
+  tf2 = Transform3s(Vec3s(0, 0, -10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+  tf2 = transform * Transform3s(Vec3s(0, 0, -10.1));
   SET_LINE;
   testShapeCollide(s, tf1, hs, tf2, false);
 }
 
 BOOST_AUTO_TEST_CASE(collide_planeplane) {
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Vec3f normal;
-  Vec3f contact;
-  FCL_REAL distance;
+  Vec3s normal;
+  Vec3s contact;
+  CoalScalar distance;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset = 3.14;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset = 3.14;
     Plane plane1(n, offset);
     Plane plane2(n, offset);
 
@@ -3250,9 +3249,9 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
   }
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset1 = 3.14;
-    FCL_REAL offset2 = offset1 + 1.19841;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset1 = 3.14;
+    CoalScalar offset2 = offset1 + 1.19841;
     Plane plane1(n, offset1);
     Plane plane2(n, offset2);
 
@@ -3268,9 +3267,9 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
   }
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset1 = 3.14;
-    FCL_REAL offset2 = offset1 - 1.19841;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset1 = 3.14;
+    CoalScalar offset2 = offset1 - 1.19841;
     Plane plane1(n, offset1);
     Plane plane2(n, offset2);
 
@@ -3286,11 +3285,11 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
   }
 
   {
-    Vec3f n1(1, 0, 0);
-    FCL_REAL offset1 = 3.14;
+    Vec3s n1(1, 0, 0);
+    CoalScalar offset1 = 3.14;
     Plane plane1(n1, offset1);
-    Vec3f n2(0, 0, 1);
-    FCL_REAL offset2 = -2.13;
+    Vec3s n2(0, 0, 1);
+    CoalScalar offset2 = -2.13;
     Plane plane2(n2, offset2);
 
     tf1.setIdentity();
@@ -3308,11 +3307,11 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
   }
 
   {
-    Vec3f n1(1, 0, 0);
-    FCL_REAL offset1 = 3.14;
+    Vec3s n1(1, 0, 0);
+    CoalScalar offset1 = 3.14;
     Plane plane1(n1, offset1);
-    Vec3f n2(1, 1, 1);
-    FCL_REAL offset2 = -2.13;
+    Vec3s n2(1, 1, 1);
+    CoalScalar offset2 = -2.13;
     Plane plane2(n2, offset2);
 
     tf1.setIdentity();
@@ -3332,19 +3331,19 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
 }
 
 BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Vec3f normal;
-  Vec3f contact;
-  FCL_REAL distance;
+  Vec3s normal;
+  Vec3s contact;
+  CoalScalar distance;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset = 3.14;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset = 3.14;
     Halfspace hf1(n, offset);
     Halfspace hf2(n, offset);
 
@@ -3362,9 +3361,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
   }
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset1 = 3.14;
-    FCL_REAL offset2 = offset1 + 1.19841;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset1 = 3.14;
+    CoalScalar offset2 = offset1 + 1.19841;
     Halfspace hf1(n, offset1);
     Halfspace hf2(n, offset2);
 
@@ -3382,9 +3381,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
   }
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset1 = 3.14;
-    FCL_REAL offset2 = offset1 - 1.19841;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset1 = 3.14;
+    CoalScalar offset2 = offset1 - 1.19841;
     Halfspace hf1(n, offset1);
     Halfspace hf2(-n, -offset2);
 
@@ -3403,11 +3402,11 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
   }
 
   {
-    Vec3f n1(1, 0, 0);
-    FCL_REAL offset1 = 3.14;
+    Vec3s n1(1, 0, 0);
+    CoalScalar offset1 = 3.14;
     Halfspace hf1(n1, offset1);
-    Vec3f n2(0, 0, 1);
-    FCL_REAL offset2 = -2.13;
+    Vec3s n2(0, 0, 1);
+    CoalScalar offset2 = -2.13;
     Halfspace hf2(n2, offset2);
 
     tf1.setIdentity();
@@ -3424,11 +3423,11 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
   }
 
   {
-    Vec3f n1(1, 0, 0);
-    FCL_REAL offset1 = 3.14;
+    Vec3s n1(1, 0, 0);
+    CoalScalar offset1 = 3.14;
     Halfspace hf1(n1, offset1);
-    Vec3f n2(1, 1, 1);
-    FCL_REAL offset2 = -2.13;
+    Vec3s n2(1, 1, 1);
+    CoalScalar offset2 = -2.13;
     Halfspace hf2(n2, offset2);
 
     tf1.setIdentity();
@@ -3448,19 +3447,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
 }
 
 BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
-  Transform3f tf1;
-  Transform3f tf2;
+  Transform3s tf1;
+  Transform3s tf2;
 
-  Vec3f normal;
-  Vec3f contact;
-  FCL_REAL distance;
+  Vec3s normal;
+  Vec3s contact;
+  CoalScalar distance;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset = 3.14;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset = 3.14;
     Halfspace hf(n, offset);
     Plane plane(n, offset);
 
@@ -3479,9 +3478,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
   }
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset1 = 3.14;
-    FCL_REAL offset2 = offset1 + 1.19841;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset1 = 3.14;
+    CoalScalar offset2 = offset1 + 1.19841;
     Halfspace hf(n, offset1);
     Plane plane(n, offset2);
 
@@ -3500,9 +3499,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
   }
 
   {
-    Vec3f n = Vec3f::Random().normalized();
-    FCL_REAL offset1 = 3.14;
-    FCL_REAL offset2 = offset1 - 1.19841;
+    Vec3s n = Vec3s::Random().normalized();
+    CoalScalar offset1 = 3.14;
+    CoalScalar offset2 = offset1 - 1.19841;
     Halfspace hf(n, offset1);
     Plane plane(n, offset2);
 
@@ -3521,11 +3520,11 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
   }
 
   {
-    Vec3f n1(1, 0, 0);
-    FCL_REAL offset1 = 3.14;
+    Vec3s n1(1, 0, 0);
+    CoalScalar offset1 = 3.14;
     Halfspace hf(n1, offset1);
-    Vec3f n2(0, 0, 1);
-    FCL_REAL offset2 = -2.13;
+    Vec3s n2(0, 0, 1);
+    CoalScalar offset2 = -2.13;
     Plane plane(n2, offset2);
 
     tf1.setIdentity();
@@ -3542,11 +3541,11 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
   }
 
   {
-    Vec3f n1(1, 0, 0);
-    FCL_REAL offset1 = 3.14;
+    Vec3s n1(1, 0, 0);
+    CoalScalar offset1 = 3.14;
     Halfspace hf(n1, offset1);
-    Vec3f n2(1, 1, 1);
-    FCL_REAL offset2 = -2.13;
+    Vec3s n2(1, 1, 1);
+    CoalScalar offset2 = -2.13;
     Plane plane(n2, offset2);
 
     tf1.setIdentity();
@@ -3568,70 +3567,70 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
 BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) {
   Sphere s1(20);
   Sphere s2(10);
-  Vec3f closest_p1, closest_p2, normal;
+  Vec3s closest_p1, closest_p2, normal;
   bool compute_penetration = true;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  FCL_REAL dist = -1;
+  CoalScalar dist = -1;
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration,
+      s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration,
       closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)),
+      s1, Transform3s(), s2, Transform3s(Vec3s(30.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)),
+      s1, Transform3s(), s2, Transform3s(Vec3s(29.9, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
 
-  dist = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2,
-                               Transform3f(), compute_penetration, closest_p1,
+  dist = solver1.shapeDistance(s1, Transform3s(Vec3s(40, 0, 0)), s2,
+                               Transform3s(), compute_penetration, closest_p1,
                                closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
 
-  dist = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2,
-                               Transform3f(), compute_penetration, closest_p1,
+  dist = solver1.shapeDistance(s1, Transform3s(Vec3s(30.1, 0, 0)), s2,
+                               Transform3s(), compute_penetration, closest_p1,
                                closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
-  dist = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2,
-                               Transform3f(), compute_penetration, closest_p1,
+  dist = solver1.shapeDistance(s1, Transform3s(Vec3s(29.9, 0, 0)), s2,
+                               Transform3s(), compute_penetration, closest_p1,
                                closest_p2, normal);
   BOOST_CHECK(dist < 0);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(30.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(29.9, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist < 0);
 
-  dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2,
+  dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(40, 0, 0)), s2,
                                transform, compute_penetration, closest_p1,
                                closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
 
-  dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)),
+  dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(30.1, 0, 0)),
                                s2, transform, compute_penetration, closest_p1,
                                closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
-  dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)),
+  dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(29.9, 0, 0)),
                                s2, transform, compute_penetration, closest_p1,
                                closest_p2, normal);
   BOOST_CHECK(dist < 0);
@@ -3640,15 +3639,15 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) {
 BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) {
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
-  Vec3f closest_p1, closest_p2, normal;
+  Vec3s closest_p1, closest_p2, normal;
   bool compute_penetration = true;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  FCL_REAL dist;
+  CoalScalar dist;
 
-  dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+  dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(),
                                compute_penetration, closest_p1, closest_p2,
                                normal);
   BOOST_CHECK(dist <= 0);
@@ -3659,57 +3658,57 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) {
   BOOST_CHECK(dist <= 0);
 
   dist = solver1.shapeDistance(
-      s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+      s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)),
+      s2, Transform3s(), s2, Transform3s(Vec3s(20.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)),
+      s2, Transform3s(), s2, Transform3s(Vec3s(0, 20.2, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10.2) < 0.001);
 
   dist = solver1.shapeDistance(
-      s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)),
+      s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 10.1, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
 
   dist = solver2.shapeDistance(
-      s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+      s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver2.shapeDistance(
-      s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)),
+      s2, Transform3s(), s2, Transform3s(Vec3s(20.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10.1) < 0.001);
 
   dist = solver2.shapeDistance(
-      s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)),
+      s2, Transform3s(), s2, Transform3s(Vec3s(0, 20.1, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10.1) < 0.001);
 
   dist = solver2.shapeDistance(
-      s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)),
+      s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 10.1, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(15.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), compute_penetration,
+      s1, Transform3s(), s2, Transform3s(Vec3s(20, 0, 0)), compute_penetration,
       closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 5) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(20, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 5) < 0.001);
 }
@@ -3718,25 +3717,25 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) {
   Cylinder s1(0.029, 0.1);
   Box s2(1.6, 0.6, 0.025);
 
-  Transform3f tf1(
+  Transform3s tf1(
       Quatf(0.5279170511703305, -0.50981118132505521, -0.67596178682051911,
             0.0668715876735793),
-      Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
+      Vec3s(0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
 
-  Transform3f tf2(
+  Transform3s tf2(
       Quatf(0.70738826916719977, 0, 0, 0.70682518110536596),
-      Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
+      Vec3s(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
 
   GJKSolver solver;
-  Vec3f p1, p2, normal;
+  Vec3s p1, p2, normal;
   bool compute_penetration = true;
   solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal);
   // If objects are not colliding, p2 should be outside the cylinder and
   // p1 should be outside the box
-  Vec3f p2Loc(tf1.inverse().transform(p2));
+  Vec3s p2Loc(tf1.inverse().transform(p2));
   bool p2_in_cylinder((fabs(p2Loc[2]) <= s1.halfLength) &&
                       (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius));
-  Vec3f p1Loc(tf2.inverse().transform(p1));
+  Vec3s p1Loc(tf2.inverse().transform(p1));
   bool p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all();
   std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl;
   std::cout << "p1 in box = (" << p1Loc.transpose() << ")" << std::endl;
@@ -3760,7 +3759,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) {
 
   s1 = Cylinder(0.06, 0.1);
   tf1.setTranslation(
-      Vec3f(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293));
+      Vec3s(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293));
   tf1.setQuatRotation(Quatf(0.52613359459338371, 0.32189408354839893,
                             0.70415587451837913, -0.35175580165512249));
   solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal);
@@ -3769,34 +3768,34 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) {
 BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) {
   Sphere s1(20);
   Box s2(5, 5, 5);
-  Vec3f closest_p1, closest_p2, normal;
+  Vec3s closest_p1, closest_p2, normal;
   bool compute_penetration = true;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  FCL_REAL dist;
+  CoalScalar dist;
 
   int N = 10;
   for (int i = 0; i < N + 1; ++i) {
-    FCL_REAL dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N);
-    dist = solver1.shapeDistance(s1, Transform3f(Vec3f(dbox, 0., 0.)), s2,
-                                 Transform3f(), compute_penetration, closest_p1,
+    CoalScalar dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N);
+    dist = solver1.shapeDistance(s1, Transform3s(Vec3s(dbox, 0., 0.)), s2,
+                                 Transform3s(), compute_penetration, closest_p1,
                                  closest_p2, normal);
     BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6);
-    EIGEN_VECTOR_IS_APPROX(normal, -Vec3f(1, 0, 0), 1e-6);
+    EIGEN_VECTOR_IS_APPROX(normal, -Vec3s(1, 0, 0), 1e-6);
 
     dist =
         solver1.shapeDistance(s1, transform, s2, transform, compute_penetration,
                               closest_p1, closest_p2, normal);
     dist = solver1.shapeDistance(
-        s1, transform * Transform3f(Vec3f(dbox, 0., 0.)), s2, transform,
+        s1, transform * Transform3s(Vec3s(dbox, 0., 0.)), s2, transform,
         compute_penetration, closest_p1, closest_p2, normal);
     BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6);
     EIGEN_VECTOR_IS_APPROX(normal, -transform.getRotation().col(0), 1e-6);
   }
 
-  dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+  dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(),
                                compute_penetration, closest_p1, closest_p2,
                                normal);
   BOOST_CHECK(dist <= 0);
@@ -3807,22 +3806,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) {
   BOOST_CHECK(dist <= 0);
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)),
+      s1, Transform3s(), s2, Transform3s(Vec3s(22.6, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(22.6, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration,
+      s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration,
       closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
 }
@@ -3830,19 +3829,19 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) {
 BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) {
   Cylinder s1(5, 10);
   Cylinder s2(5, 10);
-  Vec3f closest_p1, closest_p2, normal;
+  Vec3s closest_p1, closest_p2, normal;
   bool compute_penetration = true;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  FCL_REAL dist;
+  CoalScalar dist;
 
   {
     // The following situations corresponds to the case where the two cylinders
     // are exactly superposed. This is the worst case for EPA which will take
     // forever to converge with default parameters.
-    dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+    dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(),
                                  compute_penetration, closest_p1, closest_p2,
                                  normal);
     BOOST_CHECK(dist <= 0);
@@ -3854,12 +3853,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) {
 
     // To handle the superposing case, we have to decrease the tolerance of EPA
     // and allow it to work with more vertices and faces.
-    FCL_REAL epa_tolerance_backup = solver1.epa_tolerance;
+    CoalScalar epa_tolerance_backup = solver1.epa_tolerance;
     size_t epa_max_iterations_backup = solver1.epa_max_iterations;
     solver1.epa_tolerance = 1e-2;
     solver1.epa_max_iterations = 1000;
 
-    dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+    dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(),
                                  compute_penetration, closest_p1, closest_p2,
                                  normal);
     BOOST_CHECK(dist <= 0);
@@ -3875,22 +3874,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) {
   }
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+      s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration,
+      s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration,
       closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.001);
 }
@@ -3898,19 +3897,19 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) {
 BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) {
   Cone s1(5, 10);
   Cone s2(5, 10);
-  Vec3f closest_p1, closest_p2, normal;
+  Vec3s closest_p1, closest_p2, normal;
   bool compute_penetration = true;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  FCL_REAL dist;
+  CoalScalar dist;
 
   {
     // The following situations corresponds to the case where the two cones
     // are exactly superposed. This is the worst case for EPA which will take
     // forever to converge with default parameters.
-    dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+    dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(),
                                  compute_penetration, closest_p1, closest_p2,
                                  normal);
     BOOST_CHECK(dist <= 0);
@@ -3922,12 +3921,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) {
 
     // To handle the superposing case, we have to decrease the tolerance of EPA
     // and allow it to work with more vertices and faces.
-    FCL_REAL epa_tolerance_backup = solver1.epa_tolerance;
+    CoalScalar epa_tolerance_backup = solver1.epa_tolerance;
     size_t epa_max_iterations_backup = solver1.epa_max_iterations;
     solver1.epa_tolerance = 1e-2;
     solver1.epa_max_iterations = 1000;
 
-    dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+    dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(),
                                  compute_penetration, closest_p1, closest_p2,
                                  normal);
     BOOST_CHECK(dist <= 0);
@@ -3943,22 +3942,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) {
   }
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+      s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), compute_penetration,
+      s1, Transform3s(), s2, Transform3s(Vec3s(0, 0, 40)), compute_penetration,
       closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 1);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)),
+      s1, transform, s2, transform * Transform3s(Vec3s(0, 0, 40)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 1);
 }
@@ -3966,19 +3965,19 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) {
 BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) {
   Cylinder s1(5, 10);
   Cone s2(5, 10);
-  Vec3f closest_p1, closest_p2, normal;
+  Vec3s closest_p1, closest_p2, normal;
   bool compute_penetration = true;
 
-  Transform3f transform;
+  Transform3s transform;
   generateRandomTransform(extents, transform);
 
-  FCL_REAL dist;
+  CoalScalar dist;
 
   {
     // The following situations corresponds to the case where the two cones
     // are exactly superposed. This is the worst case for EPA which will take
     // forever to converge with default parameters.
-    dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+    dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(),
                                  compute_penetration, closest_p1, closest_p2,
                                  normal);
     BOOST_CHECK(dist <= 0);
@@ -3990,12 +3989,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) {
 
     // To handle the superposing case, we have to decrease the tolerance of EPA
     // and allow it to work with more vertices and faces.
-    FCL_REAL epa_tolerance_backup = solver1.epa_tolerance;
+    CoalScalar epa_tolerance_backup = solver1.epa_tolerance;
     size_t epa_max_iterations_backup = solver1.epa_max_iterations;
     solver1.epa_tolerance = 1e-2;
     solver1.epa_max_iterations = 1000;
 
-    dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+    dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(),
                                  compute_penetration, closest_p1, closest_p2,
                                  normal);
     BOOST_CHECK(dist <= 0);
@@ -4011,39 +4010,39 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) {
   }
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+      s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.02);
 
   dist = solver1.shapeDistance(
-      s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration,
+      s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration,
       closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.01);
 
   dist = solver1.shapeDistance(
-      s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)),
+      s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)),
       compute_penetration, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.1);
 }
 
 template <typename S1, typename S2>
 void testReversibleShapeDistance(const S1& s1, const S2& s2,
-                                 FCL_REAL distance) {
-  Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
-  Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
-
-  FCL_REAL distA;
-  FCL_REAL distB;
-  Vec3f p1A;
-  Vec3f p1B;
-  Vec3f p2A;
-  Vec3f p2B;
-  Vec3f normalA, normalB;
+                                 CoalScalar distance) {
+  Transform3s tf1(Vec3s(-0.5 * distance, 0.0, 0.0));
+  Transform3s tf2(Vec3s(+0.5 * distance, 0.0, 0.0));
+
+  CoalScalar distA;
+  CoalScalar distB;
+  Vec3s p1A;
+  Vec3s p1B;
+  Vec3s p2A;
+  Vec3s p2B;
+  Vec3s normalA, normalB;
   bool compute_penetration = true;
 
   const double tol = 1e-6;
@@ -4084,12 +4083,12 @@ BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) {
   Capsule capsule(5, 10);
   Cone cone(5, 10);
   Cylinder cylinder(5, 10);
-  Plane plane(Vec3f(0, 0, 0), 0.0);
-  Halfspace halfspace(Vec3f(0, 0, 0), 0.0);
+  Plane plane(Vec3s(0, 0, 0), 0.0);
+  Halfspace halfspace(Vec3s(0, 0, 0), 0.0);
 
   // Use sufficiently long distance so that all the primitive shapes CANNOT
   // intersect
-  FCL_REAL distance = 15.0;
+  CoalScalar distance = 15.0;
 
   // If new shape distance algorithm is added for two distinct primitive
   // shapes, uncomment associated lines. For example, box-sphere intersection
diff --git a/test/gjk.cpp b/test/gjk.cpp
index d1877f4757278b37bbc954b3f98ba2acb0bef124..9871029d5ef7921f87651dfb0c00aa97e662636e 100644
--- a/test/gjk.cpp
+++ b/test/gjk.cpp
@@ -34,31 +34,31 @@
 
 /** \author Florent Lamiraux <florent@laas.fr> */
 
-#define BOOST_TEST_MODULE FCL_GJK
+#define BOOST_TEST_MODULE COAL_GJK
 #include <time.h>
 #include <boost/test/included/unit_test.hpp>
 
 #include <Eigen/Geometry>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/internal/tools.h>
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/internal/tools.h"
+#include "coal/internal/shape_shape_func.h"
 
 #include "utility.h"
 
-using hpp::fcl::FCL_REAL;
-using hpp::fcl::GJKSolver;
-using hpp::fcl::GJKVariant;
-using hpp::fcl::Matrix3f;
-using hpp::fcl::Quatf;
-using hpp::fcl::Transform3f;
-using hpp::fcl::TriangleP;
-using hpp::fcl::Vec3f;
+using coal::CoalScalar;
+using coal::GJKSolver;
+using coal::GJKVariant;
+using coal::Matrix3s;
+using coal::Quatf;
+using coal::Transform3s;
+using coal::TriangleP;
+using coal::Vec3s;
 
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> vector_t;
-typedef Eigen::Matrix<FCL_REAL, 6, 1> vector6_t;
-typedef Eigen::Matrix<FCL_REAL, 4, 1> vector4_t;
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 1> vector_t;
+typedef Eigen::Matrix<CoalScalar, 6, 1> vector6_t;
+typedef Eigen::Matrix<CoalScalar, 4, 1> vector4_t;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
 
 struct Result {
   bool collision;
@@ -78,51 +78,51 @@ void test_gjk_distance_triangle_triangle(
   GJKSolver solver;
   if (enable_gjk_nesterov_acceleration)
     solver.gjk.gjk_variant = GJKVariant::NesterovAcceleration;
-  Transform3f tf1, tf2;
-  Vec3f p1, p2, a1, a2;
-  Matrix3f M;
-  FCL_REAL distance(sqrt(-1));
+  Transform3s tf1, tf2;
+  Vec3s p1, p2, a1, a2;
+  Matrix3s M;
+  CoalScalar distance(sqrt(-1));
   clock_t start, end;
 
   std::size_t nCol = 0, nDiff = 0;
-  FCL_REAL eps = 1e-7;
+  CoalScalar eps = 1e-7;
   Results_t results(N);
   for (std::size_t i = 0; i < N; ++i) {
-    Vec3f P1_loc(Vec3f::Random()), P2_loc(Vec3f::Random()),
-        P3_loc(Vec3f::Random());
-    Vec3f Q1_loc(Vec3f::Random()), Q2_loc(Vec3f::Random()),
-        Q3_loc(Vec3f::Random());
+    Vec3s P1_loc(Vec3s::Random()), P2_loc(Vec3s::Random()),
+        P3_loc(Vec3s::Random());
+    Vec3s Q1_loc(Vec3s::Random()), Q2_loc(Vec3s::Random()),
+        Q3_loc(Vec3s::Random());
     if (i == 0) {
-      P1_loc = Vec3f(0.063996093749999997, -0.15320971679687501,
+      P1_loc = Vec3s(0.063996093749999997, -0.15320971679687501,
                      -0.42799999999999999);
       P2_loc =
-          Vec3f(0.069105957031249998, -0.150722900390625, -0.42999999999999999);
-      P3_loc = Vec3f(0.063996093749999997, -0.15320971679687501,
+          Vec3s(0.069105957031249998, -0.150722900390625, -0.42999999999999999);
+      P3_loc = Vec3s(0.063996093749999997, -0.15320971679687501,
                      -0.42999999999999999);
       Q1_loc =
-          Vec3f(-25.655000000000001, -1.2858199462890625, 3.7249809570312502);
-      Q2_loc = Vec3f(-10.926, -1.284259033203125, 3.7281499023437501);
-      Q3_loc = Vec3f(-10.926, -1.2866180419921875, 3.72335400390625);
-      Transform3f tf(
+          Vec3s(-25.655000000000001, -1.2858199462890625, 3.7249809570312502);
+      Q2_loc = Vec3s(-10.926, -1.284259033203125, 3.7281499023437501);
+      Q3_loc = Vec3s(-10.926, -1.2866180419921875, 3.72335400390625);
+      Transform3s tf(
           Quatf(-0.42437287410898855, -0.26862477561450587,
                 -0.46249645019513175, 0.73064726592483387),
-          Vec3f(-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
+          Vec3s(-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
       tf1 = tf;
     } else if (i == 1) {
       P1_loc =
-          Vec3f(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622);
+          Vec3s(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622);
       P2_loc =
-          Vec3f(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
+          Vec3s(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
       P3_loc =
-          Vec3f(0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
+          Vec3s(0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
       Q1_loc =
-          Vec3f(-0.224713996052742, -0.7417119741439819, 0.19999997317790985);
+          Vec3s(-0.224713996052742, -0.7417119741439819, 0.19999997317790985);
       Q2_loc =
-          Vec3f(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985);
+          Vec3s(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985);
       Q3_loc =
-          Vec3f(-0.224713996052742, -0.7417119741439819, 0.09999997168779373);
-      Matrix3f R;
-      Vec3f T;
+          Vec3s(-0.224713996052742, -0.7417119741439819, 0.09999997168779373);
+      Matrix3s R;
+      Vec3s T;
       R << 0.9657787025454787, 0.09400415350535746, 0.24173273843919627,
           -0.06713698817647556, 0.9908494114820345, -0.11709000206805695,
           -0.25052768814676646, 0.09685382227587608, 0.9632524147814993;
@@ -131,21 +131,21 @@ void test_gjk_distance_triangle_triangle(
       tf1.setRotation(R);
       tf1.setTranslation(T);
     } else {
-      tf1 = Transform3f();
+      tf1 = Transform3s();
     }
 
     TriangleP tri1(P1_loc, P2_loc, P3_loc);
     TriangleP tri2(Q1_loc, Q2_loc, Q3_loc);
-    Vec3f normal;
+    Vec3s normal;
     const bool compute_penetration = true;
-    hpp::fcl::DistanceRequest request(compute_penetration, compute_penetration);
-    hpp::fcl::DistanceResult result;
+    coal::DistanceRequest request(compute_penetration, compute_penetration);
+    coal::DistanceResult result;
 
     start = clock();
     // The specialized function TriangleP-TriangleP calls GJK to check for
     // collision and compute the witness points but it does not use EPA to
     // compute the penetration depth.
-    distance = hpp::fcl::ShapeShapeDistance<TriangleP, TriangleP>(
+    distance = coal::ShapeShapeDistance<TriangleP, TriangleP>(
         &tri1, tf1, &tri2, tf2, &solver, request, result);
     end = clock();
     p1 = result.nearest_points[0];
@@ -155,15 +155,15 @@ void test_gjk_distance_triangle_triangle(
     results[i].timeGjk = end - start;
     results[i].collision = res;
     if (res) {
-      Vec3f c1, c2, normal2;
+      Vec3s c1, c2, normal2;
       ++nCol;
       // check that moving triangle 2 by the penetration depth in the
       // direction of the normal makes the triangles collision free.
-      FCL_REAL penetration_depth(-distance);
+      CoalScalar penetration_depth(-distance);
       assert(penetration_depth >= 0);
       tf2.setTranslation((penetration_depth + 10 - 4) * normal);
       result.clear();
-      distance = hpp::fcl::ShapeShapeDistance<TriangleP, TriangleP>(
+      distance = coal::ShapeShapeDistance<TriangleP, TriangleP>(
           &tri1, tf1, &tri2, tf2, &solver, request, result);
       c1 = result.nearest_points[0];
       c2 = result.nearest_points[1];
@@ -189,15 +189,15 @@ void test_gjk_distance_triangle_triangle(
       tf2.setIdentity();
     }
     // Compute vectors between vertices
-    Vec3f P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)),
+    Vec3s P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)),
         P3(tf1.transform(P3_loc)), Q1(tf2.transform(Q1_loc)),
         Q2(tf2.transform(Q2_loc)), Q3(tf2.transform(Q3_loc));
-    Vec3f u1(P2 - P1);
-    Vec3f v1(P3 - P1);
-    Vec3f w1(u1.cross(v1));
-    Vec3f u2(Q2 - Q1);
-    Vec3f v2(Q3 - Q1);
-    Vec3f w2(u2.cross(v2));
+    Vec3s u1(P2 - P1);
+    Vec3s v1(P3 - P1);
+    Vec3s w1(u1.cross(v1));
+    Vec3s u2(Q2 - Q1);
+    Vec3s v2(Q3 - Q1);
+    Vec3s w2(u2.cross(v2));
     BOOST_CHECK(w1.squaredNorm() > eps * eps);
     M.col(0) = u1;
     M.col(1) = v1;
@@ -312,17 +312,17 @@ void test_gjk_distance_triangle_triangle(
   }
   std::cerr << "Total / average time gjk: "
             << totalTimeGjkNoColl + totalTimeGjkColl << ", "
-            << FCL_REAL(totalTimeGjkNoColl + totalTimeGjkColl) /
-                   FCL_REAL(CLOCKS_PER_SEC * N)
+            << CoalScalar(totalTimeGjkNoColl + totalTimeGjkColl) /
+                   CoalScalar(CLOCKS_PER_SEC * N)
             << "s" << std::endl;
   std::cerr << "-- Collisions -------------------------" << std::endl;
   std::cerr << "Total / average time gjk: " << totalTimeGjkColl << ", "
-            << FCL_REAL(totalTimeGjkColl) / FCL_REAL(CLOCKS_PER_SEC * nCol)
+            << CoalScalar(totalTimeGjkColl) / CoalScalar(CLOCKS_PER_SEC * nCol)
             << "s" << std::endl;
   std::cerr << "-- No collisions -------------------------" << std::endl;
   std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl << ", "
-            << FCL_REAL(totalTimeGjkNoColl) /
-                   FCL_REAL(CLOCKS_PER_SEC * (N - nCol))
+            << CoalScalar(totalTimeGjkNoColl) /
+                   CoalScalar(CLOCKS_PER_SEC * (N - nCol))
             << "s" << std::endl;
 }
 
@@ -334,17 +334,17 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_nesterov) {
   test_gjk_distance_triangle_triangle(true);
 }
 
-void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray,
+void test_gjk_unit_sphere(CoalScalar center_distance, Vec3s ray,
                           double swept_sphere_radius,
                           bool use_gjk_nesterov_acceleration) {
-  using namespace hpp::fcl;
-  const FCL_REAL r = 1.0;
+  using namespace coal;
+  const CoalScalar r = 1.0;
   Sphere sphere(r);
   sphere.setSweptSphereRadius(swept_sphere_radius);
 
-  typedef Eigen::Matrix<FCL_REAL, 4, 1> Vec4f;
-  Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3f::Zero());
-  Transform3f tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray);
+  typedef Eigen::Matrix<CoalScalar, 4, 1> Vec4f;
+  Transform3s tf0(Quatf(Vec4f::Random().normalized()), Vec3s::Zero());
+  Transform3s tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray);
 
   bool expect_collision = center_distance <= 2 * (r + swept_sphere_radius);
 
@@ -359,7 +359,7 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray,
   details::GJK gjk(2, 1e-6);
   if (use_gjk_nesterov_acceleration)
     gjk.gjk_variant = GJKVariant::NesterovAcceleration;
-  details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0));
+  details::GJK::Status status = gjk.evaluate(shape, Vec3s(1, 0, 0));
 
   if (expect_collision) {
     BOOST_CHECK((status == details::GJK::Collision) ||
@@ -372,12 +372,12 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray,
     BOOST_CHECK_EQUAL(status, details::GJK::NoCollision);
   }
 
-  Vec3f w0, w1, normal;
+  Vec3s w0, w1, normal;
   gjk.getWitnessPointsAndNormal(shape, w0, w1, normal);
 
-  Vec3f w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) +
+  Vec3s w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) +
                     swept_sphere_radius * normal);
-  Vec3f w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) -
+  Vec3s w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) -
                     swept_sphere_radius * normal);
 
   EIGEN_VECTOR_IS_APPROX(w0, w0_expected, 1e-10);
@@ -389,38 +389,38 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
   std::array<double, 5> swept_sphere_radius = {0., 0.1, 1., 10., 100.};
   for (bool nesterov_acceleration : use_nesterov_acceleration) {
     for (double ssr : swept_sphere_radius) {
-      test_gjk_unit_sphere(3, Vec3f(1, 0, 0), ssr, nesterov_acceleration);
+      test_gjk_unit_sphere(3, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
 
-      test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), ssr, nesterov_acceleration);
+      test_gjk_unit_sphere(2.01, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
 
-      test_gjk_unit_sphere(2.0, Vec3f(1, 0, 0), ssr, nesterov_acceleration);
+      test_gjk_unit_sphere(2.0, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
 
-      test_gjk_unit_sphere(1.0, Vec3f(1, 0, 0), ssr, nesterov_acceleration);
+      test_gjk_unit_sphere(1.0, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
 
       // Random rotation
-      test_gjk_unit_sphere(3, Vec3f::Random().normalized(), ssr,
+      test_gjk_unit_sphere(3, Vec3s::Random().normalized(), ssr,
                            nesterov_acceleration);
 
-      test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), ssr,
+      test_gjk_unit_sphere(2.01, Vec3s::Random().normalized(), ssr,
                            nesterov_acceleration);
 
-      test_gjk_unit_sphere(2.0, Vec3f::Random().normalized(), ssr,
+      test_gjk_unit_sphere(2.0, Vec3s::Random().normalized(), ssr,
                            nesterov_acceleration);
 
-      test_gjk_unit_sphere(1.0, Vec3f::Random().normalized(), ssr,
+      test_gjk_unit_sphere(1.0, Vec3s::Random().normalized(), ssr,
                            nesterov_acceleration);
     }
   }
 }
 
-void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
+void test_gjk_triangle_capsule(Vec3s T, bool expect_collision,
                                bool use_gjk_nesterov_acceleration,
-                               Vec3f w0_expected, Vec3f w1_expected) {
-  using namespace hpp::fcl;
+                               Vec3s w0_expected, Vec3s w1_expected) {
+  using namespace coal;
   Capsule capsule(1., 2.);  // Radius 1 and length 2
-  TriangleP triangle(Vec3f(0., 0., 0.), Vec3f(1., 0., 0.), Vec3f(1., 1., 0.));
+  TriangleP triangle(Vec3s(0., 0., 0.), Vec3s(1., 0., 0.), Vec3s(1., 1., 0.));
 
-  Transform3f tf0, tf1;
+  Transform3s tf0, tf1;
   tf1.setTranslation(T);
 
   details::MinkowskiDiff shape;
@@ -436,7 +436,7 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
   details::GJK gjk(10, 1e-6);
   if (use_gjk_nesterov_acceleration)
     gjk.gjk_variant = GJKVariant::NesterovAcceleration;
-  details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0));
+  details::GJK::Status status = gjk.evaluate(shape, Vec3s(1, 0, 0));
 
   if (expect_collision) {
     BOOST_CHECK((status == details::GJK::Collision) ||
@@ -445,19 +445,19 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
     BOOST_CHECK_EQUAL(status, details::GJK::NoCollision);
 
     // Check that guess works as expected
-    Vec3f guess = gjk.getGuessFromSimplex();
+    Vec3s guess = gjk.getGuessFromSimplex();
     details::GJK gjk2(3, 1e-6);
     details::GJK::Status status2 = gjk2.evaluate(shape, guess);
     BOOST_CHECK_EQUAL(status2, details::GJK::NoCollision);
   }
 
-  Vec3f w0, w1, normal;
+  Vec3s w0, w1, normal;
   if (status == details::GJK::NoCollision ||
       status == details::GJK::CollisionWithPenetrationInformation) {
     gjk.getWitnessPointsAndNormal(shape, w0, w1, normal);
   } else {
     details::EPA epa(64, 1e-6);
-    details::EPA::Status epa_status = epa.evaluate(gjk, Vec3f(1, 0, 0));
+    details::EPA::Status epa_status = epa.evaluate(gjk, Vec3s(1, 0, 0));
     BOOST_CHECK_EQUAL(epa_status, details::EPA::AccuracyReached);
     epa.getWitnessPointsAndNormal(shape, w0, w1, normal);
   }
@@ -468,23 +468,23 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
 
 BOOST_AUTO_TEST_CASE(triangle_capsule) {
   // GJK -> no collision
-  test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, false, Vec3f(1., 0, 0),
-                            Vec3f(0., 0, 0));
+  test_gjk_triangle_capsule(Vec3s(1.01, 0, 0), false, false, Vec3s(1., 0, 0),
+                            Vec3s(0., 0, 0));
   // GJK + Nesterov acceleration -> no collision
-  test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, true, Vec3f(1., 0, 0),
-                            Vec3f(0., 0, 0));
+  test_gjk_triangle_capsule(Vec3s(1.01, 0, 0), false, true, Vec3s(1., 0, 0),
+                            Vec3s(0., 0, 0));
 
   // GJK -> collision
-  test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, false, Vec3f(1., 0, 0),
-                            Vec3f(0., 0, 0));
+  test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, false, Vec3s(1., 0, 0),
+                            Vec3s(0., 0, 0));
   // GJK + Nesterov acceleration -> collision
-  test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, true, Vec3f(1., 0, 0),
-                            Vec3f(0., 0, 0));
+  test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, true, Vec3s(1., 0, 0),
+                            Vec3s(0., 0, 0));
 
   // GJK + EPA -> collision
-  test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, false, Vec3f(0, 1, 0),
-                            Vec3f(0.5, 0, 0));
+  test_gjk_triangle_capsule(Vec3s(-0.5, -0.01, 0), true, false, Vec3s(0, 1, 0),
+                            Vec3s(0.5, 0, 0));
   // GJK + Nesterov accleration + EPA -> collision
-  test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, true, Vec3f(0, 1, 0),
-                            Vec3f(0.5, 0, 0));
+  test_gjk_triangle_capsule(Vec3s(-0.5, -0.01, 0), true, true, Vec3s(0, 1, 0),
+                            Vec3s(0.5, 0, 0));
 }
diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp
index b06ee09a702afe3337ee7f02ed0afc83107d2a31..c4cb0be9873a3b7ef70eff571b38d4e7d70aa179 100644
--- a/test/gjk_asserts.cpp
+++ b/test/gjk_asserts.cpp
@@ -1,20 +1,20 @@
-#define BOOST_TEST_MODULE FCL_GJK_ASSERTS
+#define BOOST_TEST_MODULE COAL_GJK_ASSERTS
 
 #include <boost/test/included/unit_test.hpp>
 #include <boost/math/constants/constants.hpp>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/collision.h>
+#include "coal/BVH/BVH_model.h"
+#include "coal/collision.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
-constexpr FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
+constexpr CoalScalar pi = boost::math::constants::pi<CoalScalar>();
 
 double DegToRad(const double& deg) {
   static double degToRad = pi / 180.;
   return deg * degToRad;
 }
-std::vector<Vec3f> dirs{Vec3f::UnitZ(),  -Vec3f::UnitZ(), Vec3f::UnitY(),
-                        -Vec3f::UnitY(), Vec3f::UnitX(),  -Vec3f::UnitX()};
+std::vector<Vec3s> dirs{Vec3s::UnitZ(),  -Vec3s::UnitZ(), Vec3s::UnitY(),
+                        -Vec3s::UnitY(), Vec3s::UnitX(),  -Vec3s::UnitX()};
 
 void CreateSphereMesh(BVHModel<OBBRSS>& model, const double& radius) {
   size_t polarSteps{32};
@@ -24,7 +24,7 @@ void CreateSphereMesh(BVHModel<OBBRSS>& model, const double& radius) {
 
   const float polarStep = PI / (float)(polarSteps - 1);
   const float azimuthStep = 2.0f * PI / (float)(azimuthSteps - 1);
-  std::vector<Vec3f> vertices;
+  std::vector<Vec3s> vertices;
   std::vector<Triangle> triangles;
 
   for (size_t p = 0; p < polarSteps; ++p) {
@@ -64,8 +64,8 @@ BOOST_AUTO_TEST_CASE(TestSpheres) {
 
   ComputeCollision compute(&sphere2, &sphere1);
 
-  Transform3f sphere1Tf = Transform3f::Identity();
-  Transform3f sphere2Tf = Transform3f::Identity();
+  Transform3s sphere1Tf = Transform3s::Identity();
+  Transform3s sphere2Tf = Transform3s::Identity();
 
   for (int i = 0; i < 360; ++i) {
     for (int j = 0; j < 180; ++j) {
@@ -77,9 +77,9 @@ BOOST_AUTO_TEST_CASE(TestSpheres) {
           (i == 86 && j == 52) || (i == 89 && j == 17) ||
           (i == 89 && j == 58) || (i == 89 && j == 145)) {
         sphere2Tf.setQuatRotation(
-            Eigen::AngleAxis<double>(DegToRad(i), Vec3f::UnitZ()) *
-            Eigen::AngleAxis<double>(DegToRad(j), Vec3f::UnitY()));
-        for (const Vec3f& dir : dirs) {
+            Eigen::AngleAxis<double>(DegToRad(i), Vec3s::UnitZ()) *
+            Eigen::AngleAxis<double>(DegToRad(j), Vec3s::UnitY()));
+        for (const Vec3s& dir : dirs) {
           sphere2Tf.setTranslation(dir);
           CollisionResult result;
 
diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp
index f2ac0bdbccf969cc860dd930f3199229588218be..6002c38be13093988c6696f9438e4087c2cea6f4 100644
--- a/test/gjk_convergence_criterion.cpp
+++ b/test/gjk_convergence_criterion.cpp
@@ -34,29 +34,29 @@
 
 /** \author Louis Montaut */
 
-#include "hpp/fcl/data_types.h"
-#include <boost/test/tools/old/interface.hpp>
-#define BOOST_TEST_MODULE FCL_NESTEROV_GJK
+#define BOOST_TEST_MODULE COAL_NESTEROV_GJK
 #include <boost/test/included/unit_test.hpp>
+#include <boost/test/tools/old/interface.hpp>
 
 #include <Eigen/Geometry>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/internal/tools.h>
+#include "coal/data_types.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/internal/tools.h"
 
 #include "utility.h"
 
-using hpp::fcl::Box;
-using hpp::fcl::FCL_REAL;
-using hpp::fcl::GJKConvergenceCriterion;
-using hpp::fcl::GJKConvergenceCriterionType;
-using hpp::fcl::GJKSolver;
-using hpp::fcl::ShapeBase;
-using hpp::fcl::support_func_guess_t;
-using hpp::fcl::Transform3f;
-using hpp::fcl::Vec3f;
-using hpp::fcl::details::GJK;
-using hpp::fcl::details::MinkowskiDiff;
+using coal::Box;
+using coal::CoalScalar;
+using coal::GJKConvergenceCriterion;
+using coal::GJKConvergenceCriterionType;
+using coal::GJKSolver;
+using coal::ShapeBase;
+using coal::support_func_guess_t;
+using coal::Transform3s;
+using coal::Vec3s;
+using coal::details::GJK;
+using coal::details::MinkowskiDiff;
 using std::size_t;
 
 BOOST_AUTO_TEST_CASE(set_cv_criterion) {
@@ -96,7 +96,7 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1,
   // by default GJK uses the VDB convergence criterion, which is relative.
   GJK gjk1(max_iterations, 1e-6);
 
-  FCL_REAL tol;
+  CoalScalar tol;
   switch (cv_type) {
     // need to lower the tolerance when absolute
     case GJKConvergenceCriterionType::Absolute:
@@ -122,13 +122,13 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1,
 
   // Generate random transforms
   size_t n = 1000;
-  FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.};
-  std::vector<Transform3f> transforms;
+  CoalScalar extents[] = {-3., -3., 0, 3., 3., 3.};
+  std::vector<Transform3s> transforms;
   generateRandomTransforms(extents, transforms, n);
-  Transform3f identity = Transform3f::Identity();
+  Transform3s identity = Transform3s::Identity();
 
   // Same init for both solvers
-  Vec3f init_guess = Vec3f(1, 0, 0);
+  Vec3s init_guess = Vec3s(1, 0, 0);
   support_func_guess_t init_support_guess;
   init_support_guess.setZero();
 
@@ -138,21 +138,21 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1,
 
     GJK::Status res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(gjk1.getNumIterations() <= max_iterations);
-    Vec3f ray1 = gjk1.ray;
+    Vec3s ray1 = gjk1.ray;
     res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(res1 != GJK::Status::Failed);
     EIGEN_VECTOR_IS_APPROX(gjk1.ray, ray1, 1e-8);
 
     GJK::Status res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(gjk2.getNumIterations() <= max_iterations);
-    Vec3f ray2 = gjk2.ray;
+    Vec3s ray2 = gjk2.ray;
     res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(res2 != GJK::Status::Failed);
     EIGEN_VECTOR_IS_APPROX(gjk2.ray, ray2, 1e-8);
 
     GJK::Status res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(gjk3.getNumIterations() <= max_iterations);
-    Vec3f ray3 = gjk3.ray;
+    Vec3s ray3 = gjk3.ray;
     res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess);
     BOOST_CHECK(res3 != GJK::Status::Failed);
     EIGEN_VECTOR_IS_APPROX(gjk3.ray, ray3, 1e-8);
diff --git a/test/hfields.cpp b/test/hfields.cpp
index ca34db96d56006bf4dae4c5bc73bf8083250e0d9..09d1b74492da3a4b27c8b4c415731cb47902a792 100644
--- a/test/hfields.cpp
+++ b/test/hfields.cpp
@@ -34,45 +34,45 @@
 
 /** \author Justin Carpentier */
 
-#define BOOST_TEST_MODULE FCL_HEIGHT_FIELDS
+#define BOOST_TEST_MODULE COAL_HEIGHT_FIELDS
 #include <boost/test/included/unit_test.hpp>
 #include <boost/filesystem.hpp>
 
 #include "fcl_resources/config.h"
 
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/hfield.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/mesh_loader/assimp.h>
-#include <hpp/fcl/mesh_loader/loader.h>
+#include "coal/collision.h"
+#include "coal/hfield.h"
+#include "coal/math/transform.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/mesh_loader/assimp.h"
+#include "coal/mesh_loader/loader.h"
 
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/internal/traversal_node_hfield_shape.h>
+#include "coal/collision.h"
+#include "coal/internal/traversal_node_hfield_shape.h"
 
 #include "utility.h"
 #include <iostream>
 
-using namespace hpp::fcl;
+using namespace coal;
 
 template <typename BV>
 void test_constant_hfields(const Eigen::DenseIndex nx,
                            const Eigen::DenseIndex ny,
-                           const FCL_REAL min_altitude,
-                           const FCL_REAL max_altitude) {
-  const FCL_REAL x_dim = 1., y_dim = 2.;
-  const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);
+                           const CoalScalar min_altitude,
+                           const CoalScalar max_altitude) {
+  const CoalScalar x_dim = 1., y_dim = 2.;
+  const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude);
 
   HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude);
 
   BOOST_CHECK(hfield.getXDim() == x_dim);
   BOOST_CHECK(hfield.getYDim() == y_dim);
 
-  const VecXf& x_grid = hfield.getXGrid();
+  const VecXs& x_grid = hfield.getXGrid();
   BOOST_CHECK(x_grid[0] == -x_dim / 2.);
   BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.);
 
-  const VecXf& y_grid = hfield.getYGrid();
+  const VecXs& y_grid = hfield.getYGrid();
   BOOST_CHECK(y_grid[0] == y_dim / 2.);
   BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.);
 
@@ -81,7 +81,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
 
   for (Eigen::DenseIndex i = 0; i < nx; ++i) {
     for (Eigen::DenseIndex j = 0; j < ny; ++j) {
-      Vec3f point(x_grid[i], y_grid[j], heights(j, i));
+      Vec3s point(x_grid[i], y_grid[j], heights(j, i));
       BOOST_CHECK(hfield.aabb_local.contain(point));
     }
   }
@@ -97,24 +97,24 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
 
   // Build equivalent object
   const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude);
-  const Transform3f box_placement(
-      Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.));
+  const Transform3s box_placement(
+      Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.));
 
   // Test collision
   const Sphere sphere(1.);
-  static const Transform3f IdTransform;
+  static const Transform3s IdTransform;
 
-  const Box box(Vec3f::Ones());
+  const Box box(Vec3s::Ones());
 
-  Transform3f M_sphere, M_box;
+  Transform3s M_sphere, M_box;
 
   // No collision case
   {
-    const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
     CollisionRequest request;
 
     CollisionResult result;
@@ -137,11 +137,11 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
 
   // Collision case
   {
-    const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision));
     CollisionRequest
         request;  //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));
 
@@ -165,15 +165,15 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
 
   // Update height
   hfield.updateHeights(
-      MatrixXf::Constant(ny, nx, max_altitude / 2.));  // We change nothing
+      MatrixXs::Constant(ny, nx, max_altitude / 2.));  // We change nothing
 
   // No collision case
   {
-    const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
     CollisionRequest request;
 
     CollisionResult result;
@@ -196,11 +196,11 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
 
   // Collision case
   {
-    const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision));
     CollisionRequest
         request;  //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));
 
@@ -224,15 +224,15 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
 
   // Restore height
   hfield.updateHeights(
-      MatrixXf::Constant(ny, nx, max_altitude));  // We change nothing
+      MatrixXs::Constant(ny, nx, max_altitude));  // We change nothing
 
   // Collision case
   {
-    const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision));
     CollisionRequest
         request;  //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));
 
@@ -256,7 +256,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
 }
 
 BOOST_AUTO_TEST_CASE(building_constant_hfields) {
-  const FCL_REAL max_altitude = 1., min_altitude = 0.;
+  const CoalScalar max_altitude = 1., min_altitude = 0.;
 
   test_constant_hfields<OBBRSS>(2, 2, min_altitude,
                                 max_altitude);  // Simple case
@@ -272,33 +272,33 @@ BOOST_AUTO_TEST_CASE(building_constant_hfields) {
 template <typename BV>
 void test_negative_security_margin(const Eigen::DenseIndex nx,
                                    const Eigen::DenseIndex ny,
-                                   const FCL_REAL min_altitude,
-                                   const FCL_REAL max_altitude) {
-  const FCL_REAL x_dim = 1., y_dim = 2.;
-  const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);
+                                   const CoalScalar min_altitude,
+                                   const CoalScalar max_altitude) {
+  const CoalScalar x_dim = 1., y_dim = 2.;
+  const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude);
 
   HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude);
 
   // Build equivalent object
   const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude);
-  const Transform3f box_placement(
-      Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.));
+  const Transform3s box_placement(
+      Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.));
 
   // Test collision
   const Sphere sphere(1.);
-  static const Transform3f IdTransform;
+  static const Transform3s IdTransform;
 
-  const Box box(Vec3f::Ones());
+  const Box box(Vec3s::Ones());
 
-  Transform3f M_sphere, M_box;
+  Transform3s M_sphere, M_box;
 
   // No collision case
   {
-    const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
     CollisionRequest request;
 
     CollisionResult result;
@@ -321,11 +321,11 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
 
   // Collision case - positive security_margin
   {
-    const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
     CollisionRequest request;
     request.security_margin = eps_no_collision + 1e-6;
 
@@ -349,11 +349,11 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
 
   // Collision case
   {
-    const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
     CollisionRequest request;
 
     CollisionResult result;
@@ -376,11 +376,11 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
 
   // No collision case - negative security_margin
   {
-    const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude);
+    const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude);
     M_sphere.setTranslation(
-        Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+        Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
     M_box.setTranslation(
-        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+        Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
     CollisionRequest request;
     request.security_margin = eps_no_collision - 1e-4;
 
@@ -404,7 +404,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
 }
 
 BOOST_AUTO_TEST_CASE(negative_security_margin) {
-  const FCL_REAL max_altitude = 1., min_altitude = 0.;
+  const CoalScalar max_altitude = 1., min_altitude = 0.;
 
   //  test_negative_security_margin<OBBRSS>(100, 100, min_altitude,
   //  max_altitude);
@@ -415,23 +415,23 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) {
   const Eigen::DenseIndex nx = 100, ny = 100;
 
   typedef AABB BV;
-  const MatrixXf X =
+  const MatrixXs X =
       Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1);
-  const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);
+  const MatrixXs Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);
 
-  const FCL_REAL dim_square = 0.5;
+  const CoalScalar dim_square = 0.5;
 
   const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole =
       (X.array().abs() < dim_square) && (Y.array().abs() < dim_square);
 
-  const MatrixXf heights =
-      MatrixXf::Ones(ny, nx) - hole.cast<double>().matrix();
+  const MatrixXs heights =
+      MatrixXs::Ones(ny, nx) - hole.cast<double>().matrix();
 
   const HeightField<BV> hfield(2., 2., heights, -10.);
 
   Sphere sphere(0.48);
-  const Transform3f sphere_pos(Vec3f(0., 0., 0.5));
-  const Transform3f hfield_pos;
+  const Transform3s sphere_pos(Vec3s(0., 0., 0.5));
+  const Transform3s hfield_pos;
 
   const CollisionRequest request;
 
@@ -459,17 +459,17 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) {
   //  typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug
   //  mode), as the overlap of OBBRSS is not satisfactory yet.
   typedef AABB BV;
-  const MatrixXf X =
+  const MatrixXs X =
       Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1);
-  const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);
+  const MatrixXs Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);
 
-  const FCL_REAL dim_hole = 1;
+  const CoalScalar dim_hole = 1;
 
   const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole =
       (X.array().square() + Y.array().square() <= dim_hole);
 
-  const MatrixXf heights =
-      MatrixXf::Ones(ny, nx) - hole.cast<double>().matrix();
+  const MatrixXs heights =
+      MatrixXs::Ones(ny, nx) - hole.cast<double>().matrix();
 
   const HeightField<BV> hfield(2., 2., heights, -10.);
 
@@ -480,10 +480,10 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) {
   BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.);
 
   Sphere sphere(0.975);
-  const Transform3f sphere_pos(Vec3f(0., 0., 1.));
-  const Transform3f hfield_pos;
+  const Transform3s sphere_pos(Vec3s(0., 0., 1.));
+  const Transform3s hfield_pos;
 
-  const FCL_REAL thresholds[3] = {0., 0.01, -0.005};
+  const CoalScalar thresholds[3] = {0., 0.01, -0.005};
 
   for (int i = 0; i < 3; ++i) {
     CollisionResult result;
@@ -515,24 +515,25 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) {
   }
 }
 
-bool isApprox(const FCL_REAL v1, const FCL_REAL v2, const FCL_REAL tol = 1e-6) {
+bool isApprox(const CoalScalar v1, const CoalScalar v2,
+              const CoalScalar tol = 1e-6) {
   return std::fabs(v1 - v2) <= tol;
 }
 
-Vec3f computeFaceNormal(const Triangle& triangle,
-                        const std::vector<Vec3f>& points) {
-  const Vec3f pointA = points[triangle[0]];
-  const Vec3f pointB = points[triangle[1]];
-  const Vec3f pointC = points[triangle[2]];
+Vec3s computeFaceNormal(const Triangle& triangle,
+                        const std::vector<Vec3s>& points) {
+  const Vec3s pointA = points[triangle[0]];
+  const Vec3s pointB = points[triangle[1]];
+  const Vec3s pointC = points[triangle[2]];
 
   return (pointB - pointA).cross(pointC - pointA).normalized();
 }
 
 BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
-  const FCL_REAL sphere_radius = 1.;
+  const CoalScalar sphere_radius = 1.;
   Sphere sphere(sphere_radius);
-  MatrixXf altitutes(2, 2);
-  FCL_REAL altitude_value = 1.;
+  MatrixXs altitutes(2, 2);
+  CoalScalar altitude_value = 1.;
   altitutes.fill(altitude_value);
 
   typedef AABB BV;
@@ -563,20 +564,20 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
 
   // Check face normals for convex1
   {
-    const std::vector<Vec3f>& points = *(convex1.points);
+    const std::vector<Vec3s>& points = *(convex1.points);
     // BOTTOM
     {
       const Triangle& triangle = (*(convex1.polygons))[0];
 
       BOOST_CHECK(
-          computeFaceNormal(triangle, points).isApprox(-Vec3f::UnitZ()));
+          computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ()));
     }
 
     // TOP
     {
       const Triangle& triangle = (*(convex1.polygons))[1];
 
-      BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3f::UnitZ()));
+      BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ()));
     }
 
     // WEST sides
@@ -585,14 +586,14 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
       const Triangle& triangle2 = (*(convex1.polygons))[3];
 
       BOOST_CHECK(
-          computeFaceNormal(triangle1, points).isApprox(-Vec3f::UnitX()));
+          computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitX()));
       BOOST_CHECK(
-          computeFaceNormal(triangle2, points).isApprox(-Vec3f::UnitX()));
+          computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitX()));
     }
 
     // SOUTH-EAST sides
     {
-      const Vec3f south_east_normal = Vec3f(1., -1., 0).normalized();
+      const Vec3s south_east_normal = Vec3s(1., -1., 0).normalized();
 
       const Triangle& triangle1 = (*(convex1.polygons))[4];
       const Triangle& triangle2 = (*(convex1.polygons))[5];
@@ -612,29 +613,29 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
                 << computeFaceNormal(triangle1, points).transpose()
                 << std::endl;
       BOOST_CHECK(
-          computeFaceNormal(triangle1, points).isApprox(Vec3f::UnitY()));
+          computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitY()));
       BOOST_CHECK(
-          computeFaceNormal(triangle2, points).isApprox(Vec3f::UnitY()));
+          computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitY()));
     }
   }
 
   // Check face normals for convex2
   {
-    const std::vector<Vec3f>& points = *(convex2.points);
+    const std::vector<Vec3s>& points = *(convex2.points);
 
     // BOTTOM
     {
       const Triangle& triangle = (*(convex2.polygons))[0];
 
       BOOST_CHECK(
-          computeFaceNormal(triangle, points).isApprox(-Vec3f::UnitZ()));
+          computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ()));
     }
 
     // TOP
     {
       const Triangle& triangle = (*(convex2.polygons))[1];
 
-      BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3f::UnitZ()));
+      BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ()));
     }
 
     // SOUTH sides
@@ -643,14 +644,14 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
       const Triangle& triangle2 = (*(convex2.polygons))[3];
 
       BOOST_CHECK(
-          computeFaceNormal(triangle1, points).isApprox(-Vec3f::UnitY()));
+          computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitY()));
       BOOST_CHECK(
-          computeFaceNormal(triangle2, points).isApprox(-Vec3f::UnitY()));
+          computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitY()));
     }
 
     // NORTH-WEST sides
     {
-      const Vec3f north_west_normal = Vec3f(-1., 1., 0).normalized();
+      const Vec3s north_west_normal = Vec3s(-1., 1., 0).normalized();
 
       const Triangle& triangle1 = (*(convex2.polygons))[4];
       const Triangle& triangle2 = (*(convex2.polygons))[5];
@@ -667,19 +668,19 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
       const Triangle& triangle2 = (*(convex2.polygons))[7];
 
       BOOST_CHECK(
-          computeFaceNormal(triangle1, points).isApprox(Vec3f::UnitX()));
+          computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitX()));
       BOOST_CHECK(
-          computeFaceNormal(triangle2, points).isApprox(Vec3f::UnitX()));
+          computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitX()));
     }
   }
 }
 
 BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) {
   typedef HFNodeBase::FaceOrientation FaceOrientation;
-  const FCL_REAL sphere_radius = 1.;
+  const CoalScalar sphere_radius = 1.;
   Sphere sphere(sphere_radius);
-  MatrixXf altitutes(3, 3);
-  FCL_REAL altitude_value = 1.;
+  MatrixXs altitutes(3, 3);
+  CoalScalar altitude_value = 1.;
   altitutes.fill(altitude_value);
 
   typedef AABB BV;
@@ -713,10 +714,10 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) {
 }
 
 BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
-  const FCL_REAL sphere_radius = 1.;
+  const CoalScalar sphere_radius = 1.;
   Sphere sphere(sphere_radius);
-  MatrixXf altitutes(2, 2);
-  FCL_REAL altitude_value = 1.;
+  MatrixXs altitutes(2, 2);
+  CoalScalar altitude_value = 1.;
   altitutes.fill(altitude_value);
 
   typedef AABB BV;
@@ -727,8 +728,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
 
   // Collision from the TOP
   {
-    const Transform3f sphere_pos(Vec3f(0., 0., 2.));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(Vec3s(0., 0., 2.));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -742,8 +743,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
 
   // Same, but with a positive margin.
   {
-    const Transform3f sphere_pos(Vec3f(0., 0., 2.));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(Vec3s(0., 0., 2.));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -753,7 +754,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
     BOOST_CHECK(result.isCollision());
     if (result.isCollision()) {
       const Contact& contact = result.getContact(0);
-      BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitZ()));
+      BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitZ()));
       std::cout << "contact.penetration_depth: " << contact.penetration_depth
                 << std::endl;
       BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
@@ -762,8 +763,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
 
   // Collision from the BOTTOM
   {
-    const Transform3f sphere_pos(Vec3f(0., 0., -1.));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(Vec3s(0., 0., -1.));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -774,8 +775,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
   }
 
   {
-    const Transform3f sphere_pos(Vec3f(0., 0., -1.));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(Vec3s(0., 0., -1.));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -785,7 +786,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
     BOOST_CHECK(result.isCollision());
     {
       const Contact& contact = result.getContact(0);
-      BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitZ()));
+      BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitZ()));
       std::cout << "contact.penetration_depth: " << contact.penetration_depth
                 << std::endl;
       BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
@@ -794,9 +795,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
 
   // Collision from the WEST
   {
-    const Transform3f sphere_pos(
-        Vec3f(hfield.getXGrid()[0] - sphere_radius, 0., 0.5));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(
+        Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -807,9 +808,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
   }
 
   {
-    const Transform3f sphere_pos(
-        Vec3f(hfield.getXGrid()[0] - sphere_radius, 0., 0.5));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(
+        Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -819,16 +820,16 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
     BOOST_CHECK(result.isCollision());
     if (result.isCollision()) {
       const Contact& contact = result.getContact(0);
-      BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitX()));
+      BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitX()));
       BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
     }
   }
 
   // Collision from the EAST
   {
-    const Transform3f sphere_pos(
-        Vec3f(hfield.getXGrid()[1] + sphere_radius, 0., 0.5));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(
+        Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -839,9 +840,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
   }
 
   {
-    const Transform3f sphere_pos(
-        Vec3f(hfield.getXGrid()[1] + sphere_radius, 0., 0.5));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(
+        Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -852,16 +853,16 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
 
     if (result.isCollision()) {
       const Contact& contact = result.getContact(0);
-      BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitX()));
+      BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitX()));
       BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
     }
   }
 
   // Collision from the NORTH
   {
-    const Transform3f sphere_pos(
-        Vec3f(0., hfield.getYGrid()[0] + sphere_radius, 0.5));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(
+        Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -872,9 +873,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
   }
 
   {
-    const Transform3f sphere_pos(
-        Vec3f(0., hfield.getYGrid()[0] + sphere_radius, 0.5));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(
+        Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -885,16 +886,16 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
 
     if (result.isCollision()) {
       const Contact& contact = result.getContact(0);
-      BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitY()));
+      BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitY()));
       BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
     }
   }
 
   // Collision from the SOUTH
   {
-    const Transform3f sphere_pos(
-        Vec3f(0., hfield.getYGrid()[1] - sphere_radius, 0.5));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(
+        Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -905,9 +906,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
   }
 
   {
-    const Transform3f sphere_pos(
-        Vec3f(0., hfield.getYGrid()[1] - sphere_radius, 0.5));
-    const Transform3f hfield_pos;
+    const Transform3s sphere_pos(
+        Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5));
+    const Transform3s hfield_pos;
 
     CollisionResult result;
     CollisionRequest request;
@@ -918,7 +919,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
 
     if (result.isCollision()) {
       const Contact& contact = result.getContact(0);
-      BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitY()));
+      BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitY()));
       BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
     }
   }
diff --git a/test/math.cpp b/test/math.cpp
index 3a5be3c9e6f672e05fe1371e6fc17bdd4015a0a1..c0f16ff3ee46ba53c249cd5f6cdceaaa3e203c5e 100644
--- a/test/math.cpp
+++ b/test/math.cpp
@@ -36,25 +36,25 @@
 #define _USE_MATH_DEFINES
 #include <cmath>
 
-#define BOOST_TEST_MODULE FCL_MATH
+#define BOOST_TEST_MODULE COAL_MATH
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/math/transform.h>
+#include "coal/data_types.h"
+#include "coal/math/transform.h"
 
-#include <hpp/fcl/internal/intersect.h>
-#include <hpp/fcl/internal/tools.h>
+#include "coal/internal/intersect.h"
+#include "coal/internal/tools.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
 BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) {
-  Vec3f v1(1.0, 2.0, 3.0);
+  Vec3s v1(1.0, 2.0, 3.0);
   BOOST_CHECK(v1[0] == 1.0);
   BOOST_CHECK(v1[1] == 2.0);
   BOOST_CHECK(v1[2] == 3.0);
 
-  Vec3f v2 = v1;
-  Vec3f v3(3.3, 4.3, 5.3);
+  Vec3s v2 = v1;
+  Vec3s v3(3.3, 4.3, 5.3);
   v1 += v3;
   BOOST_CHECK(isEqual(v1, v2 + v3));
   v1 -= v3;
@@ -87,53 +87,53 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) {
   BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix()));
   v1.array() += 2.0;
 
-  BOOST_CHECK(isEqual((-Vec3f(1.0, 2.0, 3.0)), Vec3f(-1.0, -2.0, -3.0)));
+  BOOST_CHECK(isEqual((-Vec3s(1.0, 2.0, 3.0)), Vec3s(-1.0, -2.0, -3.0)));
 
-  v1 = Vec3f(1.0, 2.0, 3.0);
-  v2 = Vec3f(3.0, 4.0, 5.0);
-  BOOST_CHECK(isEqual((v1.cross(v2)), Vec3f(-2.0, 4.0, -2.0)));
+  v1 = Vec3s(1.0, 2.0, 3.0);
+  v2 = Vec3s(3.0, 4.0, 5.0);
+  BOOST_CHECK(isEqual((v1.cross(v2)), Vec3s(-2.0, 4.0, -2.0)));
   BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
 
-  v1 = Vec3f(3.0, 4.0, 5.0);
+  v1 = Vec3s(3.0, 4.0, 5.0);
   BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
   BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
   BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm()));
 
-  v1 = Vec3f(1.0, 2.0, 3.0);
-  v2 = Vec3f(3.0, 4.0, 5.0);
-  BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-2.0, 4.0, -2.0)));
+  v1 = Vec3s(1.0, 2.0, 3.0);
+  v2 = Vec3s(3.0, 4.0, 5.0);
+  BOOST_CHECK(isEqual(v1.cross(v2), Vec3s(-2.0, 4.0, -2.0)));
   BOOST_CHECK(v1.dot(v2) == 26);
 }
 
-Vec3f rotate(Vec3f input, FCL_REAL w, Vec3f vec) {
+Vec3s rotate(Vec3s input, CoalScalar w, Vec3s vec) {
   return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input +
          2 * w * vec.cross(input);
 }
 
 BOOST_AUTO_TEST_CASE(quaternion) {
   Quatf q1(Quatf::Identity()), q2, q3;
-  q2 = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
+  q2 = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2);
   q3 = q2.inverse();
 
-  Vec3f v(1, -1, 0);
+  Vec3s v(1, -1, 0);
 
   BOOST_CHECK(isEqual(v, q1 * v));
-  BOOST_CHECK(isEqual(Vec3f(1, 1, 0), q2 * v));
+  BOOST_CHECK(isEqual(Vec3s(1, 1, 0), q2 * v));
   BOOST_CHECK(
-      isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v));
+      isEqual(rotate(v, q3.w(), Vec3s(q3.x(), q3.y(), q3.z())), q3 * v));
 }
 
 BOOST_AUTO_TEST_CASE(transform) {
-  Quatf q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
-  Vec3f T(0, 1, 2);
-  Transform3f tf(q, T);
+  Quatf q = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2);
+  Vec3s T(0, 1, 2);
+  Transform3s tf(q, T);
 
-  Vec3f v(1, -1, 0);
+  Vec3s v(1, -1, 0);
 
   BOOST_CHECK(isEqual(q * v + T, q * v + T));
 
-  Vec3f rv(q * v);
-  // typename Transform3f::transform_return_type<Vec3f>::type output =
+  Vec3s rv(q * v);
+  // typename Transform3s::transform_return_type<Vec3s>::type output =
   // tf * v;
   // std::cout << rv << std::endl;
   // std::cout << output.lhs() << std::endl;
@@ -142,7 +142,7 @@ BOOST_AUTO_TEST_CASE(transform) {
 
 BOOST_AUTO_TEST_CASE(random_transform) {
   for (size_t i = 0; i < 100; ++i) {
-    const Transform3f tf = Transform3f::Random();
+    const Transform3s tf = Transform3s::Random();
     BOOST_CHECK((tf.inverseTimes(tf)).isIdentity());
   }
 }
diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp
index a3ef2a696ee141f5e155ccb64b47993658031ade..3227774808410b9faebb60bf5d18f112553ea25a 100644
--- a/test/normal_and_nearest_points.cpp
+++ b/test/normal_and_nearest_points.cpp
@@ -34,19 +34,19 @@
 
 /** \author Louis Montaut */
 
-#define BOOST_TEST_MODULE NORMAL_AND_NEAREST_POINTS
+#define BOOST_TEST_MODULE COAL_NORMAL_AND_NEAREST_POINTS
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/BV/OBBRSS.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
+#include "coal/fwd.hh"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/collision_data.h"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/shape/geometric_shape_to_BVH_model.h"
 
 #include "utility.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 typedef Eigen::Vector2d Vec2d;
 
 // This test suite is designed to operate on any pair of primitive shapes:
@@ -75,9 +75,9 @@ template <typename ShapeType1, typename ShapeType2>
 void test_normal_and_nearest_points(
     const ShapeType1& o1, const ShapeType2& o2,
     size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS,
-    FCL_REAL gjk_tolerance = GJK_DEFAULT_TOLERANCE,
+    CoalScalar gjk_tolerance = GJK_DEFAULT_TOLERANCE,
     size_t epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS,
-    FCL_REAL epa_tolerance = EPA_DEFAULT_TOLERANCE) {
+    CoalScalar epa_tolerance = EPA_DEFAULT_TOLERANCE) {
 // Generate random poses for o2
 #ifndef NDEBUG  // if debug mode
   std::size_t n = 10;
@@ -86,13 +86,13 @@ void test_normal_and_nearest_points(
 #endif
   // We want to make sure we generate poses that are in collision
   // so we take a relatively small extent for the random poses
-  FCL_REAL extents[] = {-1.5, -1.5, -1.5, 1.5, 1.5, 1.5};
-  std::vector<Transform3f> transforms;
+  CoalScalar extents[] = {-1.5, -1.5, -1.5, 1.5, 1.5, 1.5};
+  std::vector<Transform3s> transforms;
   generateRandomTransforms(extents, transforms, n);
-  Transform3f tf1 = Transform3f::Identity();
+  Transform3s tf1 = Transform3s::Identity();
 
   CollisionRequest colreq;
-  colreq.distance_upper_bound = std::numeric_limits<FCL_REAL>::max();
+  colreq.distance_upper_bound = std::numeric_limits<CoalScalar>::max();
   // For strictly convex shapes, the default tolerance of EPA is way too low.
   // Because EPA is basically trying to fit a polytope to a strictly convex
   // surface, it might take it a lot of iterations to converge to a low
@@ -114,30 +114,30 @@ void test_normal_and_nearest_points(
     // Since CollisionRequest::distance_lower_bound is set to infinity,
     // these functions should agree on the results regardless of collision or
     // not.
-    Transform3f tf2 = transforms[i];
+    Transform3s tf2 = transforms[i];
     CollisionResult colres;
     DistanceResult distres;
     size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres);
-    FCL_REAL dist = distance(&o1, tf1, &o2, tf2, distreq, distres);
+    CoalScalar dist = distance(&o1, tf1, &o2, tf2, distreq, distres);
 
-    const FCL_REAL dummy_precision(100 *
-                                   std::numeric_limits<FCL_REAL>::epsilon());
+    const CoalScalar dummy_precision(
+        100 * std::numeric_limits<CoalScalar>::epsilon());
     if (col) {
       BOOST_CHECK(dist <= 0.);
       BOOST_CHECK_CLOSE(dist, distres.min_distance, dummy_precision);
       Contact contact = colres.getContact(0);
       BOOST_CHECK_CLOSE(dist, contact.penetration_depth, dummy_precision);
 
-      Vec3f cp1 = contact.nearest_points[0];
+      Vec3s cp1 = contact.nearest_points[0];
       EIGEN_VECTOR_IS_APPROX(cp1, distres.nearest_points[0], dummy_precision);
 
-      Vec3f cp2 = contact.nearest_points[1];
+      Vec3s cp2 = contact.nearest_points[1];
       EIGEN_VECTOR_IS_APPROX(cp2, distres.nearest_points[1], dummy_precision);
       BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(),
                         epa_tolerance);
       EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, epa_tolerance);
 
-      Vec3f separation_vector = contact.penetration_depth * contact.normal;
+      Vec3s separation_vector = contact.penetration_depth * contact.normal;
       EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, epa_tolerance);
 
       if (dist < 0) {
@@ -146,27 +146,27 @@ void test_normal_and_nearest_points(
       }
 
       // Separate the shapes
-      Transform3f new_tf1 = tf1;
-      FCL_REAL eps = 1e-2;
+      Transform3s new_tf1 = tf1;
+      CoalScalar eps = 1e-2;
       new_tf1.setTranslation(tf1.getTranslation() + separation_vector -
                              eps * contact.normal);
       CollisionResult new_colres;
       DistanceResult new_distres;
       size_t new_col = collide(&o1, new_tf1, &o2, tf2, colreq, new_colres);
-      FCL_REAL new_dist =
+      CoalScalar new_dist =
           distance(&o1, new_tf1, &o2, tf2, distreq, new_distres);
       BOOST_CHECK(new_dist > 0);
       BOOST_CHECK(!new_col);
       BOOST_CHECK(!new_colres.isCollision());
       BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist,
                         epa_tolerance);
-      Vec3f new_cp1 = new_distres.nearest_points[0];
-      Vec3f new_cp2 = new_distres.nearest_points[1];
+      Vec3s new_cp1 = new_distres.nearest_points[0];
+      Vec3s new_cp2 = new_distres.nearest_points[1];
       BOOST_CHECK_CLOSE(new_dist, (new_cp1 - new_cp2).norm(), epa_tolerance);
       EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal,
                              epa_tolerance);
 
-      Vec3f new_separation_vector = new_dist * new_distres.normal;
+      Vec3s new_separation_vector = new_dist * new_distres.normal;
       EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1,
                              epa_tolerance);
 
@@ -179,12 +179,12 @@ void test_normal_and_nearest_points(
       BOOST_CHECK_CLOSE(distres.min_distance, dist, dummy_precision);
       BOOST_CHECK_CLOSE(dist, colres.distance_lower_bound, dummy_precision);
 
-      Vec3f cp1 = distres.nearest_points[0];
-      Vec3f cp2 = distres.nearest_points[1];
+      Vec3s cp1 = distres.nearest_points[0];
+      Vec3s cp2 = distres.nearest_points[1];
       BOOST_CHECK_CLOSE(dist, (cp1 - cp2).norm(), gjk_tolerance);
       EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, gjk_tolerance);
 
-      Vec3f separation_vector = dist * distres.normal;
+      Vec3s separation_vector = dist * distres.normal;
       EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, gjk_tolerance);
 
       if (dist > 0) {
@@ -199,14 +199,14 @@ void test_normal_and_nearest_points(
       // If you translate one of the cones by the separation vector and it
       // happens to be parallel to the axis of the cone, the two shapes will
       // still be disjoint.
-      FCL_REAL eps = 1e-2;
-      Transform3f new_tf1 = tf1;
+      CoalScalar eps = 1e-2;
+      Transform3s new_tf1 = tf1;
       new_tf1.setTranslation(tf1.getTranslation() + separation_vector +
                              eps * distres.normal);
       CollisionResult new_colres;
       DistanceResult new_distres;
       collide(&o1, new_tf1, &o2, tf2, colreq, new_colres);
-      FCL_REAL new_dist =
+      CoalScalar new_dist =
           distance(&o1, new_tf1, &o2, tf2, distreq, new_distres);
       BOOST_CHECK(new_dist < dist);
       BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist,
@@ -214,11 +214,11 @@ void test_normal_and_nearest_points(
       // tolerance
       if (new_colres.isCollision()) {
         Contact contact = new_colres.getContact(0);
-        Vec3f new_cp1 = contact.nearest_points[0];
+        Vec3s new_cp1 = contact.nearest_points[0];
         EIGEN_VECTOR_IS_APPROX(new_cp1, new_distres.nearest_points[0],
                                dummy_precision);
 
-        Vec3f new_cp2 = contact.nearest_points[1];
+        Vec3s new_cp2 = contact.nearest_points[1];
         EIGEN_VECTOR_IS_APPROX(new_cp2, new_distres.nearest_points[1],
                                dummy_precision);
         BOOST_CHECK_CLOSE(contact.penetration_depth,
@@ -226,7 +226,7 @@ void test_normal_and_nearest_points(
         EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal,
                                epa_tolerance);
 
-        Vec3f new_separation_vector =
+        Vec3s new_separation_vector =
             contact.penetration_depth * contact.normal;
         EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1,
                                epa_tolerance);
@@ -241,17 +241,18 @@ void test_normal_and_nearest_points(
 }
 
 template <int VecSize>
-Eigen::Matrix<FCL_REAL, VecSize, 1> generateRandomVector(FCL_REAL min,
-                                                         FCL_REAL max) {
-  typedef Eigen::Matrix<FCL_REAL, VecSize, 1> VecType;
+Eigen::Matrix<CoalScalar, VecSize, 1> generateRandomVector(CoalScalar min,
+                                                           CoalScalar max) {
+  typedef Eigen::Matrix<CoalScalar, VecSize, 1> VecType;
   // Generate a random vector in the [min, max] range
   VecType v = VecType::Random() * (max - min) * 0.5 +
               VecType::Ones() * (max + min) * 0.5;
   return v;
 }
 
-FCL_REAL generateRandomNumber(FCL_REAL min, FCL_REAL max) {
-  FCL_REAL r = static_cast<FCL_REAL>(rand()) / static_cast<FCL_REAL>(RAND_MAX);
+CoalScalar generateRandomNumber(CoalScalar min, CoalScalar max) {
+  CoalScalar r =
+      static_cast<CoalScalar>(rand()) / static_cast<CoalScalar>(RAND_MAX);
   r = 2 * r - 1.0;
   return r * (max - min) * 0.5 + (max + min) * 0.5;
 }
@@ -269,7 +270,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_sphere) {
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_capsule) {
   for (size_t i = 0; i < 10; ++i) {
     Vec2d radii = generateRandomVector<2>(0.05, 1.0);
-    FCL_REAL h = generateRandomNumber(0.15, 1.0);
+    CoalScalar h = generateRandomNumber(0.15, 1.0);
     shared_ptr<Sphere> o1(new Sphere(radii(0)));
     shared_ptr<Capsule> o2(new Capsule(radii(1), h));
 
@@ -300,9 +301,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_mesh) {
         o2_.points, o2_.num_points, o2_.polygons, o2_.num_polygons));
 
     size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS;
-    FCL_REAL gjk_tolerance = GJK_DEFAULT_TOLERANCE;
+    CoalScalar gjk_tolerance = GJK_DEFAULT_TOLERANCE;
     size_t epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS;
-    FCL_REAL epa_tolerance = EPA_DEFAULT_TOLERANCE;
+    CoalScalar epa_tolerance = EPA_DEFAULT_TOLERANCE;
     test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations,
                                    gjk_tolerance, epa_max_iterations,
                                    epa_tolerance);
@@ -328,12 +329,12 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_ellipsoid) {
         o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons));
     shared_ptr<Ellipsoid> o2(new Ellipsoid(generateRandomVector<3>(0.05, 1.0)));
 
-    FCL_REAL gjk_tolerance = 1e-6;
+    CoalScalar gjk_tolerance = 1e-6;
     // With EPA's tolerance set at 1e-3, the precision on the normal, contact
     // points and penetration depth is on the order of the milimeter. However,
     // EPA (currently) cannot converge to lower tolerances on strictly convex
     // shapes in a reasonable amount of iterations.
-    FCL_REAL epa_tolerance = 1e-3;
+    CoalScalar epa_tolerance = 1e-3;
     test_normal_and_nearest_points(*o1.get(), *o2.get(),
                                    GJK_DEFAULT_MAX_ITERATIONS, gjk_tolerance,
                                    EPA_DEFAULT_MAX_ITERATIONS, epa_tolerance);
@@ -349,13 +350,13 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_ellipsoid) {
     shared_ptr<Ellipsoid> o2(new Ellipsoid(generateRandomVector<3>(0.05, 1.0)));
 
     size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS;
-    FCL_REAL gjk_tolerance = 1e-6;
+    CoalScalar gjk_tolerance = 1e-6;
     // With EPA's tolerance set at 1e-3, the precision on the normal, contact
     // points and penetration depth is on the order of the milimeter. However,
     // EPA (currently) cannot converge to lower tolerances on strictly convex
     // shapes in a reasonable amount of iterations.
     size_t epa_max_iterations = 250;
-    FCL_REAL epa_tolerance = 1e-3;
+    CoalScalar epa_tolerance = 1e-3;
     // For EPA on ellipsoids, we need to increase the number of iterations in
     // this test. This is simply because this test checks **a lot** of cases and
     // it can generate some of the worst cases for EPA. We don't want to
@@ -370,8 +371,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_ellipsoid) {
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) {
   for (size_t i = 0; i < 10; ++i) {
     shared_ptr<Box> o1(new Box(generateRandomVector<3>(0.05, 1.0)));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Plane> o2(new Plane(n, offset));
 
@@ -383,8 +384,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) {
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) {
   for (size_t i = 0; i < 10; ++i) {
     shared_ptr<Box> o1(new Box(generateRandomVector<3>(0.05, 1.0)));
-    FCL_REAL offset = 0.1;
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = 0.1;
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Halfspace> o2(new Halfspace(n, offset));
 
@@ -395,11 +396,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) {
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) {
   for (size_t i = 0; i < 10; ++i) {
-    FCL_REAL r = generateRandomNumber(0.05, 1.0);
-    FCL_REAL h = generateRandomNumber(0.15, 1.0);
+    CoalScalar r = generateRandomNumber(0.05, 1.0);
+    CoalScalar h = generateRandomNumber(0.15, 1.0);
     shared_ptr<Capsule> o1(new Capsule(r, h));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Halfspace> o2(new Halfspace(n, offset));
 
@@ -411,8 +412,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) {
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) {
   for (size_t i = 0; i < 10; ++i) {
     shared_ptr<Sphere> o1(new Sphere(generateRandomNumber(0.05, 1.0)));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Halfspace> o2(new Halfspace(n, offset));
 
@@ -424,8 +425,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) {
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_plane) {
   for (size_t i = 0; i < 10; ++i) {
     shared_ptr<Sphere> o1(new Sphere(generateRandomNumber(0.05, 1.0)));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Plane> o2(new Plane(n, offset));
 
@@ -440,8 +441,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_halfspace) {
         Ellipsoid(generateRandomVector<3>(0.05, 1.0)));
     shared_ptr<Convex<Triangle>> o1(new Convex<Triangle>(
         o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Halfspace> o2(new Halfspace(n, offset));
 
@@ -458,9 +459,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_cylinder) {
     shared_ptr<Cylinder> o2(new Cylinder(r(1), h(1)));
 
     size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS;
-    FCL_REAL gjk_tolerance = 1e-6;
+    CoalScalar gjk_tolerance = 1e-6;
     size_t epa_max_iterations = 250;
-    FCL_REAL epa_tolerance = 1e-3;
+    CoalScalar epa_tolerance = 1e-3;
     test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations,
                                    gjk_tolerance, epa_max_iterations,
                                    epa_tolerance);
@@ -478,9 +479,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_ellipsoid) {
     shared_ptr<Cylinder> o2(new Cylinder(r(1), h(1)));
 
     size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS;
-    FCL_REAL gjk_tolerance = 1e-6;
+    CoalScalar gjk_tolerance = 1e-6;
     size_t epa_max_iterations = 250;
-    FCL_REAL epa_tolerance = 1e-3;
+    CoalScalar epa_tolerance = 1e-3;
     test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations,
                                    gjk_tolerance, epa_max_iterations,
                                    epa_tolerance);
@@ -492,11 +493,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_ellipsoid) {
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) {
   for (size_t i = 0; i < 10; ++i) {
-    FCL_REAL r = generateRandomNumber(0.05, 1.0);
-    FCL_REAL h = generateRandomNumber(0.15, 1.0);
+    CoalScalar r = generateRandomNumber(0.05, 1.0);
+    CoalScalar h = generateRandomNumber(0.15, 1.0);
     shared_ptr<Cone> o1(new Cone(r, h));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Halfspace> o2(new Halfspace(n, offset));
 
@@ -507,11 +508,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) {
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) {
   for (size_t i = 0; i < 10; ++i) {
-    FCL_REAL r = generateRandomNumber(0.05, 1.0);
-    FCL_REAL h = generateRandomNumber(0.15, 1.0);
+    CoalScalar r = generateRandomNumber(0.05, 1.0);
+    CoalScalar h = generateRandomNumber(0.15, 1.0);
     shared_ptr<Cylinder> o1(new Cylinder(r, h));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Halfspace> o2(new Halfspace(n, offset));
 
@@ -522,11 +523,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) {
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) {
   for (size_t i = 0; i < 10; ++i) {
-    FCL_REAL r = generateRandomNumber(0.05, 1.0);
-    FCL_REAL h = generateRandomNumber(0.15, 1.0);
+    CoalScalar r = generateRandomNumber(0.05, 1.0);
+    CoalScalar h = generateRandomNumber(0.15, 1.0);
     shared_ptr<Cone> o1(new Cone(r, h));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Plane> o2(new Plane(n, offset));
 
@@ -537,11 +538,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) {
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) {
   for (size_t i = 0; i < 10; ++i) {
-    FCL_REAL r = generateRandomNumber(0.05, 1.0);
-    FCL_REAL h = generateRandomNumber(0.15, 1.0);
+    CoalScalar r = generateRandomNumber(0.05, 1.0);
+    CoalScalar h = generateRandomNumber(0.15, 1.0);
     shared_ptr<Cylinder> o1(new Cylinder(r, h));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Plane> o2(new Plane(n, offset));
 
@@ -552,11 +553,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) {
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_plane) {
   for (size_t i = 0; i < 10; ++i) {
-    FCL_REAL r = generateRandomNumber(0.05, 1.0);
-    FCL_REAL h = generateRandomNumber(0.15, 1.0);
+    CoalScalar r = generateRandomNumber(0.05, 1.0);
+    CoalScalar h = generateRandomNumber(0.15, 1.0);
     shared_ptr<Capsule> o1(new Capsule(r, h));
-    FCL_REAL offset = generateRandomNumber(-0.5, 0.5);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(-0.5, 0.5);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Plane> o2(new Plane(n, offset));
 
@@ -580,7 +581,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_capsule) {
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) {
   for (size_t i = 0; i < 10; ++i) {
     Vec2d r = generateRandomVector<2>(0.05, 1.0);
-    FCL_REAL h = generateRandomNumber(0.15, 1.0);
+    CoalScalar h = generateRandomNumber(0.15, 1.0);
     shared_ptr<Sphere> o1(new Sphere(r(0)));
     shared_ptr<Cylinder> o2(new Cylinder(r(1), h));
 
@@ -591,8 +592,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) {
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) {
   for (size_t i = 0; i < 10; ++i) {
-    FCL_REAL offset = generateRandomNumber(0.15, 1.0);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(0.15, 1.0);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Ellipsoid> o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0)));
     shared_ptr<Halfspace> o2(new Halfspace(n, offset));
@@ -604,8 +605,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) {
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_plane) {
   for (size_t i = 0; i < 10; ++i) {
-    FCL_REAL offset = generateRandomNumber(0.15, 1.0);
-    Vec3f n = Vec3f::Random();
+    CoalScalar offset = generateRandomNumber(0.15, 1.0);
+    Vec3s n = Vec3s::Random();
     n.normalize();
     shared_ptr<Ellipsoid> o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0)));
     shared_ptr<Plane> o2(new Plane(n, offset));
@@ -623,25 +624,25 @@ void test_normal_and_nearest_points(const BVHModel<OBBRSS>& o1,
 #else
   size_t n = 10000;
 #endif
-  FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
-  std::vector<Transform3f> transforms;
+  CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.};
+  std::vector<Transform3s> transforms;
   generateRandomTransforms(extents, transforms, n);
-  Transform3f tf1 = Transform3f::Identity();
-  transforms[0] = Transform3f::Identity();
+  Transform3s tf1 = Transform3s::Identity();
+  transforms[0] = Transform3s::Identity();
 
   CollisionRequest colreq;
-  colreq.distance_upper_bound = std::numeric_limits<FCL_REAL>::max();
+  colreq.distance_upper_bound = std::numeric_limits<CoalScalar>::max();
   colreq.num_max_contacts = 100;
   CollisionResult colres;
   DistanceRequest distreq;
   DistanceResult distres;
 
   for (size_t i = 0; i < n; i++) {
-    Transform3f tf2 = transforms[i];
+    Transform3s tf2 = transforms[i];
     colres.clear();
     distres.clear();
     size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres);
-    FCL_REAL dist = distance(&o1, tf1, &o2, tf2, distreq, distres);
+    CoalScalar dist = distance(&o1, tf1, &o2, tf2, distreq, distres);
 
     if (col) {
       BOOST_CHECK(dist <= 0.);
@@ -651,13 +652,13 @@ void test_normal_and_nearest_points(const BVHModel<OBBRSS>& o1,
         BOOST_CHECK(contact.penetration_depth <= 0);
         BOOST_CHECK(contact.penetration_depth >= colres.distance_lower_bound);
 
-        Vec3f cp1 = contact.nearest_points[0];
-        Vec3f cp2 = contact.nearest_points[1];
+        Vec3s cp1 = contact.nearest_points[0];
+        Vec3s cp2 = contact.nearest_points[1];
         BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), 1e-6);
         EIGEN_VECTOR_IS_APPROX(
             cp1, cp2 - contact.penetration_depth * contact.normal, 1e-6);
 
-        Vec3f separation_vector = contact.penetration_depth * contact.normal;
+        Vec3s separation_vector = contact.penetration_depth * contact.normal;
         EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6);
 
         if (dist < 0) {
@@ -679,14 +680,14 @@ void test_normal_and_nearest_points(const Halfspace& o1,
 }
 
 BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) {
-  Box* box_ptr = new hpp::fcl::Box(1, 1, 1);
-  hpp::fcl::CollisionGeometryPtr_t b1(box_ptr);
-  BVHModel<hpp::fcl::OBBRSS> o1 = BVHModel<OBBRSS>();
-  generateBVHModel(o1, *box_ptr, Transform3f());
+  Box* box_ptr = new coal::Box(1, 1, 1);
+  coal::CollisionGeometryPtr_t b1(box_ptr);
+  BVHModel<coal::OBBRSS> o1 = BVHModel<OBBRSS>();
+  generateBVHModel(o1, *box_ptr, Transform3s());
   o1.buildConvexRepresentation(false);
 
-  FCL_REAL offset = 0.1;
-  Vec3f n = Vec3f::Random();
+  CoalScalar offset = 0.1;
+  Vec3s n = Vec3s::Random();
   n.normalize();
   shared_ptr<Halfspace> o2(new Halfspace(n, offset));
 
diff --git a/test/obb.cpp b/test/obb.cpp
index 117696bb758c4c995ab35439a2b724438dd67e48..134a8daa2d5d6dc5aef08e134748d0bfac9691ae 100644
--- a/test/obb.cpp
+++ b/test/obb.cpp
@@ -40,34 +40,34 @@
 
 #include <chrono>
 
-#include <hpp/fcl/narrowphase/narrowphase.h>
+#include "coal/narrowphase/narrowphase.h"
 
 #include "../src/BV/OBB.h"
-#include <hpp/fcl/internal/shape_shape_func.h>
+#include "coal/internal/shape_shape_func.h"
 #include "utility.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
-void randomOBBs(Vec3f& a, Vec3f& b, FCL_REAL extentNorm) {
+void randomOBBs(Vec3s& a, Vec3s& b, CoalScalar extentNorm) {
   // Extent norm is between 0 and extentNorm on each axis
-  // a = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3));
-  // b = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3));
+  // a = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3));
+  // b = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3));
 
-  a = extentNorm * Vec3f::Random().cwiseAbs().normalized();
-  b = extentNorm * Vec3f::Random().cwiseAbs().normalized();
+  a = extentNorm * Vec3s::Random().cwiseAbs().normalized();
+  b = extentNorm * Vec3s::Random().cwiseAbs().normalized();
 }
 
-void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b,
-                     const FCL_REAL extentNorm) {
+void randomTransform(Matrix3s& B, Vec3s& T, const Vec3s& a, const Vec3s& b,
+                     const CoalScalar extentNorm) {
   // TODO Should we scale T to a and b norm ?
   (void)a;
   (void)b;
   (void)extentNorm;
 
-  FCL_REAL N = a.norm() + b.norm();
+  CoalScalar N = a.norm() + b.norm();
   // A translation of norm N ensures there is no collision.
   // Set translation to be between 0 and 2 * N;
-  T = (Vec3f::Random() / sqrt(3)) * 1.5 * N;
+  T = (Vec3s::Random() / sqrt(3)) * 1.5 * N;
   // T.setZero();
 
   Quatf q;
@@ -90,7 +90,7 @@ typedef std::chrono::high_resolution_clock clock_type;
 typedef clock_type::duration duration_type;
 
 const char* sep = ",\t";
-const FCL_REAL eps = 1.5e-7;
+const CoalScalar eps = 1.5e-7;
 
 const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0,
                              ", ",   // Coeff separator
@@ -103,22 +103,22 @@ const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0,
 
 namespace obbDisjoint_impls {
 /// @return true if OBB are disjoint.
-bool distance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,
-              FCL_REAL& distance) {
+bool distance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,
+              CoalScalar& distance) {
   GJKSolver gjk;
   Box ba(2 * a), bb(2 * b);
-  Transform3f tfa, tfb(B, T);
+  Transform3s tfa, tfb(B, T);
 
-  Vec3f p1, p2, normal;
+  Vec3s p1, p2, normal;
   bool compute_penetration = true;
   distance =
       gjk.shapeDistance(ba, tfa, bb, tfb, compute_penetration, p1, p2, normal);
   return (distance > gjk.getDistancePrecision(compute_penetration));
 }
 
-inline FCL_REAL _computeDistanceForCase1(const Vec3f& T, const Vec3f& a,
-                                         const Vec3f& b, const Matrix3f& Bf) {
-  Vec3f AABB_corner;
+inline CoalScalar _computeDistanceForCase1(const Vec3s& T, const Vec3s& a,
+                                           const Vec3s& b, const Matrix3s& Bf) {
+  Vec3s AABB_corner;
   /* This seems to be slower
  AABB_corner.noalias() = T.cwiseAbs () - a;
  AABB_corner.noalias() -= PRODUCT(Bf,b);
@@ -132,19 +132,19 @@ inline FCL_REAL _computeDistanceForCase1(const Vec3f& T, const Vec3f& a,
   AABB_corner.noalias() = T.cwiseAbs() - Bf * b - a;
 #endif
   // */
-  return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm();
+  return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm();
 }
 
-inline FCL_REAL _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T,
-                                         const Vec3f& a, const Vec3f& b,
-                                         const Matrix3f& Bf) {
+inline CoalScalar _computeDistanceForCase2(const Matrix3s& B, const Vec3s& T,
+                                           const Vec3s& a, const Vec3s& b,
+                                           const Matrix3s& Bf) {
   /*
-  Vec3f AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b);
+  Vec3s AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b);
   AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a);
-  return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm ();
+  return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm ();
   /*/
 #if MANUAL_PRODUCT
-  FCL_REAL s, t = 0;
+  CoalScalar s, t = 0;
   s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
   if (s > 0) t += s * s;
   s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
@@ -153,18 +153,18 @@ inline FCL_REAL _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T,
   if (s > 0) t += s * s;
   return t;
 #else
-  Vec3f AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b);
-  return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm();
+  Vec3s AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b);
+  return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm();
 #endif
   // */
 }
 
-int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-                     const Vec3f& b, const FCL_REAL& breakDistance2,
-                     FCL_REAL& squaredLowerBoundDistance) {
+int separatingAxisId(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                     const Vec3s& b, const CoalScalar& breakDistance2,
+                     CoalScalar& squaredLowerBoundDistance) {
   int id = 0;
 
-  Matrix3f Bf(B.cwiseAbs());
+  Matrix3s Bf(B.cwiseAbs());
 
   // Corner of b axis aligned bounding box the closest to the origin
   squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -179,14 +179,15 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
   int ja = 1, ka = 2, jb = 1, kb = 2;
   for (int ia = 0; ia < 3; ++ia) {
     for (int ib = 0; ib < 3; ++ib) {
-      const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
+      const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
 
-      const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
-                                       b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
+      const CoalScalar diff =
+          fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
+                     b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
       // We need to divide by the norm || Aia x Bib ||
       // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2  = cosine^2
       if (diff > 0) {
-        FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
+        CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
         if (sinus2 > 1e-6) {
           squaredLowerBoundDistance = diff * diff / sinus2;
           if (squaredLowerBoundDistance > breakDistance2) {
@@ -194,7 +195,7 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
           }
         }
         /* // or
-           FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
+           CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
            squaredLowerBoundDistance = diff * diff;
            if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
            squaredLowerBoundDistance /= sinus2;
@@ -215,10 +216,10 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
 }
 
 // ------------------------ 0 --------------------------------------
-bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-                     const Vec3f& b, const FCL_REAL& breakDistance2,
-                     FCL_REAL& squaredLowerBoundDistance) {
-  Matrix3f Bf(B.cwiseAbs());
+bool withRuntimeLoop(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                     const Vec3s& b, const CoalScalar& breakDistance2,
+                     CoalScalar& squaredLowerBoundDistance) {
+  Matrix3s Bf(B.cwiseAbs());
 
   // Corner of b axis aligned bounding box the closest to the origin
   squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -231,14 +232,15 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
   int ja = 1, ka = 2, jb = 1, kb = 2;
   for (int ia = 0; ia < 3; ++ia) {
     for (int ib = 0; ib < 3; ++ib) {
-      const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
+      const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
 
-      const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
-                                       b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
+      const CoalScalar diff =
+          fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
+                     b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
       // We need to divide by the norm || Aia x Bib ||
       // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2  = cosine^2
       if (diff > 0) {
-        FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
+        CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
         if (sinus2 > 1e-6) {
           squaredLowerBoundDistance = diff * diff / sinus2;
           if (squaredLowerBoundDistance > breakDistance2) {
@@ -246,7 +248,7 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
           }
         }
         /* // or
-           FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
+           CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
            squaredLowerBoundDistance = diff * diff;
            if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
            squaredLowerBoundDistance /= sinus2;
@@ -266,30 +268,30 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
 }
 
 // ------------------------ 1 --------------------------------------
-bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
-                               const Vec3f& a, const Vec3f& b,
-                               const FCL_REAL& breakDistance2,
-                               FCL_REAL& squaredLowerBoundDistance) {
-  FCL_REAL t, s;
-  FCL_REAL diff;
-
-  // Matrix3f Bf = abs(B);
+bool withManualLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
+                               const Vec3s& a, const Vec3s& b,
+                               const CoalScalar& breakDistance2,
+                               CoalScalar& squaredLowerBoundDistance) {
+  CoalScalar t, s;
+  CoalScalar diff;
+
+  // Matrix3s Bf = abs(B);
   // Bf += reps;
-  Matrix3f Bf(B.cwiseAbs());
+  Matrix3s Bf(B.cwiseAbs());
 
   // Corner of b axis aligned bounding box the closest to the origin
-  Vec3f AABB_corner(T.cwiseAbs() - Bf * b);
-  Vec3f diff3(AABB_corner - a);
-  diff3 = diff3.cwiseMax(Vec3f::Zero());
+  Vec3s AABB_corner(T.cwiseAbs() - Bf * b);
+  Vec3s diff3(AABB_corner - a);
+  diff3 = diff3.cwiseMax(Vec3s::Zero());
 
-  // for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]);
+  // for (Vec3s::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]);
   squaredLowerBoundDistance = diff3.squaredNorm();
   if (squaredLowerBoundDistance > breakDistance2) return true;
 
   AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a;
   // diff3 = | B^T T| - b - Bf^T a
   diff3 = AABB_corner - b;
-  diff3 = diff3.cwiseMax(Vec3f::Zero());
+  diff3 = diff3.cwiseMax(Vec3s::Zero());
   squaredLowerBoundDistance = diff3.squaredNorm();
 
   if (squaredLowerBoundDistance > breakDistance2) return true;
@@ -299,7 +301,7 @@ bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
   t = ((s < 0.0) ? -s : s);
   assert(t == fabs(s));
 
-  FCL_REAL sinus2;
+  CoalScalar sinus2;
   diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
               b[2] * Bf(0, 1));
   // We need to divide by the norm || A0 x B0 ||
@@ -456,11 +458,11 @@ bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
 }
 
 // ------------------------ 2 --------------------------------------
-bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T,
-                               const Vec3f& a, const Vec3f& b,
-                               const FCL_REAL& breakDistance2,
-                               FCL_REAL& squaredLowerBoundDistance) {
-  Matrix3f Bf(B.cwiseAbs());
+bool withManualLoopUnrolling_2(const Matrix3s& B, const Vec3s& T,
+                               const Vec3s& a, const Vec3s& b,
+                               const CoalScalar& breakDistance2,
+                               CoalScalar& squaredLowerBoundDistance) {
+  Matrix3s Bf(B.cwiseAbs());
 
   // Corner of b axis aligned bounding box the closest to the origin
   squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -470,13 +472,13 @@ bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T,
   if (squaredLowerBoundDistance > breakDistance2) return true;
 
   // A0 x B0
-  FCL_REAL t, s;
+  CoalScalar t, s;
   s = T[2] * B(1, 0) - T[1] * B(2, 0);
   t = ((s < 0.0) ? -s : s);
   assert(t == fabs(s));
 
-  FCL_REAL sinus2;
-  FCL_REAL diff;
+  CoalScalar sinus2;
+  CoalScalar diff;
   diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
               b[2] * Bf(0, 1));
   // We need to divide by the norm || A0 x B0 ||
@@ -636,18 +638,18 @@ bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T,
 template <int ia, int ib, int ja = (ia + 1) % 3, int ka = (ia + 2) % 3,
           int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
 struct loop_case_1 {
-  static inline bool run(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-                         const Vec3f& b, const Matrix3f& Bf,
-                         const FCL_REAL& breakDistance2,
-                         FCL_REAL& squaredLowerBoundDistance) {
-    const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
-
-    const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
-                                     b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
+  static inline bool run(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                         const Vec3s& b, const Matrix3s& Bf,
+                         const CoalScalar& breakDistance2,
+                         CoalScalar& squaredLowerBoundDistance) {
+    const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
+
+    const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
+                                       b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
     // We need to divide by the norm || Aia x Bib ||
     // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2  = cosine^2
     if (diff > 0) {
-      FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
+      CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
       if (sinus2 > 1e-6) {
         squaredLowerBoundDistance = diff * diff / sinus2;
         if (squaredLowerBoundDistance > breakDistance2) {
@@ -655,7 +657,7 @@ struct loop_case_1 {
         }
       }
       /* // or
-         FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
+         CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
          squaredLowerBoundDistance = diff * diff;
          if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
          squaredLowerBoundDistance /= sinus2;
@@ -667,11 +669,11 @@ struct loop_case_1 {
   }
 };
 
-bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
-                                 const Vec3f& a, const Vec3f& b,
-                                 const FCL_REAL& breakDistance2,
-                                 FCL_REAL& squaredLowerBoundDistance) {
-  Matrix3f Bf(B.cwiseAbs());
+bool withTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
+                                 const Vec3s& a, const Vec3s& b,
+                                 const CoalScalar& breakDistance2,
+                                 CoalScalar& squaredLowerBoundDistance) {
+  Matrix3s Bf(B.cwiseAbs());
 
   // Corner of b axis aligned bounding box the closest to the origin
   squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -716,18 +718,18 @@ bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
 
 template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
 struct loop_case_2 {
-  static inline bool run(int ia, int ja, int ka, const Matrix3f& B,
-                         const Vec3f& T, const Vec3f& a, const Vec3f& b,
-                         const Matrix3f& Bf, const FCL_REAL& breakDistance2,
-                         FCL_REAL& squaredLowerBoundDistance) {
-    const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
-
-    const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
-                                     b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
+  static inline bool run(int ia, int ja, int ka, const Matrix3s& B,
+                         const Vec3s& T, const Vec3s& a, const Vec3s& b,
+                         const Matrix3s& Bf, const CoalScalar& breakDistance2,
+                         CoalScalar& squaredLowerBoundDistance) {
+    const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
+
+    const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
+                                       b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
     // We need to divide by the norm || Aia x Bib ||
     // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2  = cosine^2
     if (diff > 0) {
-      FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
+      CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
       if (sinus2 > 1e-6) {
         squaredLowerBoundDistance = diff * diff / sinus2;
         if (squaredLowerBoundDistance > breakDistance2) {
@@ -735,7 +737,7 @@ struct loop_case_2 {
         }
       }
       /* // or
-         FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
+         CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
          squaredLowerBoundDistance = diff * diff;
          if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
          squaredLowerBoundDistance /= sinus2;
@@ -747,11 +749,11 @@ struct loop_case_2 {
   }
 };
 
-bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
-                                        const Vec3f& a, const Vec3f& b,
-                                        const FCL_REAL& breakDistance2,
-                                        FCL_REAL& squaredLowerBoundDistance) {
-  Matrix3f Bf(B.cwiseAbs());
+bool withPartialTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
+                                        const Vec3s& a, const Vec3s& b,
+                                        const CoalScalar& breakDistance2,
+                                        CoalScalar& squaredLowerBoundDistance) {
+  Matrix3s Bf(B.cwiseAbs());
 
   // Corner of b axis aligned bounding box the closest to the origin
   squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -780,13 +782,13 @@ bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
 }
 
 // ------------------------ 5 --------------------------------------
-bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-                            const Vec3f& b, const FCL_REAL& breakDistance2,
-                            FCL_REAL& squaredLowerBoundDistance) {
-  FCL_REAL t, s;
-  FCL_REAL diff;
+bool originalWithLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                            const Vec3s& b, const CoalScalar& breakDistance2,
+                            CoalScalar& squaredLowerBoundDistance) {
+  CoalScalar t, s;
+  CoalScalar diff;
 
-  Matrix3f Bf(B.cwiseAbs());
+  Matrix3s Bf(B.cwiseAbs());
   squaredLowerBoundDistance = 0;
 
   // if any of these tests are one-sided, then the polyhedra are disjoint
@@ -852,7 +854,7 @@ bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
   s = T[2] * B(1, 0) - T[1] * B(2, 0);
   t = ((s < 0.0) ? -s : s);
 
-  FCL_REAL sinus2;
+  CoalScalar sinus2;
   diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
               b[2] * Bf(0, 1));
   // We need to divide by the norm || A0 x B0 ||
@@ -1001,15 +1003,15 @@ bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
 }
 
 // ------------------------ 6 --------------------------------------
-bool originalWithNoLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-                              const Vec3f& b, const FCL_REAL&,
-                              FCL_REAL& squaredLowerBoundDistance) {
-  FCL_REAL t, s;
-  const FCL_REAL reps = 1e-6;
+bool originalWithNoLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                              const Vec3s& b, const CoalScalar&,
+                              CoalScalar& squaredLowerBoundDistance) {
+  CoalScalar t, s;
+  const CoalScalar reps = 1e-6;
 
   squaredLowerBoundDistance = 0;
 
-  Matrix3f Bf(B.array().abs() + reps);
+  Matrix3s Bf(B.array().abs() + reps);
   // Bf += reps;
 
   // if any of these tests are one-sided, then the polyhedra are disjoint
@@ -1137,8 +1139,8 @@ struct BenchmarkResult {
   /// - 0-10 identifies a separating axes.
   /// - 11 means no separatins axes could be found. (distance should be <= 0)
   int ifId;
-  FCL_REAL distance;
-  FCL_REAL squaredLowerBoundDistance;
+  CoalScalar distance;
+  CoalScalar squaredLowerBoundDistance;
   duration_type duration[NB_METHODS];
   bool failure;
 
@@ -1171,13 +1173,13 @@ std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) {
   return br.print(os);
 }
 
-BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T,
-                                   const Vec3f& a, const Vec3f& b,
+BenchmarkResult benchmark_obb_case(const Matrix3s& B, const Vec3s& T,
+                                   const Vec3s& a, const Vec3s& b,
                                    const CollisionRequest& request,
                                    std::size_t N) {
-  const FCL_REAL breakDistance(request.break_distance +
-                               request.security_margin);
-  const FCL_REAL breakDistance2 = breakDistance * breakDistance;
+  const CoalScalar breakDistance(request.break_distance +
+                                 request.security_margin);
+  const CoalScalar breakDistance2 = breakDistance * breakDistance;
 
   BenchmarkResult result;
   // First determine which axis provide the answer
@@ -1189,7 +1191,7 @@ BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T,
   // Sanity check
   result.failure = true;
   bool overlap = (result.ifId == 11);
-  FCL_REAL dist_thr = request.break_distance + request.security_margin;
+  CoalScalar dist_thr = request.break_distance + request.security_margin;
   if (!overlap && result.distance <= 0) {
     std::cerr << "Failure: negative distance for disjoint OBBs.";
   } else if (!overlap && result.squaredLowerBoundDistance < 0) {
@@ -1216,7 +1218,7 @@ BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T,
   }
 
   // Compute time
-  FCL_REAL tmp;
+  CoalScalar tmp;
   clock_type::time_point start, end;
 
   // ------------------------ 0 --------------------------------------
@@ -1281,9 +1283,9 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) {
   std::size_t nbFailure = 0;
 
   // Create two OBBs axis
-  Vec3f a, b;
-  Matrix3f B;
-  Vec3f T;
+  Vec3s a, b;
+  Matrix3s B;
+  Vec3s T;
   CollisionRequest request;
 
 #ifndef NDEBUG  // if debug mode
@@ -1295,7 +1297,7 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) {
   static const size_t nbTransformPerOBB = 100;
   static const size_t nbRunForTimeMeas = 1000;
 #endif
-  static const FCL_REAL extentNorm = 1.;
+  static const CoalScalar extentNorm = 1.;
 
   if (output != NULL) *output << BenchmarkResult::headers << '\n';
 
diff --git a/test/octree.cpp b/test/octree.cpp
index 566fb6dd504dd0967a65f3ab8c18f91259c879a6..d7f3b5d354ed03a07debe056f9672a773dead213 100644
--- a/test/octree.cpp
+++ b/test/octree.cpp
@@ -35,29 +35,29 @@
 
 /** \author Florent Lamiraux <florent@laas.fr> */
 
-#define BOOST_TEST_MODULE FCL_OCTREE
+#define BOOST_TEST_MODULE COAL_OCTREE
 #include <fstream>
 #include <boost/test/included/unit_test.hpp>
 #include <boost/filesystem.hpp>
 
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/hfield.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/internal/BV_splitter.h>
+#include "coal/BVH/BVH_model.h"
+#include "coal/collision.h"
+#include "coal/distance.h"
+#include "coal/hfield.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/BV_splitter.h"
 
 #include "utility.h"
 #include "fcl_resources/config.h"
 
 namespace utf = boost::unit_test::framework;
 
-using namespace hpp::fcl;
+using namespace coal;
 
-void makeMesh(const std::vector<Vec3f>& vertices,
+void makeMesh(const std::vector<Vec3s>& vertices,
               const std::vector<Triangle>& triangles, BVHModel<OBBRSS>& model) {
-  hpp::fcl::SplitMethodType split_method(hpp::fcl::SPLIT_METHOD_MEAN);
+  coal::SplitMethodType split_method(coal::SPLIT_METHOD_MEAN);
   model.bv_splitter.reset(new BVSplitter<OBBRSS>(split_method));
   model.bv_splitter.reset(new BVSplitter<OBBRSS>(split_method));
 
@@ -66,27 +66,26 @@ void makeMesh(const std::vector<Vec3f>& vertices,
   model.endModel();
 }
 
-hpp::fcl::OcTree makeOctree(const BVHModel<OBBRSS>& mesh,
-                            const FCL_REAL& resolution) {
-  Vec3f m(mesh.aabb_local.min_);
-  Vec3f M(mesh.aabb_local.max_);
-  hpp::fcl::Box box(resolution, resolution, resolution);
-  CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND,
-                           1);
+coal::OcTree makeOctree(const BVHModel<OBBRSS>& mesh,
+                        const CoalScalar& resolution) {
+  Vec3s m(mesh.aabb_local.min_);
+  Vec3s M(mesh.aabb_local.max_);
+  coal::Box box(resolution, resolution, resolution);
+  CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1);
   CollisionResult result;
-  Transform3f tfBox;
+  Transform3s tfBox;
   octomap::OcTreePtr_t octree(new octomap::OcTree(resolution));
 
-  for (FCL_REAL x = resolution * floor(m[0] / resolution); x <= M[0];
+  for (CoalScalar x = resolution * floor(m[0] / resolution); x <= M[0];
        x += resolution) {
-    for (FCL_REAL y = resolution * floor(m[1] / resolution); y <= M[1];
+    for (CoalScalar y = resolution * floor(m[1] / resolution); y <= M[1];
          y += resolution) {
-      for (FCL_REAL z = resolution * floor(m[2] / resolution); z <= M[2];
+      for (CoalScalar z = resolution * floor(m[2] / resolution); z <= M[2];
            z += resolution) {
-        Vec3f center(x + .5 * resolution, y + .5 * resolution,
+        Vec3s center(x + .5 * resolution, y + .5 * resolution,
                      z + .5 * resolution);
         tfBox.setTranslation(center);
-        hpp::fcl::collide(&box, tfBox, &mesh, Transform3f(), request, result);
+        coal::collide(&box, tfBox, &mesh, Transform3s(), request, result);
         if (result.isCollision()) {
           octomap::point3d p((float)center[0], (float)center[1],
                              (float)center[2]);
@@ -105,8 +104,8 @@ hpp::fcl::OcTree makeOctree(const BVHModel<OBBRSS>& mesh,
 BOOST_AUTO_TEST_CASE(octree_mesh) {
   Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
                         "", "", "(", ")");
-  FCL_REAL resolution(10.);
-  std::vector<Vec3f> pRob, pEnv;
+  CoalScalar resolution(10.);
+  std::vector<Vec3s> pRob, pEnv;
   std::vector<Triangle> tRob, tEnv;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
   loadOBJFile((path / "rob.obj").string().c_str(), pRob, tRob);
@@ -120,7 +119,7 @@ BOOST_AUTO_TEST_CASE(octree_mesh) {
   envMesh.computeLocalAABB();
   // Load octree built from envMesh by makeOctree(envMesh, resolution)
   OcTree envOctree(
-      hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution));
+      coal::loadOctreeFile((path / "env.octree").string(), resolution));
 
   std::cout << "Finished loading octree." << std::endl;
 
@@ -137,40 +136,39 @@ BOOST_AUTO_TEST_CASE(octree_mesh) {
   {
     const std::vector<uint8_t> bytes = envOctree.tobytes();
     BOOST_CHECK(bytes.size() > 0 && bytes.size() <= envOctree.toBoxes().size() *
-                                                        3 * sizeof(FCL_REAL));
+                                                        3 * sizeof(CoalScalar));
   }
 
-  std::vector<Transform3f> transforms;
-  FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
+  std::vector<Transform3s> transforms;
+  CoalScalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
 #ifndef NDEBUG  // if debug mode
   std::size_t N = 100;
 #else
   std::size_t N = 10000;
 #endif
-  N = hpp::fcl::getNbRun(utf::master_test_suite().argc,
-                         utf::master_test_suite().argv, N);
+  N = coal::getNbRun(utf::master_test_suite().argc,
+                     utf::master_test_suite().argv, N);
 
   generateRandomTransforms(extents, transforms, 2 * N);
 
-  CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND,
-                           1);
+  CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1);
   for (std::size_t i = 0; i < N; ++i) {
     CollisionResult resultMesh;
     CollisionResult resultOctree;
-    Transform3f tf1(transforms[2 * i]);
-    Transform3f tf2(transforms[2 * i + 1]);
+    Transform3s tf1(transforms[2 * i]);
+    Transform3s tf2(transforms[2 * i + 1]);
     ;
     // Test collision between meshes with random transform for robot.
-    hpp::fcl::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh);
+    coal::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh);
     // Test collision between mesh and octree for the same transform.
-    hpp::fcl::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree);
+    coal::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree);
     bool resMesh(resultMesh.isCollision());
     bool resOctree(resultOctree.isCollision());
     BOOST_CHECK(!resMesh || resOctree);
     if (!resMesh && resOctree) {
-      hpp::fcl::DistanceRequest dreq;
-      hpp::fcl::DistanceResult dres;
-      hpp::fcl::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres);
+      coal::DistanceRequest dreq;
+      coal::DistanceResult dres;
+      coal::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres);
       std::cout << "distance mesh mesh: " << dres.min_distance << std::endl;
       BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution);
     }
@@ -180,8 +178,8 @@ BOOST_AUTO_TEST_CASE(octree_mesh) {
 BOOST_AUTO_TEST_CASE(octree_height_field) {
   Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
                         "", "", "(", ")");
-  FCL_REAL resolution(10.);
-  std::vector<Vec3f> pEnv;
+  CoalScalar resolution(10.);
+  std::vector<Vec3s> pEnv;
   std::vector<Triangle> tEnv;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
   loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv);
@@ -193,57 +191,56 @@ BOOST_AUTO_TEST_CASE(octree_height_field) {
   envMesh.computeLocalAABB();
   // Load octree built from envMesh by makeOctree(envMesh, resolution)
   OcTree envOctree(
-      hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution));
+      coal::loadOctreeFile((path / "env.octree").string(), resolution));
 
   std::cout << "Finished loading octree." << std::endl;
 
   // Building hfield
-  const FCL_REAL x_dim = 10, y_dim = 20;
+  const CoalScalar x_dim = 10, y_dim = 20;
   const int nx = 100, ny = 100;
-  const FCL_REAL max_altitude = 1., min_altitude = 0.;
-  const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);
+  const CoalScalar max_altitude = 1., min_altitude = 0.;
+  const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude);
 
   HeightField<AABB> hfield(x_dim, y_dim, heights, min_altitude);
   hfield.computeLocalAABB();
 
-  std::vector<Transform3f> transforms;
-  FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
+  std::vector<Transform3s> transforms;
+  CoalScalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
 #ifndef NDEBUG  // if debug mode
   std::size_t N = 1000;
 #else
   std::size_t N = 100000;
 #endif
-  N = hpp::fcl::getNbRun(utf::master_test_suite().argc,
-                         utf::master_test_suite().argv, N);
+  N = coal::getNbRun(utf::master_test_suite().argc,
+                     utf::master_test_suite().argv, N);
 
   generateRandomTransforms(extents, transforms, 2 * N);
 
-  CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND,
-                           1);
+  CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1);
   for (std::size_t i = 0; i < N; ++i) {
     CollisionResult resultBox;
     CollisionResult resultHfield1, resultHfield2;
-    Transform3f tf1(transforms[2 * i]);
-    Transform3f tf2(transforms[2 * i + 1]);
+    Transform3s tf1(transforms[2 * i]);
+    Transform3s tf2(transforms[2 * i + 1]);
 
     Box box;
-    Transform3f box_tf;
+    Transform3s box_tf;
     constructBox(hfield.aabb_local, tf2, box, box_tf);
 
     // Test collision between octree and equivalent box.
-    hpp::fcl::collide(&envOctree, tf1, &box, box_tf, request, resultBox);
+    coal::collide(&envOctree, tf1, &box, box_tf, request, resultBox);
     // Test collision between octree and hfield.
-    hpp::fcl::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1);
-    hpp::fcl::collide(&hfield, tf2, &envOctree, tf1, request, resultHfield2);
+    coal::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1);
+    coal::collide(&hfield, tf2, &envOctree, tf1, request, resultHfield2);
 
     bool resBox(resultBox.isCollision());
     bool resHfield(resultHfield1.isCollision());
     BOOST_CHECK(resBox == resHfield);
     BOOST_CHECK(resultHfield1.isCollision() == resultHfield2.isCollision());
     if (!resBox && resHfield) {
-      hpp::fcl::DistanceRequest dreq;
-      hpp::fcl::DistanceResult dres;
-      hpp::fcl::distance(&envMesh, tf1, &box, box_tf, dreq, dres);
+      coal::DistanceRequest dreq;
+      coal::DistanceResult dres;
+      coal::distance(&envMesh, tf1, &box, box_tf, dreq, dres);
       std::cout << "distance mesh box: " << dres.min_distance << std::endl;
       BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution);
     }
diff --git a/test/profiling.cpp b/test/profiling.cpp
index 6c11dac7df50b97387c892a10580796c9e92658a..ab97a9648ba438f2dbd8c030d7061865aa3e8590 100644
--- a/test/profiling.cpp
+++ b/test/profiling.cpp
@@ -1,33 +1,33 @@
 // Copyright (c) 2017, Joseph Mirabel
 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
 //
-// This file is part of hpp-fcl.
-// hpp-fcl is free software: you can redistribute it
+// This file is part of Coal.
+// Coal is free software: you can redistribute it
 // and/or modify it under the terms of the GNU Lesser General Public
 // License as published by the Free Software Foundation, either version
 // 3 of the License, or (at your option) any later version.
 //
-// hpp-fcl is distributed in the hope that it will be
+// Coal is distributed in the hope that it will be
 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 // General Lesser Public License for more details.  You should have
 // received a copy of the GNU Lesser General Public License along with
-// hpp-fcl. If not, see <http://www.gnu.org/licenses/>.
+// Coal. If not, see <http://www.gnu.org/licenses/>.
 
 #include <boost/filesystem.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/collision_utility.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/collision_func_matrix.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/mesh_loader/assimp.h>
+#include "coal/fwd.hh"
+#include "coal/collision.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/collision_utility.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/collision_func_matrix.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/mesh_loader/assimp.h"
 #include "utility.h"
 #include "fcl_resources/config.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
 CollisionFunctionMatrix lookupTable;
 bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) {
@@ -44,7 +44,7 @@ bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) {
 
 template <typename BV /*, SplitMethodType split_method*/>
 CollisionGeometryPtr_t objToGeom(const std::string& filename) {
-  std::vector<Vec3f> pt;
+  std::vector<Vec3s> pt;
   std::vector<Triangle> tri;
   loadOBJFile(filename.c_str(), pt, tri);
 
@@ -60,7 +60,7 @@ CollisionGeometryPtr_t objToGeom(const std::string& filename) {
 
 template <typename BV /*, SplitMethodType split_method*/>
 CollisionGeometryPtr_t meshToGeom(const std::string& filename,
-                                  const Vec3f& scale = Vec3f(1, 1, 1)) {
+                                  const Vec3s& scale = Vec3s(1, 1, 1)) {
   shared_ptr<BVHModel<BV> > model(new BVHModel<BV>());
   loadPolyhedronFromResource(filename, scale, model);
   return model;
@@ -82,10 +82,10 @@ struct Results {
   Results(size_t i) : rs(i), times((Eigen::DenseIndex)i) {}
 };
 
-void collide(const std::vector<Transform3f>& tf, const CollisionGeometry* o1,
+void collide(const std::vector<Transform3s>& tf, const CollisionGeometry* o1,
              const CollisionGeometry* o2, const CollisionRequest& request,
              Results& results) {
-  Transform3f Id;
+  Transform3s Id;
   BenchTimer timer;
   for (std::size_t i = 0; i < tf.size(); ++i) {
     timer.start();
@@ -117,7 +117,7 @@ size_t Ntransform = 1;
 #else
 size_t Ntransform = 100;
 #endif
-FCL_REAL limit = 20;
+CoalScalar limit = 20;
 bool verbose = false;
 
 #define OUT(x) \
@@ -190,16 +190,16 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) {
     iarg += 3;
     if (iarg < argc && strcmp(argv[iarg], "crop") == 0) {
       CHECK_PARAM_NB(6, Crop);
-      hpp::fcl::AABB aabb(Vec3f(atof(argv[iarg + 1]), atof(argv[iarg + 2]),
-                                atof(argv[iarg + 3])),
-                          Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]),
-                                atof(argv[iarg + 6])));
+      coal::AABB aabb(Vec3s(atof(argv[iarg + 1]), atof(argv[iarg + 2]),
+                            atof(argv[iarg + 3])),
+                      Vec3s(atof(argv[iarg + 4]), atof(argv[iarg + 5]),
+                            atof(argv[iarg + 6])));
       OUT("Cropping " << aabb.min_.transpose() << " ---- "
                       << aabb.max_.transpose() << " ...");
       o->computeLocalAABB();
       OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- "
                           << o->aabb_local.max_.transpose() << " ...");
-      o.reset(extract(o.get(), Transform3f(), aabb));
+      o.reset(extract(o.get(), Transform3s(), aabb));
       if (!o) throw std::invalid_argument("Failed to crop.");
       OUT("Crop has " << dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris
                       << " triangles");
@@ -221,7 +221,7 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) {
 }
 
 int main(int argc, char** argv) {
-  std::vector<Transform3f> transforms;
+  std::vector<Transform3s> transforms;
 
   CollisionRequest request;
 
@@ -231,14 +231,14 @@ int main(int argc, char** argv) {
     Geometry first = makeGeomFromParam(iarg, argc, argv);
     Geometry second = makeGeomFromParam(iarg, argc, argv);
 
-    FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit};
+    CoalScalar extents[] = {-limit, -limit, -limit, limit, limit, limit};
     generateRandomTransforms(extents, transforms, Ntransform);
     printResultHeaders();
     Results results(Ntransform);
     collide(transforms, first.o.get(), second.o.get(), request, results);
     printResults(first, second, results);
   } else {
-    FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit};
+    CoalScalar extents[] = {-limit, -limit, -limit, limit, limit, limit};
     generateRandomTransforms(extents, transforms, Ntransform);
     boost::filesystem::path path(TEST_RESOURCES_DIR);
 
@@ -249,11 +249,11 @@ int main(int argc, char** argv) {
     geoms.push_back(Geometry("Cone", new Cone(2, 1)));
     geoms.push_back(Geometry("Cylinder", new Cylinder(2, 1)));
     // geoms.push_back(Geometry ("Plane"     , new Plane
-    // (Vec3f(1,1,1).normalized(), 0)                  ));
+    // (Vec3s(1,1,1).normalized(), 0)                  ));
     // geoms.push_back(Geometry ("Halfspace" , new Halfspace
-    // (Vec3f(1,1,1).normalized(), 0)                  ));
+    // (Vec3s(1,1,1).normalized(), 0)                  ));
     //  not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP
-    //  (Vec3f(0,1,0), Vec3f(0,0,1), Vec3f(-1,0,0))     ));
+    //  (Vec3s(0,1,0), Vec3s(0,0,1), Vec3s(-1,0,0))     ));
 
     geoms.push_back(Geometry("rob BVHModel<OBB>",
                              objToGeom<OBB>((path / "rob.obj").string())));
diff --git a/test/python_unit/CMakeLists.txt b/test/python_unit/CMakeLists.txt
index 8f7e098e0d9faad365df273683301ec1f5b3e895..1e3a952984888553ea3d8a0fb854ebbb78ee4cc1 100644
--- a/test/python_unit/CMakeLists.txt
+++ b/test/python_unit/CMakeLists.txt
@@ -6,7 +6,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
   pickling
   )
 
-ADD_DEPENDENCIES(build_tests hppfcl)
+ADD_DEPENDENCIES(build_tests ${PROJECT_NAME}_pywrap)
 FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
-  ADD_PYTHON_UNIT_TEST("py-${TEST}" "test/python_unit/${TEST}.py" "python")
-ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
+  ADD_PYTHON_UNIT_TEST("${PROJECT_NAME}-py-${TEST}" "test/python_unit/${TEST}.py" "python")
+ENDFOREACH()
diff --git a/test/python_unit/api.py b/test/python_unit/api.py
index d982dc611962e9f18cc75029aa090db5b1c82009..f15b08f60a7bc980fbade18d3ea4d0fa62182a04 100644
--- a/test/python_unit/api.py
+++ b/test/python_unit/api.py
@@ -1,30 +1,30 @@
 import unittest
 from test_case import TestCase
-import hppfcl
+import coal
 
 import numpy as np
 
 
 class TestMainAPI(TestCase):
     def test_collision(self):
-        capsule = hppfcl.Capsule(1.0, 2.0)
-        M1 = hppfcl.Transform3f()
-        M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0]))
+        capsule = coal.Capsule(1.0, 2.0)
+        M1 = coal.Transform3s()
+        M2 = coal.Transform3s(np.eye(3), np.array([3, 0, 0]))
 
-        req = hppfcl.CollisionRequest()
-        res = hppfcl.CollisionResult()
+        req = coal.CollisionRequest()
+        res = coal.CollisionResult()
 
-        self.assertTrue(not hppfcl.collide(capsule, M1, capsule, M2, req, res))
+        self.assertTrue(not coal.collide(capsule, M1, capsule, M2, req, res))
 
     def test_distance(self):
-        capsule = hppfcl.Capsule(1.0, 2.0)
-        M1 = hppfcl.Transform3f()
-        M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0]))
+        capsule = coal.Capsule(1.0, 2.0)
+        M1 = coal.Transform3s()
+        M2 = coal.Transform3s(np.eye(3), np.array([3, 0, 0]))
 
-        req = hppfcl.DistanceRequest()
-        res = hppfcl.DistanceResult()
+        req = coal.DistanceRequest()
+        res = coal.DistanceResult()
 
-        self.assertTrue(hppfcl.distance(capsule, M1, capsule, M2, req, res) > 0)
+        self.assertTrue(coal.distance(capsule, M1, capsule, M2, req, res) > 0)
 
 
 if __name__ == "__main__":
diff --git a/test/python_unit/collision.py b/test/python_unit/collision.py
index d14ece98f7328793620f80b8d144240d5676f1f8..afd69d503181d33e16e9ad41f998cbc827f3b95f 100644
--- a/test/python_unit/collision.py
+++ b/test/python_unit/collision.py
@@ -1,47 +1,47 @@
 import unittest
 from test_case import TestCase
-import hppfcl
+import coal
 
 import numpy as np
 
 
 def tetahedron():
-    pts = hppfcl.StdVec_Vec3f()
+    pts = coal.StdVec_Vec3s()
     pts.append(np.array((0, 0, 0)))
     pts.append(np.array((0, 1, 0)))
     pts.append(np.array((1, 0, 0)))
     pts.append(np.array((0, 0, 1)))
-    tri = hppfcl.StdVec_Triangle()
-    tri.append(hppfcl.Triangle(0, 1, 2))
-    tri.append(hppfcl.Triangle(0, 1, 3))
-    tri.append(hppfcl.Triangle(0, 2, 3))
-    tri.append(hppfcl.Triangle(1, 2, 3))
-    return hppfcl.Convex(pts, tri)
+    tri = coal.StdVec_Triangle()
+    tri.append(coal.Triangle(0, 1, 2))
+    tri.append(coal.Triangle(0, 1, 3))
+    tri.append(coal.Triangle(0, 2, 3))
+    tri.append(coal.Triangle(1, 2, 3))
+    return coal.Convex(pts, tri)
 
 
 class TestMainAPI(TestCase):
     def test_convex_halfspace(self):
         convex = tetahedron()
-        halfspace = hppfcl.Halfspace(np.array((0, 0, 1)), 0)
+        halfspace = coal.Halfspace(np.array((0, 0, 1)), 0)
 
-        req = hppfcl.CollisionRequest()
-        res = hppfcl.CollisionResult()
+        req = coal.CollisionRequest()
+        res = coal.CollisionResult()
 
-        M1 = hppfcl.Transform3f()
-        M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, -0.1]))
+        M1 = coal.Transform3s()
+        M2 = coal.Transform3s(np.eye(3), np.array([0, 0, -0.1]))
         res.clear()
-        hppfcl.collide(convex, M1, halfspace, M2, req, res)
-        self.assertFalse(hppfcl.collide(convex, M1, halfspace, M2, req, res))
+        coal.collide(convex, M1, halfspace, M2, req, res)
+        self.assertFalse(coal.collide(convex, M1, halfspace, M2, req, res))
 
-        M1 = hppfcl.Transform3f()
-        M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 0.1]))
+        M1 = coal.Transform3s()
+        M2 = coal.Transform3s(np.eye(3), np.array([0, 0, 0.1]))
         res.clear()
-        self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res))
+        self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res))
 
-        M1 = hppfcl.Transform3f()
-        M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 2]))
+        M1 = coal.Transform3s()
+        M2 = coal.Transform3s(np.eye(3), np.array([0, 0, 2]))
         res.clear()
-        self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res))
+        self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res))
 
 
 if __name__ == "__main__":
diff --git a/test/python_unit/collision_manager.py b/test/python_unit/collision_manager.py
index 46f7fa925928ffe4ca2d931967173e1b418564c7..241fda1c6b18ac0ccd6c4d8b77d8d1b0aeb81113 100644
--- a/test/python_unit/collision_manager.py
+++ b/test/python_unit/collision_manager.py
@@ -1,21 +1,21 @@
-import hppfcl as fcl
+import coal
 import numpy as np
 
-sphere = fcl.Sphere(0.5)
-sphere_obj = fcl.CollisionObject(sphere)
+sphere = coal.Sphere(0.5)
+sphere_obj = coal.CollisionObject(sphere)
 
-M_sphere = fcl.Transform3f.Identity()
+M_sphere = coal.Transform3s.Identity()
 M_sphere.setTranslation(np.array([-0.6, 0.0, 0.0]))
 sphere_obj.setTransform(M_sphere)
 
-box = fcl.Box(np.array([0.5, 0.5, 0.5]))
-box_obj = fcl.CollisionObject(box)
+box = coal.Box(np.array([0.5, 0.5, 0.5]))
+box_obj = coal.CollisionObject(box)
 
-M_box = fcl.Transform3f.Identity()
+M_box = coal.Transform3s.Identity()
 M_box.setTranslation(np.array([-0.6, 0.0, 0.0]))
 box_obj.setTransform(M_box)
 
-collision_manager = fcl.DynamicAABBTreeCollisionManager()
+collision_manager = coal.DynamicAABBTreeCollisionManager()
 collision_manager.registerObject(sphere_obj)
 collision_manager.registerObject(box_obj)
 
@@ -24,7 +24,7 @@ assert collision_manager.size() == 2
 collision_manager.setup()
 
 # Perform collision detection
-callback = fcl.CollisionCallBackDefault()
+callback = coal.CollisionCallBackDefault()
 collision_manager.collide(sphere_obj, callback)
 
 assert callback.data.result.numContacts() == 1
diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py
index 4d6735b1070c96bdec1b17d45c866f07e790e950..c7774e3162cc084663e0c6da128147e4672f099e 100644
--- a/test/python_unit/geometric_shapes.py
+++ b/test/python_unit/geometric_shapes.py
@@ -1,16 +1,16 @@
 import unittest
 from test_case import TestCase
-import hppfcl
+import coal
 import numpy as np
 
 
 class TestGeometricShapes(TestCase):
     def test_capsule(self):
-        capsule = hppfcl.Capsule(1.0, 2.0)
-        self.assertIsInstance(capsule, hppfcl.Capsule)
-        self.assertIsInstance(capsule, hppfcl.ShapeBase)
-        self.assertIsInstance(capsule, hppfcl.CollisionGeometry)
-        self.assertEqual(capsule.getNodeType(), hppfcl.NODE_TYPE.GEOM_CAPSULE)
+        capsule = coal.Capsule(1.0, 2.0)
+        self.assertIsInstance(capsule, coal.Capsule)
+        self.assertIsInstance(capsule, coal.ShapeBase)
+        self.assertIsInstance(capsule, coal.CollisionGeometry)
+        self.assertEqual(capsule.getNodeType(), coal.NODE_TYPE.GEOM_CAPSULE)
         self.assertEqual(capsule.radius, 1.0)
         self.assertEqual(capsule.halfLength, 1.0)
         capsule.radius = 3.0
@@ -47,11 +47,11 @@ class TestGeometricShapes(TestCase):
         self.assertApprox(Ic, I0_ref)
 
     def test_box1(self):
-        box = hppfcl.Box(np.array([1.0, 2.0, 3.0]))
-        self.assertIsInstance(box, hppfcl.Box)
-        self.assertIsInstance(box, hppfcl.ShapeBase)
-        self.assertIsInstance(box, hppfcl.CollisionGeometry)
-        self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX)
+        box = coal.Box(np.array([1.0, 2.0, 3.0]))
+        self.assertIsInstance(box, coal.Box)
+        self.assertIsInstance(box, coal.ShapeBase)
+        self.assertIsInstance(box, coal.CollisionGeometry)
+        self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX)
         self.assertTrue(np.array_equal(box.halfSide, np.array([0.5, 1.0, 1.5])))
         box.halfSide = np.array([4.0, 5.0, 6.0])
         self.assertTrue(np.array_equal(box.halfSide, np.array([4.0, 5.0, 6.0])))
@@ -73,21 +73,21 @@ class TestGeometricShapes(TestCase):
         self.assertApprox(Ic, I0_ref)
 
     def test_box2(self):
-        box = hppfcl.Box(1.0, 2.0, 3)
-        self.assertIsInstance(box, hppfcl.Box)
-        self.assertIsInstance(box, hppfcl.ShapeBase)
-        self.assertIsInstance(box, hppfcl.CollisionGeometry)
-        self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX)
+        box = coal.Box(1.0, 2.0, 3)
+        self.assertIsInstance(box, coal.Box)
+        self.assertIsInstance(box, coal.ShapeBase)
+        self.assertIsInstance(box, coal.CollisionGeometry)
+        self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX)
         self.assertEqual(box.halfSide[0], 0.5)
         self.assertEqual(box.halfSide[1], 1.0)
         self.assertEqual(box.halfSide[2], 1.5)
 
     def test_sphere(self):
-        sphere = hppfcl.Sphere(1.0)
-        self.assertIsInstance(sphere, hppfcl.Sphere)
-        self.assertIsInstance(sphere, hppfcl.ShapeBase)
-        self.assertIsInstance(sphere, hppfcl.CollisionGeometry)
-        self.assertEqual(sphere.getNodeType(), hppfcl.NODE_TYPE.GEOM_SPHERE)
+        sphere = coal.Sphere(1.0)
+        self.assertIsInstance(sphere, coal.Sphere)
+        self.assertIsInstance(sphere, coal.ShapeBase)
+        self.assertIsInstance(sphere, coal.CollisionGeometry)
+        self.assertEqual(sphere.getNodeType(), coal.NODE_TYPE.GEOM_SPHERE)
         self.assertEqual(sphere.radius, 1.0)
         sphere.radius = 2.0
         self.assertEqual(sphere.radius, 2.0)
@@ -103,11 +103,11 @@ class TestGeometricShapes(TestCase):
         self.assertApprox(Ic, I0_ref)
 
     def test_cylinder(self):
-        cylinder = hppfcl.Cylinder(1.0, 2.0)
-        self.assertIsInstance(cylinder, hppfcl.Cylinder)
-        self.assertIsInstance(cylinder, hppfcl.ShapeBase)
-        self.assertIsInstance(cylinder, hppfcl.CollisionGeometry)
-        self.assertEqual(cylinder.getNodeType(), hppfcl.NODE_TYPE.GEOM_CYLINDER)
+        cylinder = coal.Cylinder(1.0, 2.0)
+        self.assertIsInstance(cylinder, coal.Cylinder)
+        self.assertIsInstance(cylinder, coal.ShapeBase)
+        self.assertIsInstance(cylinder, coal.CollisionGeometry)
+        self.assertEqual(cylinder.getNodeType(), coal.NODE_TYPE.GEOM_CYLINDER)
         self.assertEqual(cylinder.radius, 1.0)
         self.assertEqual(cylinder.halfLength, 1.0)
         cylinder.radius = 3.0
@@ -128,11 +128,11 @@ class TestGeometricShapes(TestCase):
         self.assertApprox(Ic, I0_ref)
 
     def test_cone(self):
-        cone = hppfcl.Cone(1.0, 2.0)
-        self.assertIsInstance(cone, hppfcl.Cone)
-        self.assertIsInstance(cone, hppfcl.ShapeBase)
-        self.assertIsInstance(cone, hppfcl.CollisionGeometry)
-        self.assertEqual(cone.getNodeType(), hppfcl.NODE_TYPE.GEOM_CONE)
+        cone = coal.Cone(1.0, 2.0)
+        self.assertIsInstance(cone, coal.Cone)
+        self.assertIsInstance(cone, coal.ShapeBase)
+        self.assertIsInstance(cone, coal.CollisionGeometry)
+        self.assertEqual(cone.getNodeType(), coal.NODE_TYPE.GEOM_CONE)
         self.assertEqual(cone.radius, 1.0)
         self.assertEqual(cone.halfLength, 1.0)
         cone.radius = 3.0
@@ -155,13 +155,13 @@ class TestGeometricShapes(TestCase):
         self.assertApprox(Ic, Ic_ref)
 
     def test_BVH(self):
-        bvh = hppfcl.BVHModelOBBRSS()
+        bvh = coal.BVHModelOBBRSS()
         self.assertEqual(bvh.num_vertices, 0)
         self.assertEqual(bvh.vertices().shape, (0, 3))
 
     def test_convex(self):
-        verts = hppfcl.StdVec_Vec3f()
-        faces = hppfcl.StdVec_Triangle()
+        verts = coal.StdVec_Vec3s()
+        faces = coal.StdVec_Triangle()
         verts.extend(
             [
                 np.array([0, 0, 0]),
@@ -169,12 +169,12 @@ class TestGeometricShapes(TestCase):
                 np.array([1, 0, 0]),
             ]
         )
-        faces.append(hppfcl.Triangle(0, 1, 2))
-        hppfcl.Convex(verts, faces)
+        faces.append(coal.Triangle(0, 1, 2))
+        coal.Convex(verts, faces)
 
         verts.append(np.array([0, 0, 1]))
         try:
-            hppfcl.Convex.convexHull(verts, False, None)
+            coal.Convex.convexHull(verts, False, None)
             qhullAvailable = True
         except Exception as e:
             self.assertIn(
@@ -183,11 +183,11 @@ class TestGeometricShapes(TestCase):
             qhullAvailable = False
 
         if qhullAvailable:
-            hppfcl.Convex.convexHull(verts, False, "")
-            hppfcl.Convex.convexHull(verts, True, "")
+            coal.Convex.convexHull(verts, False, "")
+            coal.Convex.convexHull(verts, True, "")
 
             try:
-                hppfcl.Convex.convexHull(verts[:3], False, None)
+                coal.Convex.convexHull(verts[:3], False, None)
             except Exception as e:
                 self.assertIn(
                     "You shouldn't use this function with less than 4 points.", str(e)
diff --git a/test/python_unit/pickling.py b/test/python_unit/pickling.py
index 1e20fe393d7adf284dfbeac750077d33e18cd4e9..c6143d87914db454753a2a1aaf3ba3142ca65f09 100644
--- a/test/python_unit/pickling.py
+++ b/test/python_unit/pickling.py
@@ -1,23 +1,23 @@
 import unittest
 from test_case import TestCase
-import hppfcl
+import coal
 
 import pickle
 import numpy as np
 
 
 def tetahedron():
-    pts = hppfcl.StdVec_Vec3f()
+    pts = coal.StdVec_Vec3s()
     pts.append(np.array((0, 0, 0)))
     pts.append(np.array((0, 1, 0)))
     pts.append(np.array((1, 0, 0)))
     pts.append(np.array((0, 0, 1)))
-    tri = hppfcl.StdVec_Triangle()
-    tri.append(hppfcl.Triangle(0, 1, 2))
-    tri.append(hppfcl.Triangle(0, 1, 3))
-    tri.append(hppfcl.Triangle(0, 2, 3))
-    tri.append(hppfcl.Triangle(1, 2, 3))
-    return hppfcl.Convex(pts, tri)
+    tri = coal.StdVec_Triangle()
+    tri.append(coal.Triangle(0, 1, 2))
+    tri.append(coal.Triangle(0, 1, 3))
+    tri.append(coal.Triangle(0, 2, 3))
+    tri.append(coal.Triangle(1, 2, 3))
+    return coal.Convex(pts, tri)
 
 
 class TestGeometryPickling(TestCase):
@@ -30,28 +30,28 @@ class TestGeometryPickling(TestCase):
         self.assertTrue(obj == obj2)
 
     def test_all_shapes(self):
-        box = hppfcl.Box(1.0, 2.0, 3.0)
+        box = coal.Box(1.0, 2.0, 3.0)
         self.pickling(box)
 
-        sphere = hppfcl.Sphere(1.0)
+        sphere = coal.Sphere(1.0)
         self.pickling(sphere)
 
-        ellipsoid = hppfcl.Ellipsoid(1.0, 2.0, 3.0)
+        ellipsoid = coal.Ellipsoid(1.0, 2.0, 3.0)
         self.pickling(ellipsoid)
 
         convex = tetahedron()
         self.pickling(convex)
 
-        capsule = hppfcl.Capsule(1.0, 2.0)
+        capsule = coal.Capsule(1.0, 2.0)
         self.pickling(capsule)
 
-        cylinder = hppfcl.Cylinder(1.0, 2.0)
+        cylinder = coal.Cylinder(1.0, 2.0)
         self.pickling(cylinder)
 
-        plane = hppfcl.Plane(np.array([0.0, 0.0, 1.0]), 2.0)
+        plane = coal.Plane(np.array([0.0, 0.0, 1.0]), 2.0)
         self.pickling(plane)
 
-        half_space = hppfcl.Halfspace(np.array([0.0, 0.0, 1.0]), 2.0)
+        half_space = coal.Halfspace(np.array([0.0, 0.0, 1.0]), 2.0)
         self.pickling(half_space)
 
 
diff --git a/test/security_margin.cpp b/test/security_margin.cpp
index aee5455771db75bbbedd94d3c733a2e893058439..5993343daea60b3683e9ac2b0e8669a7f53f99e1 100644
--- a/test/security_margin.cpp
+++ b/test/security_margin.cpp
@@ -32,53 +32,53 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN
+#define BOOST_TEST_MODULE COAL_SECURITY_MARGIN
 #include <boost/test/included/unit_test.hpp>
 
 #include <cmath>
 #include <iostream>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
+#include "coal/distance.h"
+#include "coal/math/transform.h"
+#include "coal/collision.h"
+#include "coal/collision_object.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/shape/geometric_shape_to_BVH_model.h"
 
 #include "utility.h"
 
-using namespace hpp::fcl;
-using hpp::fcl::CollisionGeometryPtr_t;
-using hpp::fcl::CollisionObject;
-using hpp::fcl::CollisionRequest;
-using hpp::fcl::CollisionResult;
-using hpp::fcl::DistanceRequest;
-using hpp::fcl::DistanceResult;
-using hpp::fcl::Transform3f;
-using hpp::fcl::Vec3f;
+using namespace coal;
+using coal::CollisionGeometryPtr_t;
+using coal::CollisionObject;
+using coal::CollisionRequest;
+using coal::CollisionResult;
+using coal::DistanceRequest;
+using coal::DistanceResult;
+using coal::Transform3s;
+using coal::Vec3s;
 
 #define MATH_SQUARED(x) (x * x)
 
 BOOST_AUTO_TEST_CASE(aabb_aabb) {
-  CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1));
-  CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1));
+  CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1));
+  CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1));
 
-  const Transform3f tf1;
-  const Transform3f tf2_collision(Vec3f(0, 1, 1));
-  hpp::fcl::Box s1(1, 1, 1);
-  hpp::fcl::Box s2(1, 1, 1);
+  const Transform3s tf1;
+  const Transform3s tf2_collision(Vec3s(0, 1, 1));
+  coal::Box s1(1, 1, 1);
+  coal::Box s2(1, 1, 1);
   const double tol = 1e-8;
 
   AABB bv1, bv2;
-  computeBV(s1, Transform3f(), bv1);
-  computeBV(s2, Transform3f(), bv2);
+  computeBV(s1, Transform3s(), bv1);
+  computeBV(s2, Transform3s(), bv2);
 
   // No security margin - collision
   {
     CollisionRequest collisionRequest(CONTACT, 1);
     AABB bv2_transformed;
     computeBV(s2, tf2_collision, bv2_transformed);
-    FCL_REAL sqrDistLowerBound;
+    CoalScalar sqrDistLowerBound;
     bool res =
         bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound);
     BOOST_CHECK(res);
@@ -89,11 +89,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
   {
     CollisionRequest collisionRequest(CONTACT, 1);
     const double distance = 0.01;
-    Transform3f tf2_no_collision(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+    Transform3s tf2_no_collision(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
     AABB bv2_transformed;
     computeBV(s2, tf2_no_collision, bv2_transformed);
-    FCL_REAL sqrDistLowerBound;
+    CoalScalar sqrDistLowerBound;
     bool res =
         bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound);
     BOOST_CHECK(!res);
@@ -105,11 +105,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
     CollisionRequest collisionRequest(CONTACT, 1);
     const double distance = 0.01;
     collisionRequest.security_margin = distance;
-    Transform3f tf2_no_collision(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+    Transform3s tf2_no_collision(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
     AABB bv2_transformed;
     computeBV(s2, tf2_no_collision, bv2_transformed);
-    FCL_REAL sqrDistLowerBound;
+    CoalScalar sqrDistLowerBound;
     bool res =
         bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound);
     BOOST_CHECK(res);
@@ -121,11 +121,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
     CollisionRequest collisionRequest(CONTACT, 1);
     const double distance = -0.01;
     collisionRequest.security_margin = distance;
-    const Transform3f tf2(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, distance)));
+    const Transform3s tf2(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, distance)));
     AABB bv2_transformed;
     computeBV(s2, tf2, bv2_transformed);
-    FCL_REAL sqrDistLowerBound;
+    CoalScalar sqrDistLowerBound;
     bool res =
         bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound);
     BOOST_CHECK(res);
@@ -139,7 +139,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
     collisionRequest.security_margin = distance;
     AABB bv2_transformed;
     computeBV(s2, tf2_collision, bv2_transformed);
-    FCL_REAL sqrDistLowerBound;
+    CoalScalar sqrDistLowerBound;
     bool res =
         bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound);
     BOOST_CHECK(!res);
@@ -150,17 +150,17 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
 }
 
 BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) {
-  CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1));
-  CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1));
+  CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1));
+  CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1));
 
-  const Transform3f tf1;
-  const Transform3f tf2_collision(Vec3f(0, 0, 0));
-  hpp::fcl::Box s1(1, 1, 1);
-  hpp::fcl::Box s2(1, 1, 1);
+  const Transform3s tf1;
+  const Transform3s tf2_collision(Vec3s(0, 0, 0));
+  coal::Box s1(1, 1, 1);
+  coal::Box s2(1, 1, 1);
 
   AABB bv1, bv2;
-  computeBV(s1, Transform3f(), bv1);
-  computeBV(s2, Transform3f(), bv2);
+  computeBV(s1, Transform3s(), bv1);
+  computeBV(s2, Transform3s(), bv2);
 
   // The two AABB are collocated
   {
@@ -169,7 +169,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) {
     collisionRequest.security_margin = distance;
     AABB bv2_transformed;
     computeBV(s2, tf2_collision, bv2_transformed);
-    FCL_REAL sqrDistLowerBound;
+    CoalScalar sqrDistLowerBound;
     bool res =
         bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound);
     BOOST_CHECK(!res);
@@ -180,11 +180,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) {
 }
 
 BOOST_AUTO_TEST_CASE(sphere_sphere) {
-  CollisionGeometryPtr_t s1(new hpp::fcl::Sphere(1));
-  CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(2));
+  CollisionGeometryPtr_t s1(new coal::Sphere(1));
+  CollisionGeometryPtr_t s2(new coal::Sphere(2));
 
-  const Transform3f tf1;
-  const Transform3f tf2_collision(Vec3f(0, 0, 3));
+  const Transform3s tf1;
+  const Transform3s tf2_collision(Vec3s(0, 0, 3));
 
   // NOTE: when comparing a result to zero, **do not use BOOST_CHECK_CLOSE**!
   // Zero is not close to any other number, so the test will always fail.
@@ -206,8 +206,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
     CollisionRequest collisionRequest(CONTACT, 1);
     CollisionResult collisionResult;
     const double distance = 0.01;
-    Transform3f tf2_no_collision(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+    Transform3s tf2_no_collision(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
     collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest,
             collisionResult);
     BOOST_CHECK(!collisionResult.isCollision());
@@ -220,8 +220,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
     CollisionResult collisionResult;
     const double distance = 0.01;
     collisionRequest.security_margin = distance;
-    Transform3f tf2(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+    Transform3s tf2(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
     collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult);
     BOOST_CHECK(collisionResult.isCollision());
     BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
@@ -235,8 +235,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
     CollisionResult collisionResult;
     const double distance = -0.01;
     collisionRequest.security_margin = distance;
-    Transform3f tf2(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+    Transform3s tf2(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
     collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult);
     BOOST_CHECK(collisionResult.isCollision());
     BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
@@ -258,11 +258,11 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
 }
 
 BOOST_AUTO_TEST_CASE(capsule_capsule) {
-  CollisionGeometryPtr_t c1(new hpp::fcl::Capsule(0.5, 1.));
-  CollisionGeometryPtr_t c2(new hpp::fcl::Capsule(0.5, 1.));
+  CollisionGeometryPtr_t c1(new coal::Capsule(0.5, 1.));
+  CollisionGeometryPtr_t c2(new coal::Capsule(0.5, 1.));
 
-  const Transform3f tf1;
-  const Transform3f tf2_collision(Vec3f(0, 1., 0));
+  const Transform3s tf1;
+  const Transform3s tf2_collision(Vec3s(0, 1., 0));
 
   // No security margin - collision
   {
@@ -280,8 +280,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
     CollisionRequest collisionRequest(CONTACT, 1);
     CollisionResult collisionResult;
     const double distance = 0.01;
-    Transform3f tf2_no_collision(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0)));
+    Transform3s tf2_no_collision(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0)));
     collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest,
             collisionResult);
     BOOST_CHECK(!collisionResult.isCollision());
@@ -294,8 +294,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
     CollisionResult collisionResult;
     const double distance = 0.01;
     collisionRequest.security_margin = distance;
-    Transform3f tf2_no_collision(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0)));
+    Transform3s tf2_no_collision(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0)));
     collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest,
             collisionResult);
     BOOST_CHECK(collisionResult.isCollision());
@@ -310,8 +310,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
     CollisionResult collisionResult;
     const double distance = -0.01;
     collisionRequest.security_margin = distance;
-    Transform3f tf2(
-        Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0)));
+    Transform3s tf2(
+        Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0)));
     collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult);
     BOOST_CHECK(collisionResult.isCollision());
     BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
@@ -332,11 +332,11 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
 }
 
 BOOST_AUTO_TEST_CASE(box_box) {
-  CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1));
-  CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1));
+  CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1));
+  CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1));
 
-  const Transform3f tf1;
-  const Transform3f tf2_collision(Vec3f(0, 1, 1));
+  const Transform3s tf1;
+  const Transform3s tf2_collision(Vec3s(0, 1, 1));
 
   const double tol = 1e-3;
 
@@ -355,8 +355,8 @@ BOOST_AUTO_TEST_CASE(box_box) {
   {
     CollisionRequest collisionRequest(CONTACT, 1);
     const double distance = 0.01;
-    const Transform3f tf2_no_collision(
-        (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval());
+    const Transform3s tf2_no_collision(
+        (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval());
 
     CollisionResult collisionResult;
     collide(b1.get(), tf1, b2.get(), tf2_no_collision, collisionRequest,
@@ -394,12 +394,12 @@ BOOST_AUTO_TEST_CASE(box_box) {
   // Negative security margin - collision
   {
     CollisionRequest collisionRequest(CONTACT, 1);
-    const FCL_REAL distance = -0.01;
+    const CoalScalar distance = -0.01;
     collisionRequest.security_margin = distance;
     CollisionResult collisionResult;
 
-    const Transform3f tf2((tf2_collision.getTranslation() +
-                           Vec3f(0, collisionRequest.security_margin,
+    const Transform3s tf2((tf2_collision.getTranslation() +
+                           Vec3s(0, collisionRequest.security_margin,
                                  collisionRequest.security_margin))
                               .eval());
     collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult);
@@ -411,9 +411,9 @@ BOOST_AUTO_TEST_CASE(box_box) {
 }
 
 template <typename ShapeType1, typename ShapeType2>
-void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
+void test_shape_shape(const ShapeType1& shape1, const Transform3s& tf1,
                       const ShapeType2& shape2,
-                      const Transform3f& tf2_collision, const FCL_REAL tol) {
+                      const Transform3s& tf2_collision, const CoalScalar tol) {
   // No security margin - collision
   {
     CollisionRequest collisionRequest(CONTACT, 1);
@@ -429,8 +429,8 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
   {
     CollisionRequest collisionRequest(CONTACT, 1);
     const double distance = 0.01;
-    const Transform3f tf2_no_collision(
-        (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval());
+    const Transform3s tf2_no_collision(
+        (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval());
 
     CollisionResult collisionResult;
     collide(&shape1, tf1, &shape2, tf2_no_collision, collisionRequest,
@@ -471,12 +471,12 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
   // Negative security margin - collision
   {
     CollisionRequest collisionRequest(CONTACT, 1);
-    const FCL_REAL distance = -0.01;
+    const CoalScalar distance = -0.01;
     collisionRequest.security_margin = distance;
     CollisionResult collisionResult;
 
-    const Transform3f tf2((tf2_collision.getTranslation() +
-                           Vec3f(0, collisionRequest.security_margin,
+    const Transform3s tf2((tf2_collision.getTranslation() +
+                           Vec3s(0, collisionRequest.security_margin,
                                  collisionRequest.security_margin))
                               .eval());
     collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult);
@@ -488,16 +488,16 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
 }
 
 BOOST_AUTO_TEST_CASE(sphere_box) {
-  Box* box_ptr = new hpp::fcl::Box(1, 1, 1);
+  Box* box_ptr = new coal::Box(1, 1, 1);
   CollisionGeometryPtr_t b1(box_ptr);
   BVHModel<OBBRSS> box_bvh_model = BVHModel<OBBRSS>();
-  generateBVHModel(box_bvh_model, *box_ptr, Transform3f());
+  generateBVHModel(box_bvh_model, *box_ptr, Transform3s());
   box_bvh_model.buildConvexRepresentation(false);
   ConvexBase& box_convex = *box_bvh_model.convex.get();
-  CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(0.5));
+  CollisionGeometryPtr_t s2(new coal::Sphere(0.5));
 
-  const Transform3f tf1;
-  const Transform3f tf2_collision(Vec3f(0, 0, 1));
+  const Transform3s tf1;
+  const Transform3s tf2_collision(Vec3s(0, 0, 1));
 
   const double tol = 1e-6;
 
diff --git a/test/serialization.cpp b/test/serialization.cpp
index 10fb645a86a1f5db06bd36d6630ecbb8ff7c03a9..11609d30003f536b2aa308bfa04aeb3a0f672ad6 100644
--- a/test/serialization.cpp
+++ b/test/serialization.cpp
@@ -32,34 +32,35 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#define BOOST_TEST_MODULE FCL_SERIALIZATION
+#define BOOST_TEST_MODULE COAL_SERIALIZATION
 #include <fstream>
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/fwd.hh>
-
-HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
-HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
-
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/contact_patch.h>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/BV/OBBRSS.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-
-#include <hpp/fcl/serialization/collision_data.h>
-#include <hpp/fcl/serialization/contact_patch.h>
-#include <hpp/fcl/serialization/AABB.h>
-#include <hpp/fcl/serialization/BVH_model.h>
-#include <hpp/fcl/serialization/hfield.h>
-#include <hpp/fcl/serialization/transform.h>
-#include <hpp/fcl/serialization/geometric_shapes.h>
-#include <hpp/fcl/serialization/convex.h>
-#include <hpp/fcl/serialization/archive.h>
-#include <hpp/fcl/serialization/memory.h>
-
-#ifdef HPP_FCL_HAS_OCTOMAP
-#include <hpp/fcl/serialization/octree.h>
+#include "coal/fwd.hh"
+
+COAL_COMPILER_DIAGNOSTIC_PUSH
+COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+
+#include "coal/collision.h"
+
+#include "coal/contact_patch.h"
+#include "coal/distance.h"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BVH/BVH_model.h"
+
+#include "coal/serialization/collision_data.h"
+#include "coal/serialization/contact_patch.h"
+#include "coal/serialization/AABB.h"
+#include "coal/serialization/BVH_model.h"
+#include "coal/serialization/hfield.h"
+#include "coal/serialization/transform.h"
+#include "coal/serialization/geometric_shapes.h"
+#include "coal/serialization/convex.h"
+#include "coal/serialization/archive.h"
+#include "coal/serialization/memory.h"
+
+#ifdef COAL_HAS_OCTOMAP
+#include "coal/serialization/octree.h"
 #endif
 
 #include "utility.h"
@@ -70,7 +71,7 @@ HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 
 namespace utf = boost::unit_test::framework;
 
-using namespace hpp::fcl;
+using namespace coal;
 
 template <typename T>
 bool check(const T& value, const T& other) {
@@ -158,31 +159,31 @@ void test_serialization(const T& value, T& other_value,
     // -- TXT
     {
       const std::string filename = txt_filename.string();
-      hpp::fcl::serialization::saveToText(value, filename);
+      coal::serialization::saveToText(value, filename);
       BOOST_CHECK(check(value, value));
 
-      hpp::fcl::serialization::loadFromText(other_value, filename);
+      coal::serialization::loadFromText(other_value, filename);
       BOOST_CHECK(check(value, other_value));
     }
 
     // -- String stream (TXT format)
     {
       std::stringstream ss_out;
-      hpp::fcl::serialization::saveToStringStream(value, ss_out);
+      coal::serialization::saveToStringStream(value, ss_out);
       BOOST_CHECK(check(value, value));
 
       std::istringstream ss_in(ss_out.str());
-      hpp::fcl::serialization::loadFromStringStream(other_value, ss_in);
+      coal::serialization::loadFromStringStream(other_value, ss_in);
       BOOST_CHECK(check(value, other_value));
     }
 
     // -- String
     {
-      const std::string str_out = hpp::fcl::serialization::saveToString(value);
+      const std::string str_out = coal::serialization::saveToString(value);
       BOOST_CHECK(check(value, value));
 
       const std::string str_in(str_out);
-      hpp::fcl::serialization::loadFromString(other_value, str_in);
+      coal::serialization::loadFromString(other_value, str_in);
       BOOST_CHECK(check(value, other_value));
     }
   }
@@ -192,10 +193,10 @@ void test_serialization(const T& value, T& other_value,
     {
       const std::string filename = xml_filename.string();
       const std::string xml_tag = "value";
-      hpp::fcl::serialization::saveToXML(value, filename, xml_tag);
+      coal::serialization::saveToXML(value, filename, xml_tag);
       BOOST_CHECK(check(value, value));
 
-      hpp::fcl::serialization::loadFromXML(other_value, filename, xml_tag);
+      coal::serialization::loadFromXML(other_value, filename, xml_tag);
       BOOST_CHECK(check(value, other_value));
     }
   }
@@ -204,10 +205,10 @@ void test_serialization(const T& value, T& other_value,
   if (mode & 0x4) {
     {
       const std::string filename = bin_filename.string();
-      hpp::fcl::serialization::saveToBinary(value, filename);
+      coal::serialization::saveToBinary(value, filename);
       BOOST_CHECK(check(value, value));
 
-      hpp::fcl::serialization::loadFromBinary(other_value, filename);
+      coal::serialization::loadFromBinary(other_value, filename);
       BOOST_CHECK(check(value, other_value));
     }
   }
@@ -216,10 +217,10 @@ void test_serialization(const T& value, T& other_value,
   if (mode & 0x8) {
     {
       boost::asio::streambuf buffer;
-      hpp::fcl::serialization::saveToBuffer(value, buffer);
+      coal::serialization::saveToBuffer(value, buffer);
       BOOST_CHECK(check(value, value));
 
-      hpp::fcl::serialization::loadFromBuffer(other_value, buffer);
+      coal::serialization::loadFromBuffer(other_value, buffer);
       BOOST_CHECK(check(value, other_value));
     }
   }
@@ -230,11 +231,11 @@ void test_serialization(const T& value, T& other_value,
     std::shared_ptr<T> ptr = std::make_shared<T>(value);
 
     const std::string filename = txt_ptr_filename.string();
-    hpp::fcl::serialization::saveToText(ptr, filename);
+    coal::serialization::saveToText(ptr, filename);
     BOOST_CHECK(check_ptr(ptr.get(), ptr.get()));
 
     std::shared_ptr<T> other_ptr = nullptr;
-    hpp::fcl::serialization::loadFromText(other_ptr, filename);
+    coal::serialization::loadFromText(other_ptr, filename);
     BOOST_CHECK(check_ptr(ptr.get(), other_ptr.get()));
   }
 
@@ -249,12 +250,12 @@ void test_serialization(const T& value,
 }
 
 BOOST_AUTO_TEST_CASE(test_aabb) {
-  AABB aabb(-Vec3f::Ones(), Vec3f::Ones());
+  AABB aabb(-Vec3s::Ones(), Vec3s::Ones());
   test_serialization(aabb);
 }
 
 BOOST_AUTO_TEST_CASE(test_collision_data) {
-  Contact contact(NULL, NULL, 1, 2, Vec3f::Ones(), Vec3f::Zero(), -10.);
+  Contact contact(NULL, NULL, 1, 2, Vec3s::Ones(), Vec3s::Zero(), -10.);
   test_serialization(contact);
 
   CollisionRequest collision_request(CONTACT, 10);
@@ -281,27 +282,27 @@ BOOST_AUTO_TEST_CASE(test_collision_data) {
   {
     // Serializing contact patches.
     const Halfspace hspace(0, 0, 1, 0);
-    const FCL_REAL radius = 0.25;
-    const FCL_REAL height = 1.;
+    const CoalScalar radius = 0.25;
+    const CoalScalar height = 1.;
     const Cylinder cylinder(radius, height);
 
-    const Transform3f tf1;
-    Transform3f tf2;
+    const Transform3s tf1;
+    Transform3s tf2;
     // set translation to have a collision
-    const FCL_REAL offset = 0.001;
-    tf2.setTranslation(Vec3f(0, 0, height / 2 - offset));
+    const CoalScalar offset = 0.001;
+    tf2.setTranslation(Vec3s(0, 0, height / 2 - offset));
 
     const size_t num_max_contact = 1;
     const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
                                    num_max_contact);
     CollisionResult col_res;
-    hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res);
+    coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res);
     BOOST_CHECK(col_res.isCollision());
     if (col_res.isCollision()) {
       ContactPatchRequest patch_req;
       ContactPatchResult patch_res(patch_req);
-      hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res,
-                                    patch_req, patch_res);
+      coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res,
+                                patch_req, patch_res);
       BOOST_CHECK(patch_res.numContactPatches() == 1);
 
       // Serialize patch request, result and the patch itself
@@ -325,7 +326,7 @@ void checkEqualStdVector(const std::vector<T>& v1, const std::vector<T>& v2) {
 }
 
 BOOST_AUTO_TEST_CASE(test_BVHModel) {
-  std::vector<Vec3f> p1, p2;
+  std::vector<Vec3s> p1, p2;
   std::vector<Triangle> t1, t2;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
 
@@ -364,9 +365,9 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) {
   }
 }
 
-#ifdef HPP_FCL_HAS_QHULL
+#ifdef COAL_HAS_QHULL
 BOOST_AUTO_TEST_CASE(test_Convex) {
-  std::vector<Vec3f> p1;
+  std::vector<Vec3s> p1;
   std::vector<Triangle> t1;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
 
@@ -403,12 +404,12 @@ BOOST_AUTO_TEST_CASE(test_Convex) {
     BOOST_CHECK(ptr.get());
     const std::string filename = xml_filename.string();
     const std::string tag_name = "CollisionGeometry";
-    hpp::fcl::serialization::saveToXML(ptr, filename, tag_name);
+    coal::serialization::saveToXML(ptr, filename, tag_name);
     BOOST_CHECK(check(*reinterpret_cast<Convex<Triangle>*>(ptr.get()), convex));
 
     std::shared_ptr<CollisionGeometry> other_ptr = nullptr;
     BOOST_CHECK(!other_ptr.get());
-    hpp::fcl::serialization::loadFromXML(other_ptr, filename, tag_name);
+    coal::serialization::loadFromXML(other_ptr, filename, tag_name);
     BOOST_CHECK(
         check(convex, *reinterpret_cast<Convex<Triangle>*>(other_ptr.get())));
   }
@@ -416,10 +417,10 @@ BOOST_AUTO_TEST_CASE(test_Convex) {
 #endif
 
 BOOST_AUTO_TEST_CASE(test_HeightField) {
-  const FCL_REAL min_altitude = -1.;
-  const FCL_REAL x_dim = 1., y_dim = 2.;
+  const CoalScalar min_altitude = -1.;
+  const CoalScalar x_dim = 1., y_dim = 2.;
   const Eigen::DenseIndex nx = 100, ny = 200;
-  const MatrixXf heights = MatrixXf::Random(ny, nx);
+  const MatrixXs heights = MatrixXs::Random(ny, nx);
 
   HeightField<OBBRSS> hfield(x_dim, y_dim, heights, min_altitude);
 
@@ -435,25 +436,25 @@ BOOST_AUTO_TEST_CASE(test_HeightField) {
 }
 
 BOOST_AUTO_TEST_CASE(test_transform) {
-  Transform3f T;
+  Transform3s T;
   T.setQuatRotation(Quaternion3f::UnitRandom());
-  T.setTranslation(Vec3f::Random());
+  T.setTranslation(Vec3s::Random());
 
-  Transform3f T_copy;
+  Transform3s T_copy;
   test_serialization(T, T_copy);
 }
 
 BOOST_AUTO_TEST_CASE(test_shapes) {
   {
-    TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), Vec3f::UnitZ());
+    TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(), Vec3s::UnitZ());
     triangle.setSweptSphereRadius(1.);
     triangle.computeLocalAABB();
-    TriangleP triangle_copy(Vec3f::Random(), Vec3f::Random(), Vec3f::Random());
+    TriangleP triangle_copy(Vec3s::Random(), Vec3s::Random(), Vec3s::Random());
     test_serialization(triangle, triangle_copy);
   }
 
   {
-    Box box(Vec3f::UnitX()), box_copy(Vec3f::Random());
+    Box box(Vec3s::UnitX()), box_copy(Vec3s::Random());
     box.setSweptSphereRadius(1.);
     box.computeLocalAABB();
     test_serialization(box, box_copy);
@@ -495,14 +496,14 @@ BOOST_AUTO_TEST_CASE(test_shapes) {
   }
 
   {
-    Halfspace hs(Vec3f::Random(), 1.), hs_copy(Vec3f::Zero(), 0.);
+    Halfspace hs(Vec3s::Random(), 1.), hs_copy(Vec3s::Zero(), 0.);
     hs.setSweptSphereRadius(1.);
     hs.computeLocalAABB();
     test_serialization(hs, hs_copy);
   }
 
   {
-    Plane plane(Vec3f::Random(), 1.), plane_copy(Vec3f::Zero(), 0.);
+    Plane plane(Vec3s::Random(), 1.), plane_copy(Vec3s::Zero(), 0.);
     plane.setSweptSphereRadius(1.);
     plane.computeLocalAABB();
     test_serialization(plane, plane_copy);
@@ -511,11 +512,11 @@ BOOST_AUTO_TEST_CASE(test_shapes) {
 #ifdef HPP_FCL_HAS_QHULL
   {
     const size_t num_points = 500;
-    std::shared_ptr<std::vector<Vec3f>> points =
-        std::make_shared<std::vector<Vec3f>>();
+    std::shared_ptr<std::vector<Vec3s>> points =
+        std::make_shared<std::vector<Vec3s>>();
     points->reserve(num_points);
     for (size_t i = 0; i < num_points; i++) {
-      points->emplace_back(Vec3f::Random());
+      points->emplace_back(Vec3s::Random());
     }
     using Convex = Convex<Triangle>;
     std::unique_ptr<Convex> convex =
@@ -530,10 +531,10 @@ BOOST_AUTO_TEST_CASE(test_shapes) {
 #endif
 }
 
-#ifdef HPP_FCL_HAS_OCTOMAP
+#ifdef COAL_HAS_OCTOMAP
 BOOST_AUTO_TEST_CASE(test_octree) {
-  const FCL_REAL resolution = 1e-2;
-  const Matrixx3f points = Matrixx3f::Random(1000, 3);
+  const CoalScalar resolution = 1e-2;
+  const MatrixX3s points = MatrixX3s::Random(1000, 3);
   OcTreePtr_t octree_ptr = makeOctree(points, resolution);
   const OcTree& octree = *octree_ptr.get();
 
@@ -573,7 +574,7 @@ BOOST_AUTO_TEST_CASE(test_memory_footprint) {
   Sphere sphere(1.);
   BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere));
 
-  std::vector<Vec3f> p1;
+  std::vector<Vec3s> p1;
   std::vector<Triangle> t1;
   boost::filesystem::path path(TEST_RESOURCES_DIR);
 
@@ -592,4 +593,4 @@ BOOST_AUTO_TEST_CASE(test_memory_footprint) {
               computeMemoryFootprint(m1));
 }
 
-HPP_FCL_COMPILER_DIAGNOSTIC_POP
+COAL_COMPILER_DIAGNOSTIC_POP
diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp
index e305906bd0f1a3d305f0b71a3e34ae82a559b304..cfd321024aeb80e61a8abebd582490c3d3d3a9a8 100644
--- a/test/shape_inflation.cpp
+++ b/test/shape_inflation.cpp
@@ -32,88 +32,89 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN
+#define BOOST_TEST_MODULE COAL_SECURITY_MARGIN
 #include <boost/test/included/unit_test.hpp>
 
 #include <cmath>
 #include <iostream>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/shape/geometric_shapes_utility.h>
+#include "coal/distance.h"
+#include "coal/math/transform.h"
+#include "coal/collision.h"
+#include "coal/collision_object.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/shape/geometric_shapes_utility.h"
 
 #include "utility.h"
 
-using namespace hpp::fcl;
-using hpp::fcl::CollisionGeometryPtr_t;
-using hpp::fcl::CollisionObject;
-using hpp::fcl::CollisionRequest;
-using hpp::fcl::CollisionResult;
-using hpp::fcl::DistanceRequest;
-using hpp::fcl::DistanceResult;
-using hpp::fcl::Transform3f;
-using hpp::fcl::Vec3f;
+using namespace coal;
+using coal::CollisionGeometryPtr_t;
+using coal::CollisionObject;
+using coal::CollisionRequest;
+using coal::CollisionResult;
+using coal::DistanceRequest;
+using coal::DistanceResult;
+using coal::Transform3s;
+using coal::Vec3s;
 
 #define MATH_SQUARED(x) (x * x)
 
 template <typename Shape>
-bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol);
+bool isApprox(const Shape &s1, const Shape &s2, const CoalScalar tol);
 
-bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) {
-  typedef Eigen::Matrix<FCL_REAL, 1, 1> ScalarMatrix;
-  ScalarMatrix m1;
+bool isApprox(const CoalScalar &v1, const CoalScalar &v2,
+              const CoalScalar tol) {
+  typedef Eigen::Matrix<CoalScalar, 1, 1> Matrix;
+  Matrix m1;
   m1 << v1;
-  ScalarMatrix m2;
+  Matrix m2;
   m2 << v2;
   return m1.isApprox(m2, tol);
 }
 
-bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol) {
+bool isApprox(const Box &s1, const Box &s2, const CoalScalar tol) {
   return s1.halfSide.isApprox(s2.halfSide, tol);
 }
 
-bool isApprox(const Sphere &s1, const Sphere &s2, const FCL_REAL tol) {
+bool isApprox(const Sphere &s1, const Sphere &s2, const CoalScalar tol) {
   return isApprox(s1.radius, s2.radius, tol);
 }
 
-bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const FCL_REAL tol) {
+bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const CoalScalar tol) {
   return s1.radii.isApprox(s2.radii, tol);
 }
 
-bool isApprox(const Capsule &s1, const Capsule &s2, const FCL_REAL tol) {
+bool isApprox(const Capsule &s1, const Capsule &s2, const CoalScalar tol) {
   return isApprox(s1.radius, s2.radius, tol) &&
          isApprox(s1.halfLength, s2.halfLength, tol);
 }
 
-bool isApprox(const Cylinder &s1, const Cylinder &s2, const FCL_REAL tol) {
+bool isApprox(const Cylinder &s1, const Cylinder &s2, const CoalScalar tol) {
   return isApprox(s1.radius, s2.radius, tol) &&
          isApprox(s1.halfLength, s2.halfLength, tol);
 }
 
-bool isApprox(const Cone &s1, const Cone &s2, const FCL_REAL tol) {
+bool isApprox(const Cone &s1, const Cone &s2, const CoalScalar tol) {
   return isApprox(s1.radius, s2.radius, tol) &&
          isApprox(s1.halfLength, s2.halfLength, tol);
 }
 
-bool isApprox(const TriangleP &s1, const TriangleP &s2, const FCL_REAL tol) {
+bool isApprox(const TriangleP &s1, const TriangleP &s2, const CoalScalar tol) {
   return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) &&
          s1.c.isApprox(s2.c, tol);
 }
 
-bool isApprox(const Halfspace &s1, const Halfspace &s2, const FCL_REAL tol) {
+bool isApprox(const Halfspace &s1, const Halfspace &s2, const CoalScalar tol) {
   return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol);
 }
 
 template <typename Shape>
-void test(const Shape &original_shape, const FCL_REAL inflation,
-          const FCL_REAL tol = 1e-8) {
+void test(const Shape &original_shape, const CoalScalar inflation,
+          const CoalScalar tol = 1e-8) {
   // Zero inflation
   {
-    const FCL_REAL inflation = 0.;
+    const CoalScalar inflation = 0.;
     const auto &inflation_result = original_shape.inflated(inflation);
-    const Transform3f &shift = inflation_result.second;
+    const Transform3s &shift = inflation_result.second;
     const Shape &inflated_shape = inflation_result.first;
 
     BOOST_CHECK(isApprox(original_shape, inflated_shape, tol));
@@ -124,13 +125,13 @@ void test(const Shape &original_shape, const FCL_REAL inflation,
   {
     const auto &inflation_result = original_shape.inflated(inflation);
     const Shape &inflated_shape = inflation_result.first;
-    const Transform3f &inflation_shift = inflation_result.second;
+    const Transform3s &inflation_shift = inflation_result.second;
 
     BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol));
 
     const auto &deflation_result = inflated_shape.inflated(-inflation);
     const Shape &deflated_shape = deflation_result.first;
-    const Transform3f &deflation_shift = deflation_result.second;
+    const Transform3s &deflation_shift = deflation_result.second;
 
     BOOST_CHECK(isApprox(original_shape, deflated_shape, tol));
     BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol));
@@ -140,13 +141,13 @@ void test(const Shape &original_shape, const FCL_REAL inflation,
   {
     const auto &inflation_result = original_shape.inflated(-inflation);
     const Shape &inflated_shape = inflation_result.first;
-    const Transform3f &inflation_shift = inflation_result.second;
+    const Transform3s &inflation_shift = inflation_result.second;
 
     BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol));
 
     const auto &deflation_result = inflated_shape.inflated(+inflation);
     const Shape &deflated_shape = deflation_result.first;
-    const Transform3f &deflation_shift = deflation_result.second;
+    const Transform3s &deflation_shift = deflation_result.second;
 
     BOOST_CHECK(isApprox(original_shape, deflated_shape, tol));
     BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol));
@@ -154,45 +155,45 @@ void test(const Shape &original_shape, const FCL_REAL inflation,
 }
 
 template <typename Shape>
-void test_throw(const Shape &shape, const FCL_REAL inflation) {
+void test_throw(const Shape &shape, const CoalScalar inflation) {
   BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument);
 }
 
 template <typename Shape>
-void test_no_throw(const Shape &shape, const FCL_REAL inflation) {
+void test_no_throw(const Shape &shape, const CoalScalar inflation) {
   BOOST_REQUIRE_NO_THROW(shape.inflated(inflation));
 }
 
 BOOST_AUTO_TEST_CASE(test_inflate) {
-  const hpp::fcl::Sphere sphere(1);
+  const coal::Sphere sphere(1);
   test(sphere, 0.01, 1e-8);
   test_throw(sphere, -1.1);
   test_no_throw(sphere, 1.);
 
-  const hpp::fcl::Box box(1, 1, 1);
+  const coal::Box box(1, 1, 1);
   test(box, 0.01, 1e-8);
   test_throw(box, -0.6);
 
-  const hpp::fcl::Ellipsoid ellipsoid(1, 2, 3);
+  const coal::Ellipsoid ellipsoid(1, 2, 3);
   test(ellipsoid, 0.01, 1e-8);
   test_throw(ellipsoid, -1.1);
 
-  const hpp::fcl::Capsule capsule(1., 2.);
+  const coal::Capsule capsule(1., 2.);
   test(capsule, 0.01, 1e-8);
   test_throw(capsule, -1.1);
 
-  const hpp::fcl::Cylinder cylinder(1., 2.);
+  const coal::Cylinder cylinder(1., 2.);
   test(cylinder, 0.01, 1e-8);
   test_throw(cylinder, -1.1);
 
-  const hpp::fcl::Cone cone(1., 4.);
+  const coal::Cone cone(1., 4.);
   test(cone, 0.01, 1e-8);
   test_throw(cone, -1.1);
 
-  const hpp::fcl::Halfspace halfspace(Vec3f::UnitZ(), 0.);
+  const coal::Halfspace halfspace(Vec3s::UnitZ(), 0.);
   test(halfspace, 0.01, 1e-8);
 
-  //  const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(),
-  //                                     Vec3f::UnitZ());
+  //  const coal::TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(),
+  //                                     Vec3s::UnitZ());
   //  test(triangle, 0.01, 1e-8);
 }
diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp
index 0c0e7c907b23303d2dc19277f4c026b9ccc97526..6b8d35e33b9d6f548b6382e03485421cb1110295 100644
--- a/test/shape_mesh_consistency.cpp
+++ b/test/shape_mesh_consistency.cpp
@@ -35,20 +35,20 @@
 
 /** \author Jia Pan */
 
-#define BOOST_TEST_MODULE FCL_SHAPE_MESH_CONSISTENCY
+#define BOOST_TEST_MODULE COAL_SHAPE_MESH_CONSISTENCY
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
-#include <hpp/fcl/distance.h>
-#include <hpp/fcl/collision.h>
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shape_to_BVH_model.h"
+#include "coal/distance.h"
+#include "coal/collision.h"
 #include "utility.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
 
-FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
+CoalScalar extents[6] = {0, 0, 0, 10, 10, 10};
 
 BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) {
   Sphere s1(20);
@@ -56,39 +56,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) {
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3s(), 16, 16);
 
   DistanceRequest request;
   DistanceResult res, res1;
 
-  Transform3f pose;
+  Transform3s pose;
 
-  pose.setTranslation(Vec3f(50, 0, 0));
+  pose.setTranslation(Vec3s(50, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -108,30 +108,30 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) {
                 0.05);
   }
 
-  pose.setTranslation(Vec3f(40.1, 0, 0));
+  pose.setTranslation(Vec3s(40.1, 0, 0));
 
   res.clear(), res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -159,39 +159,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) {
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3f());
-  generateBVHModel(s2_rss, s2, Transform3f());
+  generateBVHModel(s1_rss, s1, Transform3s());
+  generateBVHModel(s2_rss, s2, Transform3s());
 
   DistanceRequest request;
   DistanceResult res, res1;
 
-  Transform3f pose;
+  Transform3s pose;
 
-  pose.setTranslation(Vec3f(50, 0, 0));
+  pose.setTranslation(Vec3s(50, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -211,31 +211,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) {
                 0.01);
   }
 
-  pose.setTranslation(Vec3f(15.1, 0, 0));
+  pose.setTranslation(Vec3s(15.1, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -263,39 +263,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) {
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3s(), 16, 16);
 
   DistanceRequest request;
   DistanceResult res, res1;
 
-  Transform3f pose;
+  Transform3s pose;
 
-  pose.setTranslation(Vec3f(20, 0, 0));
+  pose.setTranslation(Vec3s(20, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -316,31 +316,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) {
   }
 
   pose.setTranslation(
-      Vec3f(15, 0, 0));  // libccd cannot use small value here :(
+      Vec3s(15, 0, 0));  // libccd cannot use small value here :(
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -368,38 +368,38 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) {
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3s(), 16, 16);
 
   DistanceRequest request;
   DistanceResult res, res1;
 
-  Transform3f pose;
+  Transform3s pose;
 
-  pose.setTranslation(Vec3f(20, 0, 0));
+  pose.setTranslation(Vec3s(20, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -420,31 +420,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) {
   }
 
   pose.setTranslation(
-      Vec3f(15, 0, 0));  // libccd cannot use small value here :(
+      Vec3s(15, 0, 0));  // libccd cannot use small value here :(
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -471,39 +471,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) {
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3s(), 16, 16);
 
   DistanceRequest request;
   DistanceResult res, res1;
 
-  Transform3f pose;
+  Transform3s pose;
 
-  pose.setTranslation(Vec3f(50, 0, 0));
+  pose.setTranslation(Vec3s(50, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -523,31 +523,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) {
                 0.05);
   }
 
-  pose.setTranslation(Vec3f(40.1, 0, 0));
+  pose.setTranslation(Vec3s(40.1, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               4);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -575,39 +575,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) {
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3f());
-  generateBVHModel(s2_rss, s2, Transform3f());
+  generateBVHModel(s1_rss, s1, Transform3s());
+  generateBVHModel(s2_rss, s2, Transform3s());
 
   DistanceRequest request;
   DistanceResult res, res1;
 
-  Transform3f pose;
+  Transform3s pose;
 
-  pose.setTranslation(Vec3f(50, 0, 0));
+  pose.setTranslation(Vec3s(50, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.01);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -627,31 +627,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) {
                 0.01);
   }
 
-  pose.setTranslation(Vec3f(15.1, 0, 0));
+  pose.setTranslation(Vec3s(15.1, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -679,39 +679,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) {
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3s(), 16, 16);
 
   DistanceRequest request;
   DistanceResult res, res1;
 
-  Transform3f pose;
+  Transform3s pose;
 
-  pose.setTranslation(Vec3f(20, 0, 0));
+  pose.setTranslation(Vec3s(20, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -743,31 +743,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) {
           fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
   }
 
-  pose.setTranslation(Vec3f(10.1, 0, 0));
+  pose.setTranslation(Vec3s(10.1, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -795,39 +795,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) {
   BVHModel<RSS> s1_rss;
   BVHModel<RSS> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3s(), 16, 16);
 
   DistanceRequest request;
   DistanceResult res, res1;
 
-  Transform3f pose;
+  Transform3s pose;
 
-  pose.setTranslation(Vec3f(20, 0, 0));
+  pose.setTranslation(Vec3s(20, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               0.05);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -847,31 +847,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) {
                 0.05);
   }
 
-  pose.setTranslation(Vec3f(10.1, 0, 0));
+  pose.setTranslation(Vec3s(10.1, 0, 0));
 
   res.clear();
   res1.clear();
-  distance(&s1, Transform3f(), &s2, pose, request, res);
-  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  distance(&s1, Transform3s(), &s2_rss, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   res1.clear();
-  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  distance(&s1_rss, Transform3s(), &s2, pose, request, res1);
   BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
               2);
 
   for (std::size_t i = 0; i < 10; ++i) {
-    Transform3f t;
+    Transform3s t;
     generateRandomTransform(extents, t);
 
-    Transform3f pose1(t);
-    Transform3f pose2 = t * pose;
+    Transform3s pose1(t);
+    Transform3s pose2 = t * pose;
 
     res.clear();
     res1.clear();
@@ -900,17 +900,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) {
   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);
+  generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3s(), 16, 16);
 
   CollisionRequest request(false, 1, false);
   CollisionResult result;
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s pose, pose_aabb, pose_obb;
 
   // s2 is within s1
   // both are shapes --> collision
@@ -918,195 +918,195 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) {
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(40, 0, 0));
-  pose_aabb.setTranslation(Vec3f(40, 0, 0));
-  pose_obb.setTranslation(Vec3f(40, 0, 0));
+  pose.setTranslation(Vec3s(40, 0, 0));
+  pose_aabb.setTranslation(Vec3s(40, 0, 0));
+  pose_obb.setTranslation(Vec3s(40, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(30, 0, 0));
-  pose_aabb.setTranslation(Vec3f(30, 0, 0));
-  pose_obb.setTranslation(Vec3f(30, 0, 0));
+  pose.setTranslation(Vec3s(30, 0, 0));
+  pose_aabb.setTranslation(Vec3s(30, 0, 0));
+  pose_obb.setTranslation(Vec3s(30, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(29.9, 0, 0));
+  pose.setTranslation(Vec3s(29.9, 0, 0));
   pose_aabb.setTranslation(
-      Vec3f(29.8, 0, 0));  // 29.9 fails, result depends on mesh precision
+      Vec3s(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
+      Vec3s(29.8, 0, 0));  // 29.9 fails, result depends on mesh precision
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(-29.9, 0, 0));
+  pose.setTranslation(Vec3s(-29.9, 0, 0));
   pose_aabb.setTranslation(
-      Vec3f(-29.8, 0, 0));  // 29.9 fails, result depends on mesh precision
+      Vec3s(-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
+      Vec3s(-29.8, 0, 0));  // 29.9 fails, result depends on mesh precision
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(-30, 0, 0));
-  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
-  pose_obb.setTranslation(Vec3f(-30, 0, 0));
+  pose.setTranslation(Vec3s(-30, 0, 0));
+  pose_aabb.setTranslation(Vec3s(-30, 0, 0));
+  pose_obb.setTranslation(Vec3s(-30, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 }
 
@@ -1119,17 +1119,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) {
   BVHModel<OBB> s1_obb;
   BVHModel<OBB> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3f());
-  generateBVHModel(s2_aabb, s2, Transform3f());
-  generateBVHModel(s1_obb, s1, Transform3f());
-  generateBVHModel(s2_obb, s2, Transform3f());
+  generateBVHModel(s1_aabb, s1, Transform3s());
+  generateBVHModel(s2_aabb, s2, Transform3s());
+  generateBVHModel(s1_obb, s1, Transform3s());
+  generateBVHModel(s2_obb, s2, Transform3s());
 
   CollisionRequest request(false, 1, false);
   CollisionResult result;
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s pose, pose_aabb, pose_obb;
 
   // s2 is within s1
   // both are shapes --> collision
@@ -1137,95 +1137,95 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) {
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(15.01, 0, 0));
-  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
-  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+  pose.setTranslation(Vec3s(15.01, 0, 0));
+  pose_aabb.setTranslation(Vec3s(15.01, 0, 0));
+  pose_obb.setTranslation(Vec3s(15.01, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(14.99, 0, 0));
-  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
-  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+  pose.setTranslation(Vec3s(14.99, 0, 0));
+  pose_aabb.setTranslation(Vec3s(14.99, 0, 0));
+  pose_obb.setTranslation(Vec3s(14.99, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 }
 
@@ -1238,17 +1238,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) {
   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());
+  generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3s());
+  generateBVHModel(s1_obb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3s());
 
   CollisionRequest request(false, 1, false);
   CollisionResult result;
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s pose, pose_aabb, pose_obb;
 
   // s2 is within s1
   // both are shapes --> collision
@@ -1256,95 +1256,95 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) {
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_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));
+  pose.setTranslation(Vec3s(22.4, 0, 0));
+  pose_aabb.setTranslation(Vec3s(22.4, 0, 0));
+  pose_obb.setTranslation(Vec3s(22.4, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(22.51, 0, 0));
-  pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
-  pose_obb.setTranslation(Vec3f(22.51, 0, 0));
+  pose.setTranslation(Vec3s(22.51, 0, 0));
+  pose_aabb.setTranslation(Vec3s(22.51, 0, 0));
+  pose_obb.setTranslation(Vec3s(22.51, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 }
 
@@ -1357,80 +1357,80 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) {
   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);
+  generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3s(), 16, 16);
 
   CollisionRequest request(false, 1, false);
   CollisionResult result;
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s 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));
+  pose.setTranslation(Vec3s(9.99, 0, 0));
+  pose_aabb.setTranslation(Vec3s(9.99, 0, 0));
+  pose_obb.setTranslation(Vec3s(9.99, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(10.01, 0, 0));
-  pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
-  pose_obb.setTranslation(Vec3f(10.01, 0, 0));
+  pose.setTranslation(Vec3s(10.01, 0, 0));
+  pose_aabb.setTranslation(Vec3s(10.01, 0, 0));
+  pose_obb.setTranslation(Vec3s(10.01, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 }
 
@@ -1443,144 +1443,144 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) {
   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);
+  generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3s(), 16, 16);
 
   CollisionRequest request(false, 1, false);
   CollisionResult result;
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s 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));
+  pose.setTranslation(Vec3s(9.9, 0, 0));
+  pose_aabb.setTranslation(Vec3s(9.9, 0, 0));
+  pose_obb.setTranslation(Vec3s(9.9, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(10.1, 0, 0));
-  pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
-  pose_obb.setTranslation(Vec3f(10.1, 0, 0));
+  pose.setTranslation(Vec3s(10.1, 0, 0));
+  pose_aabb.setTranslation(Vec3s(10.1, 0, 0));
+  pose_obb.setTranslation(Vec3s(10.1, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_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));
+  pose.setTranslation(Vec3s(0, 0, 9.9));
+  pose_aabb.setTranslation(Vec3s(0, 0, 9.9));
+  pose_obb.setTranslation(Vec3s(0, 0, 9.9));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(0, 0, 10.1));
-  pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
-  pose_obb.setTranslation(Vec3f(0, 0, 10.1));
+  pose.setTranslation(Vec3s(0, 0, 10.1));
+  pose_aabb.setTranslation(Vec3s(0, 0, 10.1));
+  pose_obb.setTranslation(Vec3s(0, 0, 10.1));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 }
 
@@ -1592,10 +1592,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
   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);
+  generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3s(), 16, 16);
 
   CollisionRequest request(false, 1, false);
 
@@ -1603,7 +1603,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s pose, pose_aabb, pose_obb;
 
   // s2 is within s1
   // both are shapes --> collision
@@ -1611,195 +1611,195 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(40, 0, 0));
-  pose_aabb.setTranslation(Vec3f(40, 0, 0));
-  pose_obb.setTranslation(Vec3f(40, 0, 0));
+  pose.setTranslation(Vec3s(40, 0, 0));
+  pose_aabb.setTranslation(Vec3s(40, 0, 0));
+  pose_obb.setTranslation(Vec3s(40, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(30, 0, 0));
-  pose_aabb.setTranslation(Vec3f(30, 0, 0));
-  pose_obb.setTranslation(Vec3f(30, 0, 0));
+  pose.setTranslation(Vec3s(30, 0, 0));
+  pose_aabb.setTranslation(Vec3s(30, 0, 0));
+  pose_obb.setTranslation(Vec3s(30, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(29.9, 0, 0));
+  pose.setTranslation(Vec3s(29.9, 0, 0));
   pose_aabb.setTranslation(
-      Vec3f(29.8, 0, 0));  // 29.9 fails, result depends on mesh precision
+      Vec3s(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
+      Vec3s(29.8, 0, 0));  // 29.9 fails, result depends on mesh precision
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(-29.9, 0, 0));
+  pose.setTranslation(Vec3s(-29.9, 0, 0));
   pose_aabb.setTranslation(
-      Vec3f(-29.8, 0, 0));  // 29.9 fails, result depends on mesh precision
+      Vec3s(-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
+      Vec3s(-29.8, 0, 0));  // 29.9 fails, result depends on mesh precision
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(-30, 0, 0));
-  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
-  pose_obb.setTranslation(Vec3f(-30, 0, 0));
+  pose.setTranslation(Vec3s(-30, 0, 0));
+  pose_aabb.setTranslation(Vec3s(-30, 0, 0));
+  pose_obb.setTranslation(Vec3s(-30, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 }
 
@@ -1812,10 +1812,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) {
   BVHModel<OBB> s1_obb;
   BVHModel<OBB> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3f());
-  generateBVHModel(s2_aabb, s2, Transform3f());
-  generateBVHModel(s1_obb, s1, Transform3f());
-  generateBVHModel(s2_obb, s2, Transform3f());
+  generateBVHModel(s1_aabb, s1, Transform3s());
+  generateBVHModel(s2_aabb, s2, Transform3s());
+  generateBVHModel(s1_obb, s1, Transform3s());
+  generateBVHModel(s2_obb, s2, Transform3s());
 
   CollisionRequest request(false, 1, false);
 
@@ -1823,7 +1823,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) {
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s pose, pose_aabb, pose_obb;
 
   // s2 is within s1
   // both are shapes --> collision
@@ -1831,95 +1831,95 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) {
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(15.01, 0, 0));
-  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
-  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+  pose.setTranslation(Vec3s(15.01, 0, 0));
+  pose_aabb.setTranslation(Vec3s(15.01, 0, 0));
+  pose_obb.setTranslation(Vec3s(15.01, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(14.99, 0, 0));
-  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
-  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+  pose.setTranslation(Vec3s(14.99, 0, 0));
+  pose_aabb.setTranslation(Vec3s(14.99, 0, 0));
+  pose_obb.setTranslation(Vec3s(14.99, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 }
 
@@ -1932,10 +1932,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) {
   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());
+  generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3s());
+  generateBVHModel(s1_obb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3s());
 
   CollisionRequest request(false, 1, false);
 
@@ -1943,7 +1943,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) {
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s pose, pose_aabb, pose_obb;
 
   // s2 is within s1
   // both are shapes --> collision
@@ -1951,95 +1951,95 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) {
   // s1 is mesh, s2 is shape --> collision free
   // all are reasonable
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_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));
+  pose.setTranslation(Vec3s(22.4, 0, 0));
+  pose_aabb.setTranslation(Vec3s(22.4, 0, 0));
+  pose_obb.setTranslation(Vec3s(22.4, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(22.51, 0, 0));
-  pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
-  pose_obb.setTranslation(Vec3f(22.51, 0, 0));
+  pose.setTranslation(Vec3s(22.51, 0, 0));
+  pose_aabb.setTranslation(Vec3s(22.51, 0, 0));
+  pose_obb.setTranslation(Vec3s(22.51, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 }
 
@@ -2052,10 +2052,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) {
   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);
+  generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3s(), 16, 16);
 
   CollisionRequest request(false, 1, false);
 
@@ -2063,70 +2063,70 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) {
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s 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));
+  pose.setTranslation(Vec3s(9.99, 0, 0));
+  pose_aabb.setTranslation(Vec3s(9.99, 0, 0));
+  pose_obb.setTranslation(Vec3s(9.99, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(10.01, 0, 0));
-  pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
-  pose_obb.setTranslation(Vec3f(10.01, 0, 0));
+  pose.setTranslation(Vec3s(10.01, 0, 0));
+  pose_aabb.setTranslation(Vec3s(10.01, 0, 0));
+  pose_obb.setTranslation(Vec3s(10.01, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 }
 
@@ -2139,10 +2139,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) {
   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);
+  generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3s(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3s(), 16, 16);
 
   CollisionRequest request(false, 1, false);
 
@@ -2150,133 +2150,133 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) {
 
   bool res;
 
-  Transform3f pose, pose_aabb, pose_obb;
+  Transform3s 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));
+  pose.setTranslation(Vec3s(9.9, 0, 0));
+  pose_aabb.setTranslation(Vec3s(9.9, 0, 0));
+  pose_obb.setTranslation(Vec3s(9.9, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(10.1, 0, 0));
-  pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
-  pose_obb.setTranslation(Vec3f(10.1, 0, 0));
+  pose.setTranslation(Vec3s(10.1, 0, 0));
+  pose_aabb.setTranslation(Vec3s(10.1, 0, 0));
+  pose_obb.setTranslation(Vec3s(10.1, 0, 0));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_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));
+  pose.setTranslation(Vec3s(0, 0, 9.9));
+  pose_aabb.setTranslation(Vec3s(0, 0, 9.9));
+  pose_obb.setTranslation(Vec3s(0, 0, 9.9));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
 
-  pose.setTranslation(Vec3f(0, 0, 10.1));
-  pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
-  pose_obb.setTranslation(Vec3f(0, 0, 10.1));
+  pose.setTranslation(Vec3s(0, 0, 10.1));
+  pose_aabb.setTranslation(Vec3s(0, 0, 10.1));
+  pose_obb.setTranslation(Vec3s(0, 0, 10.1));
 
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
   result.clear();
-  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 }
diff --git a/test/simple.cpp b/test/simple.cpp
index 99a7ee5217e8e8b2c8e11d17dc71fe93c1fbacc8..fd1f7a39971286b334d490434f6db53180a70641 100644
--- a/test/simple.cpp
+++ b/test/simple.cpp
@@ -1,244 +1,246 @@
-#define BOOST_TEST_MODULE FCL_SIMPLE
-#include <boost/test/included/unit_test.hpp>
-
-#include <hpp/fcl/internal/intersect.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include "fcl_resources/config.h"
-#include <sstream>
-
-using namespace hpp::fcl;
-
-static FCL_REAL epsilon = 1e-6;
-
-static bool approx(FCL_REAL x, FCL_REAL y) { return std::abs(x - y) < epsilon; }
-
-BOOST_AUTO_TEST_CASE(projection_test_line) {
-  Vec3f v1(0, 0, 0);
-  Vec3f v2(2, 0, 0);
-
-  Vec3f p(1, 0, 0);
-  Project::ProjectResult res = Project::projectLine(v1, v2, p);
-  BOOST_CHECK(res.encode == 3);
-  BOOST_CHECK(approx(res.sqr_distance, 0));
-  BOOST_CHECK(approx(res.parameterization[0], 0.5));
-  BOOST_CHECK(approx(res.parameterization[1], 0.5));
-
-  p = Vec3f(-1, 0, 0);
-  res = Project::projectLine(v1, v2, p);
-  BOOST_CHECK(res.encode == 1);
-  BOOST_CHECK(approx(res.sqr_distance, 1));
-  BOOST_CHECK(approx(res.parameterization[0], 1));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-
-  p = Vec3f(3, 0, 0);
-  res = Project::projectLine(v1, v2, p);
-  BOOST_CHECK(res.encode == 2);
-  BOOST_CHECK(approx(res.sqr_distance, 1));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 1));
-}
-
-BOOST_AUTO_TEST_CASE(projection_test_triangle) {
-  Vec3f v1(0, 0, 1);
-  Vec3f v2(0, 1, 0);
-  Vec3f v3(1, 0, 0);
-
-  Vec3f p(1, 1, 1);
-  Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p);
-  BOOST_CHECK(res.encode == 7);
-  BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
-
-  p = Vec3f(0, 0, 1.5);
-  res = Project::projectTriangle(v1, v2, v3, p);
-  BOOST_CHECK(res.encode == 1);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 1));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-
-  p = Vec3f(1.5, 0, 0);
-  res = Project::projectTriangle(v1, v2, v3, p);
-  BOOST_CHECK(res.encode == 4);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 1));
-
-  p = Vec3f(0, 1.5, 0);
-  res = Project::projectTriangle(v1, v2, v3, p);
-  BOOST_CHECK(res.encode == 2);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 1));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-
-  p = Vec3f(1, 1, 0);
-  res = Project::projectTriangle(v1, v2, v3, p);
-  BOOST_CHECK(res.encode == 6);
-  BOOST_CHECK(approx(res.sqr_distance, 0.5));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 0.5));
-  BOOST_CHECK(approx(res.parameterization[2], 0.5));
-
-  p = Vec3f(1, 0, 1);
-  res = Project::projectTriangle(v1, v2, v3, p);
-  BOOST_CHECK(res.encode == 5);
-  BOOST_CHECK(approx(res.sqr_distance, 0.5));
-  BOOST_CHECK(approx(res.parameterization[0], 0.5));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 0.5));
-
-  p = Vec3f(0, 1, 1);
-  res = Project::projectTriangle(v1, v2, v3, p);
-  BOOST_CHECK(res.encode == 3);
-  BOOST_CHECK(approx(res.sqr_distance, 0.5));
-  BOOST_CHECK(approx(res.parameterization[0], 0.5));
-  BOOST_CHECK(approx(res.parameterization[1], 0.5));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-}
-
-BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
-  Vec3f v1(0, 0, 1);
-  Vec3f v2(0, 1, 0);
-  Vec3f v3(1, 0, 0);
-  Vec3f v4(1, 1, 1);
-
-  Vec3f p(0.5, 0.5, 0.5);
-  Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 15);
-  BOOST_CHECK(approx(res.sqr_distance, 0));
-  BOOST_CHECK(approx(res.parameterization[0], 0.25));
-  BOOST_CHECK(approx(res.parameterization[1], 0.25));
-  BOOST_CHECK(approx(res.parameterization[2], 0.25));
-  BOOST_CHECK(approx(res.parameterization[3], 0.25));
-
-  p = Vec3f(0, 0, 0);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 7);
-  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[3], 0));
-
-  p = Vec3f(0, 1, 1);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 11);
-  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
-
-  p = Vec3f(1, 1, 0);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 14);
-  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
-
-  p = Vec3f(1, 0, 1);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 13);
-  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
-  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
-
-  p = Vec3f(1.5, 1.5, 1.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 8);
-  BOOST_CHECK(approx(res.sqr_distance, 0.75));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-  BOOST_CHECK(approx(res.parameterization[3], 1));
-
-  p = Vec3f(1.5, -0.5, -0.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 4);
-  BOOST_CHECK(approx(res.sqr_distance, 0.75));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 1));
-  BOOST_CHECK(approx(res.parameterization[3], 0));
-
-  p = Vec3f(-0.5, -0.5, 1.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 1);
-  BOOST_CHECK(approx(res.sqr_distance, 0.75));
-  BOOST_CHECK(approx(res.parameterization[0], 1));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-  BOOST_CHECK(approx(res.parameterization[3], 0));
-
-  p = Vec3f(-0.5, 1.5, -0.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 2);
-  BOOST_CHECK(approx(res.sqr_distance, 0.75));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 1));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-  BOOST_CHECK(approx(res.parameterization[3], 0));
-
-  p = Vec3f(0.5, -0.5, 0.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 5);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 0.5));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 0.5));
-  BOOST_CHECK(approx(res.parameterization[3], 0));
-
-  p = Vec3f(0.5, 1.5, 0.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 10);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 0.5));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-  BOOST_CHECK(approx(res.parameterization[3], 0.5));
-
-  p = Vec3f(1.5, 0.5, 0.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 12);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 0.5));
-  BOOST_CHECK(approx(res.parameterization[3], 0.5));
-
-  p = Vec3f(-0.5, 0.5, 0.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 3);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 0.5));
-  BOOST_CHECK(approx(res.parameterization[1], 0.5));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-  BOOST_CHECK(approx(res.parameterization[3], 0));
-
-  p = Vec3f(0.5, 0.5, 1.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 9);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 0.5));
-  BOOST_CHECK(approx(res.parameterization[1], 0));
-  BOOST_CHECK(approx(res.parameterization[2], 0));
-  BOOST_CHECK(approx(res.parameterization[3], 0.5));
-
-  p = Vec3f(0.5, 0.5, -0.5);
-  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
-  BOOST_CHECK(res.encode == 6);
-  BOOST_CHECK(approx(res.sqr_distance, 0.25));
-  BOOST_CHECK(approx(res.parameterization[0], 0));
-  BOOST_CHECK(approx(res.parameterization[1], 0.5));
-  BOOST_CHECK(approx(res.parameterization[2], 0.5));
-  BOOST_CHECK(approx(res.parameterization[3], 0));
-}
+#define BOOST_TEST_MODULE COAL_SIMPLE
+#include <boost/test/included/unit_test.hpp>
+
+#include "coal/internal/intersect.h"
+#include "coal/collision.h"
+#include "coal/BVH/BVH_model.h"
+#include "fcl_resources/config.h"
+#include <sstream>
+
+using namespace coal;
+
+static CoalScalar epsilon = 1e-6;
+
+static bool approx(CoalScalar x, CoalScalar y) {
+  return std::abs(x - y) < epsilon;
+}
+
+BOOST_AUTO_TEST_CASE(projection_test_line) {
+  Vec3s v1(0, 0, 0);
+  Vec3s v2(2, 0, 0);
+
+  Vec3s p(1, 0, 0);
+  Project::ProjectResult res = Project::projectLine(v1, v2, p);
+  BOOST_CHECK(res.encode == 3);
+  BOOST_CHECK(approx(res.sqr_distance, 0));
+  BOOST_CHECK(approx(res.parameterization[0], 0.5));
+  BOOST_CHECK(approx(res.parameterization[1], 0.5));
+
+  p = Vec3s(-1, 0, 0);
+  res = Project::projectLine(v1, v2, p);
+  BOOST_CHECK(res.encode == 1);
+  BOOST_CHECK(approx(res.sqr_distance, 1));
+  BOOST_CHECK(approx(res.parameterization[0], 1));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+
+  p = Vec3s(3, 0, 0);
+  res = Project::projectLine(v1, v2, p);
+  BOOST_CHECK(res.encode == 2);
+  BOOST_CHECK(approx(res.sqr_distance, 1));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 1));
+}
+
+BOOST_AUTO_TEST_CASE(projection_test_triangle) {
+  Vec3s v1(0, 0, 1);
+  Vec3s v2(0, 1, 0);
+  Vec3s v3(1, 0, 0);
+
+  Vec3s p(1, 1, 1);
+  Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p);
+  BOOST_CHECK(res.encode == 7);
+  BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
+
+  p = Vec3s(0, 0, 1.5);
+  res = Project::projectTriangle(v1, v2, v3, p);
+  BOOST_CHECK(res.encode == 1);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 1));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+
+  p = Vec3s(1.5, 0, 0);
+  res = Project::projectTriangle(v1, v2, v3, p);
+  BOOST_CHECK(res.encode == 4);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 1));
+
+  p = Vec3s(0, 1.5, 0);
+  res = Project::projectTriangle(v1, v2, v3, p);
+  BOOST_CHECK(res.encode == 2);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 1));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+
+  p = Vec3s(1, 1, 0);
+  res = Project::projectTriangle(v1, v2, v3, p);
+  BOOST_CHECK(res.encode == 6);
+  BOOST_CHECK(approx(res.sqr_distance, 0.5));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 0.5));
+  BOOST_CHECK(approx(res.parameterization[2], 0.5));
+
+  p = Vec3s(1, 0, 1);
+  res = Project::projectTriangle(v1, v2, v3, p);
+  BOOST_CHECK(res.encode == 5);
+  BOOST_CHECK(approx(res.sqr_distance, 0.5));
+  BOOST_CHECK(approx(res.parameterization[0], 0.5));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 0.5));
+
+  p = Vec3s(0, 1, 1);
+  res = Project::projectTriangle(v1, v2, v3, p);
+  BOOST_CHECK(res.encode == 3);
+  BOOST_CHECK(approx(res.sqr_distance, 0.5));
+  BOOST_CHECK(approx(res.parameterization[0], 0.5));
+  BOOST_CHECK(approx(res.parameterization[1], 0.5));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+}
+
+BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
+  Vec3s v1(0, 0, 1);
+  Vec3s v2(0, 1, 0);
+  Vec3s v3(1, 0, 0);
+  Vec3s v4(1, 1, 1);
+
+  Vec3s p(0.5, 0.5, 0.5);
+  Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 15);
+  BOOST_CHECK(approx(res.sqr_distance, 0));
+  BOOST_CHECK(approx(res.parameterization[0], 0.25));
+  BOOST_CHECK(approx(res.parameterization[1], 0.25));
+  BOOST_CHECK(approx(res.parameterization[2], 0.25));
+  BOOST_CHECK(approx(res.parameterization[3], 0.25));
+
+  p = Vec3s(0, 0, 0);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 7);
+  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[3], 0));
+
+  p = Vec3s(0, 1, 1);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 11);
+  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
+
+  p = Vec3s(1, 1, 0);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 14);
+  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
+
+  p = Vec3s(1, 0, 1);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 13);
+  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
+  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
+
+  p = Vec3s(1.5, 1.5, 1.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 8);
+  BOOST_CHECK(approx(res.sqr_distance, 0.75));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+  BOOST_CHECK(approx(res.parameterization[3], 1));
+
+  p = Vec3s(1.5, -0.5, -0.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 4);
+  BOOST_CHECK(approx(res.sqr_distance, 0.75));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 1));
+  BOOST_CHECK(approx(res.parameterization[3], 0));
+
+  p = Vec3s(-0.5, -0.5, 1.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 1);
+  BOOST_CHECK(approx(res.sqr_distance, 0.75));
+  BOOST_CHECK(approx(res.parameterization[0], 1));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+  BOOST_CHECK(approx(res.parameterization[3], 0));
+
+  p = Vec3s(-0.5, 1.5, -0.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 2);
+  BOOST_CHECK(approx(res.sqr_distance, 0.75));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 1));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+  BOOST_CHECK(approx(res.parameterization[3], 0));
+
+  p = Vec3s(0.5, -0.5, 0.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 5);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 0.5));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 0.5));
+  BOOST_CHECK(approx(res.parameterization[3], 0));
+
+  p = Vec3s(0.5, 1.5, 0.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 10);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 0.5));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+  BOOST_CHECK(approx(res.parameterization[3], 0.5));
+
+  p = Vec3s(1.5, 0.5, 0.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 12);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 0.5));
+  BOOST_CHECK(approx(res.parameterization[3], 0.5));
+
+  p = Vec3s(-0.5, 0.5, 0.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 3);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 0.5));
+  BOOST_CHECK(approx(res.parameterization[1], 0.5));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+  BOOST_CHECK(approx(res.parameterization[3], 0));
+
+  p = Vec3s(0.5, 0.5, 1.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 9);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 0.5));
+  BOOST_CHECK(approx(res.parameterization[1], 0));
+  BOOST_CHECK(approx(res.parameterization[2], 0));
+  BOOST_CHECK(approx(res.parameterization[3], 0.5));
+
+  p = Vec3s(0.5, 0.5, -0.5);
+  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
+  BOOST_CHECK(res.encode == 6);
+  BOOST_CHECK(approx(res.sqr_distance, 0.25));
+  BOOST_CHECK(approx(res.parameterization[0], 0));
+  BOOST_CHECK(approx(res.parameterization[1], 0.5));
+  BOOST_CHECK(approx(res.parameterization[2], 0.5));
+  BOOST_CHECK(approx(res.parameterization[3], 0));
+}
diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp
index 3bf8f538e156e6d738c818a83e68eeedea43d648..26df2cca3e25c7885efb24b2e21f24afdf706eb3 100644
--- a/test/swept_sphere_radius.cpp
+++ b/test/swept_sphere_radius.cpp
@@ -34,20 +34,20 @@
 
 /** \author Louis Montaut */
 
-#define BOOST_TEST_MODULE SWEPT_SPHERE_RADIUS
+#define BOOST_TEST_MODULE COAL_SWEPT_SPHERE_RADIUS
 #include <boost/test/included/unit_test.hpp>
 
-#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/collision_utility.h>
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/collision_utility.h"
 
-#include <hpp/fcl/serialization/geometric_shapes.h>
-#include <hpp/fcl/serialization/convex.h>
-#include <hpp/fcl/serialization/transform.h>
-#include <hpp/fcl/serialization/archive.h>
+#include "coal/serialization/geometric_shapes.h"
+#include "coal/serialization/convex.h"
+#include "coal/serialization/transform.h"
+#include "coal/serialization/archive.h"
 
 #include "utility.h"
 
-using namespace hpp::fcl;
+using namespace coal;
 
 NODE_TYPE node1_type;
 NODE_TYPE node2_type;
@@ -58,7 +58,7 @@ int line;
   node2_type = shape2.getNodeType(); \
   line = __LINE__
 
-#define HPP_FCL_CHECK(cond)                                                  \
+#define COAL_CHECK(cond)                                                     \
   BOOST_CHECK_MESSAGE(                                                       \
       cond, "from line " << line << ", for collision pair: "                 \
                          << get_node_type_name(node1_type) << " - "          \
@@ -67,17 +67,17 @@ int line;
                          << ", ssr2 = " << shape2.getSweptSphereRadius()     \
                          << ": " #cond)
 
-#define HPP_FCL_CHECK_VECTOR_CLOSE(v1, v2, tol) \
-  EIGEN_VECTOR_IS_APPROX(v1, v2, tol);          \
-  HPP_FCL_CHECK(((v1) - (v2)).isZero(tol))
+#define COAL_CHECK_VECTOR_CLOSE(v1, v2, tol) \
+  EIGEN_VECTOR_IS_APPROX(v1, v2, tol);       \
+  COAL_CHECK(((v1) - (v2)).isZero(tol))
 
-#define HPP_FCL_CHECK_REAL_CLOSE(v1, v2, tol) \
-  FCL_REAL_IS_APPROX(v1, v2, tol);            \
-  HPP_FCL_CHECK(std::abs((v1) - (v2)) < tol)
+#define COAL_CHECK_REAL_CLOSE(v1, v2, tol) \
+  CoalScalar_IS_APPROX(v1, v2, tol);       \
+  COAL_CHECK(std::abs((v1) - (v2)) < tol)
 
-#define HPP_FCL_CHECK_CONDITION(cond) \
-  BOOST_CHECK(cond);                  \
-  HPP_FCL_CHECK(cond)
+#define COAL_CHECK_CONDITION(cond) \
+  BOOST_CHECK(cond);               \
+  COAL_CHECK(cond)
 
 // Preambule: swept sphere radius allows to virually convolve geometric shapes
 // by a sphere with positive radius (Minkowski sum).
@@ -99,7 +99,7 @@ int line;
 // So we can also easily recover the witness points of the swept sphere shapes.
 //
 // This suite of test is designed to verify that property and generally test for
-// swept-sphere radius support in hpp-fcl.
+// swept-sphere radius support in Coal.
 // Notes:
 //   - not all collision pairs use GJK/EPA, so this test makes sure that
 //     swept-sphere radius is taken into account even for specialized collision
@@ -113,19 +113,19 @@ int line;
 
 struct SweptSphereGJKSolver : public GJKSolver {
   template <typename S1, typename S2>
-  FCL_REAL shapeDistance(
-      const S1& s1, const Transform3f& tf1, const S2& s2,
-      const Transform3f& tf2, bool compute_penetration, Vec3f& p1, Vec3f& p2,
-      Vec3f& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const {
+  CoalScalar shapeDistance(
+      const S1& s1, const Transform3s& tf1, const S2& s2,
+      const Transform3s& tf2, bool compute_penetration, Vec3s& p1, Vec3s& p2,
+      Vec3s& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const {
     if (use_swept_sphere_radius_in_gjk_epa_iterations) {
-      FCL_REAL distance;
+      CoalScalar distance;
       this->runGJKAndEPA<S1, S2, details::SupportOptions::WithSweptSphere>(
           s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal);
       return distance;
     }
 
-    // Default behavior of hppfcl's GJKSolver
-    FCL_REAL distance;
+    // Default behavior of coal's GJKSolver
+    CoalScalar distance;
     this->runGJKAndEPA<S1, S2, details::SupportOptions::NoSweptSphere>(
         s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal);
     return distance;
@@ -138,36 +138,36 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) {
   // The swept sphere radius is detrimental to the convergence of GJK
   // and EPA. This gets worse as the radius of the swept sphere increases.
   // So we need to increase the number of iterations to get a good result.
-  const FCL_REAL tol = 1e-6;
+  const CoalScalar tol = 1e-6;
   solver.gjk_tolerance = tol;
   solver.epa_tolerance = tol;
   solver.epa_max_iterations = 1000;
   const bool compute_penetration = true;
 
-  FCL_REAL extents[] = {-2, -2, -2, 2, 2, 2};
+  CoalScalar extents[] = {-2, -2, -2, 2, 2, 2};
   std::size_t n = 10;
-  std::vector<Transform3f> tf1s;
-  std::vector<Transform3f> tf2s;
+  std::vector<Transform3s> tf1s;
+  std::vector<Transform3s> tf2s;
   generateRandomTransforms(extents, tf1s, n);
   generateRandomTransforms(extents, tf2s, n);
-  const std::array<FCL_REAL, 4> swept_sphere_radius = {0, 0.1, 1., 10.};
+  const std::array<CoalScalar, 4> swept_sphere_radius = {0, 0.1, 1., 10.};
 
-  for (const FCL_REAL& ssr1 : swept_sphere_radius) {
+  for (const CoalScalar& ssr1 : swept_sphere_radius) {
     shape1.setSweptSphereRadius(ssr1);
-    for (const FCL_REAL& ssr2 : swept_sphere_radius) {
+    for (const CoalScalar& ssr2 : swept_sphere_radius) {
       shape2.setSweptSphereRadius(ssr2);
       for (std::size_t i = 0; i < n; ++i) {
-        Transform3f tf1 = tf1s[i];
-        Transform3f tf2 = tf2s[i];
+        Transform3s tf1 = tf1s[i];
+        Transform3s tf2 = tf2s[i];
 
         SET_LINE;
 
-        std::array<FCL_REAL, 2> distance;
-        std::array<Vec3f, 2> p1;
-        std::array<Vec3f, 2> p2;
-        std::array<Vec3f, 2> normal;
+        std::array<CoalScalar, 2> distance;
+        std::array<Vec3s, 2> p1;
+        std::array<Vec3s, 2> p2;
+        std::array<Vec3s, 2> normal;
 
-        // Default hppfcl behavior - Don't take swept sphere radius into account
+        // Default coal behavior - Don't take swept sphere radius into account
         // during GJK/EPA iterations. Correct the solution afterwards.
         distance[0] =
             solver.shapeDistance(shape1, tf1, shape2, tf2, compute_penetration,
@@ -180,31 +180,31 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) {
 
         // Precision is dependent on the swept-sphere radius.
         // The issue of precision does not come from the default behavior of
-        // hppfcl, but from the result in which we manually take the swept
+        // coal, but from the result in which we manually take the swept
         // sphere radius into account in GJK/EPA iterations.
-        const FCL_REAL precision =
+        const CoalScalar precision =
             3 * sqrt(tol) +
             (1 / 100.0) * std::max(shape1.getSweptSphereRadius(),
                                    shape2.getSweptSphereRadius());
 
         // Check that the distance is the same
-        HPP_FCL_CHECK_REAL_CLOSE(distance[0], distance[1], precision);
+        COAL_CHECK_REAL_CLOSE(distance[0], distance[1], precision);
 
         // Check that the normal is the same
-        HPP_FCL_CHECK_CONDITION(normal[0].dot(normal[1]) > 0);
-        HPP_FCL_CHECK_CONDITION(std::abs(1 - normal[0].dot(normal[1])) <
-                                precision);
+        COAL_CHECK_CONDITION(normal[0].dot(normal[1]) > 0);
+        COAL_CHECK_CONDITION(std::abs(1 - normal[0].dot(normal[1])) <
+                             precision);
 
         // Check that the witness points are the same
-        HPP_FCL_CHECK_VECTOR_CLOSE(p1[0], p1[1], precision);
-        HPP_FCL_CHECK_VECTOR_CLOSE(p2[0], p2[1], precision);
+        COAL_CHECK_VECTOR_CLOSE(p1[0], p1[1], precision);
+        COAL_CHECK_VECTOR_CLOSE(p2[0], p2[1], precision);
       }
     }
   }
 }
 
-static const FCL_REAL min_shape_size = 0.1;
-static const FCL_REAL max_shape_size = 0.5;
+static const CoalScalar min_shape_size = 0.1;
+static const CoalScalar max_shape_size = 0.5;
 
 BOOST_AUTO_TEST_CASE(ssr_mesh_mesh) {
   Convex<Triangle> shape1 = makeRandomConvex(min_shape_size, max_shape_size);
@@ -281,21 +281,21 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) {
             << std::string(get_node_type_name(shape1.getNodeType())) << " and "
             << std::string(get_node_type_name(shape2.getNodeType())) << '\n';
 
-  FCL_REAL extents[] = {-2, -2, -2, 2, 2, 2};
+  CoalScalar extents[] = {-2, -2, -2, 2, 2, 2};
   std::size_t n = 1;
-  std::vector<Transform3f> tf1s;
-  std::vector<Transform3f> tf2s;
+  std::vector<Transform3s> tf1s;
+  std::vector<Transform3s> tf2s;
   generateRandomTransforms(extents, tf1s, n);
   generateRandomTransforms(extents, tf2s, n);
 
-  const std::array<FCL_REAL, 4> swept_sphere_radius = {0, 0.1, 1., 10.};
-  for (const FCL_REAL& ssr1 : swept_sphere_radius) {
+  const std::array<CoalScalar, 4> swept_sphere_radius = {0, 0.1, 1., 10.};
+  for (const CoalScalar& ssr1 : swept_sphere_radius) {
     shape1.setSweptSphereRadius(ssr1);
-    for (const FCL_REAL& ssr2 : swept_sphere_radius) {
+    for (const CoalScalar& ssr2 : swept_sphere_radius) {
       shape2.setSweptSphereRadius(ssr2);
       for (std::size_t i = 0; i < n; ++i) {
-        Transform3f tf1 = tf1s[i];
-        Transform3f tf2 = tf2s[i];
+        Transform3s tf1 = tf1s[i];
+        Transform3s tf2 = tf2s[i];
 
         SET_LINE;
         CollisionRequest request;
@@ -303,24 +303,24 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) {
         // We make sure we get witness points by setting the security margin to
         // infinity. That way shape1 and shape2 will always be considered in
         // collision.
-        request.security_margin = std::numeric_limits<FCL_REAL>::max();
-        const FCL_REAL tol = 1e-6;
+        request.security_margin = std::numeric_limits<CoalScalar>::max();
+        const CoalScalar tol = 1e-6;
         request.gjk_tolerance = tol;
         request.epa_tolerance = tol;
 
         std::array<CollisionResult, 2> result;
 
         // Without swept sphere radius
-        const FCL_REAL ssr1 = shape1.getSweptSphereRadius();
-        const FCL_REAL ssr2 = shape2.getSweptSphereRadius();
+        const CoalScalar ssr1 = shape1.getSweptSphereRadius();
+        const CoalScalar ssr2 = shape2.getSweptSphereRadius();
         shape1.setSweptSphereRadius(0.);
         shape2.setSweptSphereRadius(0.);
-        hpp::fcl::collide(&shape1, tf1, &shape2, tf2, request, result[0]);
+        coal::collide(&shape1, tf1, &shape2, tf2, request, result[0]);
 
         // With swept sphere radius
         shape1.setSweptSphereRadius(ssr1);
         shape2.setSweptSphereRadius(ssr2);
-        hpp::fcl::collide(&shape1, tf1, &shape2, tf2, request, result[1]);
+        coal::collide(&shape1, tf1, &shape2, tf2, request, result[1]);
 
         BOOST_CHECK(result[0].isCollision());
         BOOST_CHECK(result[1].isCollision());
@@ -331,28 +331,27 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) {
 
           // Precision is dependent on the swept sphere radii.
           // The issue of precision does not come from the default behavior of
-          // hppfcl, but from the result in which we manually take the swept
+          // coal, but from the result in which we manually take the swept
           // sphere radius into account in GJK/EPA iterations.
-          const FCL_REAL precision =
+          const CoalScalar precision =
               3 * sqrt(tol) + (1 / 100.0) * std::max(ssr1, ssr2);
-          const FCL_REAL ssr = ssr1 + ssr2;
+          const CoalScalar ssr = ssr1 + ssr2;
 
           // Check that the distance is the same
-          HPP_FCL_CHECK_REAL_CLOSE(contact[0].penetration_depth - ssr,
-                                   contact[1].penetration_depth, precision);
+          COAL_CHECK_REAL_CLOSE(contact[0].penetration_depth - ssr,
+                                contact[1].penetration_depth, precision);
 
           // Check that the normal is the same
-          HPP_FCL_CHECK_CONDITION((contact[0].normal).dot(contact[1].normal) >
-                                  0);
-          HPP_FCL_CHECK_CONDITION(
+          COAL_CHECK_CONDITION((contact[0].normal).dot(contact[1].normal) > 0);
+          COAL_CHECK_CONDITION(
               std::abs(1 - (contact[0].normal).dot(contact[1].normal)) <
               precision);
 
           // Check that the witness points are the same
-          HPP_FCL_CHECK_VECTOR_CLOSE(
+          COAL_CHECK_VECTOR_CLOSE(
               contact[0].nearest_points[0] + ssr1 * contact[0].normal,
               contact[1].nearest_points[0], precision);
-          HPP_FCL_CHECK_VECTOR_CLOSE(
+          COAL_CHECK_VECTOR_CLOSE(
               contact[0].nearest_points[1] - ssr2 * contact[0].normal,
               contact[1].nearest_points[1], precision);
         }
diff --git a/test/utility.cpp b/test/utility.cpp
index cf20cdd149345e90563e46f5f42cf09bf45608fd..714469f019e57d14cd638f7af944287e49afa9af 100644
--- a/test/utility.cpp
+++ b/test/utility.cpp
@@ -1,21 +1,20 @@
 #include "utility.h"
 
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
-#include <hpp/fcl/collision_utility.h>
-#include <hpp/fcl/fwd.hh>
+#include "coal/BV/BV.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/shape/geometric_shape_to_BVH_model.h"
+#include "coal/collision_utility.h"
+#include "coal/fwd.hh"
 
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/distance.h>
+#include "coal/collision.h"
+#include "coal/distance.h"
 
 #include <cstdio>
 #include <cstddef>
 #include <fstream>
 #include <iostream>
 
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 BenchTimer::BenchTimer() {
 #ifdef _WIN32
@@ -86,16 +85,16 @@ const Eigen::IOFormat vfmt = Eigen::IOFormat(
 const Eigen::IOFormat pyfmt = Eigen::IOFormat(
     Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
 
-const Vec3f UnitX = Vec3f(1, 0, 0);
-const Vec3f UnitY = Vec3f(0, 1, 0);
-const Vec3f UnitZ = Vec3f(0, 0, 1);
+const Vec3s UnitX = Vec3s(1, 0, 0);
+const Vec3s UnitY = Vec3s(0, 1, 0);
+const Vec3s UnitZ = Vec3s(0, 0, 1);
 
-FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) {
-  FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1);
+CoalScalar rand_interval(CoalScalar rmin, CoalScalar rmax) {
+  CoalScalar t = rand() / ((CoalScalar)RAND_MAX + 1);
   return (t * (rmax - rmin) + rmin);
 }
 
-void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
+void loadOBJFile(const char* filename, std::vector<Vec3s>& points,
                  std::vector<Triangle>& triangles) {
   FILE* file = fopen(filename, "rb");
   if (!file) {
@@ -122,10 +121,10 @@ void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
           strtok(NULL, "\t ");
           has_texture = true;
         } else {
-          FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t "));
-          FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t "));
-          FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t "));
-          Vec3f p(x, y, z);
+          CoalScalar x = (CoalScalar)atof(strtok(NULL, "\t "));
+          CoalScalar y = (CoalScalar)atof(strtok(NULL, "\t "));
+          CoalScalar z = (CoalScalar)atof(strtok(NULL, "\t "));
+          Vec3s p(x, y, z);
           points.push_back(p);
         }
       } break;
@@ -161,7 +160,7 @@ void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
   }
 }
 
-void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
+void saveOBJFile(const char* filename, std::vector<Vec3s>& points,
                  std::vector<Triangle>& triangles) {
   std::ofstream os(filename);
   if (!os) {
@@ -182,8 +181,9 @@ void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
   os.close();
 }
 
-#ifdef HPP_FCL_HAS_OCTOMAP
-OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) {
+#ifdef COAL_HAS_OCTOMAP
+OcTree loadOctreeFile(const std::string& filename,
+                      const CoalScalar& resolution) {
   octomap::OcTreePtr_t octree(new octomap::OcTree(filename));
   if (octree->getResolution() != resolution) {
     std::ostringstream oss;
@@ -191,97 +191,97 @@ OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) {
         << " and not " << resolution;
     throw std::invalid_argument(oss.str());
   }
-  return hpp::fcl::OcTree(octree);
+  return coal::OcTree(octree);
 }
 #endif
 
-void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) {
-  FCL_REAL c1 = cos(a);
-  FCL_REAL c2 = cos(b);
-  FCL_REAL c3 = cos(c);
-  FCL_REAL s1 = sin(a);
-  FCL_REAL s2 = sin(b);
-  FCL_REAL s3 = sin(c);
+void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3s& R) {
+  CoalScalar c1 = cos(a);
+  CoalScalar c2 = cos(b);
+  CoalScalar c3 = cos(c);
+  CoalScalar s1 = sin(a);
+  CoalScalar s2 = sin(b);
+  CoalScalar s3 = sin(c);
 
   R << c1 * c2, -c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3,
       -c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3;
 }
 
-void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) {
-  FCL_REAL x = rand_interval(extents[0], extents[3]);
-  FCL_REAL y = rand_interval(extents[1], extents[4]);
-  FCL_REAL z = rand_interval(extents[2], extents[5]);
+void generateRandomTransform(CoalScalar extents[6], Transform3s& transform) {
+  CoalScalar x = rand_interval(extents[0], extents[3]);
+  CoalScalar y = rand_interval(extents[1], extents[4]);
+  CoalScalar z = rand_interval(extents[2], extents[5]);
 
-  const FCL_REAL pi = 3.1415926;
-  FCL_REAL a = rand_interval(0, 2 * pi);
-  FCL_REAL b = rand_interval(0, 2 * pi);
-  FCL_REAL c = rand_interval(0, 2 * pi);
+  const CoalScalar pi = 3.1415926;
+  CoalScalar a = rand_interval(0, 2 * pi);
+  CoalScalar b = rand_interval(0, 2 * pi);
+  CoalScalar c = rand_interval(0, 2 * pi);
 
-  Matrix3f R;
+  Matrix3s R;
   eulerToMatrix(a, b, c, R);
-  Vec3f T(x, y, z);
+  Vec3s T(x, y, z);
   transform.setTransform(R, T);
 }
 
-void generateRandomTransforms(FCL_REAL extents[6],
-                              std::vector<Transform3f>& transforms,
+void generateRandomTransforms(CoalScalar extents[6],
+                              std::vector<Transform3s>& transforms,
                               std::size_t n) {
   transforms.resize(n);
   for (std::size_t i = 0; i < n; ++i) {
-    FCL_REAL x = rand_interval(extents[0], extents[3]);
-    FCL_REAL y = rand_interval(extents[1], extents[4]);
-    FCL_REAL z = rand_interval(extents[2], extents[5]);
+    CoalScalar x = rand_interval(extents[0], extents[3]);
+    CoalScalar y = rand_interval(extents[1], extents[4]);
+    CoalScalar z = rand_interval(extents[2], extents[5]);
 
-    const FCL_REAL pi = 3.1415926;
-    FCL_REAL a = rand_interval(0, 2 * pi);
-    FCL_REAL b = rand_interval(0, 2 * pi);
-    FCL_REAL c = rand_interval(0, 2 * pi);
+    const CoalScalar pi = 3.1415926;
+    CoalScalar a = rand_interval(0, 2 * pi);
+    CoalScalar b = rand_interval(0, 2 * pi);
+    CoalScalar c = rand_interval(0, 2 * pi);
 
     {
-      Matrix3f R;
+      Matrix3s R;
       eulerToMatrix(a, b, c, R);
-      Vec3f T(x, y, z);
+      Vec3s T(x, y, z);
       transforms[i].setTransform(R, T);
     }
   }
 }
 
-void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3],
-                              FCL_REAL delta_rot,
-                              std::vector<Transform3f>& transforms,
-                              std::vector<Transform3f>& transforms2,
+void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3],
+                              CoalScalar delta_rot,
+                              std::vector<Transform3s>& transforms,
+                              std::vector<Transform3s>& transforms2,
                               std::size_t n) {
   transforms.resize(n);
   transforms2.resize(n);
   for (std::size_t i = 0; i < n; ++i) {
-    FCL_REAL x = rand_interval(extents[0], extents[3]);
-    FCL_REAL y = rand_interval(extents[1], extents[4]);
-    FCL_REAL z = rand_interval(extents[2], extents[5]);
+    CoalScalar x = rand_interval(extents[0], extents[3]);
+    CoalScalar y = rand_interval(extents[1], extents[4]);
+    CoalScalar z = rand_interval(extents[2], extents[5]);
 
-    const FCL_REAL pi = 3.1415926;
-    FCL_REAL a = rand_interval(0, 2 * pi);
-    FCL_REAL b = rand_interval(0, 2 * pi);
-    FCL_REAL c = rand_interval(0, 2 * pi);
+    const CoalScalar pi = 3.1415926;
+    CoalScalar a = rand_interval(0, 2 * pi);
+    CoalScalar b = rand_interval(0, 2 * pi);
+    CoalScalar c = rand_interval(0, 2 * pi);
 
     {
-      Matrix3f R;
+      Matrix3s R;
       eulerToMatrix(a, b, c, R);
-      Vec3f T(x, y, z);
+      Vec3s T(x, y, z);
       transforms[i].setTransform(R, T);
     }
 
-    FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
-    FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
-    FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
+    CoalScalar deltax = rand_interval(-delta_trans[0], delta_trans[0]);
+    CoalScalar deltay = rand_interval(-delta_trans[1], delta_trans[1]);
+    CoalScalar deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
 
-    FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot);
-    FCL_REAL deltab = rand_interval(-delta_rot, delta_rot);
-    FCL_REAL deltac = rand_interval(-delta_rot, delta_rot);
+    CoalScalar deltaa = rand_interval(-delta_rot, delta_rot);
+    CoalScalar deltab = rand_interval(-delta_rot, delta_rot);
+    CoalScalar deltac = rand_interval(-delta_rot, delta_rot);
 
     {
-      Matrix3f R;
+      Matrix3s R;
       eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
-      Vec3f T(x + deltax, y + deltay, z + deltaz);
+      Vec3s T(x + deltax, y + deltay, z + deltaz);
       transforms2[i].setTransform(R, T);
     }
   }
@@ -305,7 +305,7 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
 }
 
 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
-                             void* cdata_, FCL_REAL& dist) {
+                             void* cdata_, CoalScalar& dist) {
   DistanceData* cdata = static_cast<DistanceData*>(cdata_);
   const DistanceRequest& request = cdata->request;
   DistanceResult& result = cdata->result;
@@ -367,7 +367,7 @@ std::string getNodeTypeName(NODE_TYPE node_type) {
     return std::string("invalid");
 }
 
-Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) {
+Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z) {
   Quatf q;
   q.w() = w;
   q.x() = x;
@@ -376,7 +376,7 @@ Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) {
   return q;
 }
 
-std::ostream& operator<<(std::ostream& os, const Transform3f& tf) {
+std::ostream& operator<<(std::ostream& os, const Transform3s& tf) {
   return os << "[ " << tf.getTranslation().format(vfmt) << ", "
             << tf.getQuatRotation().coeffs().format(vfmt) << " ]";
 }
@@ -390,10 +390,10 @@ std::size_t getNbRun(const int& argc, char const* const* argv,
 }
 
 void generateEnvironments(std::vector<CollisionObject*>& env,
-                          FCL_REAL 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(n);
+                          CoalScalar env_scale, std::size_t n) {
+  CoalScalar extents[] = {-env_scale, env_scale,  -env_scale,
+                          env_scale,  -env_scale, env_scale};
+  std::vector<Transform3s> transforms(n);
 
   generateRandomTransforms(extents, transforms, n);
   for (std::size_t i = 0; i < n; ++i) {
@@ -421,16 +421,16 @@ void generateEnvironments(std::vector<CollisionObject*>& env,
 }
 
 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
-                              FCL_REAL 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;
+                              CoalScalar env_scale, std::size_t n) {
+  CoalScalar extents[] = {-env_scale, env_scale,  -env_scale,
+                          env_scale,  -env_scale, env_scale};
+  std::vector<Transform3s> 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::Identity());
+    generateBVHModel(*model, box, Transform3s::Identity());
     env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
                                       transforms[i]));
     env.back()->collisionGeometry()->computeLocalAABB();
@@ -440,7 +440,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
   Sphere sphere(30);
   for (std::size_t i = 0; i < n; ++i) {
     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
-    generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16);
+    generateBVHModel(*model, sphere, Transform3s::Identity(), 16, 16);
     env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
                                       transforms[i]));
     env.back()->collisionGeometry()->computeLocalAABB();
@@ -450,18 +450,18 @@ void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
   Cylinder cylinder(10, 40);
   for (std::size_t i = 0; i < n; ++i) {
     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
-    generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16);
+    generateBVHModel(*model, cylinder, Transform3s::Identity(), 16, 16);
     env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
                                       transforms[i]));
     env.back()->collisionGeometry()->computeLocalAABB();
   }
 }
 
-Convex<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) {
-  std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>(
-      {Vec3f(l, w, d), Vec3f(l, w, -d), Vec3f(l, -w, d), Vec3f(l, -w, -d),
-       Vec3f(-l, w, d), Vec3f(-l, w, -d), Vec3f(-l, -w, d),
-       Vec3f(-l, -w, -d)}));
+Convex<Quadrilateral> buildBox(CoalScalar l, CoalScalar w, CoalScalar d) {
+  std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>(
+      {Vec3s(l, w, d), Vec3s(l, w, -d), Vec3s(l, -w, d), Vec3s(l, -w, -d),
+       Vec3s(-l, w, d), Vec3s(-l, w, -d), Vec3s(-l, -w, d),
+       Vec3s(-l, -w, -d)}));
 
   std::shared_ptr<std::vector<Quadrilateral>> polygons(
       new std::vector<Quadrilateral>(6));
@@ -480,7 +480,7 @@ Convex<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) {
 }
 
 /// Takes a point and projects it onto the surface of the unit sphere
-void toSphere(Vec3f& point) {
+void toSphere(Vec3s& point) {
   assert(point.norm() > 1e-8);
   point /= point.norm();
 }
@@ -491,7 +491,7 @@ void toSphere(Vec3f& point) {
 /// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y *
 /// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) =
 /// diag(r).
-void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) {
+void toEllipsoid(Vec3s& point, const Ellipsoid& ellipsoid) {
   toSphere(point);
   point[0] *= ellipsoid.radii[0];
   point[1] *= ellipsoid.radii[1];
@@ -499,27 +499,27 @@ void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) {
 }
 
 Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
-  FCL_REAL PHI = (1 + std::sqrt(5)) / 2;
+  CoalScalar PHI = (1 + std::sqrt(5)) / 2;
 
   // vertices
-  std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
-      Vec3f(-1, PHI, 0),
-      Vec3f(1, PHI, 0),
-      Vec3f(-1, -PHI, 0),
-      Vec3f(1, -PHI, 0),
-
-      Vec3f(0, -1, PHI),
-      Vec3f(0, 1, PHI),
-      Vec3f(0, -1, -PHI),
-      Vec3f(0, 1, -PHI),
-
-      Vec3f(PHI, 0, -1),
-      Vec3f(PHI, 0, 1),
-      Vec3f(-PHI, 0, -1),
-      Vec3f(-PHI, 0, 1),
+  std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+      Vec3s(-1, PHI, 0),
+      Vec3s(1, PHI, 0),
+      Vec3s(-1, -PHI, 0),
+      Vec3s(1, -PHI, 0),
+
+      Vec3s(0, -1, PHI),
+      Vec3s(0, 1, PHI),
+      Vec3s(0, -1, -PHI),
+      Vec3s(0, 1, -PHI),
+
+      Vec3s(PHI, 0, -1),
+      Vec3s(PHI, 0, 1),
+      Vec3s(-PHI, 0, -1),
+      Vec3s(-PHI, 0, 1),
   }));
 
-  std::vector<Vec3f>& pts_ = *pts;
+  std::vector<Vec3s>& pts_ = *pts;
   for (size_t i = 0; i < 12; ++i) {
     toEllipsoid(pts_[i], ellipsoid);
   }
@@ -556,61 +556,61 @@ Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
   );
 }
 
-Box makeRandomBox(FCL_REAL min_size, FCL_REAL max_size) {
-  return Box(Vec3f(rand_interval(min_size, max_size),
+Box makeRandomBox(CoalScalar min_size, CoalScalar max_size) {
+  return Box(Vec3s(rand_interval(min_size, max_size),
                    rand_interval(min_size, max_size),
                    rand_interval(min_size, max_size)));
 }
 
-Sphere makeRandomSphere(FCL_REAL min_size, FCL_REAL max_size) {
+Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size) {
   return Sphere(rand_interval(min_size, max_size));
 }
 
-Ellipsoid makeRandomEllipsoid(FCL_REAL min_size, FCL_REAL max_size) {
-  return Ellipsoid(Vec3f(rand_interval(min_size, max_size),
+Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size) {
+  return Ellipsoid(Vec3s(rand_interval(min_size, max_size),
                          rand_interval(min_size, max_size),
                          rand_interval(min_size, max_size)));
 }
 
-Capsule makeRandomCapsule(std::array<FCL_REAL, 2> min_size,
-                          std::array<FCL_REAL, 2> max_size) {
+Capsule makeRandomCapsule(std::array<CoalScalar, 2> min_size,
+                          std::array<CoalScalar, 2> max_size) {
   return Capsule(rand_interval(min_size[0], max_size[0]),
                  rand_interval(min_size[1], max_size[1]));
 }
 
-Cone makeRandomCone(std::array<FCL_REAL, 2> min_size,
-                    std::array<FCL_REAL, 2> max_size) {
+Cone makeRandomCone(std::array<CoalScalar, 2> min_size,
+                    std::array<CoalScalar, 2> max_size) {
   return Cone(rand_interval(min_size[0], max_size[0]),
               rand_interval(min_size[1], max_size[1]));
 }
 
-Cylinder makeRandomCylinder(std::array<FCL_REAL, 2> min_size,
-                            std::array<FCL_REAL, 2> max_size) {
+Cylinder makeRandomCylinder(std::array<CoalScalar, 2> min_size,
+                            std::array<CoalScalar, 2> max_size) {
   return Cylinder(rand_interval(min_size[0], max_size[0]),
                   rand_interval(min_size[1], max_size[1]));
 }
 
-Convex<Triangle> makeRandomConvex(FCL_REAL min_size, FCL_REAL max_size) {
+Convex<Triangle> makeRandomConvex(CoalScalar min_size, CoalScalar max_size) {
   Ellipsoid ellipsoid = makeRandomEllipsoid(min_size, max_size);
   return constructPolytopeFromEllipsoid(ellipsoid);
 }
 
-Plane makeRandomPlane(FCL_REAL min_size, FCL_REAL max_size) {
-  return Plane(Vec3f::Random().normalized(), rand_interval(min_size, max_size));
+Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size) {
+  return Plane(Vec3s::Random().normalized(), rand_interval(min_size, max_size));
 }
 
-Halfspace makeRandomHalfspace(FCL_REAL min_size, FCL_REAL max_size) {
-  return Halfspace(Vec3f::Random().normalized(),
+Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size) {
+  return Halfspace(Vec3s::Random().normalized(),
                    rand_interval(min_size, max_size));
 }
 
 std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type) {
   switch (node_type) {
     case GEOM_TRIANGLE:
-      HPP_FCL_THROW_PRETTY(std::string(HPP_FCL_PRETTY_FUNCTION) + " for " +
-                               std::string(get_node_type_name(node_type)) +
-                               " is not yet implemented.",
-                           std::invalid_argument);
+      COAL_THROW_PRETTY(std::string(COAL_PRETTY_FUNCTION) + " for " +
+                            std::string(get_node_type_name(node_type)) +
+                            " is not yet implemented.",
+                        std::invalid_argument);
       break;
     case GEOM_BOX:
       return std::make_shared<Box>(makeRandomBox(0.1, 1.0));
@@ -642,13 +642,11 @@ std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type) {
       return std::make_shared<Halfspace>(makeRandomHalfspace(0.1, 1.0));
       break;
     default:
-      HPP_FCL_THROW_PRETTY(std::string(get_node_type_name(node_type)) +
-                               " is not a primitive shape.",
-                           std::invalid_argument);
+      COAL_THROW_PRETTY(std::string(get_node_type_name(node_type)) +
+                            " is not a primitive shape.",
+                        std::invalid_argument);
       break;
   }
 }
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
diff --git a/test/utility.h b/test/utility.h
index b81723ab9b4bd44138abb32258699bde878ae490..6be386e5e012dfc18b8f26c0faac7f813452da7e 100644
--- a/test/utility.h
+++ b/test/utility.h
@@ -35,17 +35,17 @@
 
 /** \author Jia Pan */
 
-#ifndef TEST_HPP_FCL_UTILITY_H
-#define TEST_HPP_FCL_UTILITY_H
+#ifndef TEST_COAL_UTILITY_H
+#define TEST_COAL_UTILITY_H
 
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/broadphase/default_broadphase_callbacks.h>
-#include <hpp/fcl/shape/convex.h>
+#include "coal/math/transform.h"
+#include "coal/collision_data.h"
+#include "coal/collision_object.h"
+#include "coal/broadphase/default_broadphase_callbacks.h"
+#include "coal/shape/convex.h"
 
-#ifdef HPP_FCL_HAS_OCTOMAP
-#include <hpp/fcl/octree.h>
+#ifdef COAL_HAS_OCTOMAP
+#include "coal/octree.h"
 #endif
 
 #ifdef _WIN32
@@ -70,7 +70,7 @@
                           << (Va) << "\n!=\n"                                \
                           << (Vb) << "\n]")
 
-#define FCL_REAL_IS_APPROX(Va, Vb, precision)                                \
+#define CoalScalar_IS_APPROX(Va, Vb, precision)                              \
   BOOST_CHECK_MESSAGE(std::abs((Va) - (Vb)) < precision,                     \
                       "check " #Va ".isApprox(" #Vb ") failed at precision " \
                           << precision << " [\n"                             \
@@ -78,12 +78,11 @@
                           << Vb << "\n]")
 
 namespace octomap {
-#ifdef HPP_FCL_HAS_OCTOMAP
-typedef hpp::fcl::shared_ptr<octomap::OcTree> OcTreePtr_t;
+#ifdef COAL_HAS_OCTOMAP
+typedef coal::shared_ptr<octomap::OcTree> OcTreePtr_t;
 #endif
 }  // namespace octomap
-namespace hpp {
-namespace fcl {
+namespace coal {
 
 class BenchTimer {
  public:
@@ -126,50 +125,50 @@ struct TStruct {
 
 extern const Eigen::IOFormat vfmt;
 extern const Eigen::IOFormat pyfmt;
-typedef Eigen::AngleAxis<FCL_REAL> AngleAxis;
-extern const Vec3f UnitX;
-extern const Vec3f UnitY;
-extern const Vec3f UnitZ;
+typedef Eigen::AngleAxis<CoalScalar> AngleAxis;
+extern const Vec3s UnitX;
+extern const Vec3s UnitY;
+extern const Vec3s UnitZ;
 
 /// @brief Load an obj mesh file
-void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
+void loadOBJFile(const char* filename, std::vector<Vec3s>& points,
                  std::vector<Triangle>& triangles);
 
-void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
+void saveOBJFile(const char* filename, std::vector<Vec3s>& points,
                  std::vector<Triangle>& triangles);
 
-#ifdef HPP_FCL_HAS_OCTOMAP
-fcl::OcTree loadOctreeFile(const std::string& filename,
-                           const FCL_REAL& resolution);
+#ifdef COAL_HAS_OCTOMAP
+coal::OcTree loadOctreeFile(const std::string& filename,
+                            const CoalScalar& resolution);
 #endif
 
 /// @brief Generate one random transform whose translation is constrained by
 /// extents and rotation without constraints. The translation is (x, y, z), and
 /// extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <=
 /// z <= extents[5]
-void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform);
+void generateRandomTransform(CoalScalar extents[6], Transform3s& transform);
 
 /// @brief Generate n random transforms whose translations are constrained by
 /// extents.
-void generateRandomTransforms(FCL_REAL extents[6],
-                              std::vector<Transform3f>& transforms,
+void generateRandomTransforms(CoalScalar extents[6],
+                              std::vector<Transform3s>& transforms,
                               std::size_t n);
 
 /// @brief Generate n random transforms whose translations are constrained by
 /// extents. Also generate another transforms2 which have additional random
 /// translation & rotation to the transforms generated.
-void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3],
-                              FCL_REAL delta_rot,
-                              std::vector<Transform3f>& transforms,
-                              std::vector<Transform3f>& transforms2,
+void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3],
+                              CoalScalar delta_rot,
+                              std::vector<Transform3s>& transforms,
+                              std::vector<Transform3s>& transforms2,
                               std::size_t n);
 
 /// @ brief Structure for minimum distance between two meshes and the
 /// corresponding nearest point pair
 struct DistanceRes {
   double distance;
-  Vec3f p1;
-  Vec3f p2;
+  Vec3s p1;
+  Vec3s p2;
 };
 
 /// @brief Default collision callback for two objects o1 and o2 in broad phase.
@@ -181,28 +180,28 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
 /// return value means whether the broad phase can stop now. also return dist,
 /// i.e. the bmin distance till now
 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
-                             void* cdata, FCL_REAL& dist);
+                             void* cdata, CoalScalar& dist);
 
 std::string getNodeTypeName(NODE_TYPE node_type);
 
-Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);
+Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z);
 
-std::ostream& operator<<(std::ostream& os, const Transform3f& tf);
+std::ostream& operator<<(std::ostream& os, const Transform3s& tf);
 
 /// Get the argument --nb-run from argv
 std::size_t getNbRun(const int& argc, char const* const* argv,
                      std::size_t defaultValue);
 
 void generateEnvironments(std::vector<CollisionObject*>& env,
-                          FCL_REAL env_scale, std::size_t n);
+                          CoalScalar env_scale, std::size_t n);
 
 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
-                              FCL_REAL env_scale, std::size_t n);
+                              CoalScalar env_scale, std::size_t n);
 
 /// @brief Constructs a box with halfsides (l, w, d), centered around 0.
 /// The box is 2*l wide on the x-axis, 2*w wide on the y-axis and 2*d wide on
 /// the z-axis.
-Convex<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d);
+Convex<Quadrilateral> buildBox(CoalScalar l, CoalScalar w, CoalScalar d);
 
 /// @brief We give an ellipsoid as input. The output is a 20 faces polytope
 /// which vertices belong to the original ellipsoid surface. The procedure is
@@ -211,31 +210,29 @@ Convex<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d);
 /// ellipsoid tranformation to each vertex of the icosahedron.
 Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid);
 
-Box makeRandomBox(FCL_REAL min_size, FCL_REAL max_size);
+Box makeRandomBox(CoalScalar min_size, CoalScalar max_size);
 
-Sphere makeRandomSphere(FCL_REAL min_size, FCL_REAL max_size);
+Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size);
 
-Ellipsoid makeRandomEllipsoid(FCL_REAL min_size, FCL_REAL max_size);
+Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size);
 
-Capsule makeRandomCapsule(std::array<FCL_REAL, 2> min_size,
-                          std::array<FCL_REAL, 2> max_size);
+Capsule makeRandomCapsule(std::array<CoalScalar, 2> min_size,
+                          std::array<CoalScalar, 2> max_size);
 
-Cone makeRandomCone(std::array<FCL_REAL, 2> min_size,
-                    std::array<FCL_REAL, 2> max_size);
+Cone makeRandomCone(std::array<CoalScalar, 2> min_size,
+                    std::array<CoalScalar, 2> max_size);
 
-Cylinder makeRandomCylinder(std::array<FCL_REAL, 2> min_size,
-                            std::array<FCL_REAL, 2> max_size);
+Cylinder makeRandomCylinder(std::array<CoalScalar, 2> min_size,
+                            std::array<CoalScalar, 2> max_size);
 
-Convex<Triangle> makeRandomConvex(FCL_REAL min_size, FCL_REAL max_size);
+Convex<Triangle> makeRandomConvex(CoalScalar min_size, CoalScalar max_size);
 
-Plane makeRandomPlane(FCL_REAL min_size, FCL_REAL max_size);
+Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size);
 
-Halfspace makeRandomHalfspace(FCL_REAL min_size, FCL_REAL max_size);
+Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size);
 
 std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type);
 
-}  // namespace fcl
-
-}  // namespace hpp
+}  // namespace coal
 
 #endif