diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index b376bc74613ffa3aa646a405c30d57dad97fcafb..0d75d70213fb6c9666a2612a2fa2a62076effe3d 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -33,7 +33,7 @@ link_directories(${OCTOMAP_LIBRARY_DIRS})
 
 add_definitions(-DUSE_SVMLIGHT=0)
 
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/BV/kDOP.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/ccd/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/math/transform.cpp src/traversal_node_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broadphase/broadphase_bruteforce.cpp src/broadphase/broadphase_spatialhash.cpp src/broadphase/broadphase_SaP.cpp src/broadphase/broadphase_SSaP.cpp src/broadphase/broadphase_interval_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree_array.cpp src/collision.cpp src/collision_func_matrix.cpp src/broadphase/interval_tree.cpp src/ccd/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/broadphase/hierarchy_tree.cpp src/profile.cpp src/collision_data.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/BV/kDOP.cpp src/traversal/traversal_node_base.cpp src/traversal/traversal_node_bvhs.cpp src/intersect.cpp src/ccd/motion.cpp src/BVH/BV_fitter.cpp src/BVH/BV_splitter.cpp src/BVH/BVH_model.cpp src/BVH/BVH_utility.cpp src/math/transform.cpp src/traversal/traversal_node_setup.cpp src/shape/geometric_shapes.cpp src/shape/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal/traversal_recurse.cpp src/broadphase/broadphase_bruteforce.cpp src/broadphase/broadphase_spatialhash.cpp src/broadphase/broadphase_SaP.cpp src/broadphase/broadphase_SSaP.cpp src/broadphase/broadphase_interval_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree_array.cpp src/collision.cpp src/collision_func_matrix.cpp src/broadphase/interval_tree.cpp src/ccd/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/broadphase/hierarchy_tree.cpp src/profile.cpp src/collision_data.cpp)
 
 
 target_link_libraries(${PROJECT_NAME} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index 558af5959272167d7c37134d4af889b2430de656..ec04609b043b141f2aea2043d692d88cc31d500b 100644
--- a/trunk/fcl/include/fcl/BV/AABB.h
+++ b/trunk/fcl/include/fcl/BV/AABB.h
@@ -38,7 +38,6 @@
 #define FCL_AABB_H
 
 
-#include "fcl/BVH_internal.h"
 #include "fcl/math/vec_3f.h"
 
 namespace fcl
diff --git a/trunk/fcl/include/fcl/BV.h b/trunk/fcl/include/fcl/BV/BV.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV.h
rename to trunk/fcl/include/fcl/BV/BV.h
diff --git a/trunk/fcl/include/fcl/BV_node.h b/trunk/fcl/include/fcl/BV/BV_node.h
similarity index 99%
rename from trunk/fcl/include/fcl/BV_node.h
rename to trunk/fcl/include/fcl/BV/BV_node.h
index 9776d660b567ea655c14bac5f81dca6d4844ad1d..8082b2d13881fb056c1cbafd44cf5c73ebf374bc 100644
--- a/trunk/fcl/include/fcl/BV_node.h
+++ b/trunk/fcl/include/fcl/BV/BV_node.h
@@ -41,7 +41,7 @@
 #include "fcl/math/vec_3f.h"
 #include "fcl/math/matrix_3f.h"
 
-#include "fcl/BV.h"
+#include "fcl/BV/BV.h"
 #include <iostream>
 
 namespace fcl
diff --git a/trunk/fcl/include/fcl/BV/OBB.h b/trunk/fcl/include/fcl/BV/OBB.h
index 7138b9ad0960a090780040c823991d5d7a42f92c..7dbb2fa552233b857f7b04fd76c8b3384eca7eb7 100644
--- a/trunk/fcl/include/fcl/BV/OBB.h
+++ b/trunk/fcl/include/fcl/BV/OBB.h
@@ -37,7 +37,7 @@
 #ifndef FCL_OBB_H
 #define FCL_OBB_H
 
-#include "fcl/BVH_internal.h"
+
 #include "fcl/math/vec_3f.h"
 #include "fcl/math/matrix_3f.h"
 
diff --git a/trunk/fcl/include/fcl/BV/OBBRSS.h b/trunk/fcl/include/fcl/BV/OBBRSS.h
index 7474f2b81c2ea9849e6e673e137a29e2d79f8de8..be30bc9b87d43d5214fd8e136ec6e6dbcbc051e8 100644
--- a/trunk/fcl/include/fcl/BV/OBBRSS.h
+++ b/trunk/fcl/include/fcl/BV/OBBRSS.h
@@ -37,9 +37,7 @@
 #ifndef FCL_OBBRSS_H
 #define FCL_OBBRSS_H
 
-#include "fcl/BVH_internal.h"
-#include "fcl/math/vec_3f.h"
-#include "fcl/math/matrix_3f.h"
+
 #include "fcl/BV/OBB.h"
 #include "fcl/BV/RSS.h"
 
diff --git a/trunk/fcl/include/fcl/BV/RSS.h b/trunk/fcl/include/fcl/BV/RSS.h
index f1e97dfa7d0352a3e1c1e25f7818051b328f97af..fd556f6ef36a17a88c538c62bc0eefe24fbffbd3 100644
--- a/trunk/fcl/include/fcl/BV/RSS.h
+++ b/trunk/fcl/include/fcl/BV/RSS.h
@@ -37,7 +37,7 @@
 #ifndef FCL_RSS_H
 #define FCL_RSS_H
 
-#include "fcl/BVH_internal.h"
+
 #include "fcl/math/vec_3f.h"
 #include "fcl/math/matrix_3f.h"
 #include <boost/math/constants/constants.hpp>
diff --git a/trunk/fcl/include/fcl/BV/kDOP.h b/trunk/fcl/include/fcl/BV/kDOP.h
index ecc5f900c53e1d73095541f8bad7786611f70a02..4560270c29f9e498c9c891bbf1715afbdd2a20bd 100644
--- a/trunk/fcl/include/fcl/BV/kDOP.h
+++ b/trunk/fcl/include/fcl/BV/kDOP.h
@@ -37,7 +37,7 @@
 #ifndef FCL_KDOP_H
 #define FCL_KDOP_H
 
-#include "fcl/BVH_internal.h"
+
 #include "fcl/math/vec_3f.h"
 
 namespace fcl
diff --git a/trunk/fcl/include/fcl/BVH_front.h b/trunk/fcl/include/fcl/BVH/BVH_front.h
similarity index 100%
rename from trunk/fcl/include/fcl/BVH_front.h
rename to trunk/fcl/include/fcl/BVH/BVH_front.h
diff --git a/trunk/fcl/include/fcl/BVH_internal.h b/trunk/fcl/include/fcl/BVH/BVH_internal.h
similarity index 100%
rename from trunk/fcl/include/fcl/BVH_internal.h
rename to trunk/fcl/include/fcl/BVH/BVH_internal.h
diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH/BVH_model.h
similarity index 98%
rename from trunk/fcl/include/fcl/BVH_model.h
rename to trunk/fcl/include/fcl/BVH/BVH_model.h
index 63e91f16745c7c471d831feaae897ca2e81d9eea..1708098e69074b686c48a229dad9e1c73c7e1ab9 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH/BVH_model.h
@@ -38,11 +38,10 @@
 #define FCL_BVH_MODEL_H
 
 #include "fcl/collision_object.h"
-#include "fcl/BVH_internal.h"
-#include "fcl/BV.h"
-#include "fcl/BV_node.h"
-#include "fcl/BV_splitter.h"
-#include "fcl/BV_fitter.h"
+#include "fcl/BVH/BVH_internal.h"
+#include "fcl/BV/BV_node.h"
+#include "fcl/BVH/BV_splitter.h"
+#include "fcl/BVH/BV_fitter.h"
 #include <vector>
 #include <boost/shared_ptr.hpp>
 
diff --git a/trunk/fcl/include/fcl/BVH_utility.h b/trunk/fcl/include/fcl/BVH/BVH_utility.h
similarity index 99%
rename from trunk/fcl/include/fcl/BVH_utility.h
rename to trunk/fcl/include/fcl/BVH/BVH_utility.h
index 1025a4eb0216c5c6a3b3e09df7f5c4a24826f3dd..5937b46948259105effe153d3c714a2bf15ece32 100644
--- a/trunk/fcl/include/fcl/BVH_utility.h
+++ b/trunk/fcl/include/fcl/BVH/BVH_utility.h
@@ -38,7 +38,7 @@
 #ifndef FCL_BVH_UTILITY_H
 #define FCL_BVH_UTILITY_H
 
-#include "fcl/BVH_model.h"
+#include "fcl/BVH/BVH_model.h"
 
 
 namespace fcl
diff --git a/trunk/fcl/include/fcl/BV_fitter.h b/trunk/fcl/include/fcl/BVH/BV_fitter.h
similarity index 99%
rename from trunk/fcl/include/fcl/BV_fitter.h
rename to trunk/fcl/include/fcl/BVH/BV_fitter.h
index d5380da39db1a9eca087d190dce644a38376b3e9..8cd548528a3cfce43b849ab1b9d4bb76dfa18b3f 100644
--- a/trunk/fcl/include/fcl/BV_fitter.h
+++ b/trunk/fcl/include/fcl/BVH/BV_fitter.h
@@ -38,8 +38,7 @@
 #ifndef FCL_BV_FITTER_H
 #define FCL_BV_FITTER_H
 
-#include "fcl/BVH_internal.h"
-#include "fcl/data_types.h"
+#include "fcl/BVH/BVH_internal.h"
 #include "fcl/BV/kIOS.h"
 #include "fcl/BV/OBBRSS.h"
 #include <iostream>
diff --git a/trunk/fcl/include/fcl/BV_splitter.h b/trunk/fcl/include/fcl/BVH/BV_splitter.h
similarity index 98%
rename from trunk/fcl/include/fcl/BV_splitter.h
rename to trunk/fcl/include/fcl/BVH/BV_splitter.h
index 8f1b6094f889cf11ac15c5db75bcf6e1ac11ec3f..39e819d70c963cc7d402ae974599c20ee2c53c96 100644
--- a/trunk/fcl/include/fcl/BV_splitter.h
+++ b/trunk/fcl/include/fcl/BVH/BV_splitter.h
@@ -37,11 +37,7 @@
 #ifndef FCL_BV_SPLITTER_H
 #define FCL_BV_SPLITTER_H
 
-
-#include "fcl/BVH_internal.h"
-#include "fcl/data_types.h"
-#include "fcl/BV/OBB.h"
-#include "fcl/BV/RSS.h"
+#include "fcl/BVH/BVH_internal.h"
 #include "fcl/BV/kIOS.h"
 #include "fcl/BV/OBBRSS.h"
 #include <vector>
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
index db8b49a04045d2843a0bc2ffbb3b7f147c87b9b8..2e44690d4a7782f1152e1602e2b42040defd24d1 100644
--- a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
@@ -41,8 +41,8 @@
 #include "fcl/broadphase/broadphase.h"
 #include "fcl/broadphase/hierarchy_tree.h"
 #include "fcl/octree.h"
-#include "fcl/BV.h"
-#include "fcl/geometric_shapes_utility.h"
+#include "fcl/BV/BV.h"
+#include "fcl/shape/geometric_shapes_utility.h"
 #include <boost/unordered_map.hpp>
 #include <boost/bind.hpp>
 #include <limits>
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
index de146fe2e2730253fa57adfccd5e56c6f4c4f194..f5c6640625a282aec670159e1b6737ecc45aa513 100644
--- a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
@@ -40,8 +40,8 @@
 #include "fcl/broadphase/broadphase.h"
 #include "fcl/broadphase/hierarchy_tree.h"
 #include "fcl/octree.h"
-#include "fcl/BV.h"
-#include "fcl/geometric_shapes_utility.h"
+#include "fcl/BV/BV.h"
+#include "fcl/shape/geometric_shapes_utility.h"
 #include <boost/unordered_map.hpp>
 #include <boost/bind.hpp>
 #include <limits>
diff --git a/trunk/fcl/include/fcl/ccd/interval.h b/trunk/fcl/include/fcl/ccd/interval.h
index 6870ba0011be5dd6027537b008d8a6bd9a77568d..e6e96b69fee52aa59206c2730018337433077367 100644
--- a/trunk/fcl/include/fcl/ccd/interval.h
+++ b/trunk/fcl/include/fcl/ccd/interval.h
@@ -38,8 +38,7 @@
 #ifndef FCL_INTERVAL_H
 #define FCL_INTERVAL_H
 
-#include "fcl/BVH_internal.h"
-#include <cstdlib>
+#include "fcl/data_types.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/include/fcl/collision_node.h b/trunk/fcl/include/fcl/collision_node.h
index 3774dcbb5d514c4e58cdc3b9497b59a5fb0b1a69..4bc7b01524b7e5da7e5602c24f029c564673ab13 100644
--- a/trunk/fcl/include/fcl/collision_node.h
+++ b/trunk/fcl/include/fcl/collision_node.h
@@ -38,9 +38,9 @@
 #ifndef FCL_COLLISION_NODE_H
 #define FCL_COLLISION_NODE_H
 
-#include "fcl/traversal_node_base.h"
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/BVH_front.h"
+#include "fcl/traversal/traversal_node_base.h"
+#include "fcl/traversal/traversal_node_bvhs.h"
+#include "fcl/BVH/BVH_front.h"
 
 
 
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index 16a1ec34bcaed95a4798d1493395a5faa851500b..13d282f85418c1c6d4e2dce8717047d0fae2f215 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -38,9 +38,6 @@
 #define FCL_INTERSECT_H
 
 #include "fcl/math/transform.h"
-#include "fcl/BVH_internal.h"
-#include "fcl/data_types.h"
-
 
 namespace fcl
 {
diff --git a/trunk/fcl/include/fcl/math/vec_3f.h b/trunk/fcl/include/fcl/math/vec_3f.h
index fb6e67215d9115eb6cbb09f65e280f6dd2390b18..37e0c5969cb5120a5a2a8e0bd4dc1d580cc52176 100644
--- a/trunk/fcl/include/fcl/math/vec_3f.h
+++ b/trunk/fcl/include/fcl/math/vec_3f.h
@@ -37,7 +37,7 @@
 #ifndef FCL_VEC_3F_H
 #define FCL_VEC_3F_H
 
-#include "fcl/BVH_internal.h"
+#include "fcl/data_types.h"
 #include "fcl/math/math_details.h"
 #include "fcl/simd/math_simd_details.h"
 #include <cmath>
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk.h b/trunk/fcl/include/fcl/narrowphase/gjk.h
index 1943851fd19366b94cc651160b272a8ca7aaec3d..3cebbd8457c11c7b249a3826363367539eed4cf9 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk.h
@@ -37,7 +37,7 @@
 #ifndef FCL_GJK_H
 #define FCL_GJK_H
 
-#include "fcl/geometric_shapes.h"
+#include "fcl/shape/geometric_shapes.h"
 #include "fcl/math/transform.h"
 
 namespace fcl
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
index 985d8d6d8c99ea0aed9839d90800ceb8504b75f7..20c24709e2191b4df3c6a60c585d4f48210974c5 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
@@ -38,7 +38,7 @@
 #ifndef FCL_GJK_LIBCCD_H
 #define FCL_GJK_LIBCCD_H
 
-#include "fcl/geometric_shapes.h"
+#include "fcl/shape/geometric_shapes.h"
 #include "fcl/math/transform.h"
 
 #include <ccd/ccd.h>
diff --git a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h b/trunk/fcl/include/fcl/shape/geometric_shape_to_BVH_model.h
similarity index 99%
rename from trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
rename to trunk/fcl/include/fcl/shape/geometric_shape_to_BVH_model.h
index 628c1f240e6c2f9fdb7eac63c24caac099a06791..7a9f83f90998c6a4cad716f6e3090ef6352a1268 100644
--- a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/trunk/fcl/include/fcl/shape/geometric_shape_to_BVH_model.h
@@ -38,8 +38,8 @@
 #ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H
 #define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
 
-#include "fcl/geometric_shapes.h"
-#include "fcl/BVH_model.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "fcl/BVH/BVH_model.h"
 #include <boost/math/constants/constants.hpp>
 
 namespace fcl
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/shape/geometric_shapes.h
similarity index 100%
rename from trunk/fcl/include/fcl/geometric_shapes.h
rename to trunk/fcl/include/fcl/shape/geometric_shapes.h
diff --git a/trunk/fcl/include/fcl/geometric_shapes_utility.h b/trunk/fcl/include/fcl/shape/geometric_shapes_utility.h
similarity index 98%
rename from trunk/fcl/include/fcl/geometric_shapes_utility.h
rename to trunk/fcl/include/fcl/shape/geometric_shapes_utility.h
index 68cb2869521364082f0616fa148db383539e761b..018a01876505b54c9695a8ce155309f002101809 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/shape/geometric_shapes_utility.h
@@ -39,8 +39,8 @@
 #define FCL_GEOMETRIC_SHAPES_UTILITY_H
 
 #include <vector>
-#include "fcl/geometric_shapes.h"
-#include "fcl/BV.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "fcl/BV/BV.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal/traversal_node_base.h
similarity index 100%
rename from trunk/fcl/include/fcl/traversal_node_base.h
rename to trunk/fcl/include/fcl/traversal/traversal_node_base.h
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal/traversal_node_bvh_shape.h
similarity index 99%
rename from trunk/fcl/include/fcl/traversal_node_bvh_shape.h
rename to trunk/fcl/include/fcl/traversal/traversal_node_bvh_shape.h
index 73010b8a74e19532d5944022668b86611cc145f5..d2bd5cfa4fe33f1c0d493dc48d6e0e9dfe9675d5 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_node_bvh_shape.h
@@ -39,10 +39,11 @@
 #define FCL_TRAVERSAL_NODE_MESH_SHAPE_H
 
 #include "fcl/collision_data.h"
-#include "fcl/geometric_shapes.h"
-#include "fcl/traversal_node_base.h"
-#include "fcl/BVH_model.h"
-#include "fcl/geometric_shapes_utility.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "fcl/shape/geometric_shapes_utility.h"
+#include "fcl/traversal/traversal_node_base.h"
+#include "fcl/BVH/BVH_model.h"
+
 
 namespace fcl
 {
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal/traversal_node_bvhs.h
similarity index 99%
rename from trunk/fcl/include/fcl/traversal_node_bvhs.h
rename to trunk/fcl/include/fcl/traversal/traversal_node_bvhs.h
index e43f7e94faf19a45fcc5933bf6f8158fbfb31583..5e9dd63c0a34c2a8fdac4f753f243e2b56daaaef 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_node_bvhs.h
@@ -39,10 +39,10 @@
 #define FCL_TRAVERSAL_NODE_MESHES_H
 
 #include "fcl/collision_data.h"
-#include "fcl/traversal_node_base.h"
-#include "fcl/BV_node.h"
-#include "fcl/BV.h"
-#include "fcl/BVH_model.h"
+#include "fcl/traversal/traversal_node_base.h"
+#include "fcl/BV/BV_node.h"
+#include "fcl/BV/BV.h"
+#include "fcl/BVH/BVH_model.h"
 #include "fcl/intersect.h"
 #include "fcl/ccd/motion.h"
 
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal/traversal_node_octree.h
similarity index 99%
rename from trunk/fcl/include/fcl/traversal_node_octree.h
rename to trunk/fcl/include/fcl/traversal/traversal_node_octree.h
index 779df4a41df58f58810f74bddcb853a39df576c0..f9a54216056379974d9a318d55e0715af7d13127 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_node_octree.h
@@ -38,11 +38,11 @@
 #define FCL_TRAVERSAL_NODE_OCTREE_H
 
 #include "fcl/collision_data.h"
-#include "fcl/traversal_node_base.h"
+#include "fcl/traversal/traversal_node_base.h"
 #include "fcl/narrowphase/narrowphase.h"
-#include "fcl/geometric_shapes_utility.h"
+#include "fcl/shape/geometric_shapes_utility.h"
 #include "fcl/octree.h"
-#include "fcl/BVH_model.h"
+#include "fcl/BVH/BVH_model.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/include/fcl/traversal_node_setup.h b/trunk/fcl/include/fcl/traversal/traversal_node_setup.h
similarity index 99%
rename from trunk/fcl/include/fcl/traversal_node_setup.h
rename to trunk/fcl/include/fcl/traversal/traversal_node_setup.h
index cd29c3797a76e47f4e4d65c8657dcf99904b43cc..4a231f25507e86731d22864fb35a088002b7caa4 100644
--- a/trunk/fcl/include/fcl/traversal_node_setup.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_node_setup.h
@@ -38,11 +38,11 @@
 #ifndef FCL_TRAVERSAL_NODE_SETUP_H
 #define FCL_TRAVERSAL_NODE_SETUP_H
 
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/traversal_node_shapes.h"
-#include "fcl/traversal_node_bvh_shape.h"
-#include "fcl/traversal_node_octree.h"
-#include "fcl/BVH_utility.h"
+#include "fcl/traversal/traversal_node_bvhs.h"
+#include "fcl/traversal/traversal_node_shapes.h"
+#include "fcl/traversal/traversal_node_bvh_shape.h"
+#include "fcl/traversal/traversal_node_octree.h"
+#include "fcl/BVH/BVH_utility.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal/traversal_node_shapes.h
similarity index 98%
rename from trunk/fcl/include/fcl/traversal_node_shapes.h
rename to trunk/fcl/include/fcl/traversal/traversal_node_shapes.h
index 0ea0845aa1e97d9bc4fd709c848de533f5df3c69..f015079da9ed8ac1c4868bf00320691d6d14c924 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_node_shapes.h
@@ -39,9 +39,9 @@
 #define FCL_TRAVERSAL_NODE_SHAPES_H
 
 #include "fcl/collision_data.h"
-#include "fcl/traversal_node_base.h"
+#include "fcl/traversal/traversal_node_base.h"
 #include "fcl/narrowphase/narrowphase.h"
-#include "fcl/geometric_shapes_utility.h"
+#include "fcl/shape/geometric_shapes_utility.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/include/fcl/traversal_recurse.h b/trunk/fcl/include/fcl/traversal/traversal_recurse.h
similarity index 96%
rename from trunk/fcl/include/fcl/traversal_recurse.h
rename to trunk/fcl/include/fcl/traversal/traversal_recurse.h
index ef8b59193fcff55bfa104fafdddd1212d60c21ab..bf621b469c0e540feb7dd9e697fdeeded4eaa3b6 100644
--- a/trunk/fcl/include/fcl/traversal_recurse.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_recurse.h
@@ -38,9 +38,9 @@
 #ifndef FCL_TRAVERSAL_RECURSE_H
 #define FCL_TRAVERSAL_RECURSE_H
 
-#include "fcl/traversal_node_base.h"
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/BVH_front.h"
+#include "fcl/traversal/traversal_node_base.h"
+#include "fcl/traversal/traversal_node_bvhs.h"
+#include "fcl/BVH/BVH_front.h"
 #include <queue>
 
 namespace fcl
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index 6df3e995af9c5c7a3a61abb77aef595eb5d13c4e..d83f8533b94a0b7e3c6dd3dd16cf0a2b188cff88 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -35,7 +35,7 @@
 /** \author Jia Pan */
 
 #include "fcl/BV/OBB.h"
-#include "fcl/BVH_utility.h"
+#include "fcl/BVH/BVH_utility.h"
 #include "fcl/math/transform.h"
 
 #include <iostream>
diff --git a/trunk/fcl/src/BV/RSS.cpp b/trunk/fcl/src/BV/RSS.cpp
index ee296fcee80eddf9d5ff50fbbc2039963fc7834a..363e752d590d8dca0177ea5a2d2b799facb056fa 100644
--- a/trunk/fcl/src/BV/RSS.cpp
+++ b/trunk/fcl/src/BV/RSS.cpp
@@ -35,7 +35,7 @@
 /** \author Jia Pan */
 
 #include "fcl/BV/RSS.h"
-#include "fcl/BVH_utility.h"
+#include "fcl/BVH/BVH_utility.h"
 #include <iostream>
 namespace fcl
 {
diff --git a/trunk/fcl/src/BV/kIOS.cpp b/trunk/fcl/src/BV/kIOS.cpp
index 3960ad5c331962cb0b73f860f8af7caaf152af43..2dbdce96cdba5e08c6d0e01cf86f50afe5923108 100644
--- a/trunk/fcl/src/BV/kIOS.cpp
+++ b/trunk/fcl/src/BV/kIOS.cpp
@@ -35,7 +35,7 @@
 /** \author Jia Pan */
 
 #include "fcl/BV/kIOS.h"
-#include "fcl/BVH_utility.h"
+#include "fcl/BVH/BVH_utility.h"
 #include "fcl/math/transform.h"
 
 #include <iostream>
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH/BVH_model.cpp
similarity index 99%
rename from trunk/fcl/src/BVH_model.cpp
rename to trunk/fcl/src/BVH/BVH_model.cpp
index 73f768bc48a22ec00f190fba3471240b01eab5ff..8b45dcef772ad7fd43b86239bc6397f292554ee9 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH/BVH_model.cpp
@@ -34,8 +34,8 @@
 
 /** \author Jia Pan */
 
-#include "fcl/BVH_model.h"
-#include "fcl/BV.h"
+#include "fcl/BVH/BVH_model.h"
+#include "fcl/BV/BV.h"
 #include <iostream>
 #include <string.h>
 
diff --git a/trunk/fcl/src/BVH_utility.cpp b/trunk/fcl/src/BVH/BVH_utility.cpp
similarity index 99%
rename from trunk/fcl/src/BVH_utility.cpp
rename to trunk/fcl/src/BVH/BVH_utility.cpp
index 12ab357f0c3e24e81921711a50b30905ea5ae834..4377ad35ded008026e73e2e758ffa4701e87c58d 100644
--- a/trunk/fcl/src/BVH_utility.cpp
+++ b/trunk/fcl/src/BVH/BVH_utility.cpp
@@ -35,7 +35,7 @@
 /** \author Jia Pan */
 
 
-#include "fcl/BVH_utility.h"
+#include "fcl/BVH/BVH_utility.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/src/BV_fitter.cpp b/trunk/fcl/src/BVH/BV_fitter.cpp
similarity index 99%
rename from trunk/fcl/src/BV_fitter.cpp
rename to trunk/fcl/src/BVH/BV_fitter.cpp
index 68859b61f6239e4860fe40f6837ba8c9f24ea826..7455f53ba925cfcc32f35cdddac22708966d6145 100644
--- a/trunk/fcl/src/BV_fitter.cpp
+++ b/trunk/fcl/src/BVH/BV_fitter.cpp
@@ -34,8 +34,8 @@
 
 /** \author Jia Pan */
 
-#include "fcl/BV_fitter.h"
-#include "fcl/BVH_utility.h"
+#include "fcl/BVH/BV_fitter.h"
+#include "fcl/BVH/BVH_utility.h"
 #include <limits>
 
 namespace fcl
diff --git a/trunk/fcl/src/BV_splitter.cpp b/trunk/fcl/src/BVH/BV_splitter.cpp
similarity index 99%
rename from trunk/fcl/src/BV_splitter.cpp
rename to trunk/fcl/src/BVH/BV_splitter.cpp
index b657363e2fd744ab1df3ce0ccd028ce53185b99c..847ef9e9f377700fc1d4f39d552975567484ca99 100644
--- a/trunk/fcl/src/BV_splitter.cpp
+++ b/trunk/fcl/src/BVH/BV_splitter.cpp
@@ -34,7 +34,7 @@
 
 /** \author Jia Pan */
 
-#include "fcl/BV_splitter.h"
+#include "fcl/BVH/BV_splitter.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/src/ccd/conservative_advancement.cpp b/trunk/fcl/src/ccd/conservative_advancement.cpp
index 0eb67a299ffaefea7044b5aca3e3d0248f4e0240..298397b762ec4d888ca4b31c9fee262b1cde8c91 100644
--- a/trunk/fcl/src/ccd/conservative_advancement.cpp
+++ b/trunk/fcl/src/ccd/conservative_advancement.cpp
@@ -37,12 +37,14 @@
 #include "fcl/ccd/conservative_advancement.h"
 #include "fcl/ccd/motion.h"
 #include "fcl/collision_node.h"
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/traversal_node_setup.h"
+#include "fcl/traversal/traversal_node_bvhs.h"
+#include "fcl/traversal/traversal_node_setup.h"
+#include "fcl/traversal/traversal_recurse.h"
 #include "fcl/collision.h"
-#include "fcl/traversal_recurse.h"
+
 
 namespace fcl
+
 {
 
 template<typename BV>
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 48b3ae5690c9183e598d68d42dde6e7ea4c3d240..742ef826ac665b574cd7e31c12bf7011144a923e 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -37,7 +37,7 @@
 
 #include "fcl/collision_func_matrix.h"
 
-#include "fcl/traversal_node_setup.h"
+#include "fcl/traversal/traversal_node_setup.h"
 #include "fcl/collision_node.h"
 #include "fcl/narrowphase/narrowphase.h"
 
diff --git a/trunk/fcl/src/collision_node.cpp b/trunk/fcl/src/collision_node.cpp
index ef1eb832861cdaa4d3572dbf92e23801cc4de798..3a83d1a301a409c120a460065bc6600b9a4d068a 100644
--- a/trunk/fcl/src/collision_node.cpp
+++ b/trunk/fcl/src/collision_node.cpp
@@ -36,7 +36,7 @@
 
 
 #include "fcl/collision_node.h"
-#include "fcl/traversal_recurse.h"
+#include "fcl/traversal/traversal_recurse.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index d09f82e32bfc97befacdedcda421556b3821c905..d5a5509530e952ba54ff985e20038e853083ddb9 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -37,7 +37,7 @@
 #include "fcl/distance_func_matrix.h"
 
 #include "fcl/collision_node.h"
-#include "fcl/traversal_node_setup.h"
+#include "fcl/traversal/traversal_node_setup.h"
 #include "fcl/narrowphase/narrowphase.h"
 
 namespace fcl
diff --git a/trunk/fcl/src/geometric_shapes.cpp b/trunk/fcl/src/shape/geometric_shapes.cpp
similarity index 98%
rename from trunk/fcl/src/geometric_shapes.cpp
rename to trunk/fcl/src/shape/geometric_shapes.cpp
index dec91fb82095e1f61eae932b33d2b8cdfae6c12b..3d74f0ed8cdece5504eef39941f65a880bc4c478 100644
--- a/trunk/fcl/src/geometric_shapes.cpp
+++ b/trunk/fcl/src/shape/geometric_shapes.cpp
@@ -35,8 +35,8 @@
 /** \author Jia Pan */
 
 
-#include "fcl/geometric_shapes.h"
-#include "fcl/geometric_shapes_utility.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "fcl/shape/geometric_shapes_utility.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/shape/geometric_shapes_utility.cpp
similarity index 99%
rename from trunk/fcl/src/geometric_shapes_utility.cpp
rename to trunk/fcl/src/shape/geometric_shapes_utility.cpp
index 6a88184aebfc05aa4c68f8ff196100d981e35f6e..b12aa52befb8d66fecb5d3a8756718d5455e58fa 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/shape/geometric_shapes_utility.cpp
@@ -35,8 +35,8 @@
 /** \author Jia Pan */
 
 
-#include "fcl/geometric_shapes_utility.h"
-#include "fcl/BV_fitter.h"
+#include "fcl/shape/geometric_shapes_utility.h"
+#include "fcl/BVH/BV_fitter.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/src/traversal_node_base.cpp b/trunk/fcl/src/traversal/traversal_node_base.cpp
similarity index 98%
rename from trunk/fcl/src/traversal_node_base.cpp
rename to trunk/fcl/src/traversal/traversal_node_base.cpp
index ebcb1a6a67aa7658dbd2f7afe1c64225e16c7216..f228395d072e6dbcad3eba4e8dc254ea177e4a69 100644
--- a/trunk/fcl/src/traversal_node_base.cpp
+++ b/trunk/fcl/src/traversal/traversal_node_base.cpp
@@ -35,7 +35,7 @@
 /** \author Jia Pan */
 
 
-#include "fcl/traversal_node_base.h"
+#include "fcl/traversal/traversal_node_base.h"
 #include <limits>
 
 namespace fcl
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal/traversal_node_bvhs.cpp
similarity index 99%
rename from trunk/fcl/src/traversal_node_bvhs.cpp
rename to trunk/fcl/src/traversal/traversal_node_bvhs.cpp
index 637d5f1424cd2f3f65381bf91e23359c93706e4e..b9f8a9d6068c3c0a1f3a92718af0dbee87990943 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal/traversal_node_bvhs.cpp
@@ -35,7 +35,7 @@
 /** \author Jia Pan */
 
 
-#include "fcl/traversal_node_bvhs.h"
+#include "fcl/traversal/traversal_node_bvhs.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/src/traversal_node_setup.cpp b/trunk/fcl/src/traversal/traversal_node_setup.cpp
similarity index 99%
rename from trunk/fcl/src/traversal_node_setup.cpp
rename to trunk/fcl/src/traversal/traversal_node_setup.cpp
index 46a25c22a97695464a97dda511d7f56096e076ea..88fc4de565174ea6fe9a9718db19b5fb971687e4 100644
--- a/trunk/fcl/src/traversal_node_setup.cpp
+++ b/trunk/fcl/src/traversal/traversal_node_setup.cpp
@@ -35,7 +35,7 @@
 /** \author Jia Pan */
 
 
-#include "fcl/traversal_node_setup.h"
+#include "fcl/traversal/traversal_node_setup.h"
 
 namespace fcl
 {
diff --git a/trunk/fcl/src/traversal_recurse.cpp b/trunk/fcl/src/traversal/traversal_recurse.cpp
similarity index 99%
rename from trunk/fcl/src/traversal_recurse.cpp
rename to trunk/fcl/src/traversal/traversal_recurse.cpp
index 5b4f4e65bf85ed8e8efe1435ac5faa284b00dab8..68ea258dce24bc35362f88145155c7af689dc486 100644
--- a/trunk/fcl/src/traversal_recurse.cpp
+++ b/trunk/fcl/src/traversal/traversal_recurse.cpp
@@ -35,7 +35,7 @@
 /** \author Jia Pan */
 
 
-#include "fcl/traversal_recurse.h"
+#include "fcl/traversal/traversal_recurse.h"
 
 namespace fcl
 {