Added libccd and FCL

3rdparty/fcl/.appveyor.yml

@@ -0,0 +1,60 @@
+# AppVeyor file
+# http://www.appveyor.com/docs/appveyor-yml
+
+version: "{build}"
+os: Visual Studio 2015
+
+clone_folder: C:\projects\fcl
+shallow_clone: true
+
+# build platform, i.e. Win32 (instead of x86), x64, Any CPU. This setting is optional.
+platform:
+  - Win32
+  - x64
+
+environment:
+  CTEST_OUTPUT_ON_FAILURE: 1
+
+cache:
+  - C:\Program Files\libccd -> .appveyor.yml
+
+configuration:
+  - Debug
+  - Release
+
+before_build:
+  - cmd: if "%platform%"=="Win32" set CMAKE_GENERATOR_NAME=Visual Studio 14 2015
+  - cmd: if "%platform%"=="x64"   set CMAKE_GENERATOR_NAME=Visual Studio 14 2015 Win64
+  - cmd: if "%platform%"=="Win32" set PROGRAM_FILES_PATH=Program Files (x86)
+  - cmd: if "%platform%"=="x64"   set PROGRAM_FILES_PATH=Program Files
+  - cmd: if not exist C:\"Program Files"\libccd\lib\ccd.lib (
+            curl -L -o libccd-2.0.tar.gz https://github.com/danfis/libccd/archive/v2.0.tar.gz &&
+            cmake -E tar zxf libccd-2.0.tar.gz &&
+            cd libccd-2.0 &&
+            mkdir build &&
+            cd build &&
+            cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% -DCCD_DOUBLE=True .. &&
+            cmake --build . --target install --config %Configuration% &&
+            cd ..\..
+         ) else (echo Using cached libccd)
+  - cmd: if not exist C:\"Program Files"\Eigen\include\eigen3\Eigen\Core (
+            curl -L -o eigen-eigen-dc6cfdf9bcec.tar.gz https://bitbucket.org/eigen/eigen/get/3.2.9.tar.gz &&
+            cmake -E tar zxf eigen-eigen-dc6cfdf9bcec.tar.gz &&
+            cd eigen-eigen-dc6cfdf9bcec &&
+            mkdir build &&
+            cd build &&
+            cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% .. &&
+            cmake --build . --target install --config %Configuration% &&
+            cd ..\..
+         ) else (echo Using cached Eigen3)
+  - cmd: set
+  - cmd: mkdir build
+  - cmd: cd build
+  - cmd: cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% -DCCD_INCLUDE_DIR="C:\%PROGRAM_FILES_PATH%\libccd\include" -DCCD_LIBRARY="C:\%PROGRAM_FILES_PATH%\libccd\lib\ccd.lib" -DEIGEN3_INCLUDE_DIR="C:\%PROGRAM_FILES_PATH%\Eigen\include\eigen3" ..
+
+build:
+  project: C:\projects\fcl\build\fcl.sln
+  parallel: true
+
+test_script:
+  - cmd: ctest -C %Configuration%

3rdparty/fcl/.clang-format

@@ -0,0 +1,40 @@
+# -*- mode: yaml -*-
+# vi: set ft=yaml :
+
+# Copyright (c) 2018, Toyota Research Institute, 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 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 HOLDER 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: Jamie Snape, Kitware, Inc.
+
+---
+BasedOnStyle: Google
+---
+Language: Cpp
+DerivePointerAlignment: false
+PointerAlignment: Left

3rdparty/fcl/.clang-tidy

@@ -0,0 +1,36 @@
+# -*- mode: yaml -*-
+# vi: set ft=yaml :
+
+# Copyright (c) 2018, Toyota Research Institute, 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 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 HOLDER 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: Jamie Snape, Kitware, Inc.
+
+---
+Checks: 'clang-analyzer-*,clang-diagnostic-*,cppcoreguidelines-*,google-*,modernize-*,performance-*,readability-*'

3rdparty/fcl/.deprecated/articulated_model/joint.h

@@ -0,0 +1,375 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_JOINT_H
+#define FCL_ARTICULATED_MODEL_JOINT_H
+
+#include "fcl/common/data_types.h"
+
+#include <string>
+#include <vector>
+#include <map>
+#include <limits>
+#include <memory>
+
+namespace fcl
+{
+
+class JointConfig;
+class Link;
+
+enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER};
+
+/// @brief Base Joint
+template <typename S>
+class Joint
+{
+public:
+
+  Joint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
+        const Transform3<S>& transform_to_parent,
+        const std::string& name);
+
+  Joint(const std::string& name);
+
+  virtual ~Joint() {}
+
+  const std::string& getName() const;
+  void setName(const std::string& name);
+
+  virtual Transform3<S> getLocalTransform() const = 0;
+
+  virtual std::size_t getNumDofs() const = 0;
+
+  std::shared_ptr<JointConfig> getJointConfig() const;
+  void setJointConfig(const std::shared_ptr<JointConfig>& joint_cfg);
+
+  std::shared_ptr<Link> getParentLink() const;
+  std::shared_ptr<Link> getChildLink() const;
+
+  void setParentLink(const std::shared_ptr<Link>& link);
+  void setChildLink(const std::shared_ptr<Link>& link);
+
+  JointType getJointType() const;
+
+  const Transform3<S>& getTransformToParent() const;
+  void setTransformToParent(const Transform3<S>& t);
+  
+protected:
+
+  /// links to parent and child are only for connection, so weak_ptr to avoid cyclic dependency
+  std::weak_ptr<Link> link_parent_, link_child_;
+
+  JointType type_;
+
+  std::string name_;
+  
+  std::shared_ptr<JointConfig> joint_cfg_;
+
+  Transform3<S> transform_to_parent_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template <typename S>
+class PrismaticJoint : public Joint
+{
+public:
+  PrismaticJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
+                 const Transform3<S>& transform_to_parent,
+                 const std::string& name,
+                 const Vector3<S>& axis);
+
+  virtual ~PrismaticJoint() {}
+
+  Transform3<S> getLocalTransform() const;
+
+  std::size_t getNumDofs() const;
+
+  const Vector3<S>& getAxis() const;
+
+protected:
+  Vector3<S> axis_;
+};
+
+template <typename S>
+class RevoluteJoint : public Joint
+{
+public:
+  RevoluteJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
+                const Transform3<S>& transform_to_parent,
+                const std::string& name,
+                const Vector3<S>& axis);
+
+  virtual ~RevoluteJoint() {}
+
+  Transform3<S> getLocalTransform() const;
+
+  std::size_t getNumDofs() const;
+
+  const Vector3<S>& getAxis() const;
+
+protected:
+  Vector3<S> axis_;
+};
+
+template <typename S>
+class BallEulerJoint : public Joint
+{
+public:
+  BallEulerJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
+                 const Transform3<S>& transform_to_parent,
+                 const std::string& name);
+
+  virtual ~BallEulerJoint() {}
+
+  std::size_t getNumDofs() const;
+
+  Transform3<S> getLocalTransform() const;
+};
+
+//============================================================================//
+//                                                                            //
+//                              Implementations                               //
+//                                                                            //
+//============================================================================//
+
+//==============================================================================
+template <typename S>
+Joint<S>::Joint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
+             const Transform3<S>& transform_to_parent,
+             const std::string& name) :
+  link_parent_(link_parent), link_child_(link_child),
+  name_(name),
+  transform_to_parent_(transform_to_parent)
+{}
+
+//==============================================================================
+template <typename S>
+Joint<S>::Joint(const std::string& name) :
+  name_(name)
+{
+}
+
+//==============================================================================
+template <typename S>
+const std::string& Joint<S>::getName() const
+{
+  return name_;
+}
+
+//==============================================================================
+template <typename S>
+void Joint<S>::setName(const std::string& name)
+{
+  name_ = name;
+}
+
+//==============================================================================
+template <typename S>
+std::shared_ptr<JointConfig> Joint<S>::getJointConfig() const
+{
+  return joint_cfg_;
+}
+
+//==============================================================================
+template <typename S>
+void Joint<S>::setJointConfig(const std::shared_ptr<JointConfig>& joint_cfg)
+{
+  joint_cfg_ = joint_cfg;
+}
+
+//==============================================================================
+template <typename S>
+std::shared_ptr<Link> Joint<S>::getParentLink() const
+{
+  return link_parent_.lock();
+}
+
+//==============================================================================
+template <typename S>
+std::shared_ptr<Link> Joint<S>::getChildLink() const
+{
+  return link_child_.lock();
+}
+
+//==============================================================================
+template <typename S>
+void Joint<S>::setParentLink(const std::shared_ptr<Link>& link)
+{
+  link_parent_ = link;
+}
+
+//==============================================================================
+template <typename S>
+void Joint<S>::setChildLink(const std::shared_ptr<Link>& link)
+{
+  link_child_ = link;
+}
+
+//==============================================================================
+template <typename S>
+JointType Joint<S>::getJointType() const
+{
+  return type_;
+}
+
+//==============================================================================
+template <typename S>
+const Transform3<S>& Joint<S>::getTransformToParent() const
+{
+  return transform_to_parent_;
+}
+
+//==============================================================================
+template <typename S>
+void Joint<S>::setTransformToParent(const Transform3<S>& t)
+{
+  transform_to_parent_ = t;
+}
+
+//==============================================================================
+template <typename S>
+PrismaticJoint<S>::PrismaticJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
+                               const Transform3<S>& transform_to_parent,
+                               const std::string& name,
+                               const Vector3<S>& axis) :
+  Joint(link_parent, link_child, transform_to_parent, name),
+  axis_(axis)
+{
+  type_ = JT_PRISMATIC;
+}
+
+//==============================================================================
+template <typename S>
+const Vector3<S>& PrismaticJoint<S>::getAxis() const
+{
+  return axis_;
+}
+
+//==============================================================================
+template <typename S>
+std::size_t PrismaticJoint<S>::getNumDofs() const
+{
+  return 1;
+}
+
+//==============================================================================
+template <typename S>
+Transform3<S> PrismaticJoint<S>::getLocalTransform() const
+{
+  const Quaternion<S> quat(transform_to_parent_.linear());
+  const Vector3<S>& transl = transform_to_parent_.translation();
+
+  Transform3<S> tf = Transform3<S>::Identity();
+  tf.linear() = quat.toRotationMatrix();
+  tf.translation() = quat * (axis_ * (*joint_cfg_)[0]) + transl;
+
+  return tf;
+}
+
+//==============================================================================
+template <typename S>
+RevoluteJoint<S>::RevoluteJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
+                             const Transform3<S>& transform_to_parent,
+                             const std::string& name,
+                             const Vector3<S>& axis) :
+  Joint(link_parent, link_child, transform_to_parent, name),
+  axis_(axis)
+{
+  type_ = JT_REVOLUTE;
+}
+
+//==============================================================================
+template <typename S>
+const Vector3<S>& RevoluteJoint<S>::getAxis() const
+{
+  return axis_;
+}
+
+//==============================================================================
+template <typename S>
+std::size_t RevoluteJoint<S>::getNumDofs() const
+{
+  return 1;
+}
+
+//==============================================================================
+template <typename S>
+Transform3<S> RevoluteJoint<S>::getLocalTransform() const
+{
+  Transform3<S> tf = Transform3<S>::Identity();
+  tf.linear() = transform_to_parent_.linear() * AngleAxis<S>((*joint_cfg_)[0], axis_);
+  tf.translation() = transform_to_parent_.translation();
+
+  return tf;
+}
+
+//==============================================================================
+template <typename S>
+BallEulerJoint<S>::BallEulerJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
+                               const Transform3<S>& transform_to_parent,
+                               const std::string& name) :
+  Joint(link_parent, link_child, transform_to_parent, name)
+{}
+
+//==============================================================================
+template <typename S>
+std::size_t BallEulerJoint<S>::getNumDofs() const
+{
+  return 3;
+}
+
+//==============================================================================
+template <typename S>
+Transform3<S> BallEulerJoint<S>::getLocalTransform() const
+{
+  Matrix3<S> rot(
+      AngleAxis<S>((*joint_cfg_)[0], Eigen::Vector3<S>::UnitX())
+        * AngleAxis<S>((*joint_cfg_)[1], Eigen::Vector3<S>::UnitY())
+        * AngleAxis<S>((*joint_cfg_)[2], Eigen::Vector3<S>::UnitZ()));
+
+  Transform3<S> tf = Transform3<S>::Identity();
+  tf.linear() = rot;
+
+  return transform_to_parent_ * tf;
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/.deprecated/articulated_model/joint_config.h

@@ -0,0 +1,189 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
+#define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
+
+#include "fcl/common/data_types.h"
+#include <memory>
+#include <vector>
+
+namespace fcl
+{
+
+template <typename S>
+class Joint;
+
+template <typename S>
+class JointConfig
+{
+public:
+  JointConfig();
+
+  JointConfig(const JointConfig& joint_cfg);
+
+  JointConfig(const std::shared_ptr<Joint>& joint,
+              S default_value = 0,
+              S default_value_min = 0,
+              S default_value_max = 0);
+
+  std::size_t getDim() const;
+
+  inline S operator [] (std::size_t i) const
+  {
+    return values_[i];
+  }
+
+  inline S& operator [] (std::size_t i)
+  {
+    return values_[i];
+  }
+
+  S getValue(std::size_t i) const;
+
+  S& getValue(std::size_t i);
+  
+  S getLimitMin(std::size_t i) const;
+  
+  S& getLimitMin(std::size_t i);
+  
+  S getLimitMax(std::size_t i) const;
+  
+  S& getLimitMax(std::size_t i);
+  
+  std::shared_ptr<Joint> getJoint() const;
+
+private:
+  std::weak_ptr<Joint> joint_;
+  
+  std::vector<S> values_;
+  std::vector<S> limits_min_;
+  std::vector<S> limits_max_;
+};
+
+//============================================================================//
+//                                                                            //
+//                              Implementations                               //
+//                                                                            //
+//============================================================================//
+
+//==============================================================================
+template <typename S>
+JointConfig<S>::JointConfig() {}
+
+//==============================================================================
+template <typename S>
+JointConfig<S>::JointConfig(const JointConfig& joint_cfg) :
+  joint_(joint_cfg.joint_),
+  values_(joint_cfg.values_),
+  limits_min_(joint_cfg.limits_min_),
+  limits_max_(joint_cfg.limits_max_)
+{
+}
+
+//==============================================================================
+template <typename S>
+JointConfig<S>::JointConfig(const std::shared_ptr<Joint>& joint,
+                         S default_value,
+                         S default_value_min,
+                         S default_value_max) :
+  joint_(joint)
+{
+  values_.resize(joint->getNumDofs(), default_value);
+  limits_min_.resize(joint->getNumDofs(), default_value_min);
+  limits_max_.resize(joint->getNumDofs(), default_value_max);
+}
+
+//==============================================================================
+template <typename S>
+std::size_t JointConfig<S>::getDim() const
+{
+  return values_.size();
+}
+
+//==============================================================================
+template <typename S>
+S JointConfig<S>::getValue(std::size_t i) const
+{
+  return values_[i];
+}
+
+//==============================================================================
+template <typename S>
+S& JointConfig<S>::getValue(std::size_t i)
+{
+  return values_[i];
+}
+
+//==============================================================================
+template <typename S>
+S JointConfig<S>::getLimitMin(std::size_t i) const
+{
+  return limits_min_[i];
+}
+
+//==============================================================================
+template <typename S>
+S& JointConfig<S>::getLimitMin(std::size_t i)
+{
+  return limits_min_[i];
+}
+
+//==============================================================================
+template <typename S>
+S JointConfig<S>::getLimitMax(std::size_t i) const
+{
+  return limits_max_[i];
+}
+
+//==============================================================================
+template <typename S>
+S& JointConfig<S>::getLimitMax(std::size_t i)
+{
+  return limits_max_[i];
+}
+
+//==============================================================================
+template <typename S>
+std::shared_ptr<Joint> JointConfig<S>::getJoint() const
+{
+  return joint_.lock();
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/.deprecated/articulated_model/link.h

@@ -0,0 +1,145 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_LINK_H
+#define FCL_ARTICULATED_MODEL_LINK_H
+
+#include "fcl/common/data_types.h"
+#include "fcl/object/collision_object.h"
+
+#include <memory>
+#include <vector>
+
+namespace fcl
+{
+
+template <typename S>
+class Joint;
+
+template <typename S>
+class Link
+{
+public:
+  Link(const std::string& name);
+
+  const std::string& getName() const;
+  
+  void setName(const std::string& name);
+  
+  void addChildJoint(const std::shared_ptr<Joint>& joint);
+  
+  void setParentJoint(const std::shared_ptr<Joint>& joint);
+  
+  void addObject(const std::shared_ptr<CollisionObject<S>>& object);
+  
+  std::size_t getNumChildJoints() const;
+  
+  std::size_t getNumObjects() const;
+  
+protected:
+  std::string name_;
+
+  std::vector<std::shared_ptr<CollisionObject<S>> > objects_;
+
+  std::vector<std::shared_ptr<Joint> > children_joints_;
+
+  std::shared_ptr<Joint> parent_joint_;
+};
+
+//============================================================================//
+//                                                                            //
+//                              Implementations                               //
+//                                                                            //
+//============================================================================//
+
+//==============================================================================
+template <typename S>
+Link<S>::Link(const std::string& name) : name_(name)
+{}
+
+//==============================================================================
+template <typename S>
+const std::string& Link<S>::getName() const
+{
+  return name_;
+}
+
+//==============================================================================
+template <typename S>
+void Link<S>::setName(const std::string& name)
+{
+  name_ = name;
+}
+
+//==============================================================================
+template <typename S>
+void Link<S>::addChildJoint(const std::shared_ptr<Joint>& joint)
+{
+  children_joints_.push_back(joint);
+}
+
+//==============================================================================
+template <typename S>
+void Link<S>::setParentJoint(const std::shared_ptr<Joint>& joint)
+{
+  parent_joint_ = joint;
+}
+
+//==============================================================================
+template <typename S>
+void Link<S>::addObject(const std::shared_ptr<CollisionObject<S>>& object)
+{
+  objects_.push_back(object);
+}
+
+//==============================================================================
+template <typename S>
+std::size_t Link<S>::getNumChildJoints() const
+{
+  return children_joints_.size();
+}
+
+//==============================================================================
+template <typename S>
+std::size_t Link<S>::getNumObjects() const
+{
+  return objects_.size();
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/.deprecated/articulated_model/model.h

@@ -0,0 +1,237 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_MODEL_H
+#define FCL_ARTICULATED_MODEL_MODEL_H
+
+#include "fcl/articulated_model/joint.h"
+#include "fcl/articulated_model/link.h"
+
+#include "fcl/common/data_types.h"
+#include <memory>
+
+#include <map>
+#include <stdexcept>
+
+namespace fcl
+{
+
+class ModelParseError : public std::runtime_error
+{
+public:
+  ModelParseError(const std::string& error_msg) : std::runtime_error(error_msg) {}
+};
+
+template <typename S>
+class Model
+{
+public:
+  Model() {}
+  
+  virtual ~Model() {}
+
+  const std::string& getName() const;
+  
+  void addLink(const std::shared_ptr<Link>& link);
+
+  void addJoint(const std::shared_ptr<Joint>& joint);
+
+  void initRoot(const std::map<std::string, std::string>& link_parent_tree);
+
+  void initTree(std::map<std::string, std::string>& link_parent_tree);
+
+  std::size_t getNumDofs() const;
+
+  std::size_t getNumLinks() const;
+
+  std::size_t getNumJoints() const;
+  
+  std::shared_ptr<Link> getRoot() const;
+  std::shared_ptr<Link> getLink(const std::string& name) const;
+  std::shared_ptr<Joint> getJoint(const std::string& name) const;
+
+  std::vector<std::shared_ptr<Link> > getLinks() const;
+  std::vector<std::shared_ptr<Joint> > getJoints() const;
+protected:
+  std::shared_ptr<Link> root_link_;
+  std::map<std::string, std::shared_ptr<Link> > links_;
+  std::map<std::string, std::shared_ptr<Joint> > joints_;
+
+  std::string name_;
+  
+};
+
+//==============================================================================
+template <typename S>
+std::shared_ptr<Link> Model::getRoot() const
+{
+  return root_link_;
+}
+
+//==============================================================================
+template <typename S>
+std::shared_ptr<Link> Model::getLink(const std::string& name) const
+{
+  std::shared_ptr<Link> ptr;
+  std::map<std::string, std::shared_ptr<Link> >::const_iterator it = links_.find(name);
+  if(it == links_.end())
+    ptr.reset();
+  else
+    ptr = it->second;
+  return ptr;
+}
+
+//==============================================================================
+template <typename S>
+std::shared_ptr<Joint> Model::getJoint(const std::string& name) const
+{
+  std::shared_ptr<Joint> ptr;
+  std::map<std::string, std::shared_ptr<Joint> >::const_iterator it = joints_.find(name);
+  if(it == joints_.end())
+    ptr.reset();
+  else
+    ptr = it->second;
+  return ptr;
+}
+
+//==============================================================================
+template <typename S>
+const std::string& Model::getName() const
+{
+  return name_;
+}
+
+//==============================================================================
+template <typename S>
+std::vector<std::shared_ptr<Link> > Model::getLinks() const
+{
+  std::vector<std::shared_ptr<Link> > links;
+  for(std::map<std::string, std::shared_ptr<Link> >::const_iterator it = links_.begin(); it != links_.end(); ++it)
+  {
+    links.push_back(it->second);
+  }
+
+  return links;
+}
+
+//==============================================================================
+template <typename S>
+std::size_t Model::getNumLinks() const
+{
+  return links_.size();
+}
+
+//==============================================================================
+template <typename S>
+std::size_t Model::getNumJoints() const
+{
+  return joints_.size();
+}
+
+//==============================================================================
+template <typename S>
+std::size_t Model::getNumDofs() const
+{
+  std::size_t dof = 0;
+  for(std::map<std::string, std::shared_ptr<Joint> >::const_iterator it = joints_.begin(); it != joints_.end(); ++it)
+  {
+    dof += it->second->getNumDofs();
+  }
+
+  return dof;
+}
+
+//==============================================================================
+template <typename S>
+void Model::addLink(const std::shared_ptr<Link>& link)
+{
+  links_[link->getName()] = link;
+}
+
+//==============================================================================
+template <typename S>
+void Model::addJoint(const std::shared_ptr<Joint>& joint)
+{
+  joints_[joint->getName()] = joint;
+}
+
+//==============================================================================
+template <typename S>
+void Model::initRoot(const std::map<std::string, std::string>& link_parent_tree)
+{
+  root_link_.reset();
+
+  /// find the links that have no parent in the tree
+  for(std::map<std::string, std::shared_ptr<Link> >::const_iterator it = links_.begin(); it != links_.end(); ++it)
+  {
+    std::map<std::string, std::string>::const_iterator parent = link_parent_tree.find(it->first);
+    if(parent == link_parent_tree.end())
+    {
+      if(!root_link_)
+      {
+        root_link_ = getLink(it->first);
+      }
+      else
+      {
+        throw ModelParseError("Two root links found: [" + root_link_->getName() + "] and [" + it->first + "]");
+      }
+    }
+  }
+
+  if(!root_link_)
+    throw ModelParseError("No root link found.");
+}
+
+//==============================================================================
+template <typename S>
+void Model::initTree(std::map<std::string, std::string>& link_parent_tree)
+{
+  for(std::map<std::string, std::shared_ptr<Joint> >::iterator it = joints_.begin(); it != joints_.end(); ++it)
+  {
+    std::string parent_link_name = it->second->getParentLink()->getName();
+    std::string child_link_name = it->second->getChildLink()->getName();
+
+    it->second->getParentLink()->addChildJoint(it->second);
+    it->second->getChildLink()->setParentJoint(it->second);
+
+    link_parent_tree[child_link_name] = parent_link_name;
+  }
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/.deprecated/articulated_model/model_config.h

@@ -0,0 +1,137 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_MODEL_CONFIG_H
+#define FCL_ARTICULATED_MODEL_MODEL_CONFIG_H
+
+#include <string>
+#include <map>
+#include <memory>
+#include "fcl/common/data_types.h"
+#include "fcl/articulated_model/joint_config.h"
+
+namespace fcl
+{
+
+template <typename S>
+class ModelConfig
+{
+public:
+  ModelConfig();
+
+  ModelConfig(const ModelConfig& model_cfg);
+
+  ModelConfig(std::map<std::string, std::shared_ptr<Joint> > joints_map);
+
+  JointConfig getJointConfigByJointName(const std::string& joint_name) const;
+  JointConfig& getJointConfigByJointName(const std::string& joint_name);
+
+  JointConfig getJointConfigByJoint(std::shared_ptr<Joint> joint) const;
+  JointConfig& getJointConfigByJoint(std::shared_ptr<Joint> joint);
+
+  std::map<std::string, JointConfig> getJointCfgsMap() const
+  { return joint_cfgs_map_; }
+
+private:
+  std::map<std::string, JointConfig> joint_cfgs_map_;
+};
+
+//============================================================================//
+//                                                                            //
+//                              Implementations                               //
+//                                                                            //
+//============================================================================//
+
+//==============================================================================
+template <typename S>
+ModelConfig<S>::ModelConfig()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+ModelConfig<S>::ModelConfig(const ModelConfig& model_cfg) :
+  joint_cfgs_map_(model_cfg.joint_cfgs_map_)
+{}
+
+//==============================================================================
+template <typename S>
+ModelConfig<S>::ModelConfig(std::map<std::string, std::shared_ptr<Joint> > joints_map)
+{
+  std::map<std::string, std::shared_ptr<Joint> >::iterator it;
+  for(it = joints_map.begin(); it != joints_map.end(); ++it)
+    joint_cfgs_map_[it->first] = JointConfig(it->second);
+}
+
+//==============================================================================
+template <typename S>
+JointConfig ModelConfig<S>::getJointConfigByJointName(const std::string& joint_name) const
+{
+  std::map<std::string, JointConfig>::const_iterator it = joint_cfgs_map_.find(joint_name);
+  assert(it != joint_cfgs_map_.end());
+
+  return it->second;
+}
+
+//==============================================================================
+template <typename S>
+JointConfig& ModelConfig<S>::getJointConfigByJointName(const std::string& joint_name)
+{
+  std::map<std::string, JointConfig>::iterator it = joint_cfgs_map_.find(joint_name);
+  assert(it != joint_cfgs_map_.end());
+
+  return it->second;
+}
+
+//==============================================================================
+template <typename S>
+JointConfig ModelConfig<S>::getJointConfigByJoint(std::shared_ptr<Joint> joint) const
+{
+  return getJointConfigByJointName(joint->getName());
+}
+
+//==============================================================================
+template <typename S>
+JointConfig& ModelConfig<S>::getJointConfigByJoint(std::shared_ptr<Joint> joint)
+{
+  return getJointConfigByJointName(joint->getName());
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/.deprecated/learning/classifier.h

@@ -0,0 +1,260 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_LEARNING_CLASSIFIER_H
+#define FCL_LEARNING_CLASSIFIER_H
+
+#include <vector>
+
+#include "fcl/common/data_types.h"
+
+namespace fcl
+{
+
+template<typename S, std::size_t N>
+struct Item
+{
+  VectorN<S, N> q;
+  bool label;
+  S w;
+
+  Item(const VectorN<S, N>& q_, bool label_, S w_ = 1);
+
+  Item();
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(S, N)
+};
+
+template<typename S, std::size_t N>
+struct Scaler
+{
+  VectorN<S, N> v_min, v_max;
+  Scaler();
+
+  Scaler(const VectorN<S, N>& v_min_, const VectorN<S, N>& v_max_);
+
+  VectorN<S, N> scale(const VectorN<S, N>& v) const;
+
+  VectorN<S, N> unscale(const VectorN<S, N>& v) const;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(S, N)
+};
+
+template<typename S>
+struct PredictResult
+{
+  bool label;
+  S prob;
+
+  PredictResult();
+  PredictResult(bool label_, S prob_ = 1);
+};
+
+template<typename S, std::size_t N>
+class SVMClassifier
+{
+public:
+
+  ~SVMClassifier();
+  
+  virtual PredictResult<S> predict(const VectorN<S, N>& q) const = 0;
+  virtual std::vector<PredictResult<S>> predict(
+      const std::vector<VectorN<S, N> >& qs) const = 0;
+
+  virtual std::vector<Item<S, N> > getSupportVectors() const = 0;
+  virtual void setScaler(const Scaler<S, N>& scaler) = 0;
+
+  virtual void learn(const std::vector<Item<S, N> >& data) = 0;
+
+  S error_rate(const std::vector<Item<S, N> >& data) const;
+};
+
+template<typename S, std::size_t N>
+Scaler<S, N> computeScaler(const std::vector<Item<S, N> >& data);
+
+template<typename S, std::size_t N>
+Scaler<S, N> computeScaler(const std::vector<VectorN<S, N> >& data);
+
+//============================================================================//
+//                                                                            //
+//                              Implementations                               //
+//                                                                            //
+//============================================================================//
+
+//==============================================================================
+template<typename S, std::size_t N>
+Item<S, N>::Item(const VectorN<S, N>& q_, bool label_, S w_)
+  : q(q_), label(label_), w(w_)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+Item<S, N>::Item()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+Scaler<S, N>::Scaler()
+{
+  // default no scale
+  for(std::size_t i = 0; i < N; ++i)
+  {
+    v_min[i] = 0;
+    v_max[i] = 1;
+  }
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+Scaler<S, N>::Scaler(const VectorN<S, N>& v_min_, const VectorN<S, N>& v_max_)
+  : v_min(v_min_), v_max(v_max_)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+VectorN<S, N> Scaler<S, N>::scale(const VectorN<S, N>& v) const
+{
+  VectorN<S, N> res;
+  for(std::size_t i = 0; i < N; ++i)
+    res[i] = (v[i] - v_min[i]) / (v_max[i] - v_min[i]);
+  return res;
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+VectorN<S, N> Scaler<S, N>::unscale(const VectorN<S, N>& v) const
+{
+  VectorN<S, N> res;
+  for(std::size_t i = 0; i < N; ++i)
+    res[i] = v[i] * (v_max[i] - v_min[i]) + v_min[i];
+  return res;
+}
+
+//==============================================================================
+template<typename S>
+PredictResult<S>::PredictResult()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template<typename S>
+PredictResult<S>::PredictResult(bool label_, S prob_)
+  : label(label_), prob(prob_)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+SVMClassifier<S, N>::~SVMClassifier()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+S SVMClassifier<S, N>::error_rate(const std::vector<Item<S, N> >& data) const
+{
+  std::size_t num = data.size();
+
+  std::size_t error_num = 0;
+  for(std::size_t i = 0; i < data.size(); ++i)
+  {
+    PredictResult<S> res = predict(data[i].q);
+    if(res.label != data[i].label)
+      error_num++;
+  }
+
+  return error_num / (S)num;
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+Scaler<S, N> computeScaler(const std::vector<Item<S, N> >& data)
+{
+  VectorN<S, N> lower_bound, upper_bound;
+  for(std::size_t j = 0; j < N; ++j)
+  {
+    lower_bound[j] = std::numeric_limits<S>::max();
+    upper_bound[j] = -std::numeric_limits<S>::max();
+  }
+
+  for(std::size_t i = 0; i < data.size(); ++i)
+  {
+    for(std::size_t j = 0; j < N; ++j)
+    {
+      if(data[i].q[j] < lower_bound[j]) lower_bound[j] = data[i].q[j];
+      if(data[i].q[j] > upper_bound[j]) upper_bound[j] = data[i].q[j];
+    }
+  }
+
+  return Scaler<S, N>(lower_bound, upper_bound);
+}
+
+//==============================================================================
+template<typename S, std::size_t N>
+Scaler<S, N> computeScaler(const std::vector<VectorN<S, N> >& data)
+{
+  VectorN<S, N> lower_bound, upper_bound;
+  for(std::size_t j = 0; j < N; ++j)
+  {
+    lower_bound[j] = std::numeric_limits<S>::max();
+    upper_bound[j] = -std::numeric_limits<S>::max();
+  }
+
+  for(std::size_t i = 0; i < data.size(); ++i)
+  {
+    for(std::size_t j = 0; j < N; ++j)
+    {
+      if(data[i][j] < lower_bound[j]) lower_bound[j] = data[i][j];
+      if(data[i][j] > upper_bound[j]) upper_bound[j] = data[i][j];
+    }
+  }
+
+  return Scaler<S, N>(lower_bound, upper_bound);
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/.deprecated/math/motion/taylor_model/interpolation/interpolation.cpp

@@ -0,0 +1,76 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#include "fcl/math/motion/taylor_model/interpolation/interpolation.h"
+
+namespace fcl 
+{
+
+Interpolation::Interpolation() :
+  value_0_(0.0),
+  value_1_(1.0) 
+{}
+
+Interpolation::Interpolation(real start_value, real end_value) :
+  value_0_(start_value),
+  value_1_(end_value)
+{}
+
+void Interpolation::setStartValue(real start_value)
+{
+  value_0_ = start_value;
+}
+
+void Interpolation::setEndValue(real end_value)
+{
+  value_1_ = end_value;
+}
+
+bool Interpolation::operator == (const Interpolation& interpolation) const
+{
+  return 
+    (this->getType() == interpolation.getType()) &&
+    (this->value_0_ == interpolation.value_0_) &&
+    (this->value_1_ == interpolation.value_1_);
+}
+
+bool Interpolation::operator != (const Interpolation& interpolation) const
+{
+  return !(*this == interpolation);
+}
+
+}

3rdparty/fcl/.deprecated/math/motion/taylor_model/interpolation/interpolation.h

@@ -0,0 +1,91 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_H
+#define FCL_CCD_INTERPOLATION_INTERPOLATION_H
+
+#include "fcl/common/types.h"
+
+namespace fcl
+{
+
+enum InterpolationType
+{
+  LINEAR,
+  STANDARD
+};
+
+class Interpolation
+{
+public:
+  Interpolation();
+
+  virtual ~Interpolation() {}
+
+  Interpolation(real start_value, real end_value);
+
+  void setStartValue(real start_value);
+  void setEndValue(real end_value);
+
+  virtual real getValue(real time) const = 0;
+
+  /// @brief return the smallest value in time interval [0, 1]
+  virtual real getValueLowerBound() const = 0;
+
+  /// @brief return the biggest value in time interval [0, 1]
+  virtual real getValueUpperBound() const = 0;
+
+  virtual InterpolationType getType() const = 0;
+
+  bool operator == (const Interpolation& interpolation) const;
+  bool operator != (const Interpolation& interpolation) const;
+
+  virtual real getMovementLengthBound(real time) const = 0;
+
+  virtual real getVelocityBound(real time) const = 0;
+
+protected:
+  real value_0_; // value at time = 0.0
+  real value_1_; // value at time = 1.0
+
+};
+
+
+
+}
+
+#endif

3rdparty/fcl/.deprecated/math/motion/taylor_model/interpolation/interpolation_factory.cpp

@@ -0,0 +1,72 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#include "fcl/math/motion/taylor_model/interpolation/interpolation_factory.h"
+#include "fcl/math/motion/taylor_model/interpolation/interpolation_linear.h"
+#include <cassert>
+
+namespace fcl 
+{
+
+InterpolationFactory::InterpolationFactory()
+{
+  InterpolationLinear::registerToFactory();
+}
+
+InterpolationFactory& InterpolationFactory::instance()
+{
+  static InterpolationFactory instance;
+
+  return instance;
+}
+
+void InterpolationFactory::registerClass(const InterpolationType type, const CreateFunction create_function)
+{
+  this->creation_map_[type] = create_function;
+}
+
+std::shared_ptr<Interpolation> 
+InterpolationFactory::create(const InterpolationType type, const real start_value, const real end_value)
+{
+  std::map<InterpolationType, CreateFunction>::const_iterator it = creation_map_.find(type);
+
+  assert(it != creation_map_.end());
+
+  return (it->second)(start_value, end_value);  
+}
+
+}

3rdparty/fcl/.deprecated/math/motion/taylor_model/interpolation/interpolation_factory.h

@@ -0,0 +1,82 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
+#define FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
+
+#include "fcl/common/types.h"
+#include "fcl/math/motion/taylor_model/interpolation/interpolation.h"
+
+#include <map>
+
+#include <memory>
+#include <functional>
+
+namespace fcl 
+{
+
+class InterpolationFactory
+{	
+public:
+  typedef std::function<std::shared_ptr<Interpolation>(real, real)> CreateFunction;
+
+public:
+  void registerClass(const InterpolationType type, const CreateFunction create_function);
+
+  std::shared_ptr<Interpolation> create(const InterpolationType type, real start_value, real end_value);
+
+public:
+  static InterpolationFactory& instance();
+
+private:
+  InterpolationFactory();
+
+  InterpolationFactory(const InterpolationFactory&)
+  {}
+
+  InterpolationFactory& operator = (const InterpolationFactory&)
+  {
+    return *this;
+  }
+
+private:
+  std::map<InterpolationType, CreateFunction> creation_map_;
+};
+
+}
+
+#endif

3rdparty/fcl/.deprecated/math/motion/taylor_model/interpolation/interpolation_linear.cpp

@@ -0,0 +1,92 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#include "fcl/math/motion/taylor_model/interpolation/interpolation_linear.h"
+#include "fcl/math/motion/taylor_model/interpolation/interpolation_factory.h"
+
+namespace fcl 
+{
+
+InterpolationType interpolation_linear_type = LINEAR;
+
+InterpolationLinear::InterpolationLinear() : Interpolation(0.0, 1.0)
+{}
+
+InterpolationLinear::InterpolationLinear(real start_value, real end_value) : Interpolation(start_value, end_value)
+{}
+
+real InterpolationLinear::getValue(real time) const
+{
+  return value_0_ + (value_1_ - value_0_) * time;
+}
+
+real InterpolationLinear::getValueLowerBound() const
+{
+  return value_0_;
+}
+
+real InterpolationLinear::getValueUpperBound() const
+{
+  return value_1_;
+}
+
+InterpolationType InterpolationLinear::getType() const
+{
+  return interpolation_linear_type;
+}
+
+std::shared_ptr<Interpolation> InterpolationLinear::create(real start_value, real end_value)
+{
+  return std::shared_ptr<Interpolation>(new InterpolationLinear(start_value, end_value) );
+}
+
+void InterpolationLinear::registerToFactory()
+{
+  InterpolationFactory::instance().registerClass(interpolation_linear_type, create);
+}
+
+real InterpolationLinear::getMovementLengthBound(real time) const
+{
+  return getValueUpperBound() - getValue(time);
+}
+
+real InterpolationLinear::getVelocityBound(real time) const
+{
+  return (value_1_ - value_0_);
+}
+
+}

3rdparty/fcl/.deprecated/math/motion/taylor_model/interpolation/interpolation_linear.h

@@ -0,0 +1,77 @@
+/*
+ * 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 Dalibor Matura, Jia Pan */
+
+#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
+#define FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
+
+#include "fcl/common/types.h"
+#include "fcl/math/motion/taylor_model/interpolation/interpolation.h"
+
+#include <memory>
+
+namespace fcl 
+{
+
+class InterpolationFactory;
+
+class InterpolationLinear : public Interpolation
+{
+public:
+  InterpolationLinear();
+
+  InterpolationLinear(real start_value, real end_value);
+
+  virtual real getValue(real time) const;
+
+  virtual real getValueLowerBound() const;
+  virtual real getValueUpperBound() const;
+
+  virtual InterpolationType getType() const;
+
+  virtual real getMovementLengthBound(real time) const;
+
+  virtual real getVelocityBound(real time) const;
+
+public:
+  static std::shared_ptr<Interpolation> create(real start_value, real end_value);
+
+  static void registerToFactory();
+};
+
+}
+
+#endif

3rdparty/fcl/.deprecated/narrowphase/penetration_depth_request.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 FCL_PENETRATIONDEPTHREQUEST_H
+#define FCL_PENETRATIONDEPTHREQUEST_H
+
+#include "fcl/common/types.h"
+#include "fcl/narrowphase/gjk_solver.h"
+
+namespace fcl
+{
+
+enum PenetrationDepthType {PDT_TRANSLATIONAL, PDT_GENERAL_EULER, PDT_GENERAL_QUAT, PDT_GENERAL_EULER_BALL, PDT_GENERAL_QUAT_BALL};
+
+template <typename S>
+struct PenetrationDepthRequest
+{
+  void* classifier;
+
+  /// @brief PD algorithm type
+  PenetrationDepthType pd_type;
+
+  /// @brief gjk solver type
+  GJKSolverType gjk_solver_type;
+
+  Eigen::aligned_vector<Transform3<S>> contact_vectors;
+
+  PenetrationDepthRequest(void* classifier_,
+                          PenetrationDepthType pd_type_ = PDT_TRANSLATIONAL,
+                          GJKSolverType gjk_solver_type_ = GST_LIBCCD);
+};
+
+//============================================================================//
+//                                                                            //
+//                              Implementations                               //
+//                                                                            //
+//============================================================================//
+
+//==============================================================================
+template <typename S>
+PenetrationDepthRequest<S>::PenetrationDepthRequest(
+    void* classifier_,
+    PenetrationDepthType pd_type_,
+    GJKSolverType gjk_solver_type_)
+  : classifier(classifier_),
+    pd_type(pd_type_),
+    gjk_solver_type(gjk_solver_type_)
+{
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/.deprecated/narrowphase/penetration_depth_result.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 FCL_PENETRATIONDEPTHRESULT_H
+#define FCL_PENETRATIONDEPTHRESULT_H
+
+#include "fcl/common/types.h"
+
+namespace fcl
+{
+
+template <typename S>
+struct PenetrationDepthResult
+{
+  /// @brief penetration depth value
+  S pd_value;
+
+  /// @brief the transform where the collision is resolved
+  Transform3<S> resolved_tf;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/.deprecated/simd/simd_intersect.h

@@ -0,0 +1,241 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_MULTIPLE_INTERSECT
+#define FCL_MULTIPLE_INTERSECT
+
+#include <xmmintrin.h>
+#include <pmmintrin.h>
+
+
+namespace fcl
+{
+
+
+static inline __m128 sse_four_spheres_intersect(const Vector3d& o1, FCL_REAL r1,
+                                                const Vector3d& o2, FCL_REAL r2,
+                                                const Vector3d& o3, FCL_REAL r3,
+                                                const Vector3d& o4, FCL_REAL r4,
+                                                const Vector3d& o5, FCL_REAL r5,
+                                                const Vector3d& o6, FCL_REAL r6,
+                                                const Vector3d& o7, FCL_REAL r7,
+                                                const Vector3d& o8, FCL_REAL r8)
+{
+  __m128 PX, PY, PZ, PR, QX, QY, QZ, QR;
+  PX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
+  PY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]);
+  PZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]);
+  PR = _mm_set_ps(r1, r2, r3, r4);
+  QX = _mm_set_ps(o5[0], o6[0], o7[0], o8[0]);
+  QY = _mm_set_ps(o5[1], o6[1], o7[1], o8[1]);
+  QZ = _mm_set_ps(o5[2], o6[2], o7[2], o8[2]);
+  QR = _mm_set_ps(r5, r6, r7, r8);
+
+  __m128 T1 = _mm_sub_ps(PX, QX);
+  __m128 T2 = _mm_sub_ps(PY, QY);
+  __m128 T3 = _mm_sub_ps(PZ, QZ);
+  __m128 T4 = _mm_add_ps(PR, QR);
+  T1 = _mm_mul_ps(T1, T1);
+  T2 = _mm_mul_ps(T2, T2);
+  T3 = _mm_mul_ps(T3, T3);
+  T4 = _mm_mul_ps(T4, T4);
+  T1 = _mm_add_ps(T1, T2);
+  T2 = _mm_sub_ps(R2, T3);
+  return _mm_cmple_ps(T1, T2);
+}
+
+
+static inline __m128 sse_four_spheres_four_AABBs_intersect(const Vector3d& o1, FCL_REAL r1,
+                                                           const Vector3d& o2, FCL_REAL r2,
+                                                           const Vector3d& o3, FCL_REAL r3,
+                                                           const Vector3d& o4, FCL_REAL r4,
+                                                           const Vector3d& min1, const Vector3d& max1,
+                                                           const Vector3d& min2, const Vector3d& max2,
+                                                           const Vector3d& min3, const Vector3d& max3,
+                                                           const Vector3d& min4, const Vector3d& max4)
+{
+  __m128 MINX, MINY, MINZ, MAXX, MAXX, MAXY, MAXZ, SX, SY, SZ, SR;
+  MINX = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]);
+  MAXX = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]);
+  MINY = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]);
+  MAXY = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]);
+  MINZ = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]);
+  MAXZ = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]);
+  SX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
+  SY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]);
+  SZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]);
+  SR = _mm_set_ps(r1, r2, r3, r4);
+
+
+  __m128 TX = _mm_max_ps(SX, MINX);
+  __m128 TY = _mm_max_ps(SY, MINY);
+  __m128 TZ = _mm_max_ps(SZ, MINZ);
+  TX = _mm_min_ps(TX, MAXX);
+  TY = _mm_min_ps(TY, MAXY);
+  TZ = _mm_min_ps(TZ, MAXZ);
+  __m128 DX = _mm_sub_ps(SX, TX);
+  __m128 DY = _mm_sub_ps(SY, TY);
+  __m128 DZ = _mm_sub_ps(SZ, TZ);
+  __m128 R2 = _mm_mul_ps(SR, SR);
+  DX = _mm_mul_ps(DX, DX);
+  DY = _mm_mul_ps(DY, DY);
+  DZ = _mm_mul_ps(DZ, DZ);
+  __m128 T1 = _mm_add_ps(DX, DY);
+  __m128 T2 = _mm_sub_ps(R2, DZ);
+  return _mm_cmple_ps(T1, T2);  
+}
+
+
+static inline __m128 sse_four_AABBs_intersect(const Vector3d& min1, const Vector3d& max1,
+                                              const Vector3d& min2, const Vector3d& max2,
+                                              const Vector3d& min3, const Vector3d& max3,
+                                              const Vector3d& min4, const Vector3d& max4,
+                                              const Vector3d& min5, const Vector3d& max5,
+                                              const Vector3d& min6, const Vector3d& max6,
+                                              const Vector3d& min7, const Vector3d& max7,
+                                              const Vector3d& min8, const Vector3d& max8)
+{
+  __m128 MIN1X, MIN1Y, MIN1Z, MAX1X, MAX1Y, MAX1Z, MIN2X, MIN2Y, MIN2Z, MAX2X, MAX2Y, MAX2Z;
+  MIN1X = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]);
+  MAX1X = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]);
+  MIN1Y = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]);
+  MAX1Y = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]);
+  MIN1Z = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]);
+  MAX1Z = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]);
+  MIN2X = _mm_set_ps(min5[0], min6[0], min7[0], min8[0]);
+  MAX2X = _mm_set_ps(max5[0], max6[0], max7[0], max8[0]);
+  MIN2Y = _mm_set_ps(min5[1], min6[1], min7[1], min8[1]);
+  MAX2Y = _mm_set_ps(max5[1], max6[1], max7[1], max8[1]);
+  MIN2Z = _mm_set_ps(min5[2], min6[2], min7[2], min8[2]);
+  MAX2Z = _mm_set_ps(max5[2], max6[2], max7[2], max8[2]);
+
+  __m128 AX = _mm_max_ps(MIN1X, MIN2X);
+  __m128 BX = _mm_min_ps(MAX1X, MAX2X);
+  __m128 AY = _mm_max_ps(MIN1Y, MIN2Y);
+  __m128 BY = _mm_min_ps(MAX1Y, MAX2Y);
+  __m128 AZ = _mm_max_ps(MIN1Z, MIN2Z);
+  __m128 BZ = _mm_min_ps(MAX1Z, MAX2Z);
+  __m128 T1 = _mm_cmple_ps(AX, BX);
+  __m128 T2 = _mm_cmple_ps(AY, BY);
+  __m128 T3 = _mm_cmple_ps(AZ, BZ);
+  __m128 T4 = _mm_and_ps(T1, T2);
+  return _mm_and_ps(T3, T4);
+}
+
+static bool four_spheres_intersect_and(const Vector3d& o1, FCL_REAL r1,
+                                       const Vector3d& o2, FCL_REAL r2,
+                                       const Vector3d& o3, FCL_REAL r3,
+                                       const Vector3d& o4, FCL_REAL r4,
+                                       const Vector3d& o5, FCL_REAL r5,
+                                       const Vector3d& o6, FCL_REAL r6,
+                                       const Vector3d& o7, FCL_REAL r7,
+                                       const Vector3d& o8, FCL_REAL r8)
+{
+  __m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
+  return _mm_movemask_ps(CMP) == 15.f;
+}
+
+static bool four_spheres_intersect_or(const Vector3d& o1, FCL_REAL r1,
+                                      const Vector3d& o2, FCL_REAL r2,
+                                      const Vector3d& o3, FCL_REAL r3,
+                                      const Vector3d& o4, FCL_REAL r4,
+                                      const Vector3d& o5, FCL_REAL r5,
+                                      const Vector3d& o6, FCL_REAL r6,
+                                      const Vector3d& o7, FCL_REAL r7,
+                                      const Vector3d& o8, FCL_REAL r8)
+{
+  __m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
+  return __mm_movemask_ps(CMP) > 0;
+}
+
+/** @brief four spheres versus four AABBs SIMD test */
+static bool four_spheres_four_AABBs_intersect_and(const Vector3d& o1, FCL_REAL r1,
+                                                  const Vector3d& o2, FCL_REAL r2,
+                                                  const Vector3d& o3, FCL_REAL r3,
+                                                  const Vector3d& o4, FCL_REAL r4,
+                                                  const Vector3d& min1, const Vector3d& max1,
+                                                  const Vector3d& min2, const Vector3d& max2,
+                                                  const Vector3d& min3, const Vector3d& max3,
+                                                  const Vector3d& min4, const Vector3d& max4)
+{
+  __m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4);
+  return _mm_movemask_ps(CMP) == 15.f;
+}
+
+static bool four_spheres_four_AABBs_intersect_or(const Vector3d& o1, FCL_REAL r1,
+                                                 const Vector3d& o2, FCL_REAL r2,
+                                                 const Vector3d& o3, FCL_REAL r3,
+                                                 const Vector3d& o4, FCL_REAL r4,
+                                                 const Vector3d& min1, const Vector3d& max1,
+                                                 const Vector3d& min2, const Vector3d& max2,
+                                                 const Vector3d& min3, const Vector3d& max3,
+                                                 const Vector3d& min4, const Vector3d& max4)
+{
+  __m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4);
+  return _mm_movemask_ps(CMP) > 0;
+}
+
+/** @brief four AABBs versus four AABBs SIMD test */
+static bool four_AABBs_intersect_and(const Vector3d& min1, const Vector3d& max1,
+                                     const Vector3d& min2, const Vector3d& max2,
+                                     const Vector3d& min3, const Vector3d& max3,
+                                     const Vector3d& min4, const Vector3d& max4,
+                                     const Vector3d& min5, const Vector3d& max5,
+                                     const Vector3d& min6, const Vector3d& max6,
+                                     const Vector3d& min7, const Vector3d& max7,
+                                     const Vector3d& min8, const Vector3d& max8)
+{
+  __m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8);
+  return _mm_movemask_ps(CMP) == 15.f;
+}
+
+static bool four_AABBs_intersect_or(const Vector3d& min1, const Vector3d& max1,
+                                    const Vector3d& min2, const Vector3d& max2,
+                                    const Vector3d& min3, const Vector3d& max3,
+                                    const Vector3d& min4, const Vector3d& max4,
+                                    const Vector3d& min5, const Vector3d& max5,
+                                    const Vector3d& min6, const Vector3d& max6,
+                                    const Vector3d& min7, const Vector3d& max7,
+                                    const Vector3d& min8, const Vector3d& max8)
+{
+  __m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8);
+  return _mm_movemask_ps(CMP) > 0;
+}
+
+}
+
+#endif

