diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5a839e00cc2ac53361d993e06adaacb5e02543fc..74ca10f73433f5a54b0d426db84a8dfd40375fb0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -149,11 +149,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/traversal/traversal_node_bvh_shape.h
include/hpp/fcl/traversal/traversal_node_base.h
include/hpp/fcl/data_types.h
- include/hpp/fcl/articulated_model/model.h
- include/hpp/fcl/articulated_model/joint_config.h
- include/hpp/fcl/articulated_model/model_config.h
- include/hpp/fcl/articulated_model/joint.h
- include/hpp/fcl/articulated_model/link.h
include/hpp/fcl/BVH/BV_splitter.h
include/hpp/fcl/BVH/BVH_internal.h
include/hpp/fcl/BVH/BVH_model.h
diff --git a/include/hpp/fcl/articulated_model/joint.h b/include/hpp/fcl/articulated_model/joint.h
deleted file mode 100644
index 7c5b36ad1801683448980c898aec0b025e05c54f..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/articulated_model/joint.h
+++ /dev/null
@@ -1,167 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#ifndef FCL_ARTICULATED_MODEL_JOINT_H
-#define FCL_ARTICULATED_MODEL_JOINT_H
-
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/data_types.h>
-
-#include <string>
-#include <vector>
-#include <map>
-#include <limits>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-
-namespace fcl
-{
-
-class JointConfig;
-class Link;
-
-enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER};
-
-/// @brief Base Joint
-class Joint
-{
-public:
-
- Joint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
- const Transform3f& 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 Transform3f getLocalTransform() const = 0;
-
- virtual std::size_t getNumDofs() const = 0;
-
- boost::shared_ptr<JointConfig> getJointConfig() const;
- void setJointConfig(const boost::shared_ptr<JointConfig>& joint_cfg);
-
- boost::shared_ptr<Link> getParentLink() const;
- boost::shared_ptr<Link> getChildLink() const;
-
- void setParentLink(const boost::shared_ptr<Link>& link);
- void setChildLink(const boost::shared_ptr<Link>& link);
-
- JointType getJointType() const;
-
- const Transform3f& getTransformToParent() const;
- void setTransformToParent(const Transform3f& t);
-
-protected:
-
- /// links to parent and child are only for connection, so weak_ptr to avoid cyclic dependency
- boost::weak_ptr<Link> link_parent_, link_child_;
-
- JointType type_;
-
- std::string name_;
-
- boost::shared_ptr<JointConfig> joint_cfg_;
-
- Transform3f transform_to_parent_;
-};
-
-
-class PrismaticJoint : public Joint
-{
-public:
- PrismaticJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
- const Transform3f& transform_to_parent,
- const std::string& name,
- const Vec3f& axis);
-
- virtual ~PrismaticJoint() {}
-
- Transform3f getLocalTransform() const;
-
- std::size_t getNumDofs() const;
-
- const Vec3f& getAxis() const;
-
-protected:
- Vec3f axis_;
-};
-
-class RevoluteJoint : public Joint
-{
-public:
- RevoluteJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
- const Transform3f& transform_to_parent,
- const std::string& name,
- const Vec3f& axis);
-
- virtual ~RevoluteJoint() {}
-
- Transform3f getLocalTransform() const;
-
- std::size_t getNumDofs() const;
-
- const Vec3f& getAxis() const;
-
-protected:
- Vec3f axis_;
-};
-
-
-
-class BallEulerJoint : public Joint
-{
-public:
- BallEulerJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
- const Transform3f& transform_to_parent,
- const std::string& name);
-
- virtual ~BallEulerJoint() {}
-
- std::size_t getNumDofs() const;
-
- Transform3f getLocalTransform() const;
-};
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/articulated_model/joint_config.h b/include/hpp/fcl/articulated_model/joint_config.h
deleted file mode 100644
index bd5ddf18c73b497291f2ea5457a582de5bde8714..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/articulated_model/joint_config.h
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#ifndef FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
-#define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
-
-#include <hpp/fcl/data_types.h>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include <vector>
-
-namespace fcl
-{
-
-class Joint;
-
-class JointConfig
-{
-public:
- JointConfig();
-
- JointConfig(const JointConfig& joint_cfg);
-
- JointConfig(const boost::shared_ptr<Joint>& joint,
- FCL_REAL default_value = 0,
- FCL_REAL default_value_min = 0,
- FCL_REAL default_value_max = 0);
-
- std::size_t getDim() const;
-
- inline FCL_REAL operator [] (std::size_t i) const
- {
- return values_[i];
- }
-
- inline FCL_REAL& operator [] (std::size_t i)
- {
- return values_[i];
- }
-
- FCL_REAL getValue(std::size_t i) const;
-
- FCL_REAL& getValue(std::size_t i);
-
- FCL_REAL getLimitMin(std::size_t i) const;
-
- FCL_REAL& getLimitMin(std::size_t i);
-
- FCL_REAL getLimitMax(std::size_t i) const;
-
- FCL_REAL& getLimitMax(std::size_t i);
-
- boost::shared_ptr<Joint> getJoint() const;
-
-private:
- boost::weak_ptr<Joint> joint_;
-
- std::vector<FCL_REAL> values_;
- std::vector<FCL_REAL> limits_min_;
- std::vector<FCL_REAL> limits_max_;
-};
-
-}
-
-
-
-#endif
diff --git a/include/hpp/fcl/articulated_model/link.h b/include/hpp/fcl/articulated_model/link.h
deleted file mode 100644
index 41c7c48a80faa8ee6559f44bb34174bc9f210c98..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/articulated_model/link.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#ifndef FCL_ARTICULATED_MODEL_LINK_H
-#define FCL_ARTICULATED_MODEL_LINK_H
-
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/collision_object.h>
-
-#include <boost/shared_ptr.hpp>
-#include <vector>
-
-namespace fcl
-{
-
-class Joint;
-
-class Link
-{
-public:
- Link(const std::string& name);
-
- const std::string& getName() const;
-
- void setName(const std::string& name);
-
- void addChildJoint(const boost::shared_ptr<Joint>& joint);
-
- void setParentJoint(const boost::shared_ptr<Joint>& joint);
-
- void addObject(const boost::shared_ptr<CollisionObject>& object);
-
- std::size_t getNumChildJoints() const;
-
- std::size_t getNumObjects() const;
-
-protected:
- std::string name_;
-
- std::vector<boost::shared_ptr<CollisionObject> > objects_;
-
- std::vector<boost::shared_ptr<Joint> > children_joints_;
-
- boost::shared_ptr<Joint> parent_joint_;
-};
-
-}
-
-#endif
diff --git a/include/hpp/fcl/articulated_model/model.h b/include/hpp/fcl/articulated_model/model.h
deleted file mode 100644
index c235984955a90b020676ee0e5589ad31b99a65f0..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/articulated_model/model.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#ifndef FCL_ARTICULATED_MODEL_MODEL_H
-#define FCL_ARTICULATED_MODEL_MODEL_H
-
-#include <hpp/fcl/articulated_model/joint.h>
-#include <hpp/fcl/articulated_model/link.h>
-
-#include <hpp/fcl/data_types.h>
-#include <boost/shared_ptr.hpp>
-
-#include <map>
-#include <stdexcept>
-namespace fcl
-{
-
-class ModelParseError : public std::runtime_error
-{
-public:
- ModelParseError(const std::string& error_msg) : std::runtime_error(error_msg) {}
-};
-
-class Model
-{
-public:
- Model() {}
-
- virtual ~Model() {}
-
- const std::string& getName() const;
-
- void addLink(const boost::shared_ptr<Link>& link);
-
- void addJoint(const boost::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;
-
- boost::shared_ptr<Link> getRoot() const;
- boost::shared_ptr<Link> getLink(const std::string& name) const;
- boost::shared_ptr<Joint> getJoint(const std::string& name) const;
-
- std::vector<boost::shared_ptr<Link> > getLinks() const;
- std::vector<boost::shared_ptr<Joint> > getJoints() const;
-protected:
- boost::shared_ptr<Link> root_link_;
- std::map<std::string, boost::shared_ptr<Link> > links_;
- std::map<std::string, boost::shared_ptr<Joint> > joints_;
-
- std::string name_;
-
-};
-
-}
-
-#endif
-
diff --git a/include/hpp/fcl/articulated_model/model_config.h b/include/hpp/fcl/articulated_model/model_config.h
deleted file mode 100644
index a0699ca2751bb96f94fdb5a20d83c239c7d68351..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/articulated_model/model_config.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#ifndef FCL_ARTICULATED_MODEL_MODEL_CONFIG_H
-#define FCL_ARTICULATED_MODEL_MODEL_CONFIG_H
-
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/articulated_model/joint_config.h>
-#include <string>
-#include <map>
-#include <boost/weak_ptr.hpp>
-#include <boost/shared_ptr.hpp>
-
-namespace fcl
-{
-
-class ModelConfig
-{
-public:
- ModelConfig();
-
- ModelConfig(const ModelConfig& model_cfg);
-
- ModelConfig(std::map<std::string, boost::shared_ptr<Joint> > joints_map);
-
- JointConfig getJointConfigByJointName(const std::string& joint_name) const;
- JointConfig& getJointConfigByJointName(const std::string& joint_name);
-
- JointConfig getJointConfigByJoint(boost::shared_ptr<Joint> joint) const;
- JointConfig& getJointConfigByJoint(boost::shared_ptr<Joint> joint);
-
- std::map<std::string, JointConfig> getJointCfgsMap() const
- { return joint_cfgs_map_; }
-
-private:
- std::map<std::string, JointConfig> joint_cfgs_map_;
-};
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/math/sampling.h b/include/hpp/fcl/math/sampling.h
deleted file mode 100644
index 3af1fdd6c5d8a511832ee58476074fcd1e8285de..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/math/sampling.h
+++ /dev/null
@@ -1,473 +0,0 @@
-#ifndef FCL_MATH_SAMPLING_H
-#define FCL_MATH_SAMPLING_H
-
-#include <boost/random/mersenne_twister.hpp>
-#include <boost/random/uniform_real.hpp>
-#include <boost/random/normal_distribution.hpp>
-#include <boost/random/variate_generator.hpp>
-#include <boost/math/constants/constants.hpp>
-#include <boost/assign/list_of.hpp>
-#include <hpp/fcl/math/vec_nf.h>
-#include <hpp/fcl/math/transform.h>
-
-namespace fcl
-{
-
-/// @brief Random number generation. An instance of this class
-/// cannot be used by multiple threads at once (member functions
-/// are not const). However, the constructor is thread safe and
-/// different instances can be used safely in any number of
-/// threads. It is also guaranteed that all created instances will
-/// have a different random seed.
-
-class RNG
-{
-public:
-
- /// @brief Constructor. Always sets a different random seed
- RNG();
-
- /// @brief Generate a random real between 0 and 1
- double uniform01()
- {
- return uni_();
- }
-
- /// @brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound)
- double uniformReal(double lower_bound, double upper_bound)
- {
- assert(lower_bound <= upper_bound);
- return (upper_bound - lower_bound) * uni_() + lower_bound;
- }
-
- /// @brief Generate a random integer within given bounds: [\e lower_bound, \e upper_bound]
- int uniformInt(int lower_bound, int upper_bound)
- {
- int r = (int)floor(uniformReal((double)lower_bound, (double)(upper_bound) + 1.0));
- return (r > upper_bound) ? upper_bound : r;
- }
-
- /// @brief Generate a random boolean
- bool uniformBool()
- {
- return uni_() <= 0.5;
- }
-
- /// @brief Generate a random real using a normal distribution with mean 0 and variance 1
- double gaussian01()
- {
- return normal_();
- }
-
- /// @brief Generate a random real using a normal distribution with given mean and variance
- double gaussian(double mean, double stddev)
- {
- return normal_() * stddev + mean;
- }
-
- /// @brief Generate a random real using a half-normal distribution. The value is within specified bounds [\e r_min, \e r_max], but with a bias towards \e r_max. The function is implemended using a Gaussian distribution with mean at \e r_max - \e r_min. The distribution is 'folded' around \e r_max axis towards \e r_min. The variance of the distribution is (\e r_max - \e r_min) / \e focus. The higher the focus, the more probable it is that generated numbers are close to \e r_max.
- double halfNormalReal(double r_min, double r_max, double focus = 3.0);
-
- /// @brief Generate a random integer using a half-normal distribution. The value is within specified bounds ([\e r_min, \e r_max]), but with a bias towards \e r_max. The function is implemented on top of halfNormalReal()
- int halfNormalInt(int r_min, int r_max, double focus = 3.0);
-
- /// @brief Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w)
- void quaternion(double value[4]);
-
- /// @brief Uniform random sampling of Euler roll-pitch-yaw angles, each in the range [-pi, pi). The computed value has the order (roll, pitch, yaw) */
- void eulerRPY(double value[3]);
-
- /// @brief Uniform random sample on a disk with radius from r_min to r_max
- void disk(double r_min, double r_max, double& x, double& y);
-
- /// @brief Uniform random sample in a ball with radius from r_min to r_max
- void ball(double r_min, double r_max, double& x, double& y, double& z);
-
- /// @brief Set the seed for random number generation. Use this function to ensure the same sequence of random numbers is generated.
- static void setSeed(boost::uint32_t seed);
-
- /// @brief Get the seed used for random number generation. Passing the returned value to setSeed() at a subsequent execution of the code will ensure deterministic (repeatable) behaviour. Useful for debugging.
- static boost::uint32_t getSeed();
-
-private:
-
- boost::mt19937 generator_;
- boost::uniform_real<> uniDist_;
- boost::normal_distribution<> normalDist_;
- boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni_;
- boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > normal_;
-
-};
-
-class SamplerBase
-{
-public:
- mutable RNG rng;
-};
-
-template<std::size_t N>
-class SamplerR : public SamplerBase
-{
-public:
- SamplerR() {}
-
- SamplerR(const Vecnf<N>& lower_bound_,
- const Vecnf<N>& upper_bound_) : lower_bound(lower_bound_),
- upper_bound(upper_bound_)
- {
- }
-
- void setBound(const Vecnf<N>& lower_bound_,
- const Vecnf<N>& upper_bound_)
- {
- lower_bound = lower_bound_;
- upper_bound = upper_bound_;
- }
-
- void getBound(Vecnf<N>& lower_bound_,
- Vecnf<N>& upper_bound_) const
- {
- lower_bound_ = lower_bound;
- upper_bound_ = upper_bound;
- }
-
- Vecnf<N> sample() const
- {
- Vecnf<N> q;
-
- for(std::size_t i = 0; i < N; ++i)
- {
- q[i] = rng.uniformReal(lower_bound[i], upper_bound[i]);
- }
-
- return q;
- }
-
-private:
- Vecnf<N> lower_bound;
- Vecnf<N> upper_bound;
-};
-
-
-class SamplerSE2 : public SamplerBase
-{
-public:
- SamplerSE2() {}
-
- SamplerSE2(const Vecnf<2>& lower_bound_,
- const Vecnf<2>& upper_bound_) : lower_bound(lower_bound_),
- upper_bound(upper_bound_)
- {}
-
- SamplerSE2(FCL_REAL x_min, FCL_REAL x_max,
- FCL_REAL y_min, FCL_REAL y_max) : lower_bound(boost::assign::list_of<FCL_REAL>(x_min)(y_min).convert_to_container<std::vector<FCL_REAL> >()),
- upper_bound(boost::assign::list_of<FCL_REAL>(x_max)(y_max).convert_to_container<std::vector<FCL_REAL> >())
-
- {}
-
-
- void setBound(const Vecnf<2>& lower_bound_,
- const Vecnf<2>& upper_bound_)
- {
- lower_bound = lower_bound_;
- upper_bound = upper_bound_;
- }
-
- void getBound(Vecnf<2>& lower_bound_,
- Vecnf<2>& upper_bound_) const
- {
- lower_bound_ = lower_bound;
- upper_bound_ = upper_bound;
- }
-
-
- Vecnf<3> sample() const
- {
- Vecnf<3> q;
- q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]);
- q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]);
- q[2] = rng.uniformReal(-boost::math::constants::pi<FCL_REAL>(),
- boost::math::constants::pi<FCL_REAL>());
-
- return q;
- }
-
-protected:
- Vecnf<2> lower_bound;
- Vecnf<2> upper_bound;
-};
-
-
-class SamplerSE2_disk : public SamplerBase
-{
-public:
- SamplerSE2_disk() {}
-
- SamplerSE2_disk(FCL_REAL cx, FCL_REAL cy,
- FCL_REAL r1, FCL_REAL r2,
- FCL_REAL crefx, FCL_REAL crefy)
- {
- setBound(cx, cy, r1, r2, crefx, crefy);
- }
-
- void setBound(FCL_REAL cx, FCL_REAL cy,
- FCL_REAL r1, FCL_REAL r2,
- FCL_REAL crefx, FCL_REAL crefy)
- {
- c[0] = cx; c[1] = cy;
- cref[0] = crefx; cref[1] = crefy;
- r_min = r1;
- r_max = r2;
- }
-
- Vecnf<3> sample() const
- {
- Vecnf<3> q;
- FCL_REAL x, y;
- rng.disk(r_min, r_max, x, y);
- q[0] = x + c[0] - cref[0];
- q[1] = y + c[1] - cref[1];
- q[2] = rng.uniformReal(-boost::math::constants::pi<FCL_REAL>(),
- boost::math::constants::pi<FCL_REAL>());
-
- return q;
- }
-
-protected:
- FCL_REAL c[2];
- FCL_REAL cref[2];
- FCL_REAL r_min, r_max;
-};
-
-class SamplerSE3Euler : public SamplerBase
-{
-public:
- SamplerSE3Euler() {}
-
- SamplerSE3Euler(const Vecnf<3>& lower_bound_,
- const Vecnf<3>& upper_bound_) : lower_bound(lower_bound_),
- upper_bound(upper_bound_)
- {}
-
- SamplerSE3Euler(const Vec3f& lower_bound_,
- const Vec3f& upper_bound_) : lower_bound(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >()),
- upper_bound(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >())
- {}
-
- void setBound(const Vecnf<3>& lower_bound_,
- const Vecnf<3>& upper_bound_)
-
- {
- lower_bound = lower_bound_;
- upper_bound = upper_bound_;
- }
-
- void setBound(const Vec3f& lower_bound_,
- const Vec3f& upper_bound_)
- {
- lower_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
- upper_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
- }
-
- void getBound(Vecnf<3>& lower_bound_,
- Vecnf<3>& upper_bound_) const
- {
- lower_bound_ = lower_bound;
- upper_bound_ = upper_bound;
- }
-
- void getBound(Vec3f& lower_bound_,
- Vec3f& upper_bound_) const
- {
- lower_bound_ = Vec3f(lower_bound[0], lower_bound[1], lower_bound[2]);
- upper_bound_ = Vec3f(upper_bound[0], upper_bound[1], upper_bound[2]);
- }
-
- Vecnf<6> sample() const
- {
- Vecnf<6> q;
- q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]);
- q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]);
- q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]);
-
- FCL_REAL s[4];
- rng.quaternion(s);
-
- Quaternion3f quat(s[0], s[1], s[2], s[3]);
- FCL_REAL a, b, c;
- quat.toEuler(a, b, c);
-
- q[3] = a;
- q[4] = b;
- q[5] = c;
-
- return q;
- }
-
-protected:
- Vecnf<3> lower_bound;
- Vecnf<3> upper_bound;
-
-};
-
-class SamplerSE3Quat : public SamplerBase
-{
-public:
- SamplerSE3Quat() {}
-
- SamplerSE3Quat(const Vecnf<3>& lower_bound_,
- const Vecnf<3>& upper_bound_) : lower_bound(lower_bound_),
- upper_bound(upper_bound_)
- {}
-
- SamplerSE3Quat(const Vec3f& lower_bound_,
- const Vec3f& upper_bound_) : lower_bound(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >()),
- upper_bound(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >())
- {}
-
-
- void setBound(const Vecnf<3>& lower_bound_,
- const Vecnf<3>& upper_bound_)
-
- {
- lower_bound = lower_bound_;
- upper_bound = upper_bound_;
- }
-
- void setBound(const Vec3f& lower_bound_,
- const Vec3f& upper_bound_)
- {
- lower_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
- upper_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
- }
-
-
- void getBound(Vecnf<3>& lower_bound_,
- Vecnf<3>& upper_bound_) const
- {
- lower_bound_ = lower_bound;
- upper_bound_ = upper_bound;
- }
-
- void getBound(Vec3f& lower_bound_,
- Vec3f& upper_bound_) const
- {
- lower_bound_ = Vec3f(lower_bound[0], lower_bound[1], lower_bound[2]);
- upper_bound_ = Vec3f(upper_bound[0], upper_bound[1], upper_bound[2]);
- }
-
- Vecnf<7> sample() const
- {
- Vecnf<7> q;
- q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]);
- q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]);
- q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]);
-
- FCL_REAL s[4];
- rng.quaternion(s);
-
- q[3] = s[0];
- q[4] = s[1];
- q[5] = s[2];
- q[6] = s[3];
- return q;
- }
-
-protected:
- Vecnf<3> lower_bound;
- Vecnf<3> upper_bound;
-};
-
-class SamplerSE3Euler_ball : public SamplerBase
-{
-public:
- SamplerSE3Euler_ball() {}
-
- SamplerSE3Euler_ball(FCL_REAL r_) : r(r_)
- {
- }
-
- void setBound(const FCL_REAL& r_)
- {
- r = r_;
- }
-
- void getBound(FCL_REAL& r_) const
- {
- r_ = r;
- }
-
- Vecnf<6> sample() const
- {
- Vecnf<6> q;
- FCL_REAL x, y, z;
- rng.ball(0, r, x, y, z);
- q[0] = x;
- q[1] = y;
- q[2] = z;
-
- FCL_REAL s[4];
- rng.quaternion(s);
-
- Quaternion3f quat(s[0], s[1], s[2], s[3]);
- FCL_REAL a, b, c;
- quat.toEuler(a, b, c);
- q[3] = a;
- q[4] = b;
- q[5] = c;
-
- return q;
- }
-
-protected:
- FCL_REAL r;
-
-};
-
-
-class SamplerSE3Quat_ball : public SamplerBase
-{
-public:
- SamplerSE3Quat_ball() {}
-
- SamplerSE3Quat_ball(FCL_REAL r_) : r(r_)
- {}
-
- void setBound(const FCL_REAL& r_)
- {
- r = r_;
- }
-
- void getBound(FCL_REAL& r_) const
- {
- r_ = r;
- }
-
- Vecnf<7> sample() const
- {
- Vecnf<7> q;
- FCL_REAL x, y, z;
- rng.ball(0, r, x, y, z);
- q[0] = x;
- q[1] = y;
- q[2] = z;
-
- FCL_REAL s[4];
- rng.quaternion(s);
-
- q[3] = s[0];
- q[4] = s[1];
- q[5] = s[2];
- q[6] = s[3];
- return q;
- }
-
-protected:
- FCL_REAL r;
-};
-
-
-
-}
-
-#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 2cbf558ef7c3baf7c2ed423da4de82fa241e66d8..fd57890fb7e621f2b5f6ceb2762b2e2930d62e3d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -79,11 +79,6 @@ set(${LIBRARY_NAME}_SOURCES
traversal/traversal_node_base.cpp
profile.cpp
continuous_collision.cpp
- articulated_model/link.cpp
- articulated_model/model.cpp
- articulated_model/model_config.cpp
- articulated_model/joint.cpp
- articulated_model/joint_config.cpp
distance.cpp
BVH/BVH_utility.cpp
BVH/BV_fitter.cpp
diff --git a/src/articulated_model/joint.cpp b/src/articulated_model/joint.cpp
deleted file mode 100644
index 965bd12d4b505a05825e1bca2a705a6c39a8d6ee..0000000000000000000000000000000000000000
--- a/src/articulated_model/joint.cpp
+++ /dev/null
@@ -1,192 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#include <hpp/fcl/articulated_model/joint.h>
-#include <hpp/fcl/articulated_model/link.h>
-#include <hpp/fcl/articulated_model/joint_config.h>
-
-namespace fcl
-{
-
-Joint::Joint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
- const Transform3f& transform_to_parent,
- const std::string& name) :
- link_parent_(link_parent), link_child_(link_child),
- name_(name),
- transform_to_parent_(transform_to_parent)
-{}
-
-Joint::Joint(const std::string& name) :
- name_(name)
-{
-}
-
-const std::string& Joint::getName() const
-{
- return name_;
-}
-
-void Joint::setName(const std::string& name)
-{
- name_ = name;
-}
-
-boost::shared_ptr<JointConfig> Joint::getJointConfig() const
-{
- return joint_cfg_;
-}
-
-void Joint::setJointConfig(const boost::shared_ptr<JointConfig>& joint_cfg)
-{
- joint_cfg_ = joint_cfg;
-}
-
-boost::shared_ptr<Link> Joint::getParentLink() const
-{
- return link_parent_.lock();
-}
-
-boost::shared_ptr<Link> Joint::getChildLink() const
-{
- return link_child_.lock();
-}
-
-void Joint::setParentLink(const boost::shared_ptr<Link>& link)
-{
- link_parent_ = link;
-}
-
-void Joint::setChildLink(const boost::shared_ptr<Link>& link)
-{
- link_child_ = link;
-}
-
-JointType Joint::getJointType() const
-{
- return type_;
-}
-
-const Transform3f& Joint::getTransformToParent() const
-{
- return transform_to_parent_;
-}
-
-void Joint::setTransformToParent(const Transform3f& t)
-{
- transform_to_parent_ = t;
-}
-
-
-PrismaticJoint::PrismaticJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
- const Transform3f& transform_to_parent,
- const std::string& name,
- const Vec3f& axis) :
- Joint(link_parent, link_child, transform_to_parent, name),
- axis_(axis)
-{
- type_ = JT_PRISMATIC;
-}
-
-const Vec3f& PrismaticJoint::getAxis() const
-{
- return axis_;
-}
-
-std::size_t PrismaticJoint::getNumDofs() const
-{
- return 1;
-}
-
-Transform3f PrismaticJoint::getLocalTransform() const
-{
- const Quaternion3f& quat = transform_to_parent_.getQuatRotation();
- const Vec3f& transl = transform_to_parent_.getTranslation();
- return Transform3f(quat, quat.transform(axis_ * (*joint_cfg_)[0]) + transl);
-}
-
-
-RevoluteJoint::RevoluteJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
- const Transform3f& transform_to_parent,
- const std::string& name,
- const Vec3f& axis) :
- Joint(link_parent, link_child, transform_to_parent, name),
- axis_(axis)
-{
- type_ = JT_REVOLUTE;
-}
-
-const Vec3f& RevoluteJoint::getAxis() const
-{
- return axis_;
-}
-
-std::size_t RevoluteJoint::getNumDofs() const
-{
- return 1;
-}
-
-Transform3f RevoluteJoint::getLocalTransform() const
-{
- Quaternion3f quat;
- quat.fromAxisAngle(axis_, (*joint_cfg_)[0]);
- return Transform3f(transform_to_parent_.getQuatRotation() * quat, transform_to_parent_.getTranslation());
-}
-
-
-BallEulerJoint::BallEulerJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
- const Transform3f& transform_to_parent,
- const std::string& name) :
- Joint(link_parent, link_child, transform_to_parent, name)
-{}
-
-std::size_t BallEulerJoint::getNumDofs() const
-{
- return 3;
-}
-
-Transform3f BallEulerJoint::getLocalTransform() const
-{
- Matrix3f rot;
- rot.setEulerYPR((*joint_cfg_)[0], (*joint_cfg_)[1], (*joint_cfg_)[2]);
- return transform_to_parent_ * Transform3f(rot);
-}
-
-
-
-
-
-}
diff --git a/src/articulated_model/joint_config.cpp b/src/articulated_model/joint_config.cpp
deleted file mode 100644
index 7c8c3b4101278e276dc9e08d5c6aaa3a026b2552..0000000000000000000000000000000000000000
--- a/src/articulated_model/joint_config.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#include <hpp/fcl/articulated_model/joint_config.h>
-#include <hpp/fcl/articulated_model/joint.h>
-
-namespace fcl
-{
-
-JointConfig::JointConfig() {}
-
-JointConfig::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_)
-{
-}
-
-JointConfig::JointConfig(const boost::shared_ptr<Joint>& joint,
- FCL_REAL default_value,
- FCL_REAL default_value_min,
- FCL_REAL 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);
-}
-
-std::size_t JointConfig::getDim() const
-{
- return values_.size();
-}
-
-FCL_REAL JointConfig::getValue(std::size_t i) const
-{
- return values_[i];
-}
-
-FCL_REAL& JointConfig::getValue(std::size_t i)
-{
- return values_[i];
-}
-
-FCL_REAL JointConfig::getLimitMin(std::size_t i) const
-{
- return limits_min_[i];
-}
-
-FCL_REAL& JointConfig::getLimitMin(std::size_t i)
-{
- return limits_min_[i];
-}
-
-FCL_REAL JointConfig::getLimitMax(std::size_t i) const
-{
- return limits_max_[i];
-}
-
-FCL_REAL& JointConfig::getLimitMax(std::size_t i)
-{
- return limits_max_[i];
-}
-
-
-
-boost::shared_ptr<Joint> JointConfig::getJoint() const
-{
- return joint_.lock();
-}
-
-}
diff --git a/src/articulated_model/link.cpp b/src/articulated_model/link.cpp
deleted file mode 100644
index faf85193d0256dd618aedde1d2ddecfc35fed4f3..0000000000000000000000000000000000000000
--- a/src/articulated_model/link.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#include <hpp/fcl/articulated_model/link.h>
-
-#include <hpp/fcl/articulated_model/joint.h>
-
-namespace fcl
-{
-
-Link::Link(const std::string& name) : name_(name)
-{}
-
-const std::string& Link::getName() const
-{
- return name_;
-}
-
-void Link::setName(const std::string& name)
-{
- name_ = name;
-}
-
-void Link::addChildJoint(const boost::shared_ptr<Joint>& joint)
-{
- children_joints_.push_back(joint);
-}
-
-void Link::setParentJoint(const boost::shared_ptr<Joint>& joint)
-{
- parent_joint_ = joint;
-}
-
-void Link::addObject(const boost::shared_ptr<CollisionObject>& object)
-{
- objects_.push_back(object);
-}
-
-std::size_t Link::getNumChildJoints() const
-{
- return children_joints_.size();
-}
-
-std::size_t Link::getNumObjects() const
-{
- return objects_.size();
-}
-
-
-}
diff --git a/src/articulated_model/model.cpp b/src/articulated_model/model.cpp
deleted file mode 100644
index 906e0252f6f20b1246358bc6ff1043f47c8bb61e..0000000000000000000000000000000000000000
--- a/src/articulated_model/model.cpp
+++ /dev/null
@@ -1,159 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#include <hpp/fcl/articulated_model/model.h>
-#include <hpp/fcl/articulated_model/model_config.h>
-#include <boost/assert.hpp>
-
-namespace fcl
-{
-
-
-boost::shared_ptr<Link> Model::getRoot() const
-{
- return root_link_;
-}
-
-boost::shared_ptr<Link> Model::getLink(const std::string& name) const
-{
- boost::shared_ptr<Link> ptr;
- std::map<std::string, boost::shared_ptr<Link> >::const_iterator it = links_.find(name);
- if(it == links_.end())
- ptr.reset();
- else
- ptr = it->second;
- return ptr;
-}
-
-boost::shared_ptr<Joint> Model::getJoint(const std::string& name) const
-{
- boost::shared_ptr<Joint> ptr;
- std::map<std::string, boost::shared_ptr<Joint> >::const_iterator it = joints_.find(name);
- if(it == joints_.end())
- ptr.reset();
- else
- ptr = it->second;
- return ptr;
-}
-
-const std::string& Model::getName() const
-{
- return name_;
-}
-
-std::vector<boost::shared_ptr<Link> > Model::getLinks() const
-{
- std::vector<boost::shared_ptr<Link> > links;
- for(std::map<std::string, boost::shared_ptr<Link> >::const_iterator it = links_.begin(); it != links_.end(); ++it)
- {
- links.push_back(it->second);
- }
-
- return links;
-}
-
-std::size_t Model::getNumLinks() const
-{
- return links_.size();
-}
-
-std::size_t Model::getNumJoints() const
-{
- return joints_.size();
-}
-
-std::size_t Model::getNumDofs() const
-{
- std::size_t dof = 0;
- for(std::map<std::string, boost::shared_ptr<Joint> >::const_iterator it = joints_.begin(); it != joints_.end(); ++it)
- {
- dof += it->second->getNumDofs();
- }
-
- return dof;
-}
-
-void Model::addLink(const boost::shared_ptr<Link>& link)
-{
- links_[link->getName()] = link;
-}
-
-void Model::addJoint(const boost::shared_ptr<Joint>& joint)
-{
- joints_[joint->getName()] = joint;
-}
-
-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, boost::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.");
-}
-
-void Model::initTree(std::map<std::string, std::string>& link_parent_tree)
-{
- for(std::map<std::string, boost::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;
- }
-}
-
-}
diff --git a/src/articulated_model/model_config.cpp b/src/articulated_model/model_config.cpp
deleted file mode 100644
index 8f72deb8123051eacb3bf51bc4bf5b4623f241b5..0000000000000000000000000000000000000000
--- a/src/articulated_model/model_config.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Dalibor Matura, Jia Pan */
-
-#include <hpp/fcl/articulated_model/model_config.h>
-#include <hpp/fcl/articulated_model/joint.h>
-#include <algorithm>
-
-// Define for boost version < 1.47
-#ifndef BOOST_ASSERT_MSG
-#define BOOST_ASSERT_MSG(expr, msg) ((void)0)
-#endif
-
-namespace fcl
-{
-
-ModelConfig::ModelConfig() {}
-
-ModelConfig::ModelConfig(const ModelConfig& model_cfg) :
- joint_cfgs_map_(model_cfg.joint_cfgs_map_)
-{}
-
-ModelConfig::ModelConfig(std::map<std::string, boost::shared_ptr<Joint> > joints_map)
-{
- std::map<std::string, boost::shared_ptr<Joint> >::iterator it;
- for(it = joints_map.begin(); it != joints_map.end(); ++it)
- joint_cfgs_map_[it->first] = JointConfig(it->second);
-}
-
-JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name) const
-{
- std::map<std::string, JointConfig>::const_iterator it = joint_cfgs_map_.find(joint_name);
- BOOST_ASSERT_MSG((it != joint_cfgs_map_.end()), "Joint name not valid");
-
- return it->second;
-}
-
-JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_name)
-{
- std::map<std::string, JointConfig>::iterator it = joint_cfgs_map_.find(joint_name);
- BOOST_ASSERT_MSG((it != joint_cfgs_map_.end()), "Joint name not valid");
-
- return it->second;
-}
-
-JointConfig ModelConfig::getJointConfigByJoint(boost::shared_ptr<Joint> joint) const
-{
- return getJointConfigByJointName(joint->getName());
-}
-
-JointConfig& ModelConfig::getJointConfigByJoint(boost::shared_ptr<Joint> joint)
-{
- return getJointConfigByJointName(joint->getName());
-}
-
-
-}
diff --git a/src/math/sampling.cpp b/src/math/sampling.cpp
deleted file mode 100644
index f8498b59bb4a708a7e331782f464749b8457b355..0000000000000000000000000000000000000000
--- a/src/math/sampling.cpp
+++ /dev/null
@@ -1,147 +0,0 @@
-#include <hpp/fcl/math/sampling.h>
-#include <boost/random/lagged_fibonacci.hpp>
-#include <boost/random/uniform_int.hpp>
-#include <boost/thread/mutex.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <boost/math/constants/constants.hpp>
-
-namespace fcl
-{
-
-/// The seed the user asked for (cannot be 0)
-static boost::uint32_t userSetSeed = 0;
-
-/// Flag indicating whether the first seed has already been generated or not
-static bool firstSeedGenerated = false;
-
-/// The value of the first seed
-static boost::uint32_t firstSeedValue = 0;
-
-/// Compute the first seed to be used; this function should be called only once
-static boost::uint32_t firstSeed()
-{
- static boost::mutex fsLock;
- boost::mutex::scoped_lock slock(fsLock);
-
- if(firstSeedGenerated)
- return firstSeedValue;
-
- if(userSetSeed != 0)
- firstSeedValue = userSetSeed;
- else firstSeedValue = (boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() - boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds();
- firstSeedGenerated = true;
-
- return firstSeedValue;
-}
-
-/// We use a different random number generator for the seeds of the
-/// Other random generators. The root seed is from the number of
-/// nano-seconds in the current time.
-static boost::uint32_t nextSeed()
-{
- static boost::mutex rngMutex;
- boost::mutex::scoped_lock slock(rngMutex);
- static boost::lagged_fibonacci607 sGen(firstSeed());
- static boost::uniform_int<> sDist(1, 1000000000);
- static boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s(sGen, sDist);
- return s();
-}
-
-boost::uint32_t RNG::getSeed()
-{
- return firstSeed();
-}
-
-void RNG::setSeed(boost::uint32_t seed)
-{
- if(firstSeedGenerated)
- {
- std::cerr << "Random number generation already started. Changing seed now will not lead to deterministic sampling." << std::endl;
- }
- if(seed == 0)
- {
- std::cerr << "Random generator seed cannot be 0. Using 1 instead." << std::endl;
- userSetSeed = 1;
- }
- else
- userSetSeed = seed;
-}
-
-RNG::RNG() : generator_(nextSeed()),
- uniDist_(0, 1),
- normalDist_(0, 1),
- uni_(generator_, uniDist_),
- normal_(generator_, normalDist_)
-{
-}
-
-double RNG::halfNormalReal(double r_min, double r_max, double focus)
-{
- assert(r_min <= r_max);
-
- const double mean = r_max - r_min;
- double v = gaussian(mean, mean / focus);
-
- if(v > mean) v = 2.0 * mean - v;
- double r = v >= 0.0 ? v + r_min : r_min;
- return r > r_max ? r_max : r;
-}
-
-int RNG::halfNormalInt(int r_min, int r_max, double focus)
-{
- int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
- return (r > r_max) ? r_max : r;
-}
-
-// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
-// pg. 124-132
-void RNG::quaternion(double value[4])
-{
- double x0 = uni_();
- double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
- double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
- double c1 = cos(t1), s1 = sin(t1);
- double c2 = cos(t2), s2 = sin(t2);
- value[0] = s1 * r1;
- value[1] = c1 * r1;
- value[2] = s2 * r2;
- value[3] = c2 * r2;
-}
-
-// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
-void RNG::eulerRPY(double value[3])
-{
- value[0] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
- value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
- value[2] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
-}
-
-void RNG::disk(double r_min, double r_max, double& x, double& y)
-{
- double a = uniform01();
- double b = uniform01();
- double r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min);
- double theta = 2 * boost::math::constants::pi<double>() * b;
- x = r * std::cos(theta);
- y = r * std::sin(theta);
-}
-
-void RNG::ball(double r_min, double r_max, double& x, double& y, double& z)
-{
- double a = uniform01();
- double b = uniform01();
- double c = uniform01();
- double r = std::pow(a * r_max * r_max * r_max + (1 - a) * r_min * r_min * r_min, 1 / 3.0);
- double theta = std::acos(1 - 2 * b);
- double phi = 2 * boost::math::constants::pi<double>() * c;
-
- double costheta = std::cos(theta);
- double sintheta = std::sin(theta);
- double cosphi = std::cos(phi);
- double sinphi = std::sin(phi);
- x = r * costheta;
- y = r * sintheta * cosphi;
- z = r * sintheta * sinphi;
-}
-
-}