diff --git a/trunk/fcl/include/fcl/data_types.h b/trunk/fcl/include/fcl/data_types.h
new file mode 100644
index 0000000000000000000000000000000000000000..1683592898483213c408cda02e82025a4fbaf319
--- /dev/null
+++ b/trunk/fcl/include/fcl/data_types.h
@@ -0,0 +1,48 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_DATA_TYPES_H
+#define FCL_DATA_TYPES_H
+
+#include <inttypes.h>
+
+typedef double FCL_REAL;
+typedef uint64_t FCL_INT64;
+typedef int64_t FCL_UINT64;
+typedef unsigned int FCL_UINT32;
+typedef int FCL_INT32;
+
+#endif
diff --git a/trunk/fcl/include/fcl/morton.h b/trunk/fcl/include/fcl/morton.h
new file mode 100644
index 0000000000000000000000000000000000000000..b409c16ec75b9057fa2dcb4f8ef1e8535ae0e5e7
--- /dev/null
+++ b/trunk/fcl/include/fcl/morton.h
@@ -0,0 +1,174 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_MORTON_H
+#define FCL_MORTON_H
+
+#include <boost/dynamic_bitset.hpp>
+#include "fcl/data_types.h"
+#include "fcl/BV/AABB.h"
+
+namespace fcl
+{
+
+static inline FCL_UINT32 quantize(FCL_REAL x, FCL_UINT32 n)
+{
+ return std::max(std::min((FCL_UINT32)(x * (FCL_REAL)n), FCL_UINT32(n-1)), FCL_UINT32(0));
+}
+
+static inline FCL_UINT32 morton_code(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z)
+{
+ x = (x | (x << 16)) & 0x030000FF;
+ x = (x | (x << 8)) & 0x0300F00F;
+ x = (x | (x << 4)) & 0x030C30C3;
+ x = (x | (x << 2)) & 0x09249249;
+
+ y = (y | (y << 16)) & 0x030000FF;
+ y = (y | (y << 8)) & 0x0300F00F;
+ y = (y | (y << 4)) & 0x030C30C3;
+ y = (y | (y << 2)) & 0x09249249;
+
+ z = (z | (z << 16)) & 0x030000FF;
+ z = (z | (z << 8)) & 0x0300F00F;
+ z = (z | (z << 4)) & 0x030C30C3;
+ z = (z | (z << 2)) & 0x09249249;
+
+ return x | (y << 1) | (z << 2);
+}
+
+static inline FCL_UINT64 morton_code60(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z)
+{
+ FCL_UINT32 lo_x = x & 1023u;
+ FCL_UINT32 lo_y = y & 1023u;
+ FCL_UINT32 lo_z = z & 1023u;
+ FCL_UINT32 hi_x = x >> 10u;
+ FCL_UINT32 hi_y = y >> 10u;
+ FCL_UINT32 hi_z = z >> 10u;
+
+ return (FCL_UINT64(morton_code(hi_x, hi_y, hi_z)) << 30) | FCL_UINT64(morton_code(lo_x, lo_y, lo_z));
+}
+
+template<typename T>
+struct morton_functor {};
+
+template<>
+struct morton_functor<FCL_UINT32>
+{
+ 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]))
+ {}
+
+ FCL_UINT32 operator() (const Vec3f& point) const
+ {
+ FCL_UINT32 x = quantize((point[0] - base[0]) * inv[0], 1024u);
+ FCL_UINT32 y = quantize((point[1] - base[1]) * inv[1], 1024u);
+ FCL_UINT32 z = quantize((point[2] - base[2]) * inv[2], 1024u);
+
+ return morton_code(x, y, z);
+ }
+
+ const Vec3f base;
+ const Vec3f inv;
+};
+
+
+template<>
+struct morton_functor<FCL_UINT64>
+{
+ 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]))
+ {}
+
+ FCL_UINT64 operator() (const Vec3f& point) const
+ {
+ FCL_UINT32 x = quantize((point[0] - base[0]) * inv[0], 1u << 20);
+ FCL_UINT32 y = quantize((point[1] - base[1]) * inv[1], 1u << 20);
+ FCL_UINT32 z = quantize((point[2] - base[2]) * inv[2], 1u << 20);
+
+ return morton_code60(x, y, z);
+ }
+
+ const Vec3f base;
+ const Vec3f inv;
+};
+
+template<>
+struct morton_functor<boost::dynamic_bitset<> >
+{
+ morton_functor(const AABB& bbox, size_t bit_num_) : 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])),
+ bit_num(bit_num_)
+ {}
+
+ boost::dynamic_bitset<> operator() (const Vec3f& point) const
+ {
+ FCL_REAL x = (point[0] - base[0]) * inv[0];
+ FCL_REAL y = (point[1] - base[1]) * inv[1];
+ FCL_REAL z = (point[2] - base[2]) * inv[2];
+ int start_bit = bit_num * 3 - 1;
+ boost::dynamic_bitset<> bits(bit_num * 3);
+
+ x *= 2;
+ y *= 2;
+ z *= 2;
+
+ for(size_t i = 0; i < bit_num; ++i)
+ {
+ bits[start_bit--] = ((z < 1) ? 0 : 1);
+ bits[start_bit--] = ((y < 1) ? 0 : 1);
+ bits[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 bits;
+ }
+
+ const Vec3f base;
+ const Vec3f inv;
+ const size_t bit_num;
+};
+
+}
+
+#endif