3rdparty/fcl/.editorconfig

@@ -0,0 +1,44 @@
+# Copyright (c) 2018, Toyota Research Institute, 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 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 HOLDER 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: Jamie Snape, Kitware, Inc.
+
+root = true
+
+[*]
+charset = utf
+end_of_line = lf
+indent_size = 2
+indent_style = space
+insert_final_newline = true
+max_line_length = 80
+trim_trailing_whitespace = true
+
+[*.md]
+trim_trailing_whitespace = false

3rdparty/fcl/.gitignore

@@ -0,0 +1,34 @@
+# Copyright (c) 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 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 HOLDER 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: Jeongseok Lee <jslee02@gmail.com>
+
+/build
+/build-*
+/doc/doxygen

3rdparty/fcl/.travis.yml

@@ -0,0 +1,58 @@
+sudo: required
+dist: trusty
+cache:
+  apt: true
+
+language: cpp
+
+matrix:
+  include:
+    - os: linux
+      compiler: gcc
+      env: BUILD_TYPE=Debug COVERALLS=ON
+    - os: linux
+      compiler: gcc
+      env: BUILD_TYPE=Release COVERALLS=OFF
+    - os: linux
+      compiler: clang
+      env: BUILD_TYPE=Debug COVERALLS=OFF
+    - os: linux
+      compiler: clang
+      env: BUILD_TYPE=Release COVERALLS=OFF
+    - os: osx
+      osx_image: xcode9
+      compiler: clang
+      env: BUILD_TYPE=Debug COVERALLS=OFF
+    - os: osx
+      osx_image: xcode9
+      compiler: clang
+      env: BUILD_TYPE=Release COVERALLS=OFF
+
+install:
+  # Install dependencies for FCL
+  - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi
+  - if [ "$TRAVIS_OS_NAME" = "osx"   ]; then 'ci/install_osx.sh'   ; fi
+
+script:
+  # Create build directory
+  - mkdir build
+  - cd build
+
+  # Configure
+  - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DFCL_TREAT_WARNINGS_AS_ERRORS=ON -DFCL_COVERALLS=$COVERALLS ..
+
+  # Build
+  - make -j4
+  - if [ $COVERALLS = ON ] && [ "$TRAVIS_OS_NAME" = "linux" ]; then make coveralls; fi
+
+  # Run unit tests
+  - make test
+
+  # Make sure we can install and uninstall with no issues
+  - sudo make -j4 install
+  - sudo make -j4 uninstall
+
+after_failure: 
+  - cat Testing/Temporary/LastTest.log
+  - cat Testing/Temporary/LastTestsFailed.log
+

3rdparty/fcl/CHANGELOG.md

@@ -0,0 +1,61 @@
+## FCL 0
+
+### FCL 0.6.0 (2016-XX-XX)
+
+* Core/Common
+
+  * Reorganized source tree: [#161](https://github.com/flexible-collision-library/fcl/issues/161), [#163](https://github.com/flexible-collision-library/fcl/pull/163)
+  * Templatized FCL for scalar type: [#154](https://github.com/flexible-collision-library/fcl/pull/154), [#165](https://github.com/flexible-collision-library/fcl/pull/165)
+  * Removed dependency on boost: [#140](https://github.com/flexible-collision-library/fcl/pull/140), [#146](https://github.com/flexible-collision-library/fcl/pull/146), [#147](https://github.com/flexible-collision-library/fcl/pull/147), [#148](https://github.com/flexible-collision-library/fcl/pull/148)
+
+* Math
+
+  * Switched to Eigen for math operations: [#96](https://github.com/flexible-collision-library/fcl/issues/96), [#150](https://github.com/flexible-collision-library/fcl/pull/150)
+  * fcl::Transform defined to be an Isometry to facilitate inverses [#318](https://github.com/flexible-collision-library/fcl/pull/318)
+
+* Geometry
+
+  * Simplified Convex class, deprecating old constructor in favor of
+    simpler, documented constructor:
+     [#325](https://github.com/flexible-collision-library/fcl/pull/325),
+     [#338](https://github.com/flexible-collision-library/fcl/pull/338)
+
+* Broadphase
+
+  * Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156)
+
+* Narrowphase
+
+  * Add custom sphere-box collision and distance algorithms for both solvers: [#316](https://github.com/flexible-collision-library/fcl/pull/316)
+  * Add custom sphere-cylinder collision and distance algorithms for both solvers: [#321](https://github.com/flexible-collision-library/fcl/pull/321)
+
+* Distance
+
+  * Added distance request option for computing exact negative distance: [#172](https://github.com/flexible-collision-library/fcl/pull/172) 
+
+* Build/Test/Misc
+
+  * Added version check for Visual Studio in CMake (VS2015 or greater required): [#189](https://github.com/flexible-collision-library/fcl/pull/189)
+  * Added CMake targets for generating API documentation: [#174](https://github.com/flexible-collision-library/fcl/pull/174)
+  * Enabled build with SSE option by default: [#159](https://github.com/flexible-collision-library/fcl/pull/159)
+  * Added missing copyright headers:  [#149](https://github.com/flexible-collision-library/fcl/pull/149)
+  * Added test utility for performing equality between Eigen matrix-types (`CompareMatrices` in `test/eign_matrix_compare.h`): [#316](https://github.com/flexible-collision-library/fcl/pull/316)
+
+### FCL 0.5.0 (2016-07-19)
+
+* Added safe-guards to allow octree headers only if octomap enabled: [#136](https://github.com/flexible-collision-library/fcl/pull/136)
+* Added CMake option to disable octomap in build: [#135](https://github.com/flexible-collision-library/fcl/pull/135)
+* Added automatic coverage test reporting: [#125](https://github.com/flexible-collision-library/fcl/pull/125), [#98](https://github.com/flexible-collision-library/fcl/pull/98)
+* Added CMake exported targets: [#116](https://github.com/flexible-collision-library/fcl/pull/116)
+* Fixed API to support Octomap 1.8: [#129](https://github.com/flexible-collision-library/fcl/pull/129), [#126](https://github.com/flexible-collision-library/fcl/issues/126)
+* Fixed continuousCollisionNaive() wasn't resetting the returned result when no collision: [#123](https://github.com/flexible-collision-library/fcl/pull/123)
+* Fixed uninitialized tf in TranslationMotion: [#121](https://github.com/flexible-collision-library/fcl/pull/121)
+* Fixed fcl.pc populated incorrect installation paths: [#118](https://github.com/flexible-collision-library/fcl/pull/118)
+* Fixed octree vs mesh CollisionResult now returns triangle id: [#114](https://github.com/flexible-collision-library/fcl/pull/114)
+* Fixed minor typo: [#113](https://github.com/flexible-collision-library/fcl/pull/113)
+* Fixed fallback finding of libccd: [#112](https://github.com/flexible-collision-library/fcl/pull/112)
+* Fixed a nasty bug in propagate propagateBVHFrontListCollisionRecurse(): [#110](https://github.com/flexible-collision-library/fcl/pull/110)
+* Fixed test_fcl_math failures on Windows 64 bit due to non-portable use of long: [#108](https://github.com/flexible-collision-library/fcl/pull/108), [#107](https://github.com/flexible-collision-library/fcl/issues/107)
+* Fixed compilation in Visual Studio 2015, and suppressed some warnings: [#99](https://github.com/flexible-collision-library/fcl/pull/99)
+* Fixed build when libccd package config not found: [#94](https://github.com/flexible-collision-library/fcl/pull/94)
+* Removing dependency on boost: [#108](https://github.com/flexible-collision-library/fcl/pull/108), [#105](https://github.com/flexible-collision-library/fcl/pull/105), [#104](https://github.com/flexible-collision-library/fcl/pull/104), [#103](https://github.com/flexible-collision-library/fcl/pull/103)

3rdparty/fcl/CMakeLists.txt

@@ -0,0 +1,328 @@
+if(APPLE)
+  cmake_minimum_required(VERSION 3.0.00)
+elseif(MSVC)
+  cmake_minimum_required(VERSION 3.1.3)
+else()
+  cmake_minimum_required(VERSION 2.8.12)
+endif()
+
+# Set the CMAKE_CXX_COMPILER_ID variable to AppleClang instead of Clang.
+# AppleClang and Clang have different version number. This was introduced in
+# CMake 3.0.
+if(POLICY CMP0025)
+  cmake_policy(SET CMP0025 NEW)
+endif()
+
+# Use MACOSX_RPATH by default on OS X. This was added in CMake 2.8.12 and
+# became default in CMake 3.0. Explicitly setting this policy is necessary to
+# suppress a warning in CMake 3.0 and above.
+if(POLICY CMP0042)
+  cmake_policy(SET CMP0042 NEW)
+endif()
+
+project(fcl CXX C)
+
+include(CTest)
+
+configure_file(CTestCustom.cmake.in CTestCustom.cmake @ONLY)
+
+option(FCL_ENABLE_PROFILING "Enable profiling" OFF)
+option(FCL_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF)
+# Option for some bundle-like build system in order not to expose
+# any FCL binary symbols in their public ABI
+option(FCL_HIDE_ALL_SYMBOLS "Hide all binary symbols" OFF)
+
+# set the default build type
+if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING
+    "Choose the type of build; options are Debug Release RelWithDebInfo MinSizeRel."
+    FORCE)
+  set_property(CACHE CMAKE_BUILD_TYPE PROPERTY
+    STRINGS Debug Release RelWithDebInfo MinSizeRel)
+endif()
+
+# This shouldn't be necessary, but there has been trouble
+# with MSVC being set off, but MSVCXX ON.
+if(MSVC OR MSVC90 OR MSVC10)
+    set(MSVC ON)
+endif (MSVC OR MSVC90 OR MSVC10)
+
+set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
+set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules")
+include(FCLMacros)
+include(CompilerSettings)
+include(FCLVersion)
+include(GNUInstallDirs)
+include(GenerateExportHeader)
+
+if(MSVC OR IS_ICPC)
+  option(FCL_STATIC_LIBRARY "Whether the FCL library should be static rather than shared" ON)
+else()
+  option(FCL_STATIC_LIBRARY "Whether the FCL library should be static rather than shared" OFF)
+endif()
+
+# Whether to enable SSE
+set(SSE_FLAGS "")
+option(FCL_USE_X64_SSE "Whether FCL should x64 SSE instructions" ON)
+if(FCL_USE_X64_SSE)
+  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    set(SSE_FLAGS -mfpmath=sse -msse -msse2 -msse3 -mssse3)
+  elseif(MSVC)
+    # Win64 will add the flag automatically
+    if(CMAKE_VS_PLATFORM_NAME STREQUAL "Win32")
+      set(SSE_FLAGS /arch:SSE2)
+    endif()
+  endif()
+  add_compile_options(${SSE_FLAGS})
+endif()
+
+option(FCL_USE_HOST_NATIVE_ARCH "Whether FCL should use cflags from the host used to compile" OFF)
+if (FCL_USE_HOST_NATIVE_ARCH)
+  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
+  else()
+    message(WARNING "FCL_USE_HOST_NATIVE_ARCH is only supported in Linux. No effect.")
+  endif()
+endif()
+
+# DEPRECATED: old cmake option. Not strictly correct from the semantic point of view
+# it was activating march=native, not only SSE
+option(FCL_USE_SSE "(deprecated) Whether FCL should SSE instructions" OFF)
+if(FCL_USE_SSE)
+  set(FCL_HAVE_SSE TRUE)
+  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+    message(WARNING "FCL_USE_SSE is deprecated please use: FCL_USE_X64_SSE or FCL_USE_HOST_NATIVE_ARCH. "
+                    "If you want to replicate the previous behaviour use FCL_USE_HOST_NATIVE_ARCH")
+    add_definitions(-march=native)
+  elseif(MSVC)
+    # Win64 will add the flag automatically
+    if("$CMAKE_VS_PLATFORM_NAME}" STREQUAL "Win32")
+      add_definitions(/arch:SSE2)
+    endif()
+  endif()
+endif()
+
+# Coveralls support
+option(FCL_COVERALLS "Turn on coveralls support" OFF)
+option(FCL_COVERALLS_UPLOAD "Upload the generated coveralls json" ON)
+if(FCL_COVERALLS)
+  include(Coveralls)
+  coveralls_turn_on_coverage()
+endif()
+
+find_package(PkgConfig QUIET)
+set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH ON)
+
+#===============================================================================
+# Find required dependency Eigen3 (>= 3.0.5)
+#
+# If Eigen3 is not found, manually set the cache variable EIGEN3_INCLUDE_DIR
+#===============================================================================
+find_package(Eigen3 3.0.5 QUIET CONFIG)
+
+# If Eigen3Config.cmake is not found, use the FindEigen3.cmake module
+if(NOT Eigen3_FOUND)
+  find_package(Eigen3 3.0.5 QUIET MODULE)
+  set(Eigen3_FOUND ON)
+endif()
+
+if(Eigen3_FOUND)
+  set(FCL_HAVE_EIGEN TRUE)
+else()
+  message(SEND_ERROR "EIGEN3 (>= 3.0.5) is required by FCL")
+  set(FCL_HAVE_EIGEN FALSE)
+endif()
+
+#===============================================================================
+# Find required dependency libccd
+#
+# If libccd is not found, manually set the cache variables CCD_INCLUDE_DIR and
+# CCD_LIBRARY
+#===============================================================================
+find_package(ccd QUIET)
+
+# If ccd-config.cmake is not found, use pkg-config and/or find_path() and
+# find_library()
+if(NOT ccd_FOUND)
+  if(PKG_CONFIG_FOUND)
+    pkg_check_modules(PC_CCD ccd)
+    pkg_check_modules(PC_LIBCCD libccd)
+  endif()
+
+  find_path(CCD_INCLUDE_DIR ccd/ccd.h
+    HINTS "${PC_CCD_INCLUDE_DIRS}" "${PC_LIBCCD_INCLUDE_DIRS}")
+
+  # Using find_library() even if pkg-config is available ensures that the full
+  # path to the ccd library is available in CCD_LIBRARIES
+  find_library(CCD_LIBRARY ccd
+    HINTS "${PC_CCD_LIBRARY_DIRS}" "${PC_LIBCCD_LIBRARY_DIRS}")
+
+  # libccd links to LibM on UNIX.
+  if(CYGWIN OR NOT WIN32)
+    find_library(M_LIBRARY m)
+  endif()
+
+  if(CCD_INCLUDE_DIR AND CCD_LIBRARY)
+    set(CCD_INCLUDE_DIRS "${CCD_INCLUDE_DIR}")
+    set(CCD_LIBRARIES "${CCD_LIBRARY}" "${M_LIBRARY}")
+    set(ccd_FOUND ON)
+
+    mark_as_advanced(CCD_INCLUDE_DIR CCD_LIBRARY)
+  endif()
+endif()
+
+if(NOT ccd_FOUND)
+  message(FATAL_ERROR "CCD is required by FCL")
+endif()
+
+set(PKG_EXTERNAL_DEPS "ccd eigen3")
+
+#===============================================================================
+# Find optional dependency OctoMap
+#
+# If OctoMap is not found, manually set the cache variables OCTOMAP_INCLUDE_DIR
+# and OCTOMAP_LIBRARY, OCTOMATH_LIBRARY, and OCTOMAP_VERSION
+#===============================================================================
+option(FCL_WITH_OCTOMAP "OctoMap library support" ON)
+set(FCL_HAVE_OCTOMAP 0)
+
+if(FCL_WITH_OCTOMAP)
+  find_package(octomap QUIET)
+
+  # Older versions of octomap-config.cmake may not define OCTOMAP_VERSION so
+  # fall back to pkg-config
+  if(NOT octomap_FOUND OR NOT OCTOMAP_VERSION)
+    if(PKG_CONFIG_FOUND)
+      pkg_check_modules(PC_OCTOMAP octomap)
+    endif()
+
+    find_path(OCTOMAP_INCLUDE_DIR octomap/octomap.h
+      HINTS "${PC_OCTOMAP_INCLUDE_DIRS}")
+
+    # Using find_library() even if pkg-config is available ensures that the full
+    # paths to the octomap and octomath libraries are set in OCTOMAP_LIBRARIES
+    find_library(OCTOMAP_LIBRARY octomap
+      HINTS "${PC_OCTOMAP_LIBRARY_DIRS}")
+
+    find_library(OCTOMATH_LIBRARY octomath
+      HINTS "${PC_OCTOMAP_LIBRARY_DIRS}")
+
+    # Use a cache variable so that the version can be manually set if pkg-config
+    # is not available
+    set(OCTOMAP_VERSION "${PC_OCTOMAP_VERSION}"
+      CACHE STRING "octomap version (major.minor.patch)")
+
+    if(OCTOMAP_INCLUDE_DIR AND OCTOMAP_LIBRARY AND OCTOMATH_LIBRARY AND OCTOMAP_VERSION)
+      set(OCTOMAP_INCLUDE_DIRS "${OCTOMAP_INCLUDE_DIR}")
+      set(OCTOMAP_LIBRARIES "${OCTOMAP_LIBRARY}" "${OCTOMATH_LIBRARY}")
+      set(octomap_FOUND ON)
+
+      mark_as_advanced(OCTOMAP_INCLUDE_DIR OCTOMAP_LIBRARY OCTOMATH_LIBRARY OCTOMAP_VERSION)
+    else()
+      set(octomap_FOUND OFF)
+    endif()
+  endif()
+
+  if(octomap_FOUND)
+    if(NOT OCTOMAP_MAJOR_VERSION AND NOT OCTOMAP_MINOR_VERSION AND NOT OCTOMAP_PATCH_VERSION)
+      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)
+    endif()
+
+    set(FCL_HAVE_OCTOMAP 1)
+    message(STATUS "FCL uses OctoMap")
+    set(PKG_EXTERNAL_DEPS "${PKG_EXTERNAL_DEPS} octomap")
+  else()
+    message(STATUS "FCL does not use OctoMap")
+  endif()
+else()
+  message(STATUS "FCL does not use OctoMap (as requested)")
+endif()
+
+# FCL's own include dir should be at the front of the include path
+include_directories(BEFORE "include")
+include_directories(BEFORE "${CMAKE_CURRENT_BINARY_DIR}/include")
+
+add_subdirectory(src)
+add_subdirectory(include/fcl)
+
+set(pkg_conf_file_in "${CMAKE_CURRENT_SOURCE_DIR}/fcl.pc.in")
+set(pkg_conf_file_out "${CMAKE_CURRENT_BINARY_DIR}/fcl.pc")
+set(PKG_DESC "Flexible Collision Library")
+if(NOT MSVC)
+    set(PKG_CFLAGS "-std=c++11")
+endif()
+configure_file("${pkg_conf_file_in}" "${pkg_conf_file_out}" @ONLY)
+
+install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+  FILES_MATCHING PATTERN "*.h" PATTERN "*.hxx"
+  PATTERN ".DS_Store" EXCLUDE
+)
+
+install(FILES "${pkg_conf_file_out}" DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig/ COMPONENT pkgconfig)
+
+# Add uninstall target
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules/cmake_uninstall.cmake.in"
+  "${CMAKE_CURRENT_BINARY_DIR}/CMakeModules/cmake_uninstall.cmake"
+  IMMEDIATE @ONLY)
+#add_custom_target(uninstall
+#  "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/CMakeModules/cmake_uninstall.cmake")
+
+set(FCL_BUILD_TESTS "DEFAULT" CACHE INTERNAL "Deprecated; use BUILD_TESTING instead.")
+
+if(NOT FCL_BUILD_TESTS STREQUAL "DEFAULT")
+  message(DEPRECATION "FCL_BUILD_TESTS is deprecated; use BUILD_TESTING instead.")
+  if(FCL_BUILD_TESTS)
+    set(_BUILD_TESTING ON)
+  else()
+    set(_BUILD_TESTING OFF)
+  endif()
+  set(BUILD_TESTING ${_BUILD_TESTING} CACHE BOOL "Build the testing tree." FORCE)
+  unset(_BUILD_TESTING)
+endif()
+
+if(BUILD_TESTING AND NOT FCL_HIDE_ALL_SYMBOLS)
+    add_subdirectory(test)
+endif()
+
+#===============================================================================
+# API documentation using Doxygen
+# References:
+#   http://mementocodex.wordpress.com/2013/01/19/how-to-generate-code-documentation-with-doxygen-and-cmake-a-slightly-improved-approach/
+#   http://www.cmake.org/pipermail/cmake/2007-February/012796.html
+#===============================================================================
+# Doxygen
+find_package(Doxygen QUIET)
+
+if(DOXYGEN_FOUND)
+
+  set(DOXYGEN_DOXYFILE_IN      ${PROJECT_SOURCE_DIR}/doc/Doxyfile.in     )
+  set(DOXYGEN_DOXYFILE         ${PROJECT_BINARY_DIR}/doc/Doxyfile        )
+  set(DOXYGEN_HTML_INDEX       ${PROJECT_SOURCE_DIR}/doc/doxygen/index.html )
+  set(DOXYGEN_OUTPUT_ROOT      ${PROJECT_SOURCE_DIR}/doc/doxygen            )
+  set(DOXYGEN_GENERATE_TAGFILE ${DOXYGEN_OUTPUT_ROOT}/${PROJECT_NAME}.tag)
+  set(DOXYGEN_INCLUDE_PATH     ${PROJECT_SOURCE_DIR}                     )
+  set(DOXYGEN_INPUT_ROOT       "${PROJECT_SOURCE_DIR}/include ${PROJECT_SOURCE_DIR}/src")
+
+  configure_file(${DOXYGEN_DOXYFILE_IN} ${DOXYGEN_DOXYFILE} @ONLY)
+  add_custom_command(OUTPUT ${DOXYGEN_HTML_INDEX}
+                     COMMAND ${CMAKE_COMMAND} -E echo_append "Building API Documentation..."
+                     COMMAND ${DOXYGEN_EXECUTABLE} -u ${DOXYGEN_DOXYFILE}
+                     COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_DOXYFILE}
+                     COMMAND ${CMAKE_COMMAND} -E echo "Done."
+                     WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/doc
+                     DEPENDS ${DOXYGEN_DOXYFILE}
+  )
+  add_custom_target(docs DEPENDS ${DOXYGEN_HTML_INDEX})
+  add_custom_target(docs_forced
+                    COMMAND ${CMAKE_COMMAND} -E echo_append "Building API Documentation..."
+                    COMMAND ${DOXYGEN_EXECUTABLE} -u ${DOXYGEN_DOXYFILE}
+                    COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_DOXYFILE}
+                    COMMAND ${CMAKE_COMMAND} -E echo "Done."
+                    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/doc
+  )
+
+endif()

3rdparty/fcl/CMakeModules/CompilerSettings.cmake

@@ -0,0 +1,96 @@
+# GCC
+if(CMAKE_COMPILER_IS_GNUCXX)
+    add_definitions(-std=c++11 -W -Wall -Wextra -Wpedantic)
+    if(FCL_TREAT_WARNINGS_AS_ERRORS)
+        add_definitions(-Werror)
+    endif()
+endif()
+
+# Clang
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+    add_definitions(-std=c++11 -W -Wall -Wextra)
+    if(FCL_TREAT_WARNINGS_AS_ERRORS)
+        add_definitions(-Werror)
+    endif()
+endif()
+
+# AppleClang
+if(CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
+    # Require at least Apple LLVM version 6.1
+    if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS 6.1)
+        message(FATAL_ERROR "AppleClang version must be at least 6.1!")
+    endif()
+    add_definitions(-std=c++11 -W -Wall -Wextra)
+    if(FCL_TREAT_WARNINGS_AS_ERRORS)
+        add_definitions(-Werror)
+    endif()
+endif()
+
+# Visual Studio
+if(MSVC)
+    if(MSVC_VERSION VERSION_LESS 1900)
+        message(FATAL_ERROR "${PROJECT_NAME} requires Visual Studio 2015 or greater.")
+    endif()
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /W1 /bigobj")
+    if(FCL_TREAT_WARNINGS_AS_ERRORS)
+        add_definitions(/WX)
+    endif()
+endif()
+
+# Intel
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Intel")
+    set(IS_ICPC 1)
+else()
+    set(IS_ICPC 0)
+endif()
+if(IS_ICPC)
+    add_definitions(-std=c++11 -wd191 -wd411 -wd654 -wd1125 -wd1292 -wd1565 -wd1628 -wd2196)
+    set(CMAKE_AR "xiar" CACHE STRING "Intel archiver" FORCE)
+    set(CMAKE_CXX_FLAGS "-pthread" CACHE STRING "Default compile flags" FORCE)
+    set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG"
+    CACHE STRING "Flags used by the C++ compiler during release builds." FORCE)
+    set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g" CACHE STRING
+    "Flags used by the C++ compiler during debug builds." FORCE)
+    set(CMAKE_LINKER "xild" CACHE STRING "Intel linker" FORCE)
+    if(FCL_TREAT_WARNINGS_AS_ERRORS)
+        add_definitions(-Werror)
+    endif()
+endif()
+
+# XL
+if(CMAKE_CXX_COMPILER_ID STREQUAL "XL")
+    set(IS_XLC 1)
+else()
+    set(IS_XLC 0)
+endif()
+if(IS_XLC)
+    add_definitions(-std=c++11 -qpic -q64 -qmaxmem=-1)
+    set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -q64")
+    set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -q64")
+endif()
+
+# MinGW
+if((CMAKE_COMPILER_IS_GNUCXX OR IS_ICPC) AND NOT MINGW)
+    add_definitions(-fPIC)
+    if(FCL_TREAT_WARNINGS_AS_ERRORS)
+        add_definitions(-Werror)
+    endif()
+endif()
+
+# By default CMake sets RPATH for binaries in the build tree, but clears
+# it when installing. Switch this option off if the default behaviour is
+# desired.
+option(FCL_NO_DEFAULT_RPATH "Set RPATH for installed binaries" ON)
+mark_as_advanced(FCL_NO_DEFAULT_RPATH)
+
+# Set rpath http://www.paraview.org/Wiki/CMake_RPATH_handling
+if(FCL_NO_DEFAULT_RPATH)
+    message(STATUS "Set RPATH explicitly to '${CMAKE_INSTALL_LIBDIR_FULL}'")
+    set(CMAKE_SKIP_BUILD_RPATH FALSE)
+    set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+    set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_LIBDIR_FULL}")
+    set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+endif()
+
+# no prefix needed for python modules
+set(CMAKE_SHARED_MODULE_PREFIX "")

3rdparty/fcl/CMakeModules/Coveralls.cmake

@@ -0,0 +1,128 @@
+#
+# The MIT License (MIT)
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+#
+# Copyright (C) 2014 Joakim Söderberg <joakim.soderberg@gmail.com>
+#
+
+set(_CMAKE_SCRIPT_PATH ${CMAKE_CURRENT_LIST_DIR}) # must be outside coveralls_setup() to get correct path
+
+#
+# Param _COVERAGE_SRCS	A list of source files that coverage should be collected for.
+# Param _COVERALLS_UPLOAD Upload the result to coveralls?
+#
+
+function(coveralls_setup _COVERAGE_SRCS _COVERALLS_UPLOAD)
+
+	if (ARGC GREATER 2)
+		set(_CMAKE_SCRIPT_PATH ${ARGN})
+		message(STATUS "Coveralls: Using alternate CMake script dir: ${_CMAKE_SCRIPT_PATH}")
+	endif()
+
+	if (NOT EXISTS "${_CMAKE_SCRIPT_PATH}/CoverallsClear.cmake")
+		message(FATAL_ERROR "Coveralls: Missing ${_CMAKE_SCRIPT_PATH}/CoverallsClear.cmake")
+	endif()
+
+	if (NOT EXISTS "${_CMAKE_SCRIPT_PATH}/CoverallsGenerateGcov.cmake")
+		message(FATAL_ERROR "Coveralls: Missing ${_CMAKE_SCRIPT_PATH}/CoverallsGenerateGcov.cmake")
+	endif()
+
+	# When passing a CMake list to an external process, the list
+	# will be converted from the format "1;2;3" to "1 2 3".
+	# This means the script we're calling won't see it as a list
+	# of sources, but rather just one long path. We remedy this
+	# by replacing ";" with "*" and then reversing that in the script
+	# that we're calling.
+	# http://cmake.3232098.n2.nabble.com/Passing-a-CMake-list-quot-as-is-quot-to-a-custom-target-td6505681.html
+	set(COVERAGE_SRCS_TMP ${_COVERAGE_SRCS})
+	set(COVERAGE_SRCS "")
+	foreach (COVERAGE_SRC ${COVERAGE_SRCS_TMP})
+		set(COVERAGE_SRCS "${COVERAGE_SRCS}*${COVERAGE_SRC}")
+	endforeach()
+
+	#message("Coverage sources: ${COVERAGE_SRCS}")
+	set(COVERALLS_FILE ${PROJECT_BINARY_DIR}/coveralls.json)
+
+	add_custom_target(coveralls_generate
+
+		# Zero the coverage counters.
+		COMMAND ${CMAKE_COMMAND} -DPROJECT_BINARY_DIR="${PROJECT_BINARY_DIR}" -P "${_CMAKE_SCRIPT_PATH}/CoverallsClear.cmake"
+
+		# Run regress tests.
+		COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure
+
+		# Generate Gcov and translate it into coveralls JSON.
+		# We do this by executing an external CMake script.
+		# (We don't want this to run at CMake generation time, but after compilation and everything has run).
+		COMMAND ${CMAKE_COMMAND}
+				-DCOVERAGE_SRCS="${COVERAGE_SRCS}" # TODO: This is passed like: "a b c", not "a;b;c"
+				-DCOVERALLS_OUTPUT_FILE="${COVERALLS_FILE}"
+				-DCOV_PATH="${PROJECT_BINARY_DIR}"
+				-DPROJECT_ROOT="${PROJECT_SOURCE_DIR}"
+				-P "${_CMAKE_SCRIPT_PATH}/CoverallsGenerateGcov.cmake"
+
+		WORKING_DIRECTORY ${PROJECT_BINARY_DIR}
+		COMMENT "Generating coveralls output..."
+		)
+
+	if (_COVERALLS_UPLOAD)
+		message("COVERALLS UPLOAD: ON")
+
+		find_program(CURL_EXECUTABLE curl)
+
+		if (NOT CURL_EXECUTABLE)
+			message(FATAL_ERROR "Coveralls: curl not found! Aborting")
+		endif()
+
+		add_custom_target(coveralls_upload
+			# Upload the JSON to coveralls.
+			COMMAND ${CURL_EXECUTABLE}
+					-S -F json_file=@${COVERALLS_FILE}
+					https://coveralls.io/api/v1/jobs
+
+			DEPENDS coveralls_generate
+
+			WORKING_DIRECTORY ${PROJECT_BINARY_DIR}
+			COMMENT "Uploading coveralls output...")
+
+		add_custom_target(coveralls DEPENDS coveralls_upload)
+	else()
+		message("COVERALLS UPLOAD: OFF")
+		add_custom_target(coveralls DEPENDS coveralls_generate)
+	endif()
+
+endfunction()
+
+macro(coveralls_turn_on_coverage)
+	if(NOT (CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
+		AND (NOT "${CMAKE_C_COMPILER_ID}" STREQUAL "Clang"))
+		message(FATAL_ERROR "Coveralls: Compiler ${CMAKE_C_COMPILER_ID} is not GNU gcc! Aborting... You can set this on the command line using CC=/usr/bin/gcc CXX=/usr/bin/g++ cmake <options> ..")
+	endif()
+
+	if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+		message(FATAL_ERROR "Coveralls: Code coverage results with an optimised (non-Debug) build may be misleading! Add -DCMAKE_BUILD_TYPE=Debug")
+	endif()
+
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage")
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage")
+endmacro()
+
+
+

3rdparty/fcl/CMakeModules/CoverallsClear.cmake

@@ -0,0 +1,31 @@
+#
+# The MIT License (MIT)
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+#
+# Copyright (C) 2014 Joakim Söderberg <joakim.soderberg@gmail.com>
+#
+
+# do not follow symlinks in file(GLOB_RECURSE ...)
+cmake_policy(SET CMP0009 NEW)
+
+file(GLOB_RECURSE GCDA_FILES "${PROJECT_BINARY_DIR}/*.gcda")
+if(NOT GCDA_FILES STREQUAL "")
+  file(REMOVE ${GCDA_FILES})
+endif()

3rdparty/fcl/CMakeModules/CoverallsGenerateGcov.cmake

@@ -0,0 +1,480 @@
+#
+# The MIT License (MIT)
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+#
+# Copyright (C) 2014 Joakim Söderberg <joakim.soderberg@gmail.com>
+#
+# This is intended to be run by a custom target in a CMake project like this.
+# 0. Compile program with coverage support.
+# 1. Clear coverage data. (Recursively delete *.gcda in build dir)
+# 2. Run the unit tests.
+# 3. Run this script specifying which source files the coverage should be performed on.
+#
+# This script will then use gcov to generate .gcov files in the directory specified
+# via the COV_PATH var. This should probably be the same as your cmake build dir.
+#
+# It then parses the .gcov files to convert them into the Coveralls JSON format:
+# https://coveralls.io/docs/api
+#
+# Example for running as standalone CMake script from the command line:
+# (Note it is important the -P is at the end...)
+# $ cmake -DCOV_PATH=$(pwd)
+#         -DCOVERAGE_SRCS="catcierge_rfid.c;catcierge_timer.c"
+#         -P ../cmake/CoverallsGcovUpload.cmake
+#
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+
+
+#
+# Make sure we have the needed arguments.
+#
+if (NOT COVERALLS_OUTPUT_FILE)
+	message(FATAL_ERROR "Coveralls: No coveralls output file specified. Please set COVERALLS_OUTPUT_FILE")
+endif()
+
+if (NOT COV_PATH)
+	message(FATAL_ERROR "Coveralls: Missing coverage directory path where gcov files will be generated. Please set COV_PATH")
+endif()
+
+if (NOT COVERAGE_SRCS)
+	message(FATAL_ERROR "Coveralls: Missing the list of source files that we should get the coverage data for COVERAGE_SRCS")
+endif()
+
+if (NOT PROJECT_ROOT)
+	message(FATAL_ERROR "Coveralls: Missing PROJECT_ROOT.")
+endif()
+
+# Since it's not possible to pass a CMake list properly in the
+# "1;2;3" format to an external process, we have replaced the
+# ";" with "*", so reverse that here so we get it back into the
+# CMake list format.
+string(REGEX REPLACE "\\*" ";" COVERAGE_SRCS ${COVERAGE_SRCS})
+
+if (NOT DEFINED ENV{GCOV})
+  find_program(GCOV_EXECUTABLE gcov)
+else()
+  find_program(GCOV_EXECUTABLE $ENV{GCOV})
+endif()
+
+# convert all paths in COVERAGE_SRCS to absolute paths
+set(COVERAGE_SRCS_TMP "")
+foreach (COVERAGE_SRC ${COVERAGE_SRCS})
+	if (NOT "${COVERAGE_SRC}" MATCHES "^/")
+		set(COVERAGE_SRC ${PROJECT_ROOT}/${COVERAGE_SRC})
+	endif()
+	list(APPEND COVERAGE_SRCS_TMP ${COVERAGE_SRC})
+endforeach()
+set(COVERAGE_SRCS ${COVERAGE_SRCS_TMP})
+unset(COVERAGE_SRCS_TMP)
+
+if (NOT GCOV_EXECUTABLE)
+	message(FATAL_ERROR "gcov not found! Aborting...")
+endif()
+
+find_package(Git)
+
+set(JSON_REPO_TEMPLATE
+  "{
+    \"head\": {
+      \"id\": \"\@GIT_COMMIT_HASH\@\",
+      \"author_name\": \"\@GIT_AUTHOR_NAME\@\",
+      \"author_email\": \"\@GIT_AUTHOR_EMAIL\@\",
+      \"committer_name\": \"\@GIT_COMMITTER_NAME\@\",
+      \"committer_email\": \"\@GIT_COMMITTER_EMAIL\@\",
+      \"message\": \"\@GIT_COMMIT_MESSAGE\@\"
+    },
+    \"branch\": \"@GIT_BRANCH@\",
+    \"remotes\": []
+  }"
+)
+
+# TODO: Fill in git remote data
+if (GIT_FOUND)
+	# Branch.
+	execute_process(
+		COMMAND ${GIT_EXECUTABLE} rev-parse --abbrev-ref HEAD
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		OUTPUT_VARIABLE GIT_BRANCH
+		OUTPUT_STRIP_TRAILING_WHITESPACE
+	)
+
+	macro (git_log_format FORMAT_CHARS VAR_NAME)
+		execute_process(
+			COMMAND ${GIT_EXECUTABLE} log -1 --pretty=format:%${FORMAT_CHARS}
+			WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+			OUTPUT_VARIABLE ${VAR_NAME}
+			OUTPUT_STRIP_TRAILING_WHITESPACE
+		)
+	endmacro()
+
+	git_log_format(an GIT_AUTHOR_NAME)
+	git_log_format(ae GIT_AUTHOR_EMAIL)
+	git_log_format(cn GIT_COMMITTER_NAME)
+	git_log_format(ce GIT_COMMITTER_EMAIL)
+	git_log_format(B GIT_COMMIT_MESSAGE)
+	git_log_format(H GIT_COMMIT_HASH)
+
+	string(REPLACE "\n" "\\n" GIT_COMMIT_MESSAGE ${GIT_COMMIT_MESSAGE})
+
+	message("Git exe: ${GIT_EXECUTABLE}")
+	message("Git branch: ${GIT_BRANCH}")
+	message("Git author: ${GIT_AUTHOR_NAME}")
+	message("Git e-mail: ${GIT_AUTHOR_EMAIL}")
+	message("Git commiter name: ${GIT_COMMITTER_NAME}")
+	message("Git commiter e-mail: ${GIT_COMMITTER_EMAIL}")
+	message("Git commit hash: ${GIT_COMMIT_HASH}")
+	message("Git commit message: ${GIT_COMMIT_MESSAGE}")
+
+	string(CONFIGURE ${JSON_REPO_TEMPLATE} JSON_REPO_DATA)
+else()
+	set(JSON_REPO_DATA "{}")
+endif()
+
+############################# Macros #########################################
+
+#
+# This macro converts from the full path format gcov outputs:
+#
+#    /path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov
+#
+# to the original source file path the .gcov is for:
+#
+#   /path/to/project/root/subdir/the_file.c
+#
+macro(get_source_path_from_gcov_filename _SRC_FILENAME _GCOV_FILENAME)
+
+	# /path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov
+	# ->
+	# #path#to#project#root#subdir#the_file.c.gcov
+	get_filename_component(_GCOV_FILENAME_WEXT ${_GCOV_FILENAME} NAME)
+
+	# #path#to#project#root#subdir#the_file.c.gcov -> /path/to/project/root/subdir/the_file.c
+	string(REGEX REPLACE "\\.gcov$" "" SRC_FILENAME_TMP ${_GCOV_FILENAME_WEXT})
+	string(REGEX REPLACE "\#" "/" SRC_FILENAME_TMP ${SRC_FILENAME_TMP})
+	set(${_SRC_FILENAME} "${SRC_FILENAME_TMP}")
+endmacro()
+
+##############################################################################
+
+# Get the coverage data.
+file(GLOB_RECURSE GCDA_FILES "${COV_PATH}/*.gcda")
+message("GCDA files:")
+
+# Get a list of all the object directories needed by gcov
+# (The directories the .gcda files and .o files are found in)
+# and run gcov on those.
+foreach(GCDA ${GCDA_FILES})
+	message("Process: ${GCDA}")
+	message("------------------------------------------------------------------------------")
+	get_filename_component(GCDA_DIR ${GCDA} PATH)
+
+	#
+	# The -p below refers to "Preserve path components",
+	# This means that the generated gcov filename of a source file will
+	# keep the original files entire filepath, but / is replaced with #.
+	# Example:
+	#
+	# /path/to/project/root/build/CMakeFiles/the_file.dir/subdir/the_file.c.gcda
+	# ------------------------------------------------------------------------------
+	# File '/path/to/project/root/subdir/the_file.c'
+	# Lines executed:68.34% of 199
+	# /path/to/project/root/subdir/the_file.c:creating '#path#to#project#root#subdir#the_file.c.gcov'
+	#
+	# If -p is not specified then the file is named only "the_file.c.gcov"
+	#
+	execute_process(
+		COMMAND ${GCOV_EXECUTABLE} -p -o ${GCDA_DIR} ${GCDA}
+		WORKING_DIRECTORY ${COV_PATH}
+	)
+endforeach()
+
+# TODO: Make these be absolute path
+file(GLOB ALL_GCOV_FILES ${COV_PATH}/*.gcov)
+
+# Get only the filenames to use for filtering.
+#set(COVERAGE_SRCS_NAMES "")
+#foreach (COVSRC ${COVERAGE_SRCS})
+#	get_filename_component(COVSRC_NAME ${COVSRC} NAME)
+#	message("${COVSRC} -> ${COVSRC_NAME}")
+#	list(APPEND COVERAGE_SRCS_NAMES "${COVSRC_NAME}")
+#endforeach()
+
+#
+# Filter out all but the gcov files we want.
+#
+# We do this by comparing the list of COVERAGE_SRCS filepaths that the
+# user wants the coverage data for with the paths of the generated .gcov files,
+# so that we only keep the relevant gcov files.
+#
+# Example:
+# COVERAGE_SRCS =
+#				/path/to/project/root/subdir/the_file.c
+#
+# ALL_GCOV_FILES =
+#				/path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov
+#				/path/to/project/root/build/#path#to#project#root#subdir#other_file.c.gcov
+#
+# Result should be:
+# GCOV_FILES =
+#				/path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov
+#
+set(GCOV_FILES "")
+#message("Look in coverage sources: ${COVERAGE_SRCS}")
+message("\nFilter out unwanted GCOV files:")
+message("===============================")
+
+set(COVERAGE_SRCS_REMAINING ${COVERAGE_SRCS})
+
+foreach (GCOV_FILE ${ALL_GCOV_FILES})
+
+	#
+	# /path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov
+	# ->
+	# /path/to/project/root/subdir/the_file.c
+	get_source_path_from_gcov_filename(GCOV_SRC_PATH ${GCOV_FILE})
+	file(RELATIVE_PATH GCOV_SRC_REL_PATH "${PROJECT_ROOT}" "${GCOV_SRC_PATH}")
+
+	# Is this in the list of source files?
+	# TODO: We want to match against relative path filenames from the source file root...
+	list(FIND COVERAGE_SRCS ${GCOV_SRC_PATH} WAS_FOUND)
+
+	if (NOT WAS_FOUND EQUAL -1)
+		message("YES: ${GCOV_FILE}")
+		list(APPEND GCOV_FILES ${GCOV_FILE})
+
+		# We remove it from the list, so we don't bother searching for it again.
+		# Also files left in COVERAGE_SRCS_REMAINING after this loop ends should
+		# have coverage data generated from them (no lines are covered).
+		list(REMOVE_ITEM COVERAGE_SRCS_REMAINING ${GCOV_SRC_PATH})
+	else()
+		message("NO:  ${GCOV_FILE}")
+	endif()
+endforeach()
+
+# TODO: Enable setting these
+set(JSON_SERVICE_NAME "travis-ci")
+set(JSON_SERVICE_JOB_ID $ENV{TRAVIS_JOB_ID})
+set(JSON_REPO_TOKEN $ENV{COVERALLS_REPO_TOKEN})
+
+set(JSON_TEMPLATE
+"{
+  \"repo_token\": \"\@JSON_REPO_TOKEN\@\",
+  \"service_name\": \"\@JSON_SERVICE_NAME\@\",
+  \"service_job_id\": \"\@JSON_SERVICE_JOB_ID\@\",
+  \"source_files\": \@JSON_GCOV_FILES\@,
+  \"git\": \@JSON_REPO_DATA\@
+}"
+)
+
+set(SRC_FILE_TEMPLATE
+"{
+      \"name\": \"\@GCOV_SRC_REL_PATH\@\",
+      \"source_digest\": \"\@GCOV_CONTENTS_MD5\@\",
+      \"coverage\": \@GCOV_FILE_COVERAGE\@
+  }"
+)
+
+message("\nGenerate JSON for files:")
+message("=========================")
+
+set(JSON_GCOV_FILES "[")
+
+# Read the GCOV files line by line and get the coverage data.
+foreach (GCOV_FILE ${GCOV_FILES})
+
+	get_source_path_from_gcov_filename(GCOV_SRC_PATH ${GCOV_FILE})
+	file(RELATIVE_PATH GCOV_SRC_REL_PATH "${PROJECT_ROOT}" "${GCOV_SRC_PATH}")
+
+	# The new coveralls API doesn't need the entire source (Yay!)
+	# However, still keeping that part for now. Will cleanup in the future.
+	file(MD5 "${GCOV_SRC_PATH}" GCOV_CONTENTS_MD5)
+	message("MD5: ${GCOV_SRC_PATH} = ${GCOV_CONTENTS_MD5}")
+
+	# Loads the gcov file as a list of lines.
+	# (We first open the file and replace all occurences of [] with _
+	#  because CMake will fail to parse a line containing unmatched brackets...
+	#  also the \ to escaped \n in macros screws up things.)
+	# https://public.kitware.com/Bug/view.php?id=15369
+	file(READ ${GCOV_FILE} GCOV_CONTENTS)
+	string(REPLACE "[" "_" GCOV_CONTENTS "${GCOV_CONTENTS}")
+	string(REPLACE "]" "_" GCOV_CONTENTS "${GCOV_CONTENTS}")
+	string(REPLACE "\\" "_" GCOV_CONTENTS "${GCOV_CONTENTS}")
+
+	# Remove file contents to avoid encoding issues (cmake 2.8 has no ENCODING option)
+	string(REGEX REPLACE "([^:]*):([^:]*):([^\n]*)\n" "\\1:\\2: \n" GCOV_CONTENTS "${GCOV_CONTENTS}")
+	file(WRITE ${GCOV_FILE}_tmp "${GCOV_CONTENTS}")
+
+	file(STRINGS ${GCOV_FILE}_tmp GCOV_LINES)
+	list(LENGTH GCOV_LINES LINE_COUNT)
+
+	# Instead of trying to parse the source from the
+	# gcov file, simply read the file contents from the source file.
+	# (Parsing it from the gcov is hard because C-code uses ; in many places
+	#  which also happens to be the same as the CMake list delimeter).
+	file(READ ${GCOV_SRC_PATH} GCOV_FILE_SOURCE)
+
+	string(REPLACE "\\" "\\\\" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}")
+	string(REGEX REPLACE "\"" "\\\\\"" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}")
+	string(REPLACE "\t" "\\\\t" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}")
+	string(REPLACE "\r" "\\\\r" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}")
+	string(REPLACE "\n" "\\\\n" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}")
+	# According to http://json.org/ these should be escaped as well.
+	# Don't know how to do that in CMake however...
+	#string(REPLACE "\b" "\\\\b" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}")
+	#string(REPLACE "\f" "\\\\f" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}")
+	#string(REGEX REPLACE "\u([a-fA-F0-9]{4})" "\\\\u\\1" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}")
+
+	# We want a json array of coverage data as a single string
+	# start building them from the contents of the .gcov
+	set(GCOV_FILE_COVERAGE "[")
+
+	set(GCOV_LINE_COUNT 1) # Line number for the .gcov.
+	set(DO_SKIP 0)
+	foreach (GCOV_LINE ${GCOV_LINES})
+		#message("${GCOV_LINE}")
+		# Example of what we're parsing:
+		# Hitcount  |Line | Source
+		# "        8:   26:        if (!allowed || (strlen(allowed) == 0))"
+		string(REGEX REPLACE
+			"^([^:]*):([^:]*):(.*)$"
+			"\\1;\\2;\\3"
+			RES
+			"${GCOV_LINE}")
+
+		# Check if we should exclude lines using the Lcov syntax.
+		string(REGEX MATCH "LCOV_EXCL_START" START_SKIP "${GCOV_LINE}")
+		string(REGEX MATCH "LCOV_EXCL_END" END_SKIP "${GCOV_LINE}")
+		string(REGEX MATCH "LCOV_EXCL_LINE" LINE_SKIP "${GCOV_LINE}")
+
+		set(RESET_SKIP 0)
+		if (LINE_SKIP AND NOT DO_SKIP)
+			set(DO_SKIP 1)
+			set(RESET_SKIP 1)
+		endif()
+
+		if (START_SKIP)
+			set(DO_SKIP 1)
+			message("${GCOV_LINE_COUNT}: Start skip")
+		endif()
+
+		if (END_SKIP)
+			set(DO_SKIP 0)
+		endif()
+
+		list(LENGTH RES RES_COUNT)
+
+		if (RES_COUNT GREATER 2)
+			list(GET RES 0 HITCOUNT)
+			list(GET RES 1 LINE)
+			list(GET RES 2 SOURCE)
+
+			string(STRIP ${HITCOUNT} HITCOUNT)
+			string(STRIP ${LINE} LINE)
+
+			# Lines with 0 line numbers are metadata and can be ignored.
+			if (NOT ${LINE} EQUAL 0)
+
+				if (DO_SKIP)
+					set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}null, ")
+				else()
+					# Translate the hitcount into valid JSON values.
+					if (${HITCOUNT} STREQUAL "#####" OR ${HITCOUNT} STREQUAL "=====")
+						set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}0, ")
+					elseif (${HITCOUNT} STREQUAL "-")
+						set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}null, ")
+					else()
+						set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}${HITCOUNT}, ")
+					endif()
+				endif()
+			endif()
+		else()
+			message(WARNING "Failed to properly parse line (RES_COUNT = ${RES_COUNT}) ${GCOV_FILE}:${GCOV_LINE_COUNT}\n-->${GCOV_LINE}")
+		endif()
+
+		if (RESET_SKIP)
+			set(DO_SKIP 0)
+		endif()
+		math(EXPR GCOV_LINE_COUNT "${GCOV_LINE_COUNT}+1")
+	endforeach()
+
+	message("${GCOV_LINE_COUNT} of ${LINE_COUNT} lines read!")
+
+	# Advanced way of removing the trailing comma in the JSON array.
+	# "[1, 2, 3, " -> "[1, 2, 3"
+	string(REGEX REPLACE ",[ ]*$" "" GCOV_FILE_COVERAGE ${GCOV_FILE_COVERAGE})
+
+	# Append the trailing ] to complete the JSON array.
+	set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}]")
+
+	# Generate the final JSON for this file.
+	message("Generate JSON for file: ${GCOV_SRC_REL_PATH}...")
+	string(CONFIGURE ${SRC_FILE_TEMPLATE} FILE_JSON)
+
+	set(JSON_GCOV_FILES "${JSON_GCOV_FILES}${FILE_JSON}, ")
+endforeach()
+
+# Loop through all files we couldn't find any coverage for
+# as well, and generate JSON for those as well with 0% coverage.
+foreach(NOT_COVERED_SRC ${COVERAGE_SRCS_REMAINING})
+
+	# Set variables for json replacement
+	set(GCOV_SRC_PATH ${NOT_COVERED_SRC})
+	file(MD5 "${GCOV_SRC_PATH}" GCOV_CONTENTS_MD5)
+	file(RELATIVE_PATH GCOV_SRC_REL_PATH "${PROJECT_ROOT}" "${GCOV_SRC_PATH}")
+
+	# Loads the source file as a list of lines.
+	file(STRINGS ${NOT_COVERED_SRC} SRC_LINES)
+
+	set(GCOV_FILE_COVERAGE "[")
+	set(GCOV_FILE_SOURCE "")
+
+	foreach (SOURCE ${SRC_LINES})
+		set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}null, ")
+
+		string(REPLACE "\\" "\\\\" SOURCE "${SOURCE}")
+		string(REGEX REPLACE "\"" "\\\\\"" SOURCE "${SOURCE}")
+		string(REPLACE "\t" "\\\\t" SOURCE "${SOURCE}")
+		string(REPLACE "\r" "\\\\r" SOURCE "${SOURCE}")
+		set(GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}${SOURCE}\\n")
+	endforeach()
+
+	# Remove trailing comma, and complete JSON array with ]
+	string(REGEX REPLACE ",[ ]*$" "" GCOV_FILE_COVERAGE ${GCOV_FILE_COVERAGE})
+	set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}]")
+
+	# Generate the final JSON for this file.
+	message("Generate JSON for non-gcov file: ${NOT_COVERED_SRC}...")
+	string(CONFIGURE ${SRC_FILE_TEMPLATE} FILE_JSON)
+	set(JSON_GCOV_FILES "${JSON_GCOV_FILES}${FILE_JSON}, ")
+endforeach()
+
+# Get rid of trailing comma.
+string(REGEX REPLACE ",[ ]*$" "" JSON_GCOV_FILES ${JSON_GCOV_FILES})
+set(JSON_GCOV_FILES "${JSON_GCOV_FILES}]")
+
+# Generate the final complete JSON!
+message("Generate final JSON...")
+string(CONFIGURE ${JSON_TEMPLATE} JSON)
+
+file(WRITE "${COVERALLS_OUTPUT_FILE}" "${JSON}")
+message("###########################################################################")
+message("Generated coveralls JSON containing coverage data:")
+message("${COVERALLS_OUTPUT_FILE}")
+message("###########################################################################")

3rdparty/fcl/CMakeModules/FCLMacros.cmake

@@ -0,0 +1,46 @@
+#===============================================================================
+# Appends items to a cached list.
+# Usage:
+#   fcl_append_to_cached_string(_string _cacheDesc [items...])
+#===============================================================================
+macro(fcl_append_to_cached_string _string _cacheDesc)
+  foreach(newItem ${ARGN})
+    set(${_string} "${${_string}}${newItem}" CACHE INTERNAL ${_cacheDesc} FORCE)
+  endforeach()
+endmacro()
+
+#===============================================================================
+# Get list of file names give list of full paths.
+# Usage:
+#   fcl_get_filename_components(_var _cacheDesc [items...])
+#===============================================================================
+macro(fcl_get_filename_components _var _cacheDesc _prefix_to_remove)
+  set(${_var} "" CACHE INTERNAL ${_cacheDesc} FORCE)
+  string(LENGTH ${_prefix_to_remove} prefix_length)
+  foreach(header ${ARGN})
+    string(LENGTH ${header} full_length)
+    math(EXPR relative_path_length "${full_length} - ${prefix_length}")
+    string(SUBSTRING ${header} ${prefix_length} ${relative_path_length} header)
+    if(NOT ${header} MATCHES "/detail/" AND NOT ${header} MATCHES "-inl.h")
+      fcl_append_to_cached_string(
+        ${_var}
+        ${_cacheDesc}"_HEADER_NAMES"
+        "${header}\;")
+    endif()
+  endforeach()
+endmacro()
+
+#===============================================================================
+# Generate header file list to a cached list.
+# Usage:
+#   fcl_generate_include_header_list(_var _target_dir _cacheDesc [headers...])
+#===============================================================================
+macro(fcl_generate_include_header_list _var _target_dir _cacheDesc)
+  set(${_var} "" CACHE INTERNAL ${_cacheDesc} FORCE)
+  foreach(header ${ARGN})
+    fcl_append_to_cached_string(
+      ${_var}
+      ${_cacheDesc}"_HEADERS"
+      "#include \"${_target_dir}${header}\"\n")
+  endforeach()
+endmacro()

3rdparty/fcl/CMakeModules/FCLVersion.cmake

@@ -0,0 +1,8 @@
+# set the version in a way CMake can use
+set(FCL_MAJOR_VERSION 0)
+set(FCL_MINOR_VERSION 6)
+set(FCL_PATCH_VERSION 0)
+set(FCL_VERSION "${FCL_MAJOR_VERSION}.${FCL_MINOR_VERSION}.${FCL_PATCH_VERSION}")
+
+# increment this when we have ABI changes
+set(FCL_ABI_VERSION ${FCL_MAJOR_VERSION}.${FCL_MINOR_VERSION})

3rdparty/fcl/CMakeModules/FindEigen3.cmake

@@ -0,0 +1,90 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+#
+# This module reads hints about search locations from 
+# the following enviroment variables:
+#
+# EIGEN3_ROOT
+# EIGEN3_ROOT_DIR
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      HINTS
+      ENV EIGEN3_ROOT 
+      ENV EIGEN3_ROOT_DIR
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+

3rdparty/fcl/CMakeModules/cmake_uninstall.cmake.in

@@ -0,0 +1,21 @@
+if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+  message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+
+file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+string(REGEX REPLACE "\n" ";" files "${files}")
+foreach(file ${files})
+  message(STATUS "Uninstalling $ENV{DESTDIR}${file}")
+  if(EXISTS "$ENV{DESTDIR}${file}")
+    execute_process(
+      COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}"
+      OUTPUT_VARIABLE rm_out
+      RESULT_VARIABLE rm_retval
+      )
+    if(NOT "${rm_retval}" STREQUAL 0)
+      message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}")
+    endif(NOT "${rm_retval}" STREQUAL 0)
+  else(EXISTS "$ENV{DESTDIR}${file}")
+    message(STATUS "File $ENV{DESTDIR}${file} does not exist.")
+  endif(EXISTS "$ENV{DESTDIR}${file}")
+endforeach(file)

3rdparty/fcl/CPPLINT.cfg

@@ -0,0 +1,36 @@
+# Copyright (c) 2018, Toyota Research Institute, 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 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 HOLDER 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: Jamie Snape, Kitware, Inc.
+
+set noparent
+
+extensions=cc,cpp,h
+filter=-build/c++11,-build/c++14
+root=include

3rdparty/fcl/CTestConfig.cmake

@@ -0,0 +1,40 @@
+# -*- mode: cmake -*-
+# vi: set ft=cmake :
+
+# Copyright (c) 2018, Toyota Research Institute, 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 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 HOLDER 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: Jamie Snape, Kitware, Inc.
+
+set(CTEST_PROJECT_NAME fcl)
+set(CTEST_NIGHTLY_START_TIME "00:00:00 EST")
+set(CTEST_DROP_METHOD https)
+set(CTEST_DROP_SITE drake-cdash.csail.mit.edu)
+set(CTEST_DROP_LOCATION "/submit.php?project=${CTEST_PROJECT_NAME}")
+set(CTEST_DROP_SITE_CDASH ON)

3rdparty/fcl/CTestCustom.cmake.in

@@ -0,0 +1,38 @@
+# -*- mode: cmake -*-
+# vi: set ft=cmake :
+
+# Copyright (c) 2018, Toyota Research Institute, 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 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 HOLDER 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: Jamie Snape, Kitware, Inc.
+
+list(APPEND CTEST_CUSTOM_COVERAGE_EXCLUDE ".*/test/.*")
+
+set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS 128)
+set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS 128)

3rdparty/fcl/INSTALL

@@ -0,0 +1,32 @@
+
+Dependencies:
+============
+
+ - Eigen (available at http://eigen.tuxfamily.org/)
+ - libccd (available at http://libccd.danfis.cz/)
+ - octomap (optional dependency, available at http://octomap.github.com)
+
+Eigen and libccd are mandatory dependencies. If octomap is not found,
+collision detection with octrees will not be possible.
+
+For installation, CMake will also be needed (http://cmake.org).
+
+Install:
+=======
+
+* Linux / Mac OS:
+  The CMakeLists.txt can be used to generate makefiles; For example, one may use operations such as:
+
+  mkdir build
+  cd build
+  cmake ..
+  make -jN # N is the maximum number of parallel compile jobs
+  
+Once the compilation is finished,
+  make install
+will install the project. To specify the installation prefix,
+pass the parameter -DCMAKE_INSTALL_PREFIX=/my/prefix/ to the "cmake .." command above.
+
+
+* Visual Studio (2015 or higher is required):
+  The CMakeLists.txt can be used to generate a Visual Studio project, using the cmake build tool.

3rdparty/fcl/LICENSE

@@ -0,0 +1,32 @@
+Software License Agreement (BSD License)
+
+ Copyright (c) 2008-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.

3rdparty/fcl/README.md

@@ -0,0 +1,184 @@
+## FCL -- The Flexible Collision Library 
+
+Linux / OS X [![Build Status](https://travis-ci.org/flexible-collision-library/fcl.svg?branch=master)](https://travis-ci.org/flexible-collision-library/fcl)
+Windows [![Build status](https://ci.appveyor.com/api/projects/status/do1k727uu6e8uemf/branch/master?svg=true)](https://ci.appveyor.com/project/flexible-collision-library/fcl)
+Coverage [![Coverage Status](https://coveralls.io/repos/github/flexible-collision-library/fcl/badge.svg?branch=master)](https://coveralls.io/github/flexible-collision-library/fcl?branch=master)
+
+FCL is a library for performing three types of proximity queries on a pair of geometric models composed of triangles. 
+ - Collision detection: detecting whether the two models overlap, and optionally, all of the triangles that overlap.
+ - Distance computation: computing the minimum distance between a pair of models, i.e., the distance between the closest pair of points.
+ - Tolerance verification: determining whether two models are closer or farther than a tolerance distance.
+ - Continuous collision detection: detecting whether the two moving models overlap during the movement, and optionally, the time of contact.
+ - Contact information: for collision detection and continuous collision detection, the contact information (including contact normals and contact points) can be returned optionally.
+
+FCL has the following features
+ - C++ interface
+ - Compilable for either linux or win32 (both makefiles and Microsoft Visual projects can be generated using cmake)
+ - No special topological constraints or adjacency information required for input models – all that is necessary is a list of the model's triangles
+ - Supported different object shapes:
+  + box
+  + sphere
+  + ellipsoid
+  + capsule
+  + cone
+  + cylinder
+  + convex
+  + half-space
+  + plane
+  + mesh
+  + octree (optional, octrees are represented using the octomap library http://octomap.github.com)
+
+
+## Installation
+
+Before compiling FCL, please make sure Eigen and libccd (for collision checking between convex objects and is available here https://github.com/danfis/libccd) are installed. For libccd, make sure to compile from github version instead of the zip file from the webpage, because one bug fixing is not included in the zipped version.
+
+Some optional libraries need to be installed for some optional capability of FCL. For octree collision, please install the octomap library from http://octomap.github.com.
+
+CMakeLists.txt is used to generate makefiles in Linux or Visual studio projects in windows. In command line, run
+``` cmake
+mkdir build
+cd build
+cmake ..
+```
+Next, in linux, use make to compile the code. 
+
+In windows, there will generate a visual studio project and then you can compile the code.
+
+## Interfaces
+Before starting the proximity computation, we need first to set the geometry and transform for the objects involving in computation. The geometry of an object is represented as a mesh soup, which can be set as follows:
+
+```cpp
+// set mesh triangles and vertice indices
+std::vector<Vec3f> vertices;
+std::vector<Triangle> triangles;
+// code to set the vertices and triangles
+...
+// BVHModel is a template class for mesh geometry, for default OBBRSS template is used
+typedef BVHModel<OBBRSS> Model;
+Model* model = new Model();
+// add the mesh data into the BVHModel structure
+model->beginModel();
+model->addSubModel(vertices, triangles);
+model->endModel();
+```
+
+The transform of an object includes the rotation and translation:
+```cpp
+// R and T are the rotation matrix and translation vector
+Matrix3f R;
+Vec3f T;
+// code for setting R and T
+...
+// transform is configured according to R and T
+Transform3f pose(R, T);
+```
+
+
+Given the geometry and the transform, we can also combine them together to obtain a collision object instance and here is an example:
+```cpp
+//geom and tf are the geometry and the transform of the object
+BVHModel<OBBRSS>* geom = ...
+Transform3f tf = ...
+//Combine them together
+CollisionObject* obj = new CollisionObject(geom, tf);
+```
+
+Once the objects are set, we can perform the proximity computation between them. All the proximity queries in FCL follow a common pipeline: first, set the query request data structure and then run the query function by using request as the input. The result is returned in a query result data structure. For example, for collision checking, we first set the CollisionRequest data structure, and then run the collision function:
+```cpp
+// Given two objects o1 and o2
+CollisionObject* o1 = ...
+CollisionObject* o2 = ...
+// set the collision request structure, here we just use the default setting
+CollisionRequest request;
+// result will be returned via the collision result structure
+CollisionResult result;
+// perform collision test
+collide(o1, o2, request, result);
+```
+
+By setting the collision request, the user can easily choose whether to return contact information (which is slower) or just return binary collision results (which is faster). 
+
+
+For distance computation, the pipeline is almost the same:
+
+```cpp
+// Given two objects o1 and o2
+CollisionObject* o1 = ...
+CollisionObject* o2 = ...
+// set the distance request structure, here we just use the default setting
+DistanceRequest request;
+// result will be returned via the collision result structure
+DistanceResult result;
+// perform distance test
+distance(o1, o2, request, result);
+```
+
+For continuous collision, FCL requires the goal transform to be provided (the initial transform is included in the collision object data structure). Beside that, the pipeline is almost the same as distance/collision:
+
+```cpp
+// Given two objects o1 and o2
+CollisionObject* o1 = ...
+CollisionObject* o2 = ...
+// The goal transforms for o1 and o2
+Transform3f tf_goal_o1 = ...
+Transform3f tf_goal_o2 = ...
+// set the continuous collision request structure, here we just use the default setting
+ContinuousCollisionRequest request;
+// result will be returned via the continuous collision result structure
+ContinuousCollisionResult result;
+// perform continuous collision test
+continuousCollide(o1, tf_goal_o1, o2, tf_goal_o2, request, result);
+```
+
+FCL supports broadphase collision/distance between two groups of objects and can avoid the n square complexity. For collision, broadphase algorithm can return all the collision pairs. For distance, it can return the pair with the minimum distance. FCL uses a CollisionManager structure to manage all the objects involving the collision or distance operations.
+```cpp
+// Initialize the collision manager for the first group of objects. 
+// FCL provides various different implementations of CollisionManager.
+// Generally, the DynamicAABBTreeCollisionManager would provide the best performance.
+BroadPhaseCollisionManager* manager1 = new DynamicAABBTreeCollisionManager(); 
+// Initialize the collision manager for the second group of objects.
+BroadPhaseCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
+// To add objects into the collision manager, using BroadPhaseCollisionManager::registerObject() function to add one object
+std::vector<CollisionObject*> objects1 = ...
+for(std::size_t i = 0; i < objects1.size(); ++i)
+manager1->registerObject(objects1[i]);
+// Another choose is to use BroadPhaseCollisionManager::registerObjects() function to add a set of objects
+std::vector<CollisionObject*> objects2 = ...
+manager2->registerObjects(objects2);
+// In order to collect the information during broadphase, CollisionManager requires two settings: 
+// a) a callback to collision or distance; 
+// b) an intermediate data to store the information generated during the broadphase computation
+// For a), FCL provides the default callbacks for both collision and distance.
+// For b), FCL uses the CollisionData structure for collision and DistanceData structure for distance. CollisionData/DistanceData is just a container including both the CollisionRequest/DistanceRequest and CollisionResult/DistanceResult structures mentioned above.
+CollisionData collision_data;
+DistanceData distance_data;
+// Setup the managers, which is related with initializing the broadphase acceleration structure according to objects input
+manager1->setup();
+manager2->setup();
+// Examples for various queries
+// 1. Collision query between two object groups and get collision numbers
+manager2->collide(manager1, &collision_data, defaultCollisionFunction);
+int n_contact_num = collision_data.result.numContacts(); 
+// 2. Distance query between two object groups and get the minimum distance
+manager2->distance(manager1, &distance_data, defaultDistanceFunction);
+double min_distance = distance_data.result.min_distance;
+// 3. Self collision query for group 1
+manager1->collide(&collision_data, defaultCollisionFunction);
+// 4. Self distance query for group 1
+manager1->distance(&distance_data, defaultDistanceFunction);
+// 5. Collision query between one object in group 1 and the entire group 2
+manager2->collide(objects1[0], &collision_data, defaultCollisionFunction);
+// 6. Distance query between one object in group 1 and the entire group 2
+manager2->distance(objects1[0], &distance_data, defaultDistanceFunction); 
+```
+
+
+For more examples, please refer to the test folder:
+- test_fcl_collision.cpp: provide examples for collision test
+- test_fcl_distance.cpp: provide examples for distance test
+- test_fcl_broadphase.cpp: provide examples for broadphase collision/distance test
+- test_fcl_frontlist.cpp: provide examples for frontlist collision acceleration
+#- test_fcl_global_penetration.cpp: provide examples for global penetration depth test
+#- test_fcl_xmldata.cpp: provide examples for more global penetration depth test based on xml data
+- test_fcl_octomap.cpp: provide examples for collision/distance computation between octomap data and other data types.

3rdparty/fcl/ci/install_linux.sh

@@ -0,0 +1,15 @@
+sudo add-apt-repository --yes ppa:libccd-debs/ppa
+sudo apt-get -qq update
+
+sudo apt-get -qq --yes --force-yes install libeigen3-dev
+sudo apt-get -qq --yes --force-yes install libccd-dev
+
+# Octomap
+git clone https://github.com/OctoMap/octomap
+cd octomap
+git checkout tags/v1.8.0
+mkdir build
+cd build
+cmake ..
+make
+sudo make install

3rdparty/fcl/ci/install_osx.sh

@@ -0,0 +1,16 @@
+brew update > /dev/null
+
+brew install git
+brew install cmake
+brew install eigen
+brew install libccd
+
+# Octomap
+git clone https://github.com/OctoMap/octomap
+cd octomap
+git checkout tags/v1.8.0
+mkdir build
+cd build
+cmake ..
+make
+sudo make install

3rdparty/fcl/fcl.pc.in

@@ -0,0 +1,12 @@
+# This file was generated by CMake for @PROJECT_NAME@
+prefix=@CMAKE_INSTALL_PREFIX@
+exec_prefix=${prefix}
+libdir=@CMAKE_INSTALL_FULL_LIBDIR@
+includedir=@CMAKE_INSTALL_FULL_INCLUDEDIR@
+
+Name: @PROJECT_NAME@
+Description: @PKG_DESC@
+Version: @FCL_VERSION@
+Requires: @PKG_EXTERNAL_DEPS@
+Libs: -L${libdir} -l@PROJECT_NAME@
+Cflags: @PKG_CFLAGS@ -I${includedir}

3rdparty/fcl/include/fcl/CMakeLists.txt

@@ -0,0 +1,44 @@
+file(GLOB_RECURSE HEADERS            ${CMAKE_CURRENT_SOURCE_DIR}/*.h)
+file(GLOB_RECURSE CONFIGURED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/*.h)
+set(FCL_HEADERS ${HEADERS} ${CONFIGURED_HEADERS} PARENT_SCOPE)
+
+# Generate export header. There is no way of generating a file name
+# called just export.h. Workaround using configure and remove
+generate_export_header(${PROJECT_NAME})
+configure_file(${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_export.h
+  ${CMAKE_CURRENT_BINARY_DIR}/export.h
+  COPYONLY)
+file(REMOVE ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_export.h)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/export.h
+  DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/fcl)
+
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/config.h.in ${CMAKE_CURRENT_BINARY_DIR}/config.h)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/config.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/fcl)
+
+fcl_get_filename_components(
+  fcl_header_rel_paths
+  "fcl headers"
+  ${CMAKE_CURRENT_SOURCE_DIR}
+  ${HEADERS})
+fcl_get_filename_components(
+  fcl_config_header_rel_paths
+  "fcl configured headers"
+  ${CMAKE_CURRENT_BINARY_DIR}
+  ${CMAKE_CURRENT_BINARY_DIR}/config.h)
+list(INSERT fcl_header_rel_paths 0 ${fcl_config_header_rel_paths})
+
+fcl_generate_include_header_list(
+  fcl_headers
+  "fcl"
+  "fcl header relative paths"
+  ${fcl_header_rel_paths})
+configure_file(
+  ${CMAKE_CURRENT_SOURCE_DIR}/fcl.h.in
+  ${CMAKE_CURRENT_BINARY_DIR}/fcl.h)
+
+install(
+  FILES ${hdrs} ${CMAKE_CURRENT_BINARY_DIR}/fcl.h
+  DESTINATION include/fcl
+  COMPONENT headers)
+
+list(APPEND FCL_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/fcl.h)

3rdparty/fcl/include/fcl/broadphase/broadphase_SSaP-inl.h

@@ -0,0 +1,595 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_SSAP_INL_H
+#define FCL_BROAD_PHASE_SSAP_INL_H
+
+#include "fcl/broadphase/broadphase_SSaP.h"
+
+namespace fcl
+{
+
+//==============================================================================
+extern template
+class FCL_EXPORT SSaPCollisionManager<double>;
+
+/** @brief Functor sorting objects according to the AABB<S> lower x bound */
+template <typename S>
+struct SortByXLow
+{
+  bool operator()(const CollisionObject<S>* a, const CollisionObject<S>* b) const
+  {
+    if(a->getAABB().min_[0] < b->getAABB().min_[0])
+      return true;
+    return false;
+  }
+};
+
+/** @brief Functor sorting objects according to the AABB<S> lower y bound */
+template <typename S>
+struct SortByYLow
+{
+  bool operator()(const CollisionObject<S>* a, const CollisionObject<S>* b) const
+  {
+    if(a->getAABB().min_[1] < b->getAABB().min_[1])
+      return true;
+    return false;
+  }
+};
+
+/** @brief Functor sorting objects according to the AABB<S> lower z bound */
+template <typename S>
+struct SortByZLow
+{
+  bool operator()(const CollisionObject<S>* a, const CollisionObject<S>* b) const
+  {
+    if(a->getAABB().min_[2] < b->getAABB().min_[2])
+      return true;
+    return false;
+  }
+};
+
+/** @brief Dummy collision object with a point AABB<S> */
+template <typename S>
+class FCL_EXPORT DummyCollisionObject : public CollisionObject<S>
+{
+public:
+  DummyCollisionObject(const AABB<S>& aabb_) : CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>())
+  {
+    this->aabb = aabb_;
+  }
+
+  void computeLocalAABB() {}
+};
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::unregisterObject(CollisionObject<S>* obj)
+{
+  setup();
+
+  DummyCollisionObject<S> dummyHigh(AABB<S>(obj->getAABB().max_));
+
+  auto pos_start1 = objs_x.begin();
+  auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow<S>());
+
+  while(pos_start1 < pos_end1)
+  {
+    if(*pos_start1 == obj)
+    {
+      objs_x.erase(pos_start1);
+      break;
+    }
+    ++pos_start1;
+  }
+
+  auto pos_start2 = objs_y.begin();
+  auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow<S>());
+
+  while(pos_start2 < pos_end2)
+  {
+    if(*pos_start2 == obj)
+    {
+      objs_y.erase(pos_start2);
+      break;
+    }
+    ++pos_start2;
+  }
+
+  auto pos_start3 = objs_z.begin();
+  auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow<S>());
+
+  while(pos_start3 < pos_end3)
+  {
+    if(*pos_start3 == obj)
+    {
+      objs_z.erase(pos_start3);
+      break;
+    }
+    ++pos_start3;
+  }
+}
+
+//==============================================================================
+template <typename S>
+SSaPCollisionManager<S>::SSaPCollisionManager() : setup_(false)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::registerObject(CollisionObject<S>* obj)
+{
+  objs_x.push_back(obj);
+  objs_y.push_back(obj);
+  objs_z.push_back(obj);
+  setup_ = false;
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::setup()
+{
+  if(!setup_)
+  {
+    std::sort(objs_x.begin(), objs_x.end(), SortByXLow<S>());
+    std::sort(objs_y.begin(), objs_y.end(), SortByYLow<S>());
+    std::sort(objs_z.begin(), objs_z.end(), SortByZLow<S>());
+    setup_ = true;
+  }
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::update()
+{
+  setup_ = false;
+  setup();
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::clear()
+{
+  objs_x.clear();
+  objs_y.clear();
+  objs_z.clear();
+  setup_ = false;
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::getObjects(std::vector<CollisionObject<S>*>& objs) const
+{
+  objs.resize(objs_x.size());
+  std::copy(objs_x.begin(), objs_x.end(), objs.begin());
+}
+
+//==============================================================================
+template <typename S>
+bool SSaPCollisionManager<S>::checkColl(typename std::vector<CollisionObject<S>*>::const_iterator pos_start, typename std::vector<CollisionObject<S>*>::const_iterator pos_end,
+                                     CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  while(pos_start < pos_end)
+  {
+    if(*pos_start != obj) // no collision between the same object
+    {
+      if((*pos_start)->getAABB().overlap(obj->getAABB()))
+      {
+        if(callback(*pos_start, obj, cdata))
+          return true;
+      }
+    }
+    pos_start++;
+  }
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+bool SSaPCollisionManager<S>::checkDis(typename std::vector<CollisionObject<S>*>::const_iterator pos_start, typename std::vector<CollisionObject<S>*>::const_iterator pos_end, CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
+{
+  while(pos_start < pos_end)
+  {
+    if(*pos_start != obj) // no distance between the same object
+    {
+      if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist)
+      {
+        if(callback(*pos_start, obj, cdata, min_dist))
+          return true;
+      }
+    }
+    pos_start++;
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  collide_(obj, cdata, callback);
+}
+
+//==============================================================================
+template <typename S>
+bool SSaPCollisionManager<S>::collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  static const unsigned int CUTOFF = 100;
+
+  DummyCollisionObject<S> dummyHigh(AABB<S>(obj->getAABB().max_));
+  bool coll_res = false;
+
+  const auto pos_start1 = objs_x.begin();
+  const auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow<S>());
+  unsigned int d1 = pos_end1 - pos_start1;
+
+  if(d1 > CUTOFF)
+  {
+    const auto pos_start2 = objs_y.begin();
+    const auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow<S>());
+    unsigned int d2 = pos_end2 - pos_start2;
+
+    if(d2 > CUTOFF)
+    {
+      const auto pos_start3 = objs_z.begin();
+      const auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow<S>());
+      unsigned int d3 = pos_end3 - pos_start3;
+
+      if(d3 > CUTOFF)
+      {
+        if(d3 <= d2 && d3 <= d1)
+          coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
+        else
+        {
+          if(d2 <= d3 && d2 <= d1)
+            coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
+          else
+            coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
+        }
+      }
+      else
+        coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
+    }
+    else
+      coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
+  }
+  else
+    coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
+
+  return coll_res;
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  S min_dist = std::numeric_limits<S>::max();
+  distance_(obj, cdata, callback, min_dist);
+}
+
+//==============================================================================
+template <typename S>
+bool SSaPCollisionManager<S>::distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
+{
+  static const unsigned int CUTOFF = 100;
+  Vector3<S> delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  Vector3<S> dummy_vector = obj->getAABB().max_;
+  if(min_dist < std::numeric_limits<S>::max())
+    dummy_vector += Vector3<S>(min_dist, min_dist, min_dist);
+
+  typename std::vector<CollisionObject<S>*>::const_iterator pos_start1 = objs_x.begin();
+  typename std::vector<CollisionObject<S>*>::const_iterator pos_start2 = objs_y.begin();
+  typename std::vector<CollisionObject<S>*>::const_iterator pos_start3 = objs_z.begin();
+  typename std::vector<CollisionObject<S>*>::const_iterator pos_end1 = objs_x.end();
+  typename std::vector<CollisionObject<S>*>::const_iterator pos_end2 = objs_y.end();
+  typename std::vector<CollisionObject<S>*>::const_iterator pos_end3 = objs_z.end();
+
+  int status = 1;
+  S old_min_distance;
+
+  while(1)
+  {
+    old_min_distance = min_dist;
+    DummyCollisionObject<S> dummyHigh((AABB<S>(dummy_vector)));
+
+    pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow<S>());
+    unsigned int d1 = pos_end1 - pos_start1;
+
+    bool dist_res = false;
+
+    if(d1 > CUTOFF)
+    {
+      pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow<S>());
+      unsigned int d2 = pos_end2 - pos_start2;
+
+      if(d2 > CUTOFF)
+      {
+        pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow<S>());
+        unsigned int d3 = pos_end3 - pos_start3;
+
+        if(d3 > CUTOFF)
+        {
+          if(d3 <= d2 && d3 <= d1)
+            dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
+          else
+          {
+            if(d2 <= d3 && d2 <= d1)
+              dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
+            else
+              dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
+          }
+        }
+        else
+          dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
+      }
+      else
+        dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
+    }
+    else
+    {
+      dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
+    }
+
+    if(dist_res) return true;
+
+    if(status == 1)
+    {
+      if(old_min_distance < std::numeric_limits<S>::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_ + Vector3<S>(min_dist, min_dist, min_dist);
+          status = 0;
+        }
+        else // need more loop
+        {
+          if(dummy_vector.isApprox(obj->getAABB().max_, std::numeric_limits<S>::epsilon() * 100))
+            dummy_vector = dummy_vector + delta;
+          else
+            dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
+        }
+      }
+
+      // yes, following is wrong, will result in too large distance.
+      // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
+      // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
+      // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
+    }
+    else if(status == 0)
+      break;
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+size_t SSaPCollisionManager<S>::selectOptimalAxis(
+    const std::vector<CollisionObject<S>*>& objs_x,
+    const std::vector<CollisionObject<S>*>& objs_y,
+    const std::vector<CollisionObject<S>*>& objs_z,
+    typename std::vector<CollisionObject<S>*>::const_iterator& it_beg,
+    typename std::vector<CollisionObject<S>*>::const_iterator& it_end)
+{
+  /// simple sweep and prune method
+  S delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0];
+  S delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1];
+  S 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)
+    axis = 1;
+  else if(delta_z > delta_y && delta_z > delta_x)
+    axis = 2;
+
+  switch(axis)
+  {
+  case 0:
+    it_beg = objs_x.begin();
+    it_end = objs_x.end();
+    break;
+  case 1:
+    it_beg = objs_y.begin();
+    it_end = objs_y.end();
+    break;
+  case 2:
+    it_beg = objs_z.begin();
+    it_end = objs_z.end();
+    break;
+  }
+
+  return axis;
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::collide(void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  typename std::vector<CollisionObject<S>*>::const_iterator pos, run_pos, pos_end;
+  size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z,
+                                  pos, pos_end);
+  size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
+  size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
+
+  run_pos = pos;
+
+  while((run_pos < pos_end) && (pos < pos_end))
+  {
+    CollisionObject<S>* obj = *(pos++);
+
+    while(1)
+    {
+      if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis])
+      {
+        run_pos++;
+        if(run_pos == pos_end) break;
+        continue;
+      }
+      else
+      {
+        run_pos++;
+        break;
+      }
+    }
+
+    if(run_pos < pos_end)
+    {
+      typename std::vector<CollisionObject<S>*>::const_iterator run_pos2 = run_pos;
+
+      while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis])
+      {
+        CollisionObject<S>* obj2 = *run_pos2;
+        run_pos2++;
+
+        if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2]))
+        {
+          if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3]))
+          {
+            if(callback(obj, obj2, cdata))
+              return;
+          }
+        }
+
+        if(run_pos2 == pos_end) break;
+      }
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::distance(void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  typename std::vector<CollisionObject<S>*>::const_iterator it, it_end;
+  selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
+
+  S min_dist = std::numeric_limits<S>::max();
+  for(; it != it_end; ++it)
+  {
+    if(distance_(*it, cdata, callback, min_dist))
+      return;
+  }
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
+{
+  SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0)) return;
+
+  if(this == other_manager)
+  {
+    collide(cdata, callback);
+    return;
+  }
+
+
+  typename std::vector<CollisionObject<S>*>::const_iterator it, end;
+  if(this->size() < other_manager->size())
+  {
+    for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
+      if(other_manager->collide_(*it, cdata, callback)) return;
+  }
+  else
+  {
+    for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it)
+      if(collide_(*it, cdata, callback)) return;
+  }
+}
+
+//==============================================================================
+template <typename S>
+void SSaPCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
+{
+  SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0)) return;
+
+  if(this == other_manager)
+  {
+    distance(cdata, callback);
+    return;
+  }
+
+  S min_dist = std::numeric_limits<S>::max();
+  typename std::vector<CollisionObject<S>*>::const_iterator it, end;
+  if(this->size() < other_manager->size())
+  {
+    for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
+      if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
+  }
+  else
+  {
+    for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it)
+      if(distance_(*it, cdata, callback, min_dist)) return;
+  }
+}
+
+//==============================================================================
+template <typename S>
+bool SSaPCollisionManager<S>::empty() const
+{
+  return objs_x.empty();
+}
+
+//==============================================================================
+template <typename S>
+size_t SSaPCollisionManager<S>::size() const
+{
+  return objs_x.size();
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_SSaP.h

@@ -0,0 +1,136 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_SSAP_H
+#define FCL_BROAD_PHASE_SSAP_H
+
+#include <vector>
+#include "fcl/broadphase/broadphase_collision_manager.h"
+
+namespace fcl
+{
+
+/// @brief Simple SAP collision manager 
+template <typename S>
+class FCL_EXPORT SSaPCollisionManager : public BroadPhaseCollisionManager<S>
+{
+public:
+  SSaPCollisionManager();
+
+  /// @brief remove one object from the manager
+  void registerObject(CollisionObject<S>* obj);
+
+  /// @brief add one object to the manager
+  void unregisterObject(CollisionObject<S>* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  void update();
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject<S>*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  void collide(void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  void distance(void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager<S>* other_manager, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager<S>* other_manager, void* cdata, DistanceCallBack<S> 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<S>*>::const_iterator pos_start, typename std::vector<CollisionObject<S>*>::const_iterator pos_end,
+                 CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> 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<S>*>::const_iterator pos_start, typename std::vector<CollisionObject<S>*>::const_iterator pos_end,
+                CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const;
+
+  bool collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+  
+  bool distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const;
+
+  static size_t selectOptimalAxis(
+      const std::vector<CollisionObject<S>*>& objs_x,
+      const std::vector<CollisionObject<S>*>& objs_y,
+      const std::vector<CollisionObject<S>*>& objs_z,
+      typename std::vector<CollisionObject<S>*>::const_iterator& it_beg,
+      typename std::vector<CollisionObject<S>*>::const_iterator& it_end);
+
+  /// @brief Objects sorted according to lower x value
+  std::vector<CollisionObject<S>*> objs_x;
+
+  /// @brief Objects sorted according to lower y value
+  std::vector<CollisionObject<S>*> objs_y;
+
+  /// @brief Objects sorted according to lower z value
+  std::vector<CollisionObject<S>*> 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_;
+};
+
+using SSaPCollisionManagerf = SSaPCollisionManager<float>;
+using SSaPCollisionManagerd = SSaPCollisionManager<double>;
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_SSaP-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_SaP-inl.h

@@ -0,0 +1,973 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_SAP_INL_H
+#define FCL_BROAD_PHASE_SAP_INL_H
+
+#include "fcl/broadphase/broadphase_SaP.h"
+
+namespace fcl
+{
+
+//==============================================================================
+extern template
+class FCL_EXPORT SaPCollisionManager<double>;
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::unregisterObject(CollisionObject<S>* obj)
+{
+  auto it = AABB_arr.begin();
+  for(auto end = AABB_arr.end(); it != end; ++it)
+  {
+    if((*it)->obj == obj)
+      break;
+  }
+
+  AABB_arr.erase(it);
+  obj_aabb_map.erase(obj);
+
+  if(it == AABB_arr.end())
+    return;
+
+  SaPAABB* curr = *it;
+  *it = nullptr;
+
+  for(int coord = 0; coord < 3; ++coord)
+  {
+    //first delete the lo endpoint of the interval.
+    if(curr->lo->prev[coord] == nullptr)
+      elist[coord] = curr->lo->next[coord];
+    else
+      curr->lo->prev[coord]->next[coord] = curr->lo->next[coord];
+
+    curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord];
+
+    //then, delete the "hi" endpoint.
+    if(curr->hi->prev[coord] == nullptr)
+      elist[coord] = curr->hi->next[coord];
+    else
+      curr->hi->prev[coord]->next[coord] = curr->hi->next[coord];
+
+    if(curr->hi->next[coord] != nullptr)
+      curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord];
+  }
+
+  delete curr->lo;
+  delete curr->hi;
+  delete curr;
+
+  overlap_pairs.remove_if(isUnregistered(obj));
+}
+\
+//==============================================================================
+template <typename S>
+SaPCollisionManager<S>::SaPCollisionManager()
+{
+  elist[0] = nullptr;
+  elist[1] = nullptr;
+  elist[2] = nullptr;
+
+  optimal_axis = 0;
+}
+
+//==============================================================================
+template <typename S>
+SaPCollisionManager<S>::~SaPCollisionManager()
+{
+  clear();
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::registerObjects(const std::vector<CollisionObject<S>*>& other_objs)
+{
+  if(other_objs.empty()) return;
+
+  if(size() > 0)
+    BroadPhaseCollisionManager<S>::registerObjects(other_objs);
+  else
+  {
+    std::vector<EndPoint*> endpoints(2 * other_objs.size());
+
+    for(size_t i = 0; i < other_objs.size(); ++i)
+    {
+      SaPAABB* sapaabb = new SaPAABB();
+      sapaabb->obj = other_objs[i];
+      sapaabb->lo = new EndPoint();
+      sapaabb->hi = new EndPoint();
+      sapaabb->cached = other_objs[i]->getAABB();
+      endpoints[2 * i] = sapaabb->lo;
+      endpoints[2 * i + 1] = sapaabb->hi;
+      sapaabb->lo->minmax = 0;
+      sapaabb->hi->minmax = 1;
+      sapaabb->lo->aabb = sapaabb;
+      sapaabb->hi->aabb = sapaabb;
+      AABB_arr.push_back(sapaabb);
+      obj_aabb_map[other_objs[i]] = sapaabb;
+    }
+
+
+    S scale[3];
+    for(size_t coord = 0; coord < 3; ++coord)
+    {
+      std::sort(endpoints.begin(), endpoints.end(),
+                std::bind(std::less<S>(),
+                            std::bind(static_cast<S (EndPoint::*)(size_t) const >(&EndPoint::getVal), std::placeholders::_1, coord),
+                            std::bind(static_cast<S (EndPoint::*)(size_t) const >(&EndPoint::getVal), std::placeholders::_2, coord)));
+
+      endpoints[0]->prev[coord] = nullptr;
+      endpoints[0]->next[coord] = endpoints[1];
+      for(size_t i = 1; i < endpoints.size() - 1; ++i)
+      {
+        endpoints[i]->prev[coord] = endpoints[i-1];
+        endpoints[i]->next[coord] = endpoints[i+1];
+      }
+      endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2];
+      endpoints[endpoints.size() - 1]->next[coord] = nullptr;
+
+      elist[coord] = endpoints[0];
+
+      scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord];
+    }
+
+    int axis = 0;
+    if(scale[axis] < scale[1]) axis = 1;
+    if(scale[axis] < scale[2]) axis = 2;
+
+    EndPoint* pos = elist[axis];
+
+    while(pos != nullptr)
+    {
+      EndPoint* pos_next = nullptr;
+      SaPAABB* aabb = pos->aabb;
+      EndPoint* pos_it = pos->next[axis];
+
+      while(pos_it != nullptr)
+      {
+        if(pos_it->aabb == aabb)
+        {
+          if(pos_next == nullptr) pos_next = pos_it;
+          break;
+        }
+
+        if(pos_it->minmax == 0)
+        {
+          if(pos_next == nullptr) pos_next = pos_it;
+          if(pos_it->aabb->cached.overlap(aabb->cached))
+            overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj);
+        }
+        pos_it = pos_it->next[axis];
+      }
+
+      pos = pos_next;
+    }
+  }
+
+  updateVelist();
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::registerObject(CollisionObject<S>* obj)
+{
+  SaPAABB* curr = new SaPAABB;
+  curr->cached = obj->getAABB();
+  curr->obj = obj;
+  curr->lo = new EndPoint;
+  curr->lo->minmax = 0;
+  curr->lo->aabb = curr;
+
+  curr->hi = new EndPoint;
+  curr->hi->minmax = 1;
+  curr->hi->aabb = curr;
+
+  for(int coord = 0; coord < 3; ++coord)
+  {
+    EndPoint* current = elist[coord];
+
+    // first insert the lo end point
+    if(current == nullptr) // empty list
+    {
+      elist[coord] = curr->lo;
+      curr->lo->prev[coord] = curr->lo->next[coord] = nullptr;
+    }
+    else // otherwise, find the correct location in the list and insert
+    {
+      EndPoint* curr_lo = curr->lo;
+      S curr_lo_val = curr_lo->getVal()[coord];
+      while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != nullptr))
+        current = current->next[coord];
+
+      if(current->getVal()[coord] >= curr_lo_val)
+      {
+        curr_lo->prev[coord] = current->prev[coord];
+        curr_lo->next[coord] = current;
+        if(current->prev[coord] == nullptr)
+          elist[coord] = curr_lo;
+        else
+          current->prev[coord]->next[coord] = curr_lo;
+
+        current->prev[coord] = curr_lo;
+      }
+      else
+      {
+        curr_lo->prev[coord] = current;
+        curr_lo->next[coord] = nullptr;
+        current->next[coord] = curr_lo;
+      }
+    }
+
+    // now insert hi end point
+    current = curr->lo;
+
+    EndPoint* curr_hi = curr->hi;
+    S curr_hi_val = curr_hi->getVal()[coord];
+
+    if(coord == 0)
+    {
+      while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr))
+      {
+        if(current != curr->lo)
+          if(current->aabb->cached.overlap(curr->cached))
+            overlap_pairs.emplace_back(current->aabb->obj, obj);
+
+        current = current->next[coord];
+      }
+    }
+    else
+    {
+      while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr))
+        current = current->next[coord];
+    }
+
+    if(current->getVal()[coord] >= curr_hi_val)
+    {
+      curr_hi->prev[coord] = current->prev[coord];
+      curr_hi->next[coord] = current;
+      if(current->prev[coord] == nullptr)
+        elist[coord] = curr_hi;
+      else
+        current->prev[coord]->next[coord] = curr_hi;
+
+      current->prev[coord] = curr_hi;
+    }
+    else
+    {
+      curr_hi->prev[coord] = current;
+      curr_hi->next[coord] = nullptr;
+      current->next[coord] = curr_hi;
+    }
+  }
+
+  AABB_arr.push_back(curr);
+
+  obj_aabb_map[obj] = curr;
+
+  updateVelist();
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::setup()
+{
+  if(size() == 0) return;
+
+  S 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);
+  size_t axis = 0;
+  if(scale[axis] < scale[1]) axis = 1;
+  if(scale[axis] < scale[2]) axis = 2;
+  optimal_axis = axis;
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::update_(SaPAABB* updated_aabb)
+{
+  if(updated_aabb->cached.equal(updated_aabb->obj->getAABB()))
+    return;
+
+  SaPAABB* current = updated_aabb;
+
+  Vector3<S> new_min = current->obj->getAABB().min_;
+  Vector3<S> new_max = current->obj->getAABB().max_;
+
+  SaPAABB dummy;
+  dummy.cached = current->obj->getAABB();
+
+  for(int coord = 0; coord < 3; ++coord)
+  {
+    int direction; // -1 reverse, 0 nochange, 1 forward
+    EndPoint* temp;
+
+    if(current->lo->getVal(coord) > new_min[coord])
+      direction = -1;
+    else if(current->lo->getVal(coord) < new_min[coord])
+      direction = 1;
+    else direction = 0;
+
+    if(direction == -1)
+    {
+      //first update the "lo" endpoint of the interval
+      if(current->lo->prev[coord] != nullptr)
+      {
+        temp = current->lo;
+        while((temp != nullptr) && (temp->getVal(coord) > new_min[coord]))
+        {
+          if(temp->minmax == 1)
+            if(temp->aabb->cached.overlap(dummy.cached))
+              addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+          temp = temp->prev[coord];
+        }
+
+        if(temp == nullptr)
+        {
+          current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+          current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+          current->lo->prev[coord] = nullptr;
+          current->lo->next[coord] = elist[coord];
+          elist[coord]->prev[coord] = current->lo;
+          elist[coord] = current->lo;
+        }
+        else
+        {
+          current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+          current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+          current->lo->prev[coord] = temp;
+          current->lo->next[coord] = temp->next[coord];
+          temp->next[coord]->prev[coord] = current->lo;
+          temp->next[coord] = current->lo;
+        }
+      }
+
+      current->lo->getVal(coord) = new_min[coord];
+
+      // update hi end point
+      temp = current->hi;
+      while(temp->getVal(coord) > new_max[coord])
+      {
+        if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached)))
+          removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+        temp = temp->prev[coord];
+      }
+
+      current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+      if(current->hi->next[coord] != nullptr)
+        current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+      current->hi->prev[coord] = temp;
+      current->hi->next[coord] = temp->next[coord];
+      if(temp->next[coord] != nullptr)
+        temp->next[coord]->prev[coord] = current->hi;
+      temp->next[coord] = current->hi;
+
+      current->hi->getVal(coord) = new_max[coord];
+    }
+    else if(direction == 1)
+    {
+      //here, we first update the "hi" endpoint.
+      if(current->hi->next[coord] != nullptr)
+      {
+        temp = current->hi;
+        while((temp->next[coord] != nullptr) && (temp->getVal(coord) < new_max[coord]))
+        {
+          if(temp->minmax == 0)
+            if(temp->aabb->cached.overlap(dummy.cached))
+              addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+          temp = temp->next[coord];
+        }
+
+        if(temp->getVal(coord) < new_max[coord])
+        {
+          current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+          current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+          current->hi->prev[coord] = temp;
+          current->hi->next[coord] = nullptr;
+          temp->next[coord] = current->hi;
+        }
+        else
+        {
+          current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+          current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+          current->hi->prev[coord] = temp->prev[coord];
+          current->hi->next[coord] = temp;
+          temp->prev[coord]->next[coord] = current->hi;
+          temp->prev[coord] = current->hi;
+        }
+      }
+
+      current->hi->getVal(coord) = new_max[coord];
+
+      //then, update the "lo" endpoint of the interval.
+      temp = current->lo;
+
+      while(temp->getVal(coord) < new_min[coord])
+      {
+        if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached)))
+          removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+        temp = temp->next[coord];
+      }
+
+      if(current->lo->prev[coord] != nullptr)
+        current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+      else
+        elist[coord] = current->lo->next[coord];
+      current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+      current->lo->prev[coord] = temp->prev[coord];
+      current->lo->next[coord] = temp;
+      if(temp->prev[coord] != nullptr)
+        temp->prev[coord]->next[coord] = current->lo;
+      else
+        elist[coord] = current->lo;
+      temp->prev[coord] = current->lo;
+      current->lo->getVal(coord) = new_min[coord];
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::updateVelist()
+{
+  for(int coord = 0; coord < 3; ++coord)
+  {
+    velist[coord].resize(size() * 2);
+    EndPoint* current = elist[coord];
+    size_t id = 0;
+    while(current)
+    {
+      velist[coord][id] = current;
+      current = current->next[coord];
+      id++;
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::update(CollisionObject<S>* updated_obj)
+{
+  update_(obj_aabb_map[updated_obj]);
+
+  updateVelist();
+
+  setup();
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::update(const std::vector<CollisionObject<S>*>& updated_objs)
+{
+  for(size_t i = 0; i < updated_objs.size(); ++i)
+    update_(obj_aabb_map[updated_objs[i]]);
+
+  updateVelist();
+
+  setup();
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::update()
+{
+  for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it)
+  {
+    update_(*it);
+  }
+
+  updateVelist();
+
+  setup();
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::clear()
+{
+  for(auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
+  {
+    delete (*it)->hi;
+    delete (*it)->lo;
+    delete *it;
+    *it = nullptr;
+  }
+
+  AABB_arr.clear();
+  overlap_pairs.clear();
+
+  elist[0] = nullptr;
+  elist[1] = nullptr;
+  elist[2] = nullptr;
+
+  velist[0].clear();
+  velist[1].clear();
+  velist[2].clear();
+
+  obj_aabb_map.clear();
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::getObjects(std::vector<CollisionObject<S>*>& objs) const
+{
+  objs.resize(AABB_arr.size());
+  int i = 0;
+  for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it, ++i)
+  {
+    objs[i] = (*it)->obj;
+  }
+}
+
+//==============================================================================
+template <typename S>
+bool SaPCollisionManager<S>::collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  size_t axis = optimal_axis;
+  const AABB<S>& obj_aabb = obj->getAABB();
+
+  S min_val = obj_aabb.min_[axis];
+  //  S max_val = obj_aabb.max_[axis];
+
+  EndPoint dummy;
+  SaPAABB dummy_aabb;
+  dummy_aabb.cached = obj_aabb;
+  dummy.minmax = 1;
+  dummy.aabb = &dummy_aabb;
+
+  // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
+  const auto res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
+                                                                   std::bind(std::less<S>(),
+                                                                               std::bind(static_cast<S (EndPoint::*)(size_t) const>(&EndPoint::getVal), std::placeholders::_1, axis),
+                                                                               std::bind(static_cast<S (EndPoint::*)(size_t) const>(&EndPoint::getVal), std::placeholders::_2, axis)));
+
+  EndPoint* end_pos = nullptr;
+  if(res_it != velist[axis].end())
+    end_pos = *res_it;
+
+  EndPoint* pos = elist[axis];
+
+  while(pos != end_pos)
+  {
+    if(pos->aabb->obj != obj)
+    {
+      if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
+      {
+        if(pos->aabb->cached.overlap(obj->getAABB()))
+          if(callback(obj, pos->aabb->obj, cdata))
+            return true;
+      }
+    }
+    pos = pos->next[axis];
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::addToOverlapPairs(
+    const typename SaPCollisionManager<S>::SaPPair& p)
+{
+  bool repeated = false;
+  for(auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it)
+  {
+    if(*it == p)
+    {
+      repeated = true;
+      break;
+    }
+  }
+
+  if(!repeated)
+    overlap_pairs.push_back(p);
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::removeFromOverlapPairs(
+    const typename SaPCollisionManager<S>::SaPPair& p)
+{
+  for(auto it = overlap_pairs.begin(), end = overlap_pairs.end();
+      it != end;
+      ++it)
+  {
+    if(*it == p)
+    {
+      overlap_pairs.erase(it);
+      break;
+    }
+  }
+
+  // or overlap_pairs.remove_if(isNotValidPair(p));
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  collide_(obj, cdata, callback);
+}
+
+//==============================================================================
+template <typename S>
+bool SaPCollisionManager<S>::distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
+{
+  Vector3<S> delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  AABB<S> aabb = obj->getAABB();
+
+  if(min_dist < std::numeric_limits<S>::max())
+  {
+    Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
+    aabb.expand(min_dist_delta);
+  }
+
+  size_t axis = optimal_axis;
+
+  int status = 1;
+  S old_min_distance;
+
+  EndPoint* start_pos = elist[axis];
+
+  while(1)
+  {
+    old_min_distance = min_dist;
+    S min_val = aabb.min_[axis];
+    //    S max_val = aabb.max_[axis];
+
+    EndPoint dummy;
+    SaPAABB dummy_aabb;
+    dummy_aabb.cached = aabb;
+    dummy.minmax = 1;
+    dummy.aabb = &dummy_aabb;
+
+
+    const auto res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
+                                                                     std::bind(std::less<S>(),
+                                                                                 std::bind(static_cast<S (EndPoint::*)(size_t) const>(&EndPoint::getVal), std::placeholders::_1, axis),
+                                                                                 std::bind(static_cast<S (EndPoint::*)(size_t) const>(&EndPoint::getVal), std::placeholders::_2, axis)));
+
+    EndPoint* end_pos = nullptr;
+    if(res_it != velist[axis].end())
+      end_pos = *res_it;
+
+    EndPoint* pos = start_pos;
+
+    while(pos != end_pos)
+    {
+      // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos.
+      // but this seems slower.
+      if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
+      {
+        CollisionObject<S>* curr_obj = pos->aabb->obj;
+        if(curr_obj != obj)
+        {
+          if(!this->enable_tested_set_)
+          {
+            if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
+            {
+              if(callback(curr_obj, obj, cdata, min_dist))
+                return true;
+            }
+          }
+          else
+          {
+            if(!this->inTestedSet(curr_obj, obj))
+            {
+              if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
+              {
+                if(callback(curr_obj, obj, cdata, min_dist))
+                  return true;
+              }
+
+              this->insertTestedSet(curr_obj, obj);
+            }
+          }
+        }
+      }
+
+      pos = pos->next[axis];
+    }
+
+    if(status == 1)
+    {
+      if(old_min_distance < std::numeric_limits<S>::max())
+        break;
+      else
+      {
+        if(min_dist < old_min_distance)
+        {
+          Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
+          aabb = AABB<S>(obj->getAABB(), min_dist_delta);
+          status = 0;
+        }
+        else
+        {
+          if(aabb.equal(obj->getAABB()))
+            aabb.expand(delta);
+          else
+            aabb.expand(obj->getAABB(), 2.0);
+        }
+      }
+    }
+    else if(status == 0)
+      break;
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  S min_dist = std::numeric_limits<S>::max();
+
+  distance_(obj, cdata, callback, min_dist);
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::collide(void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  for(auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end; ++it)
+  {
+    CollisionObject<S>* obj1 = it->obj1;
+    CollisionObject<S>* obj2 = it->obj2;
+
+    if(callback(obj1, obj2, cdata))
+      return;
+  }
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::distance(void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  this->enable_tested_set_ = true;
+  this->tested_set.clear();
+
+  S min_dist = std::numeric_limits<S>::max();
+
+  for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it)
+  {
+    if(distance_((*it)->obj, cdata, callback, min_dist))
+      break;
+  }
+
+  this->enable_tested_set_ = false;
+  this->tested_set.clear();
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
+{
+  SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0)) return;
+
+  if(this == other_manager)
+  {
+    collide(cdata, callback);
+    return;
+  }
+
+  if(this->size() < other_manager->size())
+  {
+    for(auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it)
+    {
+      if(other_manager->collide_((*it)->obj, cdata, callback))
+        return;
+    }
+  }
+  else
+  {
+    for(auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it)
+    {
+      if(collide_((*it)->obj, cdata, callback))
+        return;
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void SaPCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
+{
+  SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0)) return;
+
+  if(this == other_manager)
+  {
+    distance(cdata, callback);
+    return;
+  }
+
+  S min_dist = std::numeric_limits<S>::max();
+
+  if(this->size() < other_manager->size())
+  {
+    for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it)
+    {
+      if(other_manager->distance_((*it)->obj, cdata, callback, min_dist))
+        return;
+    }
+  }
+  else
+  {
+    for(auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it)
+    {
+      if(distance_((*it)->obj, cdata, callback, min_dist))
+        return;
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+bool SaPCollisionManager<S>::empty() const
+{
+  return AABB_arr.size();
+}
+
+//==============================================================================
+template <typename S>
+size_t SaPCollisionManager<S>::size() const
+{
+  return AABB_arr.size();
+}
+
+//==============================================================================
+template <typename S>
+const Vector3<S>&SaPCollisionManager<S>::EndPoint::getVal() const
+{
+  if(minmax) return aabb->cached.max_;
+  else return aabb->cached.min_;
+}
+
+//==============================================================================
+template <typename S>
+Vector3<S>&SaPCollisionManager<S>::EndPoint::getVal()
+{
+  if(minmax) return aabb->cached.max_;
+  else return aabb->cached.min_;
+}
+
+//==============================================================================
+template <typename S>
+S SaPCollisionManager<S>::EndPoint::getVal(size_t i) const
+{
+  if(minmax)
+    return aabb->cached.max_[i];
+  else
+    return aabb->cached.min_[i];
+}
+
+//==============================================================================
+template <typename S>
+S& SaPCollisionManager<S>::EndPoint::getVal(size_t i)
+{
+  if(minmax)
+    return aabb->cached.max_[i];
+  else
+    return aabb->cached.min_[i];
+}
+
+//==============================================================================
+template <typename S>
+SaPCollisionManager<S>::SaPPair::SaPPair(CollisionObject<S>* a, CollisionObject<S>* b)
+{
+  if(a < b)
+  {
+    obj1 = a;
+    obj2 = b;
+  }
+  else
+  {
+    obj1 = b;
+    obj2 = a;
+  }
+}
+
+//==============================================================================
+template <typename S>
+bool SaPCollisionManager<S>::SaPPair::operator ==(const typename SaPCollisionManager<S>::SaPPair& other) const
+{
+  return ((obj1 == other.obj1) && (obj2 == other.obj2));
+}
+
+//==============================================================================
+template <typename S>
+SaPCollisionManager<S>::isUnregistered::isUnregistered(CollisionObject<S>* obj_) : obj(obj_)
+{}
+
+//==============================================================================
+template <typename S>
+bool SaPCollisionManager<S>::isUnregistered::operator()(const SaPPair& pair) const
+{
+  return (pair.obj1 == obj) || (pair.obj2 == obj);
+}
+
+//==============================================================================
+template <typename S>
+SaPCollisionManager<S>::isNotValidPair::isNotValidPair(CollisionObject<S>* obj1_, CollisionObject<S>* obj2_) : obj1(obj1_),
+  obj2(obj2_)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+bool SaPCollisionManager<S>::isNotValidPair::operator()(const SaPPair& pair)
+{
+  return (pair.obj1 == obj1) && (pair.obj2 == obj2);
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_SaP.h

@@ -0,0 +1,245 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_SAP_H
+#define FCL_BROAD_PHASE_SAP_H
+
+#include <map>
+#include <list>
+
+#include "fcl/broadphase/broadphase_collision_manager.h"
+
+namespace fcl
+{
+
+/// @brief Rigorous SAP collision manager
+template <typename S>
+class FCL_EXPORT SaPCollisionManager : public BroadPhaseCollisionManager<S>
+{
+public:
+
+  SaPCollisionManager();
+
+  ~SaPCollisionManager();
+
+  /// @brief add objects to the manager
+  void registerObjects(const std::vector<CollisionObject<S>*>& other_objs);
+
+  /// @brief remove one object from the manager
+  void registerObject(CollisionObject<S>* obj);
+
+  /// @brief add one object to the manager
+  void unregisterObject(CollisionObject<S>* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject<S>* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject<S>*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject<S>*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  void collide(void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  void distance(void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager<S>* other_manager, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager<S>* other_manager, void* cdata, DistanceCallBack<S> 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 interval for one object
+  struct SaPAABB;
+
+  /// @brief End point for an interval
+  struct EndPoint;
+
+  /// @brief A pair of objects that are not culling away and should further check collision
+  struct SaPPair;
+
+  /// @brief Functor to help unregister one object
+  class FCL_EXPORT isUnregistered;
+
+  /// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away)
+  class FCL_EXPORT isNotValidPair;
+
+  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;
+
+  size_t optimal_axis;
+
+  std::map<CollisionObject<S>*, SaPAABB*> obj_aabb_map;
+
+  bool distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const;
+
+  bool collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  void addToOverlapPairs(const SaPPair& p);
+
+  void removeFromOverlapPairs(const SaPPair& p);
+};
+
+using SaPCollisionManagerf = SaPCollisionManager<float>;
+using SaPCollisionManagerd = SaPCollisionManager<double>;
+
+/// @brief SAP interval for one object
+template <typename S>
+struct SaPCollisionManager<S>::SaPAABB
+{
+  /// @brief object
+  CollisionObject<S>* obj;
+
+  /// @brief lower bound end point of the interval
+  typename SaPCollisionManager<S>::EndPoint* lo;
+
+  /// @brief higher bound end point of the interval
+  typename SaPCollisionManager<S>::EndPoint* hi;
+
+  /// @brief cached AABB<S> value
+  AABB<S> cached;
+};
+
+/// @brief End point for an interval
+template <typename S>
+struct SaPCollisionManager<S>::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
+  typename SaPCollisionManager<S>::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 Vector3<S>& getVal() const;
+
+  /// @brief set the value of the end point
+  Vector3<S>& getVal();
+
+  S getVal(size_t i) const;
+
+  S& getVal(size_t i);
+
+};
+
+/// @brief A pair of objects that are not culling away and should further check collision
+template <typename S>
+struct SaPCollisionManager<S>::SaPPair
+{
+  SaPPair(CollisionObject<S>* a, CollisionObject<S>* b);
+
+  CollisionObject<S>* obj1;
+  CollisionObject<S>* obj2;
+
+  bool operator == (const SaPPair& other) const;
+};
+
+/// @brief Functor to help unregister one object
+template <typename S>
+class FCL_EXPORT SaPCollisionManager<S>::isUnregistered
+{
+  CollisionObject<S>* obj;
+
+public:
+  isUnregistered(CollisionObject<S>* obj_);
+
+  bool operator() (const SaPPair& pair) const;
+};
+
+/// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away)
+template <typename S>
+class FCL_EXPORT SaPCollisionManager<S>::isNotValidPair
+{
+  CollisionObject<S>* obj1;
+  CollisionObject<S>* obj2;
+
+public:
+  isNotValidPair(CollisionObject<S>* obj1_, CollisionObject<S>* obj2_);
+
+  bool operator() (const SaPPair& pair);
+};
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_SaP-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_bruteforce-inl.h

@@ -0,0 +1,251 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_BRUTE_FORCE_INL_H
+#define FCL_BROAD_PHASE_BRUTE_FORCE_INL_H
+
+#include "fcl/broadphase/broadphase_bruteforce.h"
+
+#include <iterator>
+
+namespace fcl {
+
+//==============================================================================
+extern template
+class FCL_EXPORT NaiveCollisionManager<double>;
+
+//==============================================================================
+template <typename S>
+NaiveCollisionManager<S>::NaiveCollisionManager()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::registerObjects(const std::vector<CollisionObject<S>*>& other_objs)
+{
+  std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::unregisterObject(CollisionObject<S>* obj)
+{
+  objs.remove(obj);
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::registerObject(CollisionObject<S>* obj)
+{
+  objs.push_back(obj);
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::setup()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::update()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::clear()
+{
+  objs.clear();
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::getObjects(std::vector<CollisionObject<S>*>& objs_) const
+{
+  objs_.resize(objs.size());
+  std::copy(objs.begin(), objs.end(), objs_.begin());
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  for(auto* obj2 : objs)
+  {
+    if(callback(obj, obj2, cdata))
+      return;
+  }
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  S min_dist = std::numeric_limits<S>::max();
+  for(auto* obj2 : objs)
+  {
+    if(obj->getAABB().distance(obj2->getAABB()) < min_dist)
+    {
+      if(callback(obj, obj2, cdata, min_dist))
+        return;
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::collide(void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  for(typename std::list<CollisionObject<S>*>::const_iterator it1 = objs.begin(), end = objs.end();
+      it1 != end; ++it1)
+  {
+    typename std::list<CollisionObject<S>*>::const_iterator it2 = it1; it2++;
+    for(; it2 != end; ++it2)
+    {
+      if((*it1)->getAABB().overlap((*it2)->getAABB()))
+      {
+        if(callback(*it1, *it2, cdata))
+          return;
+      }
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::distance(void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  S min_dist = std::numeric_limits<S>::max();
+  for(typename std::list<CollisionObject<S>*>::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1)
+  {
+    typename std::list<CollisionObject<S>*>::const_iterator it2 = it1; it2++;
+    for(; it2 != end; ++it2)
+    {
+      if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)
+      {
+        if(callback(*it1, *it2, cdata, min_dist))
+          return;
+      }
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
+{
+  NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0)) return;
+
+  if(this == other_manager)
+  {
+    collide(cdata, callback);
+    return;
+  }
+
+  for(auto* obj1 : objs)
+  {
+    for(auto* obj2 : other_manager->objs)
+    {
+      if(obj1->getAABB().overlap(obj2->getAABB()))
+      {
+        if(callback(obj1, obj2, cdata))
+          return;
+      }
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void NaiveCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
+{
+  NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0)) return;
+
+  if(this == other_manager)
+  {
+    distance(cdata, callback);
+    return;
+  }
+
+  S min_dist = std::numeric_limits<S>::max();
+  for(auto* obj1 : objs)
+  {
+    for(auto* obj2 : other_manager->objs)
+    {
+      if(obj1->getAABB().distance(obj2->getAABB()) < min_dist)
+      {
+        if(callback(obj1, obj2, cdata, min_dist))
+          return;
+      }
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+bool NaiveCollisionManager<S>::empty() const
+{
+  return objs.empty();
+}
+
+//==============================================================================
+template <typename S>
+size_t NaiveCollisionManager<S>::size() const
+{
+  return objs.size();
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/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 FCL_BROAD_PHASE_BRUTE_FORCE_H
+#define FCL_BROAD_PHASE_BRUTE_FORCE_H
+
+#include <list>
+#include "fcl/broadphase/broadphase_collision_manager.h"
+
+namespace fcl
+{
+
+/// @brief Brute force N-body collision manager
+template <typename S>
+class FCL_EXPORT NaiveCollisionManager : public BroadPhaseCollisionManager<S>
+{
+public:
+  NaiveCollisionManager();
+
+  /// @brief add objects to the manager
+  void registerObjects(const std::vector<CollisionObject<S>*>& other_objs);
+
+  /// @brief add one object to the manager
+  void registerObject(CollisionObject<S>* obj);
+
+  /// @brief remove one object from the manager
+  void unregisterObject(CollisionObject<S>* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  void update();
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject<S>*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  void collide(void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  void distance(void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager<S>* other_manager, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager<S>* other_manager, void* cdata, DistanceCallBack<S> 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<S>*> objs;
+};
+
+using NaiveCollisionManagerf = NaiveCollisionManager<float>;
+using NaiveCollisionManagerd = NaiveCollisionManager<double>;
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_bruteforce-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_collision_manager-inl.h

@@ -0,0 +1,114 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_INL_H
+#define FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_INL_H
+
+#include "fcl/broadphase/broadphase_collision_manager.h"
+
+#include "fcl/common/unused.h"
+
+namespace fcl {
+
+//==============================================================================
+extern template
+class FCL_EXPORT BroadPhaseCollisionManager<double>;
+
+//==============================================================================
+template <typename S>
+BroadPhaseCollisionManager<S>::BroadPhaseCollisionManager()
+  : enable_tested_set_(false)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+BroadPhaseCollisionManager<S>::~BroadPhaseCollisionManager()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+void BroadPhaseCollisionManager<S>::registerObjects(
+    const std::vector<CollisionObject<S>*>& other_objs)
+{
+  for(size_t i = 0; i < other_objs.size(); ++i)
+    registerObject(other_objs[i]);
+}
+
+//==============================================================================
+template <typename S>
+void BroadPhaseCollisionManager<S>::update(CollisionObject<S>* updated_obj)
+{
+  FCL_UNUSED(updated_obj);
+
+  update();
+}
+
+//==============================================================================
+template <typename S>
+void BroadPhaseCollisionManager<S>::update(
+    const std::vector<CollisionObject<S>*>& updated_objs)
+{
+  FCL_UNUSED(updated_objs);
+
+  update();
+}
+
+//==============================================================================
+template <typename S>
+bool BroadPhaseCollisionManager<S>::inTestedSet(
+    CollisionObject<S>* a, CollisionObject<S>* b) const
+{
+  if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end();
+  else return tested_set.find(std::make_pair(b, a)) != tested_set.end();
+}
+
+//==============================================================================
+template <typename S>
+void BroadPhaseCollisionManager<S>::insertTestedSet(
+    CollisionObject<S>* a, CollisionObject<S>* b) const
+{
+  if(a < b) tested_set.insert(std::make_pair(a, b));
+  else tested_set.insert(std::make_pair(b, a));
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_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 FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
+#define FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
+
+#include <set>
+#include <vector>
+
+#include "fcl/narrowphase/collision_object.h"
+
+namespace fcl
+{
+
+/// @brief Callback for collision between two objects. Return value is whether
+/// can stop now.
+template <typename S>
+using CollisionCallBack = bool (*)(
+    CollisionObject<S>* o1, CollisionObject<S>* o2, void* cdata);
+
+/// @brief Callback for distance between two objects, Return value is whether
+/// can stop now, also return the minimum distance till now.
+template <typename S>
+using DistanceCallBack = bool (*)(
+    CollisionObject<S>* o1,
+    CollisionObject<S>* o2, void* cdata, S& dist);
+
+/// @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.
+template <typename S>
+class FCL_EXPORT BroadPhaseCollisionManager
+{
+public:
+  BroadPhaseCollisionManager();
+
+  virtual ~BroadPhaseCollisionManager();
+
+  /// @brief add objects to the manager
+  virtual void registerObjects(const std::vector<CollisionObject<S>*>& other_objs);
+
+  /// @brief add one object to the manager
+  virtual void registerObject(CollisionObject<S>* obj) = 0;
+
+  /// @brief remove one object from the manager
+  virtual void unregisterObject(CollisionObject<S>* 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<S>* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  virtual void update(const std::vector<CollisionObject<S>*>& updated_objs);
+
+  /// @brief clear the manager
+  virtual void clear() = 0;
+
+  /// @brief return the objects managed by the manager
+  virtual void getObjects(std::vector<CollisionObject<S>*>& objs) const = 0;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  virtual void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const = 0;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  virtual void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const = 0;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  virtual void collide(void* cdata, CollisionCallBack<S> callback) const = 0;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  virtual void distance(void* cdata, DistanceCallBack<S> callback) const = 0;
+
+  /// @brief perform collision test with objects belonging to another manager
+  virtual void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack<S> callback) const = 0;
+
+  /// @brief perform distance test with objects belonging to another manager
+  virtual void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack<S> 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<S>*, CollisionObject<S>*> > tested_set;
+  mutable bool enable_tested_set_;
+
+  bool inTestedSet(CollisionObject<S>* a, CollisionObject<S>* b) const;
+
+  void insertTestedSet(CollisionObject<S>* a, CollisionObject<S>* b) const;
+
+};
+
+using BroadPhaseCollisionManagerf = BroadPhaseCollisionManager<float>;
+using BroadPhaseCollisionManagerd = BroadPhaseCollisionManager<double>;
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_collision_manager-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h

@@ -0,0 +1,96 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
+#define FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
+
+#include "fcl/broadphase/broadphase_continuous_collision_manager.h"
+
+#include "fcl/common/unused.h"
+
+namespace fcl {
+
+//==============================================================================
+extern template
+class FCL_EXPORT BroadPhaseContinuousCollisionManager<double>;
+
+//==============================================================================
+template <typename S>
+BroadPhaseContinuousCollisionManager<S>::BroadPhaseContinuousCollisionManager()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+BroadPhaseContinuousCollisionManager<S>::~BroadPhaseContinuousCollisionManager()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+void BroadPhaseContinuousCollisionManager<S>::registerObjects(
+    const std::vector<ContinuousCollisionObject<S>*>& other_objs)
+{
+  for(size_t i = 0; i < other_objs.size(); ++i)
+    registerObject(other_objs[i]);
+}
+
+//==============================================================================
+template <typename S>
+void BroadPhaseContinuousCollisionManager<S>::update(
+    ContinuousCollisionObject<S>* updated_obj)
+{
+  FCL_UNUSED(updated_obj);
+
+  update();
+}
+
+//==============================================================================
+template <typename S>
+void BroadPhaseContinuousCollisionManager<S>::update(
+    const std::vector<ContinuousCollisionObject<S>*>& updated_objs)
+{
+  FCL_UNUSED(updated_objs);
+
+  update();
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_continuous_collision_manager.h

@@ -0,0 +1,132 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
+#define FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
+
+#include "fcl/broadphase/broadphase_collision_manager.h"
+#include "fcl/narrowphase/collision_object.h"
+#include "fcl/narrowphase/continuous_collision_object.h"
+
+namespace fcl
+{
+
+/// @brief Callback for continuous collision between two objects. Return value
+/// is whether can stop now.
+template <typename S>
+using ContinuousCollisionCallBack = bool (*)(
+    ContinuousCollisionObject<S>* o1,
+    ContinuousCollisionObject<S>* 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<S>* o1,
+    ContinuousCollisionObject<S>* o2, void* cdata, 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 FCL_EXPORT BroadPhaseContinuousCollisionManager
+{
+public:
+  BroadPhaseContinuousCollisionManager();
+
+  virtual ~BroadPhaseContinuousCollisionManager();
+
+  /// @brief add objects to the manager
+  virtual void registerObjects(const std::vector<ContinuousCollisionObject<S>*>& other_objs);
+
+  /// @brief add one object to the manager
+  virtual void registerObject(ContinuousCollisionObject<S>* obj) = 0;
+
+  /// @brief remove one object from the manager
+  virtual void unregisterObject(ContinuousCollisionObject<S>* 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<S>* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  virtual void update(const std::vector<ContinuousCollisionObject<S>*>& updated_objs);
+
+  /// @brief clear the manager
+  virtual void clear() = 0;
+
+  /// @brief return the objects managed by the manager
+  virtual void getObjects(std::vector<ContinuousCollisionObject<S>*>& objs) const = 0;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  virtual void collide(ContinuousCollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const = 0;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  virtual void distance(ContinuousCollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const = 0;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  virtual void collide(void* cdata, CollisionCallBack<S> callback) const = 0;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  virtual void distance(void* cdata, DistanceCallBack<S> callback) const = 0;
+
+  /// @brief perform collision test with objects belonging to another manager
+  virtual void collide(BroadPhaseContinuousCollisionManager<S>* other_manager, void* cdata, CollisionCallBack<S> callback) const = 0;
+
+  /// @brief perform distance test with objects belonging to another manager
+  virtual void distance(BroadPhaseContinuousCollisionManager<S>* other_manager, void* cdata, DistanceCallBack<S> 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<double>;
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_continuous_collision_manager-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h

@@ -0,0 +1,141 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
+#define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
+
+#include <unordered_map>
+#include <functional>
+
+#include "fcl/math/bv/utility.h"
+#include "fcl/geometry/shape/box.h"
+#include "fcl/geometry/shape/utility.h"
+#include "fcl/broadphase/broadphase_collision_manager.h"
+#include "fcl/broadphase/detail/hierarchy_tree.h"
+
+namespace fcl
+{
+
+template <typename S>
+class FCL_EXPORT DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager<S>
+{
+public:
+
+  using DynamicAABBNode = detail::NodeBase<AABB<S>>;
+  using DynamicAABBTable = std::unordered_map<CollisionObject<S>*, DynamicAABBNode*> ;
+
+  int max_tree_nonbalanced_level;
+  int tree_incremental_balance_pass;
+  int& tree_topdown_balance_threshold;
+  int& tree_topdown_level;
+  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<S>*>& other_objs);
+  
+  /// @brief add one object to the manager
+  void registerObject(CollisionObject<S>* obj);
+
+  /// @brief remove one object from the manager
+  void unregisterObject(CollisionObject<S>* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject<S>* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject<S>*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject<S>*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  void collide(void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  void distance(void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> 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::HierarchyTree<AABB<S>>& getTree() const;
+
+private:
+  detail::HierarchyTree<AABB<S>> dtree;
+  std::unordered_map<CollisionObject<S>*, DynamicAABBNode*> table;
+
+  bool setup_;
+
+  void update_(CollisionObject<S>* updated_obj);
+};
+
+using DynamicAABBTreeCollisionManagerf = DynamicAABBTreeCollisionManager<float>;
+using DynamicAABBTreeCollisionManagerd = DynamicAABBTreeCollisionManager<double>;
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h

@@ -0,0 +1,142 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
+#define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
+
+#include <unordered_map>
+#include <functional>
+#include <limits>
+
+#include "fcl/math/bv/utility.h"
+#include "fcl/geometry/shape/box.h"
+#include "fcl/geometry/shape/utility.h"
+#include "fcl/broadphase/broadphase_collision_manager.h"
+#include "fcl/broadphase/detail/hierarchy_tree_array.h"
+
+namespace fcl
+{
+
+template <typename S>
+class FCL_EXPORT DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager<S>
+{
+public:
+
+  using DynamicAABBNode = detail::implementation_array::NodeBase<AABB<S>>;
+  using DynamicAABBTable = std::unordered_map<CollisionObject<S>*, size_t>;
+
+  int max_tree_nonbalanced_level;
+  int tree_incremental_balance_pass;
+  int& tree_topdown_balance_threshold;
+  int& tree_topdown_level;
+  int tree_init_level;
+
+  bool octree_as_geometry_collide;
+  bool octree_as_geometry_distance;
+  
+  DynamicAABBTreeCollisionManager_Array();
+
+  /// @brief add objects to the manager
+  void registerObjects(const std::vector<CollisionObject<S>*>& other_objs);
+  
+  /// @brief add one object to the manager
+  void registerObject(CollisionObject<S>* obj);
+
+  /// @brief remove one object from the manager
+  void unregisterObject(CollisionObject<S>* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject<S>* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject<S>*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject<S>*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  void collide(void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  void distance(void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> 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<S>>& getTree() const;
+
+private:
+  detail::implementation_array::HierarchyTree<AABB<S>> dtree;
+  std::unordered_map<CollisionObject<S>*, size_t> table;
+
+  bool setup_;
+
+  void update_(CollisionObject<S>* updated_obj);
+};
+
+using DynamicAABBTreeCollisionManager_Arrayf = DynamicAABBTreeCollisionManager_Array<float>;
+using DynamicAABBTreeCollisionManager_Arrayd = DynamicAABBTreeCollisionManager_Array<double>;
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_interval_tree-inl.h

@@ -0,0 +1,750 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_INTERVAL_TREE_INL_H
+#define FCL_BROAD_PHASE_INTERVAL_TREE_INL_H
+
+#include "fcl/broadphase/broadphase_interval_tree.h"
+
+namespace fcl
+{
+
+//==============================================================================
+extern template
+class FCL_EXPORT IntervalTreeCollisionManager<double>;
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::unregisterObject(CollisionObject<S>* obj)
+{
+  // must sorted before
+  setup();
+
+  EndPoint p;
+  p.value = obj->getAABB().min_[0];
+  auto start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p);
+  p.value = obj->getAABB().max_[0];
+  auto end1 = std::upper_bound(start1, endpoints[0].end(), p);
+
+  if(start1 < end1)
+  {
+    unsigned int start_id = start1 - endpoints[0].begin();
+    unsigned int end_id = end1 - endpoints[0].begin();
+    unsigned int cur_id = start_id;
+    for(unsigned int i = start_id; i < end_id; ++i)
+    {
+      if(endpoints[0][i].obj != obj)
+      {
+        if(i == cur_id) cur_id++;
+        else
+        {
+          endpoints[0][cur_id] = endpoints[0][i];
+          cur_id++;
+        }
+      }
+    }
+    if(cur_id < end_id)
+      endpoints[0].resize(endpoints[0].size() - 2);
+  }
+
+  p.value = obj->getAABB().min_[1];
+  auto start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p);
+  p.value = obj->getAABB().max_[1];
+  auto end2 = std::upper_bound(start2, endpoints[1].end(), p);
+
+  if(start2 < end2)
+  {
+    unsigned int start_id = start2 - endpoints[1].begin();
+    unsigned int end_id = end2 - endpoints[1].begin();
+    unsigned int cur_id = start_id;
+    for(unsigned int i = start_id; i < end_id; ++i)
+    {
+      if(endpoints[1][i].obj != obj)
+      {
+        if(i == cur_id) cur_id++;
+        else
+        {
+          endpoints[1][cur_id] = endpoints[1][i];
+          cur_id++;
+        }
+      }
+    }
+    if(cur_id < end_id)
+      endpoints[1].resize(endpoints[1].size() - 2);
+  }
+
+
+  p.value = obj->getAABB().min_[2];
+  auto start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p);
+  p.value = obj->getAABB().max_[2];
+  auto end3 = std::upper_bound(start3, endpoints[2].end(), p);
+
+  if(start3 < end3)
+  {
+    unsigned int start_id = start3 - endpoints[2].begin();
+    unsigned int end_id = end3 - endpoints[2].begin();
+    unsigned int cur_id = start_id;
+    for(unsigned int i = start_id; i < end_id; ++i)
+    {
+      if(endpoints[2][i].obj != obj)
+      {
+        if(i == cur_id) cur_id++;
+        else
+        {
+          endpoints[2][cur_id] = endpoints[2][i];
+          cur_id++;
+        }
+      }
+    }
+    if(cur_id < end_id)
+      endpoints[2].resize(endpoints[2].size() - 2);
+  }
+
+  // update the interval tree
+  if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end())
+  {
+    SAPInterval* ivl1 = obj_interval_maps[0][obj];
+    SAPInterval* ivl2 = obj_interval_maps[1][obj];
+    SAPInterval* ivl3 = obj_interval_maps[2][obj];
+
+    interval_trees[0]->deleteNode(ivl1);
+    interval_trees[1]->deleteNode(ivl2);
+    interval_trees[2]->deleteNode(ivl3);
+
+    delete ivl1;
+    delete ivl2;
+    delete ivl3;
+
+    obj_interval_maps[0].erase(obj);
+    obj_interval_maps[1].erase(obj);
+    obj_interval_maps[2].erase(obj);
+  }
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeCollisionManager<S>::IntervalTreeCollisionManager() : setup_(false)
+{
+  for(int i = 0; i < 3; ++i)
+    interval_trees[i] = nullptr;
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeCollisionManager<S>::~IntervalTreeCollisionManager()
+{
+  clear();
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::registerObject(CollisionObject<S>* obj)
+{
+  EndPoint p, q;
+
+  p.obj = obj;
+  q.obj = obj;
+  p.minmax = 0;
+  q.minmax = 1;
+  p.value = obj->getAABB().min_[0];
+  q.value = obj->getAABB().max_[0];
+  endpoints[0].push_back(p);
+  endpoints[0].push_back(q);
+
+  p.value = obj->getAABB().min_[1];
+  q.value = obj->getAABB().max_[1];
+  endpoints[1].push_back(p);
+  endpoints[1].push_back(q);
+
+  p.value = obj->getAABB().min_[2];
+  q.value = obj->getAABB().max_[2];
+  endpoints[2].push_back(p);
+  endpoints[2].push_back(q);
+  setup_ = false;
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::setup()
+{
+  if(!setup_)
+  {
+    std::sort(endpoints[0].begin(), endpoints[0].end());
+    std::sort(endpoints[1].begin(), endpoints[1].end());
+    std::sort(endpoints[2].begin(), endpoints[2].end());
+
+    for(int i = 0; i < 3; ++i)
+      delete interval_trees[i];
+
+    for(int i = 0; i < 3; ++i)
+      interval_trees[i] = new detail::IntervalTree<S>;
+
+    for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
+    {
+      EndPoint p = endpoints[0][i];
+      CollisionObject<S>* obj = p.obj;
+      if(p.minmax == 0)
+      {
+        SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj);
+        SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj);
+        SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj);
+
+        interval_trees[0]->insert(ivl1);
+        interval_trees[1]->insert(ivl2);
+        interval_trees[2]->insert(ivl3);
+
+        obj_interval_maps[0][obj] = ivl1;
+        obj_interval_maps[1][obj] = ivl2;
+        obj_interval_maps[2][obj] = ivl3;
+      }
+    }
+
+    setup_ = true;
+  }
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::update()
+{
+  setup_ = false;
+
+  for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
+  {
+    if(endpoints[0][i].minmax == 0)
+      endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0];
+    else
+      endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0];
+  }
+
+  for(unsigned int i = 0, size = endpoints[1].size(); i < size; ++i)
+  {
+    if(endpoints[1][i].minmax == 0)
+      endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1];
+    else
+      endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1];
+  }
+
+  for(unsigned int i = 0, size = endpoints[2].size(); i < size; ++i)
+  {
+    if(endpoints[2][i].minmax == 0)
+      endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2];
+    else
+      endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2];
+  }
+
+  setup();
+
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::update(CollisionObject<S>* updated_obj)
+{
+  AABB<S> old_aabb;
+  const AABB<S>& new_aabb = updated_obj->getAABB();
+  for(int i = 0; i < 3; ++i)
+  {
+    const auto it = obj_interval_maps[i].find(updated_obj);
+    interval_trees[i]->deleteNode(it->second);
+    old_aabb.min_[i] = it->second->low;
+    old_aabb.max_[i] = it->second->high;
+    it->second->low = new_aabb.min_[i];
+    it->second->high = new_aabb.max_[i];
+    interval_trees[i]->insert(it->second);
+  }
+
+  EndPoint dummy;
+  typename std::vector<EndPoint>::iterator it;
+  for(int i = 0; i < 3; ++i)
+  {
+    dummy.value = old_aabb.min_[i];
+    it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy);
+    for(; it != endpoints[i].end(); ++it)
+    {
+      if(it->obj == updated_obj && it->minmax == 0)
+      {
+        it->value = new_aabb.min_[i];
+        break;
+      }
+    }
+
+    dummy.value = old_aabb.max_[i];
+    it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy);
+    for(; it != endpoints[i].end(); ++it)
+    {
+      if(it->obj == updated_obj && it->minmax == 0)
+      {
+        it->value = new_aabb.max_[i];
+        break;
+      }
+    }
+
+    std::sort(endpoints[i].begin(), endpoints[i].end());
+  }
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::update(const std::vector<CollisionObject<S>*>& updated_objs)
+{
+  for(size_t i = 0; i < updated_objs.size(); ++i)
+    update(updated_objs[i]);
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::clear()
+{
+  endpoints[0].clear();
+  endpoints[1].clear();
+  endpoints[2].clear();
+
+  delete interval_trees[0]; interval_trees[0] = nullptr;
+  delete interval_trees[1]; interval_trees[1] = nullptr;
+  delete interval_trees[2]; interval_trees[2] = nullptr;
+
+  for(int i = 0; i < 3; ++i)
+  {
+    for(auto it = obj_interval_maps[i].cbegin(), end = obj_interval_maps[i].cend();
+        it != end; ++it)
+    {
+      delete it->second;
+    }
+  }
+
+  for(int i = 0; i < 3; ++i)
+    obj_interval_maps[i].clear();
+
+  setup_ = false;
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::getObjects(std::vector<CollisionObject<S>*>& objs) const
+{
+  objs.resize(endpoints[0].size() / 2);
+  unsigned int j = 0;
+  for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
+  {
+    if(endpoints[0][i].minmax == 0)
+    {
+      objs[j] = endpoints[0][i].obj; j++;
+    }
+  }
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+  collide_(obj, cdata, callback);
+}
+
+//==============================================================================
+template <typename S>
+bool IntervalTreeCollisionManager<S>::collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  static const unsigned int CUTOFF = 100;
+
+  std::deque<detail::SimpleInterval<S>*> results0, results1, results2;
+
+  results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]);
+  if(results0.size() > CUTOFF)
+  {
+    results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]);
+    if(results1.size() > CUTOFF)
+    {
+      results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]);
+      if(results2.size() > CUTOFF)
+      {
+        int d1 = results0.size();
+        int d2 = results1.size();
+        int d3 = results2.size();
+
+        if(d1 >= d2 && d1 >= d3)
+          return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
+        else if(d2 >= d1 && d2 >= d3)
+          return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
+        else
+          return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
+      }
+      else
+        return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
+    }
+    else
+      return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
+  }
+  else
+    return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+  S min_dist = std::numeric_limits<S>::max();
+  distance_(obj, cdata, callback, min_dist);
+}
+
+//==============================================================================
+template <typename S>
+bool IntervalTreeCollisionManager<S>::distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
+{
+  static const unsigned int CUTOFF = 100;
+
+  Vector3<S> delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  AABB<S> aabb = obj->getAABB();
+  if(min_dist < std::numeric_limits<S>::max())
+  {
+    Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
+    aabb.expand(min_dist_delta);
+  }
+
+  int status = 1;
+  S old_min_distance;
+
+  while(1)
+  {
+    bool dist_res = false;
+
+    old_min_distance = min_dist;
+
+    std::deque<detail::SimpleInterval<S>*> results0, results1, results2;
+
+    results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]);
+    if(results0.size() > CUTOFF)
+    {
+      results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]);
+      if(results1.size() > CUTOFF)
+      {
+        results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]);
+        if(results2.size() > CUTOFF)
+        {
+          int d1 = results0.size();
+          int d2 = results1.size();
+          int d3 = results2.size();
+
+          if(d1 >= d2 && d1 >= d3)
+            dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
+          else if(d2 >= d1 && d2 >= d3)
+            dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
+          else
+            dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
+        }
+        else
+          dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
+      }
+      else
+        dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
+    }
+    else
+      dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
+
+    if(dist_res) return true;
+
+    results0.clear();
+    results1.clear();
+    results2.clear();
+
+    if(status == 1)
+    {
+      if(old_min_distance < std::numeric_limits<S>::max())
+        break;
+      else
+      {
+        if(min_dist < old_min_distance)
+        {
+          Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
+          aabb = AABB<S>(obj->getAABB(), min_dist_delta);
+          status = 0;
+        }
+        else
+        {
+          if(aabb.equal(obj->getAABB()))
+            aabb.expand(delta);
+          else
+            aabb.expand(obj->getAABB(), 2.0);
+        }
+      }
+    }
+    else if(status == 0)
+      break;
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::collide(void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  std::set<CollisionObject<S>*> active;
+  std::set<std::pair<CollisionObject<S>*, CollisionObject<S>*> > overlap;
+  unsigned int n = endpoints[0].size();
+  S diff_x = endpoints[0][0].value - endpoints[0][n-1].value;
+  S diff_y = endpoints[1][0].value - endpoints[1][n-1].value;
+  S diff_z = endpoints[2][0].value - endpoints[2][n-1].value;
+
+  int axis = 0;
+  if(diff_y > diff_x && diff_y > diff_z)
+    axis = 1;
+  else if(diff_z > diff_y && diff_z > diff_x)
+    axis = 2;
+
+  for(unsigned int i = 0; i < n; ++i)
+  {
+    const EndPoint& endpoint = endpoints[axis][i];
+    CollisionObject<S>* index = endpoint.obj;
+    if(endpoint.minmax == 0)
+    {
+      auto iter = active.begin();
+      auto end = active.end();
+      for(; iter != end; ++iter)
+      {
+        CollisionObject<S>* active_index = *iter;
+        const AABB<S>& b0 = active_index->getAABB();
+        const AABB<S>& b1 = index->getAABB();
+
+        int axis2 = (axis + 1) % 3;
+        int axis3 = (axis + 2) % 3;
+
+        if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3))
+        {
+          std::pair<typename std::set<std::pair<CollisionObject<S>*, CollisionObject<S>*> >::iterator, bool> insert_res;
+          if(active_index < index)
+            insert_res = overlap.insert(std::make_pair(active_index, index));
+          else
+            insert_res = overlap.insert(std::make_pair(index, active_index));
+
+          if(insert_res.second)
+          {
+            if(callback(active_index, index, cdata))
+              return;
+          }
+        }
+      }
+      active.insert(index);
+    }
+    else
+      active.erase(index);
+  }
+
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::distance(void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+
+  this->enable_tested_set_ = true;
+  this->tested_set.clear();
+  S min_dist = std::numeric_limits<S>::max();
+
+  for(size_t i = 0; i < endpoints[0].size(); ++i)
+    if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break;
+
+  this->enable_tested_set_ = false;
+  this->tested_set.clear();
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
+{
+  IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0)) return;
+
+  if(this == other_manager)
+  {
+    collide(cdata, callback);
+    return;
+  }
+
+  if(this->size() < other_manager->size())
+  {
+    for(size_t i = 0, size = endpoints[0].size(); i < size; ++i)
+      if(other_manager->collide_(endpoints[0][i].obj, cdata, callback)) return;
+  }
+  else
+  {
+    for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
+      if(collide_(other_manager->endpoints[0][i].obj, cdata, callback)) return;
+  }
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
+{
+  IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0)) return;
+
+  if(this == other_manager)
+  {
+    distance(cdata, callback);
+    return;
+  }
+
+  S min_dist = std::numeric_limits<S>::max();
+
+  if(this->size() < other_manager->size())
+  {
+    for(size_t i = 0, size = endpoints[0].size(); i < size; ++i)
+      if(other_manager->distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return;
+  }
+  else
+  {
+    for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
+      if(distance_(other_manager->endpoints[0][i].obj, cdata, callback, min_dist)) return;
+  }
+}
+
+//==============================================================================
+template <typename S>
+bool IntervalTreeCollisionManager<S>::empty() const
+{
+  return endpoints[0].empty();
+}
+
+//==============================================================================
+template <typename S>
+size_t IntervalTreeCollisionManager<S>::size() const
+{
+  return endpoints[0].size() / 2;
+}
+
+//==============================================================================
+template <typename S>
+bool IntervalTreeCollisionManager<S>::checkColl(
+    typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_start,
+    typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_end,
+    CollisionObject<S>* obj,
+    void* cdata,
+    CollisionCallBack<S> callback) const
+{
+  while(pos_start < pos_end)
+  {
+    SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
+    if(ivl->obj != obj)
+    {
+      if(ivl->obj->getAABB().overlap(obj->getAABB()))
+      {
+        if(callback(ivl->obj, obj, cdata))
+          return true;
+      }
+    }
+
+    pos_start++;
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+bool IntervalTreeCollisionManager<S>::checkDist(
+    typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_start,
+    typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_end,
+    CollisionObject<S>* obj,
+    void* cdata,
+    DistanceCallBack<S> callback,
+    S& min_dist) const
+{
+  while(pos_start < pos_end)
+  {
+    SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
+    if(ivl->obj != obj)
+    {
+      if(!this->enable_tested_set_)
+      {
+        if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
+        {
+          if(callback(ivl->obj, obj, cdata, min_dist))
+            return true;
+        }
+      }
+      else
+      {
+        if(!this->inTestedSet(ivl->obj, obj))
+        {
+          if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
+          {
+            if(callback(ivl->obj, obj, cdata, min_dist))
+              return true;
+          }
+
+          this->insertTestedSet(ivl->obj, obj);
+        }
+      }
+    }
+
+    pos_start++;
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+bool IntervalTreeCollisionManager<S>::EndPoint::operator<(
+    const typename IntervalTreeCollisionManager<S>::EndPoint& p) const
+{
+  return value < p.value;
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeCollisionManager<S>::SAPInterval::SAPInterval(
+    S low_, S high_, CollisionObject<S>* obj_)
+  : detail::SimpleInterval<S>()
+{
+  this->low = low_;
+  this->high = high_;
+  obj = obj_;
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_interval_tree.h

@@ -0,0 +1,177 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_INTERVAL_TREE_H
+#define FCL_BROAD_PHASE_INTERVAL_TREE_H
+
+#include <deque>
+#include <map>
+#include "fcl/broadphase/broadphase_collision_manager.h"
+#include "fcl/broadphase/detail/interval_tree.h"
+
+namespace fcl
+{
+
+/// @brief Collision manager based on interval tree
+template <typename S>
+class FCL_EXPORT IntervalTreeCollisionManager : public BroadPhaseCollisionManager<S>
+{
+public:
+  IntervalTreeCollisionManager();
+
+  ~IntervalTreeCollisionManager();
+
+  /// @brief remove one object from the manager
+  void registerObject(CollisionObject<S>* obj);
+
+  /// @brief add one object to the manager
+  void unregisterObject(CollisionObject<S>* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject<S>* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject<S>*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject<S>*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance computation between one object and all the objects belonging to the manager
+  void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+  void collide(void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  void distance(void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager<S>* other_manager, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager<S>* other_manager, void* cdata, DistanceCallBack<S> 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
+  struct EndPoint;
+
+  /// @brief Extention interval tree's interval to SAP interval, adding more information
+  struct SAPInterval;
+
+  bool checkColl(
+      typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_start,
+      typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_end,
+      CollisionObject<S>* obj,
+      void* cdata,
+      CollisionCallBack<S> callback) const;
+
+  bool checkDist(
+      typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_start,
+      typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_end,
+      CollisionObject<S>* obj,
+      void* cdata,
+      DistanceCallBack<S> callback,
+      S& min_dist) const;
+
+  bool collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  bool distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const;
+
+  /// @brief vector stores all the end points
+  std::vector<EndPoint> endpoints[3];
+
+  /// @brief  interval tree manages the intervals
+  detail::IntervalTree<S>* interval_trees[3];
+
+  std::map<CollisionObject<S>*, SAPInterval*> obj_interval_maps[3];
+
+  /// @brief tag for whether the interval tree is maintained suitably
+  bool setup_;
+};
+
+using IntervalTreeCollisionManagerf = IntervalTreeCollisionManager<float>;
+using IntervalTreeCollisionManagerd = IntervalTreeCollisionManager<double>;
+
+/// @brief SAP end point
+template <typename S>
+struct FCL_EXPORT IntervalTreeCollisionManager<S>::EndPoint
+{
+  /// @brief object related with the end point
+  CollisionObject<S>* obj;
+
+  /// @brief end point value
+  S 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
+template <typename S>
+struct FCL_EXPORT IntervalTreeCollisionManager<S>::SAPInterval : public detail::SimpleInterval<S>
+{
+  CollisionObject<S>* obj;
+
+  SAPInterval(S low_, S high_, CollisionObject<S>* obj_);
+};
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_interval_tree-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_spatialhash-inl.h

@@ -0,0 +1,673 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
+#define FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
+
+#include "fcl/broadphase/broadphase_spatialhash.h"
+
+namespace fcl
+{
+
+//==============================================================================
+extern template
+class FCL_EXPORT SpatialHashingCollisionManager<
+    double,
+    detail::SimpleHashTable<
+        AABB<double>, CollisionObject<double>*, detail::SpatialHash<double>>>;
+
+//==============================================================================
+template<typename S, typename HashTable>
+SpatialHashingCollisionManager<S, HashTable>::SpatialHashingCollisionManager(
+    S cell_size,
+    const Vector3<S>& scene_min,
+    const Vector3<S>& scene_max,
+    unsigned int default_table_size)
+  : scene_limit(AABB<S>(scene_min, scene_max)),
+    hash_table(new HashTable(detail::SpatialHash<S>(scene_limit, cell_size)))
+{
+  hash_table->init(default_table_size);
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+SpatialHashingCollisionManager<S, HashTable>::~SpatialHashingCollisionManager()
+{
+  delete hash_table;
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::registerObject(
+    CollisionObject<S>* obj)
+{
+  objs.push_back(obj);
+
+  const AABB<S>& obj_aabb = obj->getAABB();
+  AABB<S> 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 S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::unregisterObject(CollisionObject<S>* obj)
+{
+  objs.remove(obj);
+
+  const AABB<S>& obj_aabb = obj->getAABB();
+  AABB<S> 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 S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::setup()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, 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<S>* obj = *it;
+    const AABB<S>& obj_aabb = obj->getAABB();
+    AABB<S> 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 S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::update(CollisionObject<S>* updated_obj)
+{
+  const AABB<S>& new_aabb = updated_obj->getAABB();
+  const AABB<S>& old_aabb = obj_aabb_map[updated_obj];
+
+  AABB<S> 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<S> 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 S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::update(const std::vector<CollisionObject<S>*>& updated_objs)
+{
+  for(size_t i = 0; i < updated_objs.size(); ++i)
+    update(updated_objs[i]);
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::clear()
+{
+  objs.clear();
+  hash_table->clear();
+  objs_outside_scene_limit.clear();
+  obj_aabb_map.clear();
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::getObjects(std::vector<CollisionObject<S>*>& objs_) const
+{
+  objs_.resize(objs.size());
+  std::copy(objs.begin(), objs.end(), objs_.begin());
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0) return;
+  collide_(obj, cdata, callback);
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0) return;
+  S min_dist = std::numeric_limits<S>::max();
+  distance_(obj, cdata, callback, min_dist);
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+bool SpatialHashingCollisionManager<S, HashTable>::collide_(
+    CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
+{
+  const auto& obj_aabb = obj->getAABB();
+  AABB<S> 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, cdata))
+        return true;
+    }
+
+    if(!scene_limit.contain(obj_aabb))
+    {
+      for(const auto& obj2 : objs_outside_scene_limit)
+      {
+        if(obj == obj2)
+          continue;
+
+        if(callback(obj, obj2, cdata))
+          return true;
+      }
+    }
+  }
+  else
+  {
+    for(const auto& obj2 : objs_partially_penetrating_scene_limit)
+    {
+      if(obj == obj2)
+        continue;
+
+      if(callback(obj, obj2, cdata))
+        return true;
+    }
+
+    for(const auto& obj2 : objs_outside_scene_limit)
+    {
+      if(obj == obj2)
+        continue;
+
+      if(callback(obj, obj2, cdata))
+        return true;
+    }
+  }
+
+  return false;
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+bool SpatialHashingCollisionManager<S, HashTable>::distance_(
+    CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
+{
+  auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  auto aabb = obj->getAABB();
+  if(min_dist < std::numeric_limits<S>::max())
+  {
+    Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
+    aabb.expand(min_dist_delta);
+  }
+
+  AABB<S> overlap_aabb;
+
+  auto status = 1;
+  S 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), cdata, callback, min_dist))
+      {
+        return true;
+      }
+
+      if(!scene_limit.contain(aabb))
+      {
+        if (distanceObjectToObjects(
+              obj, objs_outside_scene_limit, cdata, callback, min_dist))
+        {
+          return true;
+        }
+      }
+    }
+    else
+    {
+      if (distanceObjectToObjects(
+            obj, objs_partially_penetrating_scene_limit, cdata, callback, min_dist))
+      {
+        return true;
+      }
+
+      if (distanceObjectToObjects(
+            obj, objs_outside_scene_limit, cdata, callback, min_dist))
+      {
+        return true;
+      }
+    }
+
+    if(status == 1)
+    {
+      if(old_min_distance < std::numeric_limits<S>::max())
+      {
+        break;
+      }
+      else
+      {
+        if(min_dist < old_min_distance)
+        {
+          Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
+          aabb = AABB<S>(obj->getAABB(), min_dist_delta);
+          status = 0;
+        }
+        else
+        {
+          if(aabb.equal(obj->getAABB()))
+            aabb.expand(delta);
+          else
+            aabb.expand(obj->getAABB(), 2.0);
+        }
+      }
+    }
+    else if(status == 0)
+    {
+      break;
+    }
+  }
+
+  return false;
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::collide(
+    void* cdata, CollisionCallBack<S> callback) const
+{
+  if(size() == 0)
+    return;
+
+  for(const auto& obj1 : objs)
+  {
+    const auto& obj_aabb = obj1->getAABB();
+    AABB<S> 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, cdata))
+            return;
+        }
+      }
+
+      if(!scene_limit.contain(obj_aabb))
+      {
+        for(const auto& obj2 : objs_outside_scene_limit)
+        {
+          if(obj1 < obj2)
+          {
+            if(callback(obj1, obj2, cdata))
+              return;
+          }
+        }
+      }
+    }
+    else
+    {
+      for(const auto& obj2 : objs_partially_penetrating_scene_limit)
+      {
+        if(obj1 < obj2)
+        {
+          if(callback(obj1, obj2, cdata))
+            return;
+        }
+      }
+
+      for(const auto& obj2 : objs_outside_scene_limit)
+      {
+        if(obj1 < obj2)
+        {
+          if(callback(obj1, obj2, cdata))
+            return;
+        }
+      }
+    }
+  }
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::distance(
+    void* cdata, DistanceCallBack<S> callback) const
+{
+  if(size() == 0)
+    return;
+
+  this->enable_tested_set_ = true;
+  this->tested_set.clear();
+
+  S min_dist = std::numeric_limits<S>::max();
+
+  for(const auto& obj : objs)
+  {
+    if(distance_(obj, cdata, callback, min_dist))
+      break;
+  }
+
+  this->enable_tested_set_ = false;
+  this->tested_set.clear();
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
+{
+  auto* other_manager = static_cast<SpatialHashingCollisionManager<S, HashTable>* >(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0))
+    return;
+
+  if(this == other_manager)
+  {
+    collide(cdata, callback);
+    return;
+  }
+
+  if(this->size() < other_manager->size())
+  {
+    for(const auto& obj : objs)
+    {
+      if(other_manager->collide_(obj, cdata, callback))
+        return;
+    }
+  }
+  else
+  {
+    for(const auto& obj : other_manager->objs)
+    {
+      if(collide_(obj, cdata, callback))
+        return;
+    }
+  }
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
+{
+  auto* other_manager = static_cast<SpatialHashingCollisionManager<S, HashTable>* >(other_manager_);
+
+  if((size() == 0) || (other_manager->size() == 0))
+    return;
+
+  if(this == other_manager)
+  {
+    distance(cdata, callback);
+    return;
+  }
+
+  S min_dist = std::numeric_limits<S>::max();
+
+  if(this->size() < other_manager->size())
+  {
+    for(const auto& obj : objs)
+      if(other_manager->distance_(obj, cdata, callback, min_dist)) return;
+  }
+  else
+  {
+    for(const auto& obj : other_manager->objs)
+      if(distance_(obj, cdata, callback, min_dist)) return;
+  }
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+bool SpatialHashingCollisionManager<S, HashTable>::empty() const
+{
+  return objs.empty();
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+size_t SpatialHashingCollisionManager<S, HashTable>::size() const
+{
+  return objs.size();
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+void SpatialHashingCollisionManager<S, HashTable>::computeBound(
+    std::vector<CollisionObject<S>*>& objs, Vector3<S>& l, Vector3<S>& u)
+{
+  AABB<S> bound;
+  for(unsigned int i = 0; i < objs.size(); ++i)
+    bound += objs[i]->getAABB();
+
+  l = bound.min_;
+  u = bound.max_;
+}
+
+//==============================================================================
+template<typename S, typename HashTable>
+template<typename Container>
+bool SpatialHashingCollisionManager<S, HashTable>::distanceObjectToObjects(
+    CollisionObject<S>* obj,
+    const Container& objs,
+    void* cdata,
+    DistanceCallBack<S> callback,
+    S& 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, cdata, min_dist))
+          return true;
+      }
+    }
+    else
+    {
+      if(!this->inTestedSet(obj, obj2))
+      {
+        if(obj->getAABB().distance(obj2->getAABB()) < min_dist)
+        {
+          if(callback(obj, obj2, cdata, min_dist))
+            return true;
+        }
+
+        this->insertTestedSet(obj, obj2);
+      }
+    }
+  }
+
+  return false;
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/broadphase_spatialhash.h

@@ -0,0 +1,174 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_BROADPAHSESPATIALHASH_H
+#define FCL_BROADPHASE_BROADPAHSESPATIALHASH_H
+
+#include <list>
+#include <map>
+#include "fcl/math/bv/AABB.h"
+#include "fcl/broadphase/broadphase_collision_manager.h"
+#include "fcl/broadphase/detail/simple_hash_table.h"
+#include "fcl/broadphase/detail/sparse_hash_table.h"
+#include "fcl/broadphase/detail/spatial_hash.h"
+
+namespace fcl
+{
+
+/// @brief spatial hashing collision mananger
+template<typename S,
+         typename HashTable
+             = detail::SimpleHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>> >
+class FCL_EXPORT SpatialHashingCollisionManager : public BroadPhaseCollisionManager<S>
+{
+public:
+  SpatialHashingCollisionManager(
+      S cell_size,
+      const Vector3<S>& scene_min,
+      const Vector3<S>& scene_max,
+      unsigned int default_table_size = 1000);
+
+  ~SpatialHashingCollisionManager();
+
+  /// @brief add one object to the manager
+  void registerObject(CollisionObject<S>* obj);
+
+  /// @brief remove one object from the manager
+  void unregisterObject(CollisionObject<S>* obj);
+
+  /// @brief initialize the manager, related with the specific type of manager
+  void setup();
+
+  /// @brief update the condition of manager
+  void update();
+
+  /// @brief update the manager by explicitly given the object updated
+  void update(CollisionObject<S>* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  void update(const std::vector<CollisionObject<S>*>& updated_objs);
+
+  /// @brief clear the manager
+  void clear();
+
+  /// @brief return the objects managed by the manager
+  void getObjects(std::vector<CollisionObject<S>*>& objs) const;
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance computation between one object and all the objects belonging ot the manager
+  void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager (i.e, N^2 self collision)
+  void collide(void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+  void distance(void* cdata, DistanceCallBack<S> callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager<S>* other_manager, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager<S>* other_manager, void* cdata, DistanceCallBack<S> 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<S>*>& objs, Vector3<S>& l, Vector3<S>& u);
+
+protected:
+
+  /// @brief perform collision test between one object and all the objects belonging to the manager
+  bool collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
+
+  /// @brief perform distance computation between one object and all the objects belonging ot the manager
+  bool distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const;
+
+  /// @brief all objects in the scene
+  std::list<CollisionObject<S>*> objs;
+
+  /// @brief objects partially penetrating (not totally inside nor outside) the
+  /// scene limit are in another list
+  std::list<CollisionObject<S>*> objs_partially_penetrating_scene_limit;
+
+  /// @brief objects outside the scene limit are in another list
+  std::list<CollisionObject<S>*> objs_outside_scene_limit;
+
+  /// @brief the size of the scene
+  AABB<S> scene_limit;
+
+  /// @brief store the map between objects and their aabbs. will make update more convenient
+  std::map<CollisionObject<S>*, AABB<S>> 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<S>* obj,
+      const Container& objs,
+      void* cdata,
+      DistanceCallBack<S> callback,
+      S& min_dist) const;
+
+};
+
+template<typename HashTable = detail::SimpleHashTable<AABB<float>, CollisionObject<float>*, detail::SpatialHash<float>>>
+using SpatialHashingCollisionManagerf = SpatialHashingCollisionManager<float, HashTable>;
+
+template<typename HashTable = detail::SimpleHashTable<AABB<double>, CollisionObject<double>*, detail::SpatialHash<double>>>
+using SpatialHashingCollisionManagerd = SpatialHashingCollisionManager<double, HashTable>;
+
+} // namespace fcl
+
+#include "fcl/broadphase/broadphase_spatialhash-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/hierarchy_tree.h

@@ -0,0 +1,273 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan  */
+
+#ifndef FCL_HIERARCHY_TREE_H
+#define FCL_HIERARCHY_TREE_H
+
+#include <vector>
+#include <map>
+#include <functional>
+#include <iostream>
+#include "fcl/common/warning.h"
+#include "fcl/math/bv/AABB.h"
+#include "fcl/broadphase/detail/morton.h"
+#include "fcl/broadphase/detail/node_base.h"
+
+namespace fcl
+{
+
+namespace detail
+{
+
+/// @brief Class for hierarchy tree structure
+template<typename BV>
+class FCL_EXPORT HierarchyTree
+{
+public:
+
+  using S = typename BV::S;
+
+  typedef NodeBase<BV> NodeType;
+
+  /// @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<NodeType*>& leaves, int level = 0);
+
+  /// @brief Insest a node
+  NodeType* insert(const BV& bv, void* data);
+
+  /// @brief Remove a leaf node
+  void remove(NodeType* leaf);
+
+  /// @brief Clear the tree 
+  void clear();
+
+  /// @brief Whether the tree is empty 
+  bool empty() const;
+
+  /// @brief update one leaf node 
+  void update(NodeType* leaf, int lookahead_level = -1);
+
+  /// @brief update the tree when the bounding volume of a given leaf has changed
+  bool update(NodeType* leaf, const BV& bv);
+
+  /// @brief update one leaf's bounding volume, with prediction 
+  bool update(NodeType* leaf, const BV& bv, const Vector3<S>& vel, S margin);
+
+  /// @brief update one leaf's bounding volume, with prediction 
+  bool update(NodeType* leaf, const BV& bv, const Vector3<S>& 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 NodeType* root, std::vector<NodeType*>& leaves) const;
+
+  /// @brief number of leaves in the tree
+  size_t size() const;
+
+  /// @brief get the root of the tree
+  NodeType* getRoot() const;
+
+  NodeType*& getRoot();
+
+  /// @brief print the tree in a recursive way
+  void print(NodeType* 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 NodeType* a, const NodeType* 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 
+  NodeType* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief compute the maximum height of a subtree rooted from a given node
+  size_t getMaxHeight(NodeType* node) const;
+
+  /// @brief compute the maximum depth of a subtree rooted from a given node
+  void getMaxDepth(NodeType* 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<S> edge.
+  /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold.
+  NodeType* 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.
+  NodeType* 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<NodeType*>& 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<NodeType*>& 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<NodeType*>& 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<NodeType*>& leaves);
+  
+  NodeType* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32& split, int bits);
+
+  NodeType* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32& split, int bits);
+
+  NodeType* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief update one leaf node's bounding volume 
+  void update_(NodeType* leaf, const BV& bv);
+
+  /// @brief sort node n and its parent according to their memory position 
+  NodeType* sort(NodeType* n, NodeType*& r);
+  
+  /// @brief Insert a leaf node and also update its ancestors 
+  void insertLeaf(NodeType* root, NodeType* 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 
+  NodeType* removeLeaf(NodeType* leaf);
+
+  /// @brief Delete all internal nodes and return all leaves nodes with given depth from root 
+  void fetchLeaves(NodeType* root, std::vector<NodeType*>& leaves, int depth = -1);
+
+  static size_t indexOf(NodeType* node);
+
+  /// @brief create one node (leaf or internal)  
+  NodeType* createNode(NodeType* parent, 
+                       const BV& bv,
+                       void* data);
+
+  NodeType* createNode(NodeType* parent,
+                       const BV& bv1,
+                       const BV& bv2,
+                       void* data);
+  
+  NodeType* createNode(NodeType* parent,
+                       void* data);
+
+  void deleteNode(NodeType* node);
+
+  void recurseDeleteNode(NodeType* node);
+
+  void recurseRefit(NodeType* node);
+
+  static BV bounds(const std::vector<NodeType*>& leaves);
+
+  static BV bounds(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+protected:
+  NodeType* root_node;
+
+  size_t n_leaves;
+
+  unsigned int opath;
+
+  /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently. 
+  NodeType* 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
+
+#include "fcl/broadphase/detail/hierarchy_tree-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/hierarchy_tree_array.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 FCL_HIERARCHY_TREE_ARRAY_H
+#define FCL_HIERARCHY_TREE_ARRAY_H
+
+#include <vector>
+#include <map>
+#include <functional>
+#include <iostream>
+#include "fcl/common/warning.h"
+#include "fcl/math/bv/AABB.h"
+#include "fcl/broadphase/detail/morton.h"
+#include "fcl/broadphase/detail/node_base_array.h"
+
+namespace fcl
+{
+
+namespace detail
+{
+
+namespace implementation_array
+{
+
+/// @brief Class for hierarchy tree structure
+template<typename BV>
+class FCL_EXPORT HierarchyTree
+{
+  using S = typename BV::S;
+  typedef NodeBase<BV> NodeType;
+  
+  struct SortByMorton
+  {
+    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;
+    }
+
+    NodeType* nodes;
+    uint32 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(NodeType* 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 Vector3<S>& vel, S margin);
+
+  /// @brief update one leaf's bounding volume, with prediction 
+  bool update(size_t leaf, const BV& bv, const Vector3<S>& 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, NodeType*& 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
+  NodeType* 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<S> 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(NodeType* 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(NodeType* 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(NodeType* 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(NodeType* leaves, int n_leaves_);
+
+  size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32& split, int bits);
+
+  size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32& 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, NodeType*& 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;
+  NodeType* 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 = -1;
+};
+
+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<S>. 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
+
+#include "fcl/broadphase/detail/hierarchy_tree_array-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/interval_tree-inl.h

@@ -0,0 +1,563 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_INTERVAL_TREE_INL_H
+#define FCL_INTERVAL_TREE_INL_H
+
+#include "fcl/broadphase/detail/interval_tree.h"
+
+#include <algorithm>
+
+namespace fcl {
+namespace detail {
+
+//==============================================================================
+extern template
+class IntervalTree<double>;
+
+//==============================================================================
+template <typename S>
+IntervalTree<S>::IntervalTree()
+{
+  nil = new IntervalTreeNode<S>;
+  nil->left = nil->right = nil->parent = nil;
+  nil->red = false;
+  nil->key = nil->high = nil->max_high = -std::numeric_limits<double>::max();
+  nil->stored_interval = nullptr;
+
+  root = new IntervalTreeNode<S>;
+  root->parent = root->left = root->right = nil;
+  root->key = root->high = root->max_high = std::numeric_limits<double>::max();
+  root->red = false;
+  root->stored_interval = nullptr;
+
+  /// the following are used for the query function
+  recursion_node_stack_size = 128;
+  recursion_node_stack = (it_recursion_node<S>*)malloc(recursion_node_stack_size*sizeof(it_recursion_node<S>));
+  recursion_node_stack_top = 1;
+  recursion_node_stack[0].start_node = nullptr;
+}
+
+//==============================================================================
+template <typename S>
+IntervalTree<S>::~IntervalTree()
+{
+  IntervalTreeNode<S>* x = root->left;
+  std::deque<IntervalTreeNode<S>*> nodes_to_free;
+
+  if(x != nil)
+  {
+    if(x->left != nil)
+    {
+      nodes_to_free.push_back(x->left);
+    }
+    if(x->right != nil)
+    {
+      nodes_to_free.push_back(x->right);
+    }
+
+    delete x;
+    while( nodes_to_free.size() > 0)
+    {
+      x = nodes_to_free.back();
+      nodes_to_free.pop_back();
+      if(x->left != nil)
+      {
+        nodes_to_free.push_back(x->left);
+      }
+      if(x->right != nil)
+      {
+        nodes_to_free.push_back(x->right);
+      }
+      delete x;
+    }
+  }
+  delete nil;
+  delete root;
+  free(recursion_node_stack);
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTree<S>::leftRotate(IntervalTreeNode<S>* x)
+{
+  IntervalTreeNode<S>* y;
+
+  y = x->right;
+  x->right = y->left;
+
+  if(y->left != nil) y->left->parent = x;
+
+  y->parent = x->parent;
+
+  if(x == x->parent->left)
+    x->parent->left = y;
+  else
+    x->parent->right = y;
+
+  y->left = x;
+  x->parent = y;
+
+  x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high));
+  y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTree<S>::rightRotate(IntervalTreeNode<S>* y)
+{
+  IntervalTreeNode<S>* x;
+
+  x = y->left;
+  y->left = x->right;
+
+  if(nil != x->right)  x->right->parent = y;
+
+  x->parent = y->parent;
+  if(y == y->parent->left)
+    y->parent->left = x;
+  else
+    y->parent->right = x;
+
+  x->right = y;
+  y->parent = x;
+
+  y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high));
+  x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTree<S>::recursiveInsert(IntervalTreeNode<S>* z)
+{
+  IntervalTreeNode<S>* x;
+  IntervalTreeNode<S>* y;
+
+  z->left = z->right = nil;
+  y = root;
+  x = root->left;
+  while(x != nil)
+  {
+    y = x;
+    if(x->key > z->key)
+      x = x->left;
+    else
+      x = x->right;
+  }
+  z->parent = y;
+  if((y == root) || (y->key > z->key))
+    y->left = z;
+  else
+    y->right = z;
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTree<S>::fixupMaxHigh(IntervalTreeNode<S>* x)
+{
+  while(x != root)
+  {
+    x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high));
+    x = x->parent;
+  }
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeNode<S>* IntervalTree<S>::insert(SimpleInterval<S>* new_interval)
+{
+  IntervalTreeNode<S>* y;
+  IntervalTreeNode<S>* x;
+  IntervalTreeNode<S>* new_node;
+
+  x = new IntervalTreeNode<S>(new_interval);
+  recursiveInsert(x);
+  fixupMaxHigh(x->parent);
+  new_node = x;
+  x->red = true;
+  while(x->parent->red)
+  {
+    /// use sentinel instead of checking for root
+    if(x->parent == x->parent->parent->left)
+    {
+      y = x->parent->parent->right;
+      if(y->red)
+      {
+        x->parent->red = true;
+        y->red = true;
+        x->parent->parent->red = true;
+        x = x->parent->parent;
+      }
+      else
+      {
+        if(x == x->parent->right)
+        {
+          x = x->parent;
+          leftRotate(x);
+        }
+        x->parent->red = false;
+        x->parent->parent->red = true;
+        rightRotate(x->parent->parent);
+      }
+    }
+    else
+    {
+      y = x->parent->parent->left;
+      if(y->red)
+      {
+        x->parent->red = false;
+        y->red = false;
+        x->parent->parent->red = true;
+        x = x->parent->parent;
+      }
+      else
+      {
+        if(x == x->parent->left)
+        {
+          x = x->parent;
+          rightRotate(x);
+        }
+        x->parent->red = false;
+        x->parent->parent->red = true;
+        leftRotate(x->parent->parent);
+      }
+    }
+  }
+  root->left->red = false;
+  return new_node;
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeNode<S>* IntervalTree<S>::getSuccessor(IntervalTreeNode<S>* x) const
+{
+  IntervalTreeNode<S>* y;
+
+  if(nil != (y = x->right))
+  {
+    while(y->left != nil)
+      y = y->left;
+    return y;
+  }
+  else
+  {
+    y = x->parent;
+    while(x == y->right)
+    {
+      x = y;
+      y = y->parent;
+    }
+    if(y == root) return nil;
+    return y;
+  }
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeNode<S>* IntervalTree<S>::getPredecessor(IntervalTreeNode<S>* x) const
+{
+  IntervalTreeNode<S>* y;
+
+  if(nil != (y = x->left))
+  {
+    while(y->right != nil)
+      y = y->right;
+    return y;
+  }
+  else
+  {
+    y = x->parent;
+    while(x == y->left)
+    {
+      if(y == root) return nil;
+      x = y;
+      y = y->parent;
+    }
+    return y;
+  }
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTree<S>::recursivePrint(IntervalTreeNode<S>* x) const
+{
+  if(x != nil)
+  {
+    recursivePrint(x->left);
+    x->print(nil,root);
+    recursivePrint(x->right);
+  }
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTree<S>::print() const
+{
+  recursivePrint(root->left);
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTree<S>::deleteFixup(IntervalTreeNode<S>* x)
+{
+  IntervalTreeNode<S>* w;
+  IntervalTreeNode<S>* root_left_node = root->left;
+
+  while((!x->red) && (root_left_node != x))
+  {
+    if(x == x->parent->left)
+    {
+      w = x->parent->right;
+      if(w->red)
+      {
+        w->red = false;
+        x->parent->red = true;
+        leftRotate(x->parent);
+        w = x->parent->right;
+      }
+      if((!w->right->red) && (!w->left->red))
+      {
+        w->red = true;
+        x = x->parent;
+      }
+      else
+      {
+        if(!w->right->red)
+        {
+          w->left->red = false;
+          w->red = true;
+          rightRotate(w);
+          w = x->parent->right;
+        }
+        w->red = x->parent->red;
+        x->parent->red = false;
+        w->right->red = false;
+        leftRotate(x->parent);
+        x = root_left_node;
+      }
+    }
+    else
+    {
+      w = x->parent->left;
+      if(w->red)
+      {
+        w->red = false;
+        x->parent->red = true;
+        rightRotate(x->parent);
+        w = x->parent->left;
+      }
+      if((!w->right->red) && (!w->left->red))
+      {
+        w->red = true;
+        x = x->parent;
+      }
+      else
+      {
+        if(!w->left->red)
+        {
+          w->right->red = false;
+          w->red = true;
+          leftRotate(w);
+          w = x->parent->left;
+        }
+        w->red = x->parent->red;
+        x->parent->red = false;
+        w->left->red = false;
+        rightRotate(x->parent);
+        x = root_left_node;
+      }
+    }
+  }
+  x->red = false;
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTree<S>::deleteNode(SimpleInterval<S>* ivl)
+{
+  IntervalTreeNode<S>* node = recursiveSearch(root, ivl);
+  if(node)
+    deleteNode(node);
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeNode<S>* IntervalTree<S>::recursiveSearch(IntervalTreeNode<S>* node, SimpleInterval<S>* ivl) const
+{
+  if(node != nil)
+  {
+    if(node->stored_interval == ivl)
+      return node;
+
+    IntervalTreeNode<S>* left = recursiveSearch(node->left, ivl);
+    if(left != nil) return left;
+    IntervalTreeNode<S>* right = recursiveSearch(node->right, ivl);
+    if(right != nil) return right;
+  }
+
+  return nil;
+}
+
+//==============================================================================
+template <typename S>
+SimpleInterval<S>* IntervalTree<S>::deleteNode(IntervalTreeNode<S>* z)
+{
+  IntervalTreeNode<S>* y;
+  IntervalTreeNode<S>* x;
+  SimpleInterval<S>* node_to_delete = z->stored_interval;
+
+  y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z);
+  x= (y->left == nil) ? y->right : y->left;
+  if(root == (x->parent = y->parent))
+  {
+    root->left = x;
+  }
+  else
+  {
+    if(y == y->parent->left)
+    {
+      y->parent->left = x;
+    }
+    else
+    {
+      y->parent->right = x;
+    }
+  }
+
+  /// @brief y should not be nil in this case
+  /// y is the node to splice out and x is its child
+  if(y != z)
+  {
+    y->max_high = -std::numeric_limits<double>::max();
+    y->left = z->left;
+    y->right = z->right;
+    y->parent = z->parent;
+    z->left->parent = z->right->parent = y;
+    if(z == z->parent->left)
+      z->parent->left = y;
+    else
+      z->parent->right = y;
+
+    fixupMaxHigh(x->parent);
+    if(!(y->red))
+    {
+      y->red = z->red;
+      deleteFixup(x);
+    }
+    else
+      y->red = z->red;
+    delete z;
+  }
+  else
+  {
+    fixupMaxHigh(x->parent);
+    if(!(y->red)) deleteFixup(x);
+    delete y;
+  }
+
+  return node_to_delete;
+}
+
+//==============================================================================
+/// @brief returns 1 if the intervals overlap, and 0 otherwise
+template <typename S>
+bool overlap(S a1, S a2, S b1, S b2)
+{
+  if(a1 <= b1)
+  {
+    return (b1 <= a2);
+  }
+  else
+  {
+    return (a1 <= b2);
+  }
+}
+
+//==============================================================================
+template <typename S>
+std::deque<SimpleInterval<S>*> IntervalTree<S>::query(S low, S high)
+{
+  std::deque<SimpleInterval<S>*> result_stack;
+  IntervalTreeNode<S>* x = root->left;
+  bool run = (x != nil);
+
+  current_parent = 0;
+
+  while(run)
+  {
+    if(overlap(low,high,x->key,x->high))
+    {
+      result_stack.push_back(x->stored_interval);
+      recursion_node_stack[current_parent].try_right_branch = true;
+    }
+    if(x->left->max_high >= low)
+    {
+      if(recursion_node_stack_top == recursion_node_stack_size)
+      {
+        recursion_node_stack_size *= 2;
+        recursion_node_stack = (it_recursion_node<S> *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node<S>));
+        if(recursion_node_stack == nullptr)
+          exit(1);
+      }
+      recursion_node_stack[recursion_node_stack_top].start_node = x;
+      recursion_node_stack[recursion_node_stack_top].try_right_branch = false;
+      recursion_node_stack[recursion_node_stack_top].parent_index = current_parent;
+      current_parent = recursion_node_stack_top++;
+      x = x->left;
+    }
+    else
+      x = x->right;
+
+    run = (x != nil);
+    while((!run) && (recursion_node_stack_top > 1))
+    {
+      if(recursion_node_stack[--recursion_node_stack_top].try_right_branch)
+      {
+        x=recursion_node_stack[recursion_node_stack_top].start_node->right;
+        current_parent=recursion_node_stack[recursion_node_stack_top].parent_index;
+        recursion_node_stack[current_parent].try_right_branch = true;
+        run = (x != nil);
+      }
+    }
+  }
+  return result_stack;
+}
+
+} // namespace detail
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/interval_tree.h

@@ -0,0 +1,142 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_INTERVAL_TREE_H
+#define FCL_INTERVAL_TREE_H
+
+#include <deque>
+#include <limits>
+#include <cstdlib>
+#include <iostream>
+#include "fcl/broadphase/detail/interval_tree_node.h"
+
+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.
+template <typename S>
+struct FCL_EXPORT it_recursion_node
+{
+public:
+  IntervalTreeNode<S>* start_node;
+
+  unsigned int parent_index;
+
+  bool try_right_branch;
+};
+
+using it_recursion_nodef = it_recursion_node<float>;
+using it_recursion_noded = it_recursion_node<double>;
+
+extern template
+struct it_recursion_node<double>;
+
+/// @brief Interval tree
+template <typename S>
+class FCL_EXPORT IntervalTree
+{
+public:
+
+  IntervalTree();
+
+  ~IntervalTree();
+
+  /// @brief Print the whole interval tree
+  void print() const;
+
+  /// @brief Delete one node of the interval tree
+  SimpleInterval<S>* deleteNode(IntervalTreeNode<S>* node);
+
+  /// @brief delete node stored a given interval
+  void deleteNode(SimpleInterval<S>* ivl);
+
+  /// @brief Insert one node of the interval tree
+  IntervalTreeNode<S>* insert(SimpleInterval<S>* new_interval);
+
+  /// @brief get the predecessor of a given node
+  IntervalTreeNode<S>* getPredecessor(IntervalTreeNode<S>* node) const;
+
+  /// @brief Get the successor of a given node
+  IntervalTreeNode<S>* getSuccessor(IntervalTreeNode<S>* node) const;
+
+  /// @brief Return result for a given query
+  std::deque<SimpleInterval<S>*> query(S low, S high);
+
+protected:
+
+  IntervalTreeNode<S>* root;
+
+  IntervalTreeNode<S>* nil;
+
+  /// @brief left rotation of tree node
+  void leftRotate(IntervalTreeNode<S>* node);
+
+  /// @brief right rotation of tree node
+  void rightRotate(IntervalTreeNode<S>* node);
+
+  /// @brief Inserts node into the tree as if it were a regular binary tree
+  void recursiveInsert(IntervalTreeNode<S>* node);
+
+  /// @brief recursively print a subtree 
+  void recursivePrint(IntervalTreeNode<S>* node) const;
+
+  /// @brief recursively find the node corresponding to the interval
+  IntervalTreeNode<S>* recursiveSearch(IntervalTreeNode<S>* node, SimpleInterval<S>* ivl) const;
+
+  /// @brief Travels up to the root fixing the max_high fields after an insertion or deletion
+  void fixupMaxHigh(IntervalTreeNode<S>* node);
+
+  void deleteFixup(IntervalTreeNode<S>* node);
+
+private:
+  unsigned int recursion_node_stack_size;
+  it_recursion_node<S>* recursion_node_stack;
+  unsigned int current_parent;
+  unsigned int recursion_node_stack_top;
+};
+
+using IntervalTreef = IntervalTree<float>;
+using IntervalTreed = IntervalTree<double>;
+
+} // namespace detail
+} // namespace fcl
+
+#include "fcl/broadphase/detail/interval_tree-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/interval_tree_node-inl.h

@@ -0,0 +1,96 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
+#define FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
+
+#include "fcl/broadphase/detail/interval_tree_node.h"
+
+#include <iostream>
+
+namespace fcl {
+namespace detail {
+
+//==============================================================================
+extern template
+class FCL_EXPORT IntervalTreeNode<double>;
+
+//==============================================================================
+template <typename S>
+IntervalTreeNode<S>::IntervalTreeNode()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeNode<S>::IntervalTreeNode(SimpleInterval<S>* new_interval)
+  : stored_interval (new_interval),
+    key(new_interval->low),
+    high(new_interval->high),
+    max_high(high)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+IntervalTreeNode<S>::~IntervalTreeNode()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+void IntervalTreeNode<S>::print(
+    IntervalTreeNode<S>* nil, IntervalTreeNode<S>* root) const
+{
+  stored_interval->print();
+  std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high;
+  std::cout << "  l->key = ";
+  if(left == nil) std::cout << "nullptr"; else std::cout << left->key;
+  std::cout << "  r->key = ";
+  if(right == nil) std::cout << "nullptr"; else std::cout << right->key;
+  std::cout << "  p->key = ";
+  if(parent == root) std::cout << "nullptr"; else std::cout << parent->key;
+  std::cout << "  red = " << (int)red << std::endl;
+}
+
+} // namespace detail
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/interval_tree_node.h

@@ -0,0 +1,103 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_DETAIL_INTERVALTREENODE_H
+#define FCL_BROADPHASE_DETAIL_INTERVALTREENODE_H
+
+#include "fcl/broadphase/detail/simple_interval.h"
+#include "fcl/export.h"
+
+namespace fcl
+{
+
+namespace detail
+{
+
+template <typename S>
+class FCL_EXPORT IntervalTree;
+
+/// @brief The node for interval tree
+template <typename S>
+class FCL_EXPORT IntervalTreeNode
+{
+public:
+
+  template <typename>
+  friend class IntervalTree;
+
+  friend class IntervalTree<double>;
+  
+  /// @brief Create an empty node
+  IntervalTreeNode();
+
+  /// @brief Create an node storing the interval
+  IntervalTreeNode(SimpleInterval<S>* new_interval);
+
+  ~IntervalTreeNode();
+
+  /// @brief Print the interval node information: set left = nil and right = root
+  void print(IntervalTreeNode* left, IntervalTreeNode* right) const;
+
+protected:
+  /// @brief interval stored in the node
+  SimpleInterval<S>* stored_interval;
+
+  S key;
+
+  S high;
+
+  S max_high;
+
+  /// @brief red or black node: if red = false then the node is black
+  bool red;  
+
+  IntervalTreeNode* left;
+
+  IntervalTreeNode* right;
+
+  IntervalTreeNode* parent;
+};
+
+using IntervalTreeNodef = IntervalTreeNode<float>;
+using IntervalTreeNoded = IntervalTreeNode<double>;
+
+} // namespace detail
+} // namespace fcl
+
+#include "fcl/broadphase/detail/interval_tree_node-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/morton-inl.h

@@ -0,0 +1,175 @@
+/*
+ * 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 FCL_MORTON_INL_H
+#define FCL_MORTON_INL_H
+
+#include "fcl/broadphase/detail/morton.h"
+
+namespace fcl {
+/// @cond IGNORE
+namespace detail {
+
+//==============================================================================
+extern template
+uint32 quantize(double x, uint32 n);
+
+//==============================================================================
+extern template
+struct morton_functor<double, uint32>;
+
+//==============================================================================
+extern template
+struct morton_functor<double, uint64>;
+
+//==============================================================================
+template <typename S>
+uint32 quantize(S x, uint32 n)
+{
+  return std::max(std::min((uint32)(x * (S)n), uint32(n-1)), uint32(0));
+}
+
+//==============================================================================
+template<typename S>
+morton_functor<S, uint32>::morton_functor(const AABB<S>& 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 morton_functor<S, uint32>::operator()(const Vector3<S>& point) const
+{
+  uint32 x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
+  uint32 y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
+  uint32 z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
+
+  return detail::morton_code(x, y, z);
+}
+
+//==============================================================================
+template<typename S>
+morton_functor<S, uint64>::morton_functor(const AABB<S>& 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 morton_functor<S, uint64>::operator()(const Vector3<S>& point) const
+{
+  uint32 x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
+  uint32 y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
+  uint32 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>::bits()
+{
+  return 60;
+}
+
+//==============================================================================
+template<typename S>
+constexpr size_t morton_functor<S, uint32>::bits()
+{
+  return 30;
+}
+
+//==============================================================================
+template<typename S, size_t N>
+morton_functor<S, std::bitset<N>>::morton_functor(const AABB<S>& 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 Vector3<S>& 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
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/morton.h

@@ -0,0 +1,130 @@
+/*
+ * 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 FCL_MORTON_H
+#define FCL_MORTON_H
+
+#include "fcl/common/types.h"
+#include "fcl/math/bv/AABB.h"
+
+#include <bitset>
+
+namespace fcl
+{
+
+/// @cond IGNORE
+namespace detail
+{
+
+template <typename S>
+FCL_EXPORT
+uint32 quantize(S x, uint32 n);
+
+/// @brief compute 30 bit morton code
+FCL_EXPORT
+uint32 morton_code(uint32 x, uint32 y, uint32 z);
+
+/// @brief compute 60 bit morton code
+FCL_EXPORT
+uint64 morton_code60(uint32 x, uint32 y, uint32 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 FCL_EXPORT morton_functor {};
+
+/// @brief Functor to compute 30 bit morton code for a given AABB<S>
+template<typename S>
+struct FCL_EXPORT morton_functor<S, uint32>
+{
+  morton_functor(const AABB<S>& bbox);
+
+  uint32 operator() (const Vector3<S>& point) const;
+
+  const Vector3<S> base;
+  const Vector3<S> inv;
+
+  static constexpr size_t bits();
+};
+
+using morton_functoru32f = morton_functor<float, uint32>;
+using morton_functoru32d = morton_functor<double, uint32>;
+
+/// @brief Functor to compute 60 bit morton code for a given AABB<S>
+template<typename S>
+struct FCL_EXPORT morton_functor<S, uint64>
+{
+  morton_functor(const AABB<S>& bbox);
+
+  uint64 operator() (const Vector3<S>& point) const;
+
+  const Vector3<S> base;
+  const Vector3<S> inv;
+
+  static constexpr size_t bits();
+};
+
+using morton_functoru64f = morton_functor<float, uint64>;
+using morton_functoru64d = morton_functor<double, uint64>;
+
+/// @brief Functor to compute N bit morton code for a given AABB<S>
+/// N must be a multiple of 3.
+template<typename S, size_t N>
+struct FCL_EXPORT morton_functor<S, std::bitset<N>>
+{
+  static_assert(N%3==0, "Number of bits must be a multiple of 3");
+
+  morton_functor(const AABB<S>& bbox);
+
+  std::bitset<N> operator() (const Vector3<S>& point) const;
+
+  const Vector3<S> base;
+  const Vector3<S> inv;
+
+  static constexpr size_t bits();
+};
+
+} // namespace detail
+/// @endcond
+} // namespace fcl
+
+#include "fcl/broadphase/detail/morton-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/node_base-inl.h

@@ -0,0 +1,81 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan  */
+
+#ifndef FCL_BROADPHASE_DETAIL_NODEBASE_INL_H
+#define FCL_BROADPHASE_DETAIL_NODEBASE_INL_H
+
+#include "fcl/broadphase/detail/node_base.h"
+
+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
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/node_base.h

@@ -0,0 +1,83 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan  */
+
+#ifndef FCL_BROADPHASE_DETAIL_NODEBASE_H
+#define FCL_BROADPHASE_DETAIL_NODEBASE_H
+
+#include "fcl/common/types.h"
+
+namespace fcl
+{
+
+namespace detail
+{
+
+/// @brief dynamic AABB<S> 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 code;
+
+  NodeBase();
+};
+
+} // namespace detail
+} // namespace fcl
+
+#include "fcl/broadphase/detail/node_base-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/node_base_array-inl.h

@@ -0,0 +1,70 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan  */
+
+#ifndef FCL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
+#define FCL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
+
+#include "fcl/broadphase/detail/node_base_array.h"
+
+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
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/node_base_array.h

@@ -0,0 +1,81 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan  */
+
+#ifndef FCL_BROADPHASE_DETAIL_NODEBASEARRAY_H
+#define FCL_BROADPHASE_DETAIL_NODEBASEARRAY_H
+
+#include "fcl/common/types.h"
+
+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 code;
+  
+  bool isLeaf() const;
+  bool isInternal() const;
+};
+
+} // namespace implementation_array
+} // namespace detail
+} // namespace fcl
+
+#include "fcl/broadphase/detail/node_base_array-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/simple_hash_table-inl.h

@@ -0,0 +1,123 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_SIMPLEHASHTABLE_INL_H
+#define FCL_BROADPHASE_SIMPLEHASHTABLE_INL_H
+
+#include "fcl/broadphase/detail/simple_hash_table.h"
+
+#include <iterator>
+
+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)
+  {
+    throw std::logic_error("SimpleHashTable must have non-zero size.");
+  }
+
+  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)
+  {
+    unsigned int 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)
+  {
+    unsigned int 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
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/simple_hash_table.h

@@ -0,0 +1,91 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_SIMPLEHASHTABLE_H
+#define FCL_BROADPHASE_SIMPLEHASHTABLE_H
+
+#include <stdexcept>
+#include <set>
+#include <vector>
+#include <list>
+
+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 FCL_EXPORT 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
+
+#include "fcl/broadphase/detail/simple_hash_table-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/simple_interval-inl.h

@@ -0,0 +1,67 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
+#define FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
+
+#include "fcl/broadphase/detail/simple_interval.h"
+
+namespace fcl {
+namespace detail {
+
+//==============================================================================
+extern template
+struct SimpleInterval<double>;
+
+//==============================================================================
+template <typename S>
+SimpleInterval<S>::~SimpleInterval()
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+void SimpleInterval<S>::print()
+{
+  // Do nothing
+}
+
+} // namespace detail
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/simple_interval.h

@@ -0,0 +1,71 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H
+#define FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H
+
+#include "fcl/export.h"
+
+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.
+template <typename S>
+struct FCL_EXPORT SimpleInterval
+{
+public:
+  virtual ~SimpleInterval();
+  
+  virtual void print();
+
+  /// @brief interval is defined as [low, high]
+  S low, high;
+};
+
+using SimpleIntervalf = SimpleInterval<float>;
+using SimpleIntervald = SimpleInterval<double>;
+
+} // namespace detail
+} // namespace fcl
+
+#include "fcl/broadphase/detail/simple_interval-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/sparse_hash_table-inl.h

@@ -0,0 +1,121 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_SPARSEHASHTABLE_INL_H
+#define FCL_BROADPHASE_SPARSEHASHTABLE_INL_H
+
+#include "fcl/broadphase/detail/sparse_hash_table.h"
+
+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
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/sparse_hash_table.h

@@ -0,0 +1,91 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_SPARSEHASHTABLE_H
+#define FCL_BROADPHASE_SPARSEHASHTABLE_H
+
+#include <stdexcept>
+#include <set>
+#include <vector>
+#include <list>
+#include <unordered_map>
+
+namespace fcl
+{
+
+namespace detail
+{
+
+template<typename U, typename V>
+class FCL_EXPORT unordered_map_hash_table : public std::unordered_map<U, V> {};
+
+/// @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 FCL_EXPORT 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
+
+#include "fcl/broadphase/detail/sparse_hash_table-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/spatial_hash-inl.h

@@ -0,0 +1,89 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_SPATIALHASH_INL_H
+#define FCL_BROADPHASE_SPATIALHASH_INL_H
+
+#include "fcl/broadphase/detail/spatial_hash.h"
+
+namespace fcl {
+namespace detail {
+
+//==============================================================================
+extern template
+struct SpatialHash<double>;
+
+//==============================================================================
+template <typename S>
+SpatialHash<S>::SpatialHash(const AABB<S>& scene_limit_, S 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);
+}
+
+//==============================================================================
+template <typename S>
+std::vector<unsigned int> SpatialHash<S>::operator()(const AABB<S>& 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
+
+#endif

3rdparty/fcl/include/fcl/broadphase/detail/spatial_hash.h

@@ -0,0 +1,74 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */ 
+
+/** @author Jia Pan */
+
+#ifndef FCL_BROADPHASE_SPATIALHASH_H
+#define FCL_BROADPHASE_SPATIALHASH_H
+
+#include "fcl/math/bv/AABB.h"
+
+namespace fcl
+{
+
+namespace detail
+{
+
+/// @brief Spatial hash function: hash an AABB to a set of integer values
+template <typename S_>
+struct FCL_EXPORT SpatialHash
+{
+  using S = S_;
+
+  SpatialHash(const AABB<S>& scene_limit_, S cell_size_);
+    
+  std::vector<unsigned int> operator() (const AABB<S>& aabb) const;
+
+private:
+
+  S cell_size;
+  AABB<S> scene_limit;
+  unsigned int width[3];
+};
+
+using SpatialHashf = SpatialHash<float>;
+using SpatialHashd = SpatialHash<double>;
+
+} // namespace detail
+} // namespace fcl
+
+#include "fcl/broadphase/detail/spatial_hash-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/common/detail/profiler.h

@@ -0,0 +1,251 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008-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 Ioan Sucan */
+
+#ifndef FCL_COMMON_DETAIL_PROFILER_H
+#define FCL_COMMON_DETAIL_PROFILER_H
+
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <map>
+#include <cmath>
+#include <mutex>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+#include "fcl/common/time.h"
+#include "fcl/export.h"
+
+namespace fcl {
+namespace detail {
+
+/// @brief This is a simple thread-safe tool for counting time
+/// spent in various chunks of code. This is different from
+/// external profiling tools in that it allows the user to count
+/// time spent in various bits of code (sub-function granularity)
+/// or count how many times certain pieces of code are executed.
+class FCL_EXPORT Profiler
+{
+public:
+  // non-copyable
+  Profiler(const Profiler&) = delete;
+  Profiler& operator=(const Profiler&) = delete;
+
+  /// @brief This instance will call Profiler::begin() when constructed and
+  /// Profiler::end() when it goes out of scope.
+  class FCL_EXPORT ScopedBlock;
+
+  /// @brief This instance will call Profiler::start() when constructed and
+  /// Profiler::stop() when it goes out of scope.
+  /// If the profiler was already started, this block's constructor and
+  /// destructor take no action
+  class FCL_EXPORT ScopedStart;
+
+  /// @brief Return an instance of the class
+  static Profiler& Instance(void);
+
+  /// @brief Constructor. It is allowed to separately instantiate this
+  /// class (not only as a singleton)
+  Profiler(bool printOnDestroy = false, bool autoStart = false);
+
+  /// @brief Destructor
+  ~Profiler(void);
+
+  /// @brief Start counting time
+  static void Start(void);
+
+  /// @brief Stop counting time
+  static void Stop(void);
+
+  /// @brief Clear counted time and events
+  static void Clear(void);
+
+  /// @brief Start counting time
+  void start(void);
+
+  /// @brief Stop counting time
+  void stop(void);
+
+  /// @brief Clear counted time and events
+  void clear(void);
+
+  /// @brief Count a specific event for a number of times
+  static void Event(const std::string& name, const unsigned int times = 1);
+
+  /// @brief Count a specific event for a number of times
+  void event(const std::string &name, const unsigned int times = 1);
+
+  /// @brief Maintain the average of a specific value
+  static void Average(const std::string& name, const double value);
+
+  /// @brief Maintain the average of a specific value
+  void average(const std::string &name, const double value);
+
+  /// @brief Begin counting time for a specific chunk of code
+  static void Begin(const std::string &name);
+
+  /// @brief Stop counting time for a specific chunk of code
+  static void End(const std::string &name);
+
+  /// @brief Begin counting time for a specific chunk of code
+  void begin(const std::string &name);
+
+  /// @brief Stop counting time for a specific chunk of code
+  void end(const std::string &name);
+
+  /// @brief Print the status of the profiled code chunks and
+  /// events. Optionally, computation done by different threads
+  /// can be printed separately.
+  static void Status(std::ostream &out = std::cout, bool merge = true);
+
+  /// @brief Print the status of the profiled code chunks and
+  /// events. Optionally, computation done by different threads
+  /// can be printed separately.
+  void status(std::ostream &out = std::cout, bool merge = true);
+
+  /// @brief Check if the profiler is counting time or not
+  bool running(void) const;
+
+  /// @brief Check if the profiler is counting time or not
+  static bool Running(void);
+
+private:
+
+  /// @brief Information about time spent in a section of the code
+  struct TimeInfo
+  {
+    TimeInfo(void);
+
+    /// @brief Total time counted.
+    time::duration    total;
+
+    /// @brief The shortest counted time interval
+    time::duration    shortest;
+
+    /// @brief The longest counted time interval
+    time::duration    longest;
+
+    /// @brief Number of times a chunk of time was added to this structure
+    unsigned long int parts;
+
+    /// @brief The point in time when counting time started
+    time::point       start;
+
+    /// @brief Begin counting time
+    void set(void);
+
+    /// @brief Add the counted time to the total time
+    void update(void);
+  };
+
+  /// @brief Information maintained about averaged values
+  struct AvgInfo
+  {
+    /// @brief The sum of the values to average
+    double            total;
+
+    /// @brief The sub of squares of the values to average
+    double            totalSqr;
+
+    /// @brief Number of times a value was added to this structure
+    unsigned long int parts;
+  };
+
+  /// @brief Information to be maintained for each thread
+  struct PerThread
+  {
+    /// @brief The stored events
+    std::map<std::string, unsigned long int> events;
+
+    /// @brief The stored averages
+    std::map<std::string, AvgInfo>           avg;
+
+    /// @brief The amount of time spent in various places
+    std::map<std::string, TimeInfo>          time;
+  };
+
+  void printThreadInfo(std::ostream &out, const PerThread &data);
+
+  std::mutex                             lock_;
+  std::map<std::thread::id, PerThread>   data_;
+  TimeInfo                               tinfo_;
+  bool                                   running_;
+  bool                                   printOnDestroy_;
+
+};
+
+/// @brief This instance will call Profiler::begin() when constructed and
+/// Profiler::end() when it goes out of scope.
+class FCL_EXPORT Profiler::ScopedBlock
+{
+public:
+  /// @brief Start counting time for the block named \e name of the profiler
+  /// \e prof
+  ScopedBlock(const std::string &name, Profiler &prof = Profiler::Instance());
+
+  ~ScopedBlock(void);
+
+private:
+
+  std::string  name_;
+  Profiler    &prof_;
+};
+
+/// @brief This instance will call Profiler::start() when constructed and
+/// Profiler::stop() when it goes out of scope.
+/// If the profiler was already started, this block's constructor and
+/// destructor take no action
+class FCL_EXPORT Profiler::ScopedStart
+{
+public:
+
+  /// @brief Take as argument the profiler instance to operate on (\e prof)
+  ScopedStart(Profiler &prof = Profiler::Instance());
+
+  ~ScopedStart(void);
+
+private:
+
+  Profiler &prof_;
+  bool      wasRunning_;
+};
+
+} // namespace detail
+} // namespace fcl
+
+#endif // #ifndef FCL_COMMON_DETAIL_PROFILER_H

3rdparty/fcl/include/fcl/common/exception.h

@@ -0,0 +1,66 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_EXCEPTION_H
+#define FCL_EXCEPTION_H
+
+#include <stdexcept>
+#include <string>
+
+#include "fcl/export.h"
+
+namespace fcl
+{
+
+class FCL_EXPORT Exception : public std::runtime_error
+{
+public:
+
+  /** @brief This is just a wrapper on std::runtime_error */
+  explicit Exception(const std::string& what);
+
+  /** @brief This is just a wrapper on std::runtime_error with a
+      prefix added */
+  Exception(const std::string& prefix, const std::string& what);
+
+  virtual ~Exception(void) throw();
+
+};
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/common/profiler.h

@@ -0,0 +1,63 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008-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 Jeongseok Lee <jslee02@gmail.com> */
+
+#ifndef FCL_COMMON_PROFILER_H
+#define FCL_COMMON_PROFILER_H
+
+#include "fcl/config.h"
+
+#if FCL_ENABLE_PROFILING
+
+  #define FCL_PROFILE_START                ::fcl::detail::Profiler::Start();
+  #define FCL_PROFILE_STOP                 ::fcl::detail::Profiler::Stop();
+  #define FCL_PROFILE_BLOCK_BEGIN(name)    ::fcl::detail::Profiler::Begin(name);
+  #define FCL_PROFILE_BLOCK_END(name)      ::fcl::detail::Profiler::End(name);
+  #define FCL_PROFILE_STATUS(stream)       ::fcl::detail::Profiler::Status(stream);
+
+#else
+
+  #define FCL_PROFILE_START
+  #define FCL_PROFILE_STOP
+  #define FCL_PROFILE_BLOCK_BEGIN(name)
+  #define FCL_PROFILE_BLOCK_END(name)
+  #define FCL_PROFILE_STATUS(stream)
+
+#endif // #if FCL_ENABLE_PROFILING
+
+#include "fcl/common/detail/profiler.h"
+
+#endif // #ifndef FCL_COMMON_PROFILER_H

3rdparty/fcl/include/fcl/common/time.h

@@ -0,0 +1,72 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008-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 Ioan Sucan */
+
+#ifndef FCL_COMMON_TIME_H
+#define FCL_COMMON_TIME_H
+
+#include <chrono>
+#include "fcl/export.h"
+
+namespace fcl
+{
+
+/// @brief Namespace containing time datatypes and time operations
+namespace time
+{
+
+/// @brief Representation of a point in time
+using point = std::chrono::system_clock::time_point;
+
+/// @brief Representation of a time duration
+using duration = std::chrono::system_clock::duration;
+
+/// @brief Get the current time point
+FCL_EXPORT
+point now(void);
+
+/// @brief Return the time duration representing a given number of seconds
+FCL_EXPORT
+duration seconds(double sec);
+
+/// @brief Return the number of seconds that a time duration represents
+FCL_EXPORT
+double seconds(const duration &d);
+
+} // namespace time
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/common/types.h

@@ -0,0 +1,196 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_DATA_TYPES_H
+#define FCL_DATA_TYPES_H
+
+#include <cstddef>
+#include <cstdint>
+#include <vector>
+#include <map>
+#include <memory>
+#include <Eigen/Dense>
+#include <Eigen/StdVector>
+#include "fcl/export.h"
+
+namespace fcl
+{
+
+typedef FCL_DEPRECATED double FCL_REAL;
+typedef FCL_DEPRECATED std::int64_t  FCL_INT64;
+typedef FCL_DEPRECATED std::uint64_t FCL_UINT64;
+typedef FCL_DEPRECATED std::int32_t  FCL_INT32;
+typedef FCL_DEPRECATED std::uint32_t FCL_UINT32;
+
+using int64 = std::int64_t;
+using uint64 = std::uint64_t;
+using int32 = std::int32_t;
+using uint32 = std::uint32_t;
+
+template <typename S>
+using Vector2 = Eigen::Matrix<S, 2, 1>;
+
+template <typename S>
+using Vector3 = Eigen::Matrix<S, 3, 1>;
+
+template <typename S>
+using Vector6 = Eigen::Matrix<S, 6, 1>;
+
+template <typename S>
+using Vector7 = Eigen::Matrix<S, 7, 1>;
+
+template <typename S, int N>
+using VectorN = Eigen::Matrix<S, N, 1>;
+
+template <typename S>
+using VectorX = Eigen::Matrix<S, Eigen::Dynamic, 1>;
+
+template <typename S>
+using Matrix3 = Eigen::Matrix<S, 3, 3>;
+
+template <typename S>
+using Quaternion = Eigen::Quaternion<S>;
+
+template <typename S>
+using Transform3 = Eigen::Transform<S, 3, Eigen::Isometry>;
+
+template <typename S>
+using Translation3 = Eigen::Translation<S, 3>;
+
+template <typename S>
+using AngleAxis = Eigen::AngleAxis<S>;
+
+// float types
+using Vector3f = Vector3<float>;
+template <int N>
+using VectorNf = VectorN<float, N>;
+using VectorXf = VectorX<float>;
+using Matrix3f = Matrix3<float>;
+using Quaternionf = Quaternion<float>;
+using Transform3f = Transform3<float>;
+using Translation3f = Translation3<float>;
+using AngleAxisf = AngleAxis<float>;
+
+// double types
+using Vector3d = Vector3<double>;
+template <int N>
+using VectorNd = VectorN<double, N>;
+using VectorXd = VectorX<double>;
+using Matrix3d = Matrix3<double>;
+using Quaterniond = Quaternion<double>;
+using Transform3d = Transform3<double>;
+using Translation3d = Translation3<double>;
+using AngleAxisd = AngleAxis<double>;
+
+template <typename _Tp>
+using aligned_vector = std::vector<_Tp, Eigen::aligned_allocator<_Tp>>;
+
+template <typename _Key, typename _Tp, typename _Compare = std::less<_Key>>
+using aligned_map = std::map<_Key, _Tp, _Compare,
+    Eigen::aligned_allocator<std::pair<const _Key, _Tp>>>;
+
+#if EIGEN_VERSION_AT_LEAST(3,2,9)
+
+template <typename _Tp, typename... _Args>
+inline std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args)
+{
+  typedef typename std::remove_const<_Tp>::type _Tp_nc;
+  return std::allocate_shared<_Tp>(Eigen::aligned_allocator<_Tp_nc>(),
+                                   std::forward<_Args>(__args)...);
+}
+
+#else
+
+/// Aligned allocator that is compatible with c++11
+// Ref: https://bitbucket.org/eigen/eigen/commits/f5b7700
+// C++11 compatible version is available since Eigen 3.2.9 so we use this copy
+// for Eigen (< 3.2.9).
+template <class T>
+class FCL_EXPORT aligned_allocator_cpp11 : public std::allocator<T>
+{
+public:
+  typedef std::size_t     size_type;
+  typedef std::ptrdiff_t  difference_type;
+  typedef T*              pointer;
+  typedef const T*        const_pointer;
+  typedef T&              reference;
+  typedef const T&        const_reference;
+  typedef T               value_type;
+
+  template <class U>
+  struct rebind
+  {
+    typedef aligned_allocator_cpp11<U> other;
+  };
+
+  aligned_allocator_cpp11()
+    : std::allocator<T>() {}
+
+  aligned_allocator_cpp11(const aligned_allocator_cpp11& other)
+    : std::allocator<T>(other) {}
+
+  template <class U>
+  aligned_allocator_cpp11(const aligned_allocator_cpp11<U>& other)
+    : std::allocator<T>(other) {}
+
+  ~aligned_allocator_cpp11() {}
+
+  pointer allocate(size_type num, const void* /*hint*/ = 0)
+  {
+    Eigen::internal::check_size_for_overflow<T>(num);
+    return static_cast<pointer>( Eigen::internal::aligned_malloc(num * sizeof(T)) );
+  }
+
+  void deallocate(pointer p, size_type /*num*/)
+  {
+    Eigen::internal::aligned_free(p);
+  }
+};
+
+template <typename _Tp, typename... _Args>
+inline std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args)
+{
+  typedef typename std::remove_const<_Tp>::type _Tp_nc;
+  return std::allocate_shared<_Tp>(aligned_allocator_cpp11<_Tp_nc>(),
+                                   std::forward<_Args>(__args)...);
+}
+
+#endif
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/common/unused.h

@@ -0,0 +1,41 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, 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.
+ */
+
+#ifndef FCL_COMMON_UNUSED_H
+#define FCL_COMMON_UNUSED_H
+
+// Macro to suppress -Wunused-parameter and -Wunused-variable warnings.
+#define FCL_UNUSED(x) do { (void)(x); } while (0)
+
+#endif

3rdparty/fcl/include/fcl/common/warning.h

@@ -0,0 +1,117 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 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 CNRS-LAAS and AIST 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 Jeongseok Lee <jslee02@gmail.com>
+
+#ifndef FCL_COMMON_WARNING_H
+#define FCL_COMMON_WARNING_H
+
+#include "fcl/config.h"
+
+// We define two convenient macros that can be used to suppress
+// deprecated-warnings for a specific code block rather than a whole project.
+// This macros would be useful when you need to call deprecated function for
+// some reason (e.g., for backward compatibility) but don't want warnings.
+//
+// Example code:
+//
+// deprecated_function()  // warning
+//
+// FCL_SUPPRESS_DEPRECATED_BEGIN
+// deprecated_function()  // okay, no warning
+// FCL_SUPPRESS_DEPRECATED_END
+//
+#if defined (FCL_COMPILER_GCC)
+
+  #define FCL_SUPPRESS_DEPRECATED_BEGIN                             \
+    _Pragma("GCC diagnostic push")                                  \
+    _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"")
+
+  #define FCL_SUPPRESS_DEPRECATED_END \
+    _Pragma("GCC diagnostic pop")
+
+  #define FCL_SUPPRESS_UNINITIALIZED_BEGIN                           \
+    _Pragma("GCC diagnostic push")                                   \
+    _Pragma("GCC diagnostic ignored \"-Wuninitialized\"")
+
+  #define FCL_SUPPRESS_UNINITIALIZED_END \
+    _Pragma("GCC diagnostic pop")
+
+  #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN                           \
+    _Pragma("GCC diagnostic push")                                   \
+    _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"")
+
+  #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_END \
+    _Pragma("GCC diagnostic pop")
+
+#elif defined (FCL_COMPILER_CLANG)
+
+  #define FCL_SUPPRESS_DEPRECATED_BEGIN                               \
+    _Pragma("clang diagnostic push")                                  \
+    _Pragma("clang diagnostic ignored \"-Wdeprecated-declarations\"")
+
+  #define FCL_SUPPRESS_DEPRECATED_END \
+    _Pragma("clang diagnostic pop")
+
+  #define FCL_SUPPRESS_UNINITIALIZED_BEGIN                            \
+    _Pragma("clang diagnostic push")                                  \
+    _Pragma("clang diagnostic ignored \"-Wuninitialized\"")
+
+  #define FCL_SUPPRESS_UNINITIALIZED_END \
+    _Pragma("clang diagnostic pop")
+
+  #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN
+
+  #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_END
+
+#elif defined (FCL_COMPILER_MSVC)
+
+  #define FCL_SUPPRESS_DEPRECATED_BEGIN \
+    __pragma(warning(push))             \
+    __pragma(warning(disable:4996))
+
+  #define FCL_SUPPRESS_DEPRECATED_END   \
+    __pragma(warning(pop))
+
+  #define FCL_SUPPRESS_UNINITIALIZED_BEGIN  // TODO
+
+  #define FCL_SUPPRESS_UNINITIALIZED_END  // TODO
+
+  #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN  // TODO
+
+  #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_END  // TODO
+
+#endif
+
+#endif // FCL_COMMON_WARNING_H

3rdparty/fcl/include/fcl/config.h.in

@@ -0,0 +1,83 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012-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.
+ */
+
+#ifndef FCL_CONFIG_H
+#define FCL_CONFIG_H
+
+#define FCL_VERSION "@FCL_VERSION@"
+#define FCL_MAJOR_VERSION @FCL_MAJOR_VERSION@
+#define FCL_MINOR_VERSION @FCL_MINOR_VERSION@
+#define FCL_PATCH_VERSION @FCL_PATCH_VERSION@
+
+#cmakedefine01 FCL_HAVE_SSE
+#cmakedefine01 FCL_HAVE_OCTOMAP
+
+#cmakedefine01 FCL_ENABLE_PROFILING
+
+// Detect the operating systems
+#if defined(__APPLE__)
+  #define FCL_OS_MACOS
+#elif defined(__gnu_linux__)
+  #define FCL_OS_LINUX
+#elif defined(_WIN32)
+  #define FCL_OS_WINDOWS
+#endif
+
+// Detect the compiler
+#if defined(__clang__)
+  #define FCL_COMPILER_CLANG
+#elif defined(__GNUC__) || defined(__GNUG__)
+  #define FCL_COMPILER_GCC
+#elif defined(_MSC_VER)
+  #define FCL_COMPILER_MSVC
+#endif
+
+#if FCL_HAVE_OCTOMAP
+  #define OCTOMAP_MAJOR_VERSION @OCTOMAP_MAJOR_VERSION@
+  #define OCTOMAP_MINOR_VERSION @OCTOMAP_MINOR_VERSION@
+  #define OCTOMAP_PATCH_VERSION @OCTOMAP_PATCH_VERSION@
+
+  #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 // FCL_HAVE_OCTOMAP
+
+#endif

3rdparty/fcl/include/fcl/fcl.h.in

@@ -0,0 +1,44 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 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 Jeongseok Lee <jslee02@gmail.com> */
+
+// Automatically generated file by cmake
+
+#ifndef FCL_FCL_H
+#define FCL_FCL_H
+
+${fcl_headers}
+
+#endif

3rdparty/fcl/include/fcl/geometry/bvh/BVH_internal.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 FCL_BVH_INTERNAL_H
+#define FCL_BVH_INTERNAL_H
+
+#include "fcl/common/types.h"
+
+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
+  };
+
+}
+
+#endif

3rdparty/fcl/include/fcl/geometry/bvh/BVH_model.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 FCL_BVH_MODEL_H
+#define FCL_BVH_MODEL_H
+
+#include <vector>
+#include <memory>
+
+#include "fcl/math/bv/OBB.h"
+#include "fcl/math/bv/kDOP.h"
+#include "fcl/geometry/collision_geometry.h"
+#include "fcl/geometry/bvh/BVH_internal.h"
+#include "fcl/geometry/bvh/BV_node.h"
+#include "fcl/geometry/bvh/detail/BV_splitter.h"
+#include "fcl/geometry/bvh/detail/BV_fitter.h"
+
+namespace fcl
+{
+
+/// @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)
+template <typename BV>
+class FCL_EXPORT BVHModel : public CollisionGeometry<typename BV::S>
+{
+public:
+
+  using S = typename BV::S;
+
+  /// @brief Model type described by the instance
+  BVHModelType getModelType() const;
+
+  /// @brief Constructing an empty BVH
+  BVHModel();
+
+  /// @brief copy from another BVH
+  BVHModel(const BVHModel& other);
+
+  /// @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(int id) const;
+
+  /// @brief Access the bv giving the its index
+  BVNode<BV>& getBV(int id);
+
+  /// @brief Get the number of bv in the BVH
+  int getNumBVs() const;
+
+  /// @brief Get the object type: it is a BVH
+  OBJECT_TYPE getObjectType() const override;
+
+  /// @brief Get the BV type: default is unknown
+  NODE_TYPE getNodeType() const override;
+
+  /// @brief Compute the AABB for the BVH, used for broad-phase collision
+  void computeLocalAABB() override;
+
+  /// @brief Begin a new BVH model
+  int beginModel(int num_tris = 0, int num_vertices = 0);
+
+  /// @brief Add one point in the new BVH model
+  int addVertex(const Vector3<S>& p);
+
+  /// @brief Add one triangle in the new BVH model
+  int addTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3);
+
+  /// @brief Add a set of triangles in the new BVH model
+  int addSubModel(const std::vector<Vector3<S>>& ps, const std::vector<Triangle>& ts);
+
+  /// @brief Add a set of points in the new BVH model
+  int addSubModel(const std::vector<Vector3<S>>& 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 Vector3<S>& p);
+
+  /// @brief Replace one triangle in the old BVH model
+  int replaceTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3);
+
+  /// @brief Replace a set of points in the old BVH model
+  int replaceSubModel(const std::vector<Vector3<S>>& 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 Vector3<S>& p);
+
+  /// @brief Update one triangle in the old BVH model
+  int updateTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3);
+
+  /// @brief Update a set of points in the old BVH model
+  int updateSubModel(const std::vector<Vector3<S>>& ps);
+
+  /// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy
+  int endUpdateModel(bool refit = true, bool bottomup = true);
+
+  /// @brief Check the number of memory used
+  int memUsage(int 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();
+
+  Vector3<S> computeCOM() const override;
+
+  S computeVolume() const override;
+
+  Matrix3<S> computeMomentofInertia() const override;
+
+public:
+  /// @brief Geometry point data
+  Vector3<S>* vertices;
+
+  /// @brief Geometry triangle index data, will be nullptr for point clouds
+  Triangle* tri_indices;
+
+  /// @brief Geometry point data in previous frame
+  Vector3<S>* prev_vertices;
+
+  /// @brief Number of triangles
+  int num_tris;
+
+  /// @brief Number of points
+  int num_vertices;
+
+  /// @brief The state of BVH building process
+  BVHBuildState build_state;
+
+  /// @brief Split rule to split one BV node into two children
+  std::shared_ptr<detail::BVSplitterBase<BV>> bv_splitter;
+
+  /// @brief Fitting rule to fit a BV node to a set of geometry primitives
+  std::shared_ptr<detail::BVFitterBase<BV>> bv_fitter;
+
+private:
+
+  int num_tris_allocated;
+  int num_vertices_allocated;
+  int num_bvs_allocated;
+  int num_vertex_updated; /// for ccd vertex update
+  unsigned int* primitive_indices;
+
+  /// @brief Bounding volume hierarchy
+  BVNode<BV>* bvs;
+
+  /// @brief Number of BV nodes in bounding volume hierarchy
+  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, int first_primitive, 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,
+      const Matrix3<S>& parent_axis,
+      const Vector3<S>& parent_c);
+
+  template <typename, typename>
+  friend struct MakeParentRelativeRecurseImpl;
+};
+
+} // namespace fcl
+
+#include "fcl/geometry/bvh/BVH_model-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/geometry/bvh/BVH_utility-inl.h

@@ -0,0 +1,172 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BVH_UTILITY_INL_H
+#define FCL_BVH_UTILITY_INL_H
+
+#include "fcl/geometry/bvh/BVH_utility.h"
+
+#include "fcl/math/bv/utility.h"
+
+namespace fcl
+{
+
+//==============================================================================
+extern template
+void BVHExpand(
+    BVHModel<OBB<double>>& model, const Variance3<double>* ucs, double r);
+
+//==============================================================================
+extern template
+void BVHExpand(
+    BVHModel<RSS<double>>& model, const Variance3<double>* ucs, double r);
+
+//==============================================================================
+template <typename S, typename BV>
+FCL_EXPORT
+void BVHExpand(BVHModel<BV>& model, const Variance3<S>* ucs, S r)
+{
+  for(int i = 0; i < model.num_bvs; ++i)
+  {
+    BVNode<BV>& bvnode = model.getBV(i);
+
+    BV bv;
+    for(int j = 0; j < bvnode.num_primitives; ++j)
+    {
+      int v_id = bvnode.first_primitive + j;
+      const Variance3<S>& uc = ucs[v_id];
+
+      Vector3<S>& v = model.vertices[bvnode.first_primitive + j];
+
+      for(int k = 0; k < 3; ++k)
+      {
+        bv += (v + uc.axis.col(k) * (r * uc.sigma[k]));
+        bv += (v - uc.axis.col(k) * (r * uc.sigma[k]));
+      }
+    }
+
+    bvnode.bv = bv;
+  }
+}
+
+//==============================================================================
+template <typename S>
+FCL_EXPORT
+void BVHExpand(
+    BVHModel<OBB<S>>& model,
+    const Variance3<S>* ucs,
+    S r)
+{
+  for(int i = 0; i < model.getNumBVs(); ++i)
+  {
+    BVNode<OBB<S>>& bvnode = model.getBV(i);
+
+    Vector3<S>* vs = new Vector3<S>[bvnode.num_primitives * 6];
+    // TODO(JS): We could use one std::vector outside of the outter for-loop,
+    // and reuse it rather than creating and destructing the array every
+    // iteration.
+
+    for(int j = 0; j < bvnode.num_primitives; ++j)
+    {
+      int v_id = bvnode.first_primitive + j;
+      const Variance3<S>& uc = ucs[v_id];
+
+      Vector3<S>&v = model.vertices[bvnode.first_primitive + j];
+
+      for(int k = 0; k < 3; ++k)
+      {
+        const auto index1 = 6 * j + 2 * k;
+        const auto index2 = index1 + 1;
+        vs[index1] = v;
+        vs[index1].noalias() += uc.axis.col(k) * (r * uc.sigma[k]);
+        vs[index2] = v;
+        vs[index2].noalias() -= uc.axis.col(k) * (r * uc.sigma[k]);
+      }
+    }
+
+    OBB<S> bv;
+    fit(vs, bvnode.num_primitives * 6, bv);
+
+    delete [] vs;
+
+    bvnode.bv = bv;
+  }
+}
+
+//==============================================================================
+template <typename S>
+FCL_EXPORT
+void BVHExpand(
+    BVHModel<RSS<S>>& model,
+    const Variance3<S>* ucs,
+    S r)
+{
+  for(int i = 0; i < model.getNumBVs(); ++i)
+  {
+    BVNode<RSS<S>>& bvnode = model.getBV(i);
+
+    Vector3<S>* vs = new Vector3<S>[bvnode.num_primitives * 6];
+    // TODO(JS): We could use one std::vector outside of the outter for-loop,
+    // and reuse it rather than creating and destructing the array every
+    // iteration.
+
+    for(int j = 0; j < bvnode.num_primitives; ++j)
+    {
+      int v_id = bvnode.first_primitive + j;
+      const Variance3<S>& uc = ucs[v_id];
+
+      Vector3<S>&v = model.vertices[bvnode.first_primitive + j];
+
+      for(int k = 0; k < 3; ++k)
+      {
+        vs[6 * j + 2 * k] = v + uc.axis.col(k) * (r * uc.sigma[k]);
+        vs[6 * j + 2 * k + 1] = v - uc.axis.col(k) * (r * uc.sigma[k]);
+      }
+    }
+
+    RSS<S> bv;
+    fit(vs, bvnode.num_primitives * 6, bv);
+
+    delete [] vs;
+
+    bvnode.bv = bv;
+  }
+}
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/geometry/bvh/BVH_utility.h

@@ -0,0 +1,71 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BVH_UTILITY_H
+#define FCL_BVH_UTILITY_H
+
+#include "fcl/math/variance3.h"
+#include "fcl/geometry/bvh/BVH_model.h"
+
+namespace fcl
+{
+
+/// @brief Expand the BVH bounding boxes according to the variance matrix
+/// corresponding to the data stored within each BV node
+template <typename S, typename BV>
+FCL_EXPORT
+void BVHExpand(BVHModel<BV>& model, const Variance3<S>* ucs, S r);
+
+/// @brief Expand the BVH bounding boxes according to the corresponding variance
+/// information, for OBB
+template <typename S>
+FCL_EXPORT
+void BVHExpand(
+    BVHModel<OBB<S>>& model, const Variance3<S>* ucs, S r = 1.0);
+
+/// @brief Expand the BVH bounding boxes according to the corresponding variance
+/// information, for RSS
+template <typename S>
+FCL_EXPORT
+void BVHExpand(
+    BVHModel<RSS<S>>& model, const Variance3<S>* ucs, S r = 1.0);
+
+} // namespace fcl
+
+#include "fcl/geometry/bvh/BVH_utility-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/geometry/bvh/BV_node-inl.h

@@ -0,0 +1,117 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef FCL_BV_BVNODE_INL_H
+#define FCL_BV_BVNODE_INL_H
+
+#include "fcl/geometry/bvh/BV_node.h"
+
+namespace fcl
+{
+
+//==============================================================================
+template <typename BV>
+bool BVNode<BV>::overlap(const BVNode& other) const
+{
+  return bv.overlap(other.bv);
+}
+
+//==============================================================================
+template <typename BV>
+typename BVNode<BV>::S BVNode<BV>::distance(
+    const BVNode& other, Vector3<S>* P1, Vector3<S>* P2) const
+{
+  return bv.distance(other.bv, P1, P2);
+}
+
+//==============================================================================
+template <typename BV>
+Vector3<typename BVNode<BV>::S> BVNode<BV>::getCenter() const
+{
+  return bv.center();
+}
+
+//==============================================================================
+template <typename S, typename BV>
+struct GetOrientationImpl
+{
+  static Matrix3<S> run(const BVNode<BV>& /*node*/)
+  {
+    return Matrix3<S>::Identity();
+  }
+};
+
+//==============================================================================
+template <typename BV>
+Matrix3<typename BV::S> BVNode<BV>::getOrientation() const
+{
+  return GetOrientationImpl<typename BV::S, BV>::run(bv);
+}
+
+//==============================================================================
+template <typename S>
+struct GetOrientationImpl<S, OBB<S>>
+{
+  static Matrix3<S> run(const OBB<S>& bv)
+  {
+    return bv.axis;
+  }
+};
+
+//==============================================================================
+template <typename S>
+struct GetOrientationImpl<S, RSS<S>>
+{
+  static Matrix3<S> run(const RSS<S>& bv)
+  {
+    return bv.axis;
+  }
+};
+
+//==============================================================================
+template <typename S>
+struct GetOrientationImpl<S, OBBRSS<S>>
+{
+  static Matrix3<S> run(const OBBRSS<S>& bv)
+  {
+    return bv.obb.axis;
+  }
+};
+
+} // namespace fcl
+
+#endif

3rdparty/fcl/include/fcl/geometry/bvh/BV_node.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 FCL_BV_BVNODE_H
+#define FCL_BV_BVNODE_H
+
+#include <iostream>
+#include "fcl/math/bv/OBB.h"
+#include "fcl/math/bv/RSS.h"
+#include "fcl/math/bv/OBBRSS.h"
+#include "fcl/geometry/bvh/BV_node_base.h"
+
+namespace fcl
+{
+
+/// @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 FCL_EXPORT BVNode : public BVNodeBase
+{
+  using S = typename BV::S;
+
+  /// @brief bounding volume storing the geometry
+  BV bv;
+
+  /// @brief Check whether two BVNode collide
+  bool overlap(const BVNode& other) const;
+
+  /// @brief Compute the distance between two BVNode. P1 and P2, if not nullptr and
+  /// the underlying BV supports distance, return the nearest points.
+  S distance(
+      const BVNode& other,
+      Vector3<S>* P1 = nullptr,
+      Vector3<S>* P2 = nullptr) const;
+
+  /// @brief Access the center of the BV
+  Vector3<S> getCenter() const;
+
+  /// @brief Access the orientation of the BV
+  Matrix3<S> getOrientation() const;
+};
+
+} // namespace fcl
+
+#include "fcl/geometry/bvh/BV_node-inl.h"
+
+#endif

3rdparty/fcl/include/fcl/geometry/bvh/BV_node_base.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 FCL_BV_BVNODEBASE_H
+#define FCL_BV_BVNODEBASE_H
+
+#include <iostream>
+#include "fcl/math/bv/OBB.h"
+#include "fcl/math/bv/RSS.h"
+#include "fcl/math/bv/OBBRSS.h"
+
+namespace fcl
+{
+
+/// @brief BVNodeBase encodes the tree structure for BVH
+struct FCL_EXPORT 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.
+  int first_primitive;
+
+  /// @brief The number of primitives belonging to the current node 
+  int num_primitives;
+
+  /// @brief Whether current node is a leaf node (i.e. contains a primitive index
+  bool isLeaf() const;
+
+  /// @brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel
+  int primitiveId() const;
+
+  /// @brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel
+  int leftChild() const;
+
+  /// @brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel
+  int rightChild() const;
+};
+
+} // namespace fcl
+
+#endif