diff --git a/CMakeLists.txt b/CMakeLists.txt
index c138482747767ecda8fee4e3bc48ae7296f952f4..1ca49e6b3326558765c2b1c2a3d01f3796329cdc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -90,20 +90,8 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/collision_data.h
include/hpp/fcl/profile.h
include/hpp/fcl/exception.h
- include/hpp/fcl/ccd/taylor_vector.h
- include/hpp/fcl/ccd/interval_vector.h
include/hpp/fcl/ccd/simplex.h
include/hpp/fcl/ccd/support.h
- include/hpp/fcl/ccd/interval_matrix.h
- include/hpp/fcl/ccd/interval.h
- include/hpp/fcl/ccd/interpolation/interpolation_factory.h
- include/hpp/fcl/ccd/interpolation/interpolation_linear.h
- include/hpp/fcl/ccd/interpolation/interpolation.h
- include/hpp/fcl/ccd/conservative_advancement.h
- include/hpp/fcl/ccd/taylor_model.h
- include/hpp/fcl/ccd/taylor_matrix.h
- include/hpp/fcl/ccd/motion_base.h
- include/hpp/fcl/ccd/motion.h
include/hpp/fcl/deprecated.h
include/hpp/fcl/BV/kIOS.h
include/hpp/fcl/BV/BV.h
@@ -145,7 +133,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/knn/nearest_neighbors_sqrtapprox.h
include/hpp/fcl/knn/nearest_neighbors_linear.h
include/hpp/fcl/knn/nearest_neighbors.h
- include/hpp/fcl/continuous_collision.h
include/hpp/fcl/math/vec_nf.h
include/hpp/fcl/math/matrix_3f.h
include/hpp/fcl/math/matrix_3fx.h
diff --git a/include/hpp/fcl/broadphase/broadphase.h b/include/hpp/fcl/broadphase/broadphase.h
index de22336e20077fa056f5f29fbaf159602b7a3213..e34c2f723d637891a72cd7e64e39da718522d462 100644
--- a/include/hpp/fcl/broadphase/broadphase.h
+++ b/include/hpp/fcl/broadphase/broadphase.h
@@ -147,89 +147,8 @@ protected:
};
-/// @brief Callback for continuous collision between two objects. Return value is whether can stop now.
-typedef bool (*ContinuousCollisionCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata);
-
-/// @brief Callback for continuous distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
-typedef bool (*ContinuousDistanceCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata, FCL_REAL& 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.
-class BroadPhaseContinuousCollisionManager
-{
-public:
- BroadPhaseContinuousCollisionManager()
- {
- }
-
- virtual ~BroadPhaseContinuousCollisionManager() {}
-
- /// @brief add objects to the manager
- virtual void registerObjects(const std::vector<ContinuousCollisionObject*>& other_objs)
- {
- for(size_t i = 0; i < other_objs.size(); ++i)
- registerObject(other_objs[i]);
- }
-
- /// @brief add one object to the manager
- virtual void registerObject(ContinuousCollisionObject* obj) = 0;
-
- /// @brief remove one object from the manager
- virtual void unregisterObject(ContinuousCollisionObject* obj) = 0;
-
- /// @brief initialize the manager, related with the specific type of manager
- virtual void setup() = 0;
-
- /// @brief update the condition of manager
- virtual void update() = 0;
-
- /// @brief update the manager by explicitly given the object updated
- virtual void update(ContinuousCollisionObject* /*updated_obj*/)
- {
- update();
- }
-
- /// @brief update the manager by explicitly given the set of objects update
- virtual void update(const std::vector<ContinuousCollisionObject*>& /*updated_objs*/)
- {
- update();
- }
-
- /// @brief clear the manager
- virtual void clear() = 0;
-
- /// @brief return the objects managed by the manager
- virtual void getObjects(std::vector<ContinuousCollisionObject*>& objs) const = 0;
-
- /// @brief perform collision test between one object and all the objects belonging to the manager
- virtual void collide(ContinuousCollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0;
-
- /// @brief perform distance computation between one object and all the objects belonging to the manager
- virtual void distance(ContinuousCollisionObject* obj, void* cdata, DistanceCallBack 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 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 callback) const = 0;
-
- /// @brief perform collision test with objects belonging to another manager
- virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0;
-
- /// @brief perform distance test with objects belonging to another manager
- virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, DistanceCallBack 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;
-};
-
-
}
-
#include <hpp/fcl/broadphase/broadphase_bruteforce.h>
#include <hpp/fcl/broadphase/broadphase_spatialhash.h>
#include <hpp/fcl/broadphase/broadphase_SaP.h>
diff --git a/include/hpp/fcl/ccd/conservative_advancement.h b/include/hpp/fcl/ccd/conservative_advancement.h
deleted file mode 100644
index 9c0a993faf0d98c18b40957f766135fa6939f671..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/conservative_advancement.h
+++ /dev/null
@@ -1,68 +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 Jia Pan */
-
-#ifndef FCL_CONSERVATIVE_ADVANCEMENT_H
-#define FCL_CONSERVATIVE_ADVANCEMENT_H
-
-
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/ccd/motion_base.h>
-
-
-namespace fcl
-{
-
-template<typename NarrowPhaseSolver>
-struct ConservativeAdvancementFunctionMatrix
-{
- typedef FCL_REAL (*ConservativeAdvancementFunc)(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result);
-
- ConservativeAdvancementFunc conservative_advancement_matrix[NODE_COUNT][NODE_COUNT];
-
- ConservativeAdvancementFunctionMatrix();
-};
-
-
-
-}
-
-#endif
-
-
-
-
diff --git a/include/hpp/fcl/ccd/interpolation/interpolation.h b/include/hpp/fcl/ccd/interpolation/interpolation.h
deleted file mode 100644
index 07d8be7149aaba41b658e4a67bd9dbc28ab6a476..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/interpolation/interpolation.h
+++ /dev/null
@@ -1,91 +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_CCD_INTERPOLATION_INTERPOLATION_H
-#define FCL_CCD_INTERPOLATION_INTERPOLATION_H
-
-#include <hpp/fcl/data_types.h>
-
-namespace fcl
-{
-
-enum InterpolationType
-{
- LINEAR,
- STANDARD
-};
-
-class Interpolation
-{
-public:
- Interpolation();
-
- virtual ~Interpolation() {}
-
- Interpolation(FCL_REAL start_value, FCL_REAL end_value);
-
- void setStartValue(FCL_REAL start_value);
- void setEndValue(FCL_REAL end_value);
-
- virtual FCL_REAL getValue(FCL_REAL time) const = 0;
-
- /// @brief return the smallest value in time interval [0, 1]
- virtual FCL_REAL getValueLowerBound() const = 0;
-
- /// @brief return the biggest value in time interval [0, 1]
- virtual FCL_REAL getValueUpperBound() const = 0;
-
- virtual InterpolationType getType() const = 0;
-
- bool operator == (const Interpolation& interpolation) const;
- bool operator != (const Interpolation& interpolation) const;
-
- virtual FCL_REAL getMovementLengthBound(FCL_REAL time) const = 0;
-
- virtual FCL_REAL getVelocityBound(FCL_REAL time) const = 0;
-
-protected:
- FCL_REAL value_0_; // value at time = 0.0
- FCL_REAL value_1_; // value at time = 1.0
-
-};
-
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/ccd/interpolation/interpolation_factory.h b/include/hpp/fcl/ccd/interpolation/interpolation_factory.h
deleted file mode 100644
index c8aa08850c75e8294833405042275d1cd42a3229..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/interpolation/interpolation_factory.h
+++ /dev/null
@@ -1,82 +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_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
-#define FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
-
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/ccd/interpolation/interpolation.h>
-
-#include <map>
-
-#include <boost/shared_ptr.hpp>
-#include <boost/function.hpp>
-
-namespace fcl
-{
-
-class InterpolationFactory
-{
-public:
- typedef boost::function<boost::shared_ptr<Interpolation>(FCL_REAL, FCL_REAL)> CreateFunction;
-
-public:
- void registerClass(const InterpolationType type, const CreateFunction create_function);
-
- boost::shared_ptr<Interpolation> create(const InterpolationType type, FCL_REAL start_value, FCL_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
diff --git a/include/hpp/fcl/ccd/interpolation/interpolation_linear.h b/include/hpp/fcl/ccd/interpolation/interpolation_linear.h
deleted file mode 100644
index cc50ee67da1211eb4b86fe78e7f849be2e2360fd..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/interpolation/interpolation_linear.h
+++ /dev/null
@@ -1,77 +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_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
-#define FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
-
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/ccd/interpolation/interpolation.h>
-
-#include <boost/shared_ptr.hpp>
-
-namespace fcl
-{
-
-class InterpolationFactory;
-
-class InterpolationLinear : public Interpolation
-{
-public:
- InterpolationLinear();
-
- InterpolationLinear(FCL_REAL start_value, FCL_REAL end_value);
-
- virtual FCL_REAL getValue(FCL_REAL time) const;
-
- virtual FCL_REAL getValueLowerBound() const;
- virtual FCL_REAL getValueUpperBound() const;
-
- virtual InterpolationType getType() const;
-
- virtual FCL_REAL getMovementLengthBound(FCL_REAL time) const;
-
- virtual FCL_REAL getVelocityBound(FCL_REAL time) const;
-
-public:
- static boost::shared_ptr<Interpolation> create(FCL_REAL start_value, FCL_REAL end_value);
-
- static void registerToFactory();
-};
-
-}
-
-#endif
diff --git a/include/hpp/fcl/ccd/interval.h b/include/hpp/fcl/ccd/interval.h
deleted file mode 100644
index 6ea96df7b764385b1ea57877e11d1e5d45d8c66a..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/interval.h
+++ /dev/null
@@ -1,229 +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.
- */
-
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
-/** \author Jia Pan */
-
-
-#ifndef FCL_CCD_INTERVAL_H
-#define FCL_CCD_INTERVAL_H
-
-#include <hpp/fcl/data_types.h>
-
-namespace fcl
-{
-
-/// @brief Interval class for [a, b]
-struct Interval
-{
- FCL_REAL i_[2];
-
- Interval() { i_[0] = i_[1] = 0; }
-
- explicit Interval(FCL_REAL v)
- {
- i_[0] = i_[1] = v;
- }
-
- /// @brief construct interval [left, right]
- Interval(FCL_REAL left, FCL_REAL right)
- {
- i_[0] = left; i_[1] = right;
- }
-
- /// @brief construct interval [left, right]
- inline void setValue(FCL_REAL a, FCL_REAL b)
- {
- i_[0] = a; i_[1] = b;
- }
-
- /// @brief construct zero interval [x, x]
- inline void setValue(FCL_REAL x)
- {
- i_[0] = i_[1] = x;
- }
-
- /// @brief access the interval endpoints: 0 for left, 1 for right end
- inline FCL_REAL operator [] (size_t i) const
- {
- return i_[i];
- }
-
- /// @brief access the interval endpoints: 0 for left, 1 for right end
- inline FCL_REAL& operator [] (size_t i)
- {
- return i_[i];
- }
-
- /// @brief whether two intervals are the same
- inline bool operator == (const Interval& other) const
- {
- if(i_[0] != other.i_[0]) return false;
- if(i_[1] != other.i_[1]) return false;
- return true;
- }
-
- /// @brief add two intervals
- inline Interval operator + (const Interval& other) const
- {
- return Interval(i_[0] + other.i_[0], i_[1] + other.i_[1]);
- }
-
- /// @brief minus another interval
- inline Interval operator - (const Interval& other) const
- {
- return Interval(i_[0] - other.i_[1], i_[1] - other.i_[0]);
- }
-
- inline Interval& operator += (const Interval& other)
- {
- i_[0] += other.i_[0];
- i_[1] += other.i_[1];
- return *this;
- }
-
- inline Interval& operator -= (const Interval& other)
- {
- i_[0] -= other.i_[1];
- i_[1] -= other.i_[0];
- return *this;
- }
-
- Interval operator * (const Interval& other) const;
-
- Interval& operator *= (const Interval& other);
-
- inline Interval operator * (FCL_REAL d) const
- {
- if(d >= 0) return Interval(i_[0] * d, i_[1] * d);
- return Interval(i_[1] * d, i_[0] * d);
- }
-
- inline Interval& operator *= (FCL_REAL d)
- {
- if(d >= 0)
- {
- i_[0] *= d;
- i_[1] *= d;
- }
- else
- {
- FCL_REAL tmp = i_[0];
- i_[0] = i_[1] * d;
- i_[1] = tmp * d;
- }
-
- return *this;
- }
-
- /// @brief other must not contain 0
- Interval operator / (const Interval& other) const;
-
- Interval& operator /= (const Interval& other);
-
- /// @brief determine whether the intersection between intervals is empty
- inline bool overlap(const Interval& other) const
- {
- if(i_[1] < other.i_[0]) return false;
- if(i_[0] > other.i_[1]) return false;
- return true;
- }
-
- inline bool intersect(const Interval& other)
- {
- if(i_[1] < other.i_[0]) return false;
- if(i_[0] > other.i_[1]) return false;
- if(i_[1] > other.i_[1]) i_[1] = other.i_[1];
- if(i_[0] < other.i_[0]) i_[0] = other.i_[0];
- return true;
- }
-
- inline Interval operator - () const
- {
- return Interval(-i_[1], -i_[0]);
- }
-
- /// @brief Return the nearest distance for points within the interval to zero
- inline FCL_REAL getAbsLower() const
- {
- if(i_[0] >= 0) return i_[0];
- if(i_[1] >= 0) return 0;
- return -i_[1];
- }
-
- /// @brief Return the farthest distance for points within the interval to zero
- inline FCL_REAL getAbsUpper() const
- {
- if(i_[0] + i_[1] >= 0) return i_[1];
- return i_[0];
- }
-
-
- inline bool contains(FCL_REAL v) const
- {
- if(v < i_[0]) return false;
- if(v > i_[1]) return false;
- return true;
- }
-
- /// @brief Compute the minimum interval contains v and original interval
- inline Interval& bound(FCL_REAL v)
- {
- if(v < i_[0]) i_[0] = v;
- if(v > i_[1]) i_[1] = v;
- return *this;
- }
-
-
- /// @brief Compute the minimum interval contains other and original interval
- inline Interval& bound(const Interval& other)
- {
- if(other.i_[0] < i_[0]) i_[0] = other.i_[0];
- if(other.i_[1] > i_[1]) i_[1] = other.i_[1];
- return *this;
- }
-
-
- void print() const;
- inline FCL_REAL center() const { return 0.5 * (i_[0] + i_[1]); }
- inline FCL_REAL diameter() const { return i_[1] -i_[0]; }
-};
-
-Interval bound(const Interval& i, FCL_REAL v);
-
-Interval bound(const Interval& i, const Interval& other);
-
-}
-#endif
diff --git a/include/hpp/fcl/ccd/interval_matrix.h b/include/hpp/fcl/ccd/interval_matrix.h
deleted file mode 100644
index 6ad14b0daf4683257979074ffec4cfda5f8c9327..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/interval_matrix.h
+++ /dev/null
@@ -1,112 +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.
- */
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
-/** \author Jia Pan */
-
-
-#ifndef FCL_CCD_INTERVAL_MATRIX_H
-#define FCL_CCD_INTERVAL_MATRIX_H
-
-#include <hpp/fcl/ccd/interval.h>
-#include <hpp/fcl/ccd/interval_vector.h>
-#include <hpp/fcl/math/matrix_3f.h>
-
-namespace fcl
-{
-
-struct IMatrix3
-{
- IVector3 v_[3];
-
- IMatrix3();
- IMatrix3(FCL_REAL v);
- IMatrix3(const Matrix3f& m);
- IMatrix3(FCL_REAL m[3][3][2]);
- IMatrix3(FCL_REAL m[3][3]);
- IMatrix3(Interval m[3][3]);
- IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3);
-
- void setIdentity();
-
- IVector3 getColumn(size_t i) const;
- const IVector3& getRow(size_t i) const;
-
- Vec3f getColumnLow(size_t i) const;
- Vec3f getRowLow(size_t i) const;
-
- Vec3f getColumnHigh(size_t i) const;
- Vec3f getRowHigh(size_t i) const;
-
- Matrix3f getLow() const;
- Matrix3f getHigh() const;
-
- inline const Interval& operator () (size_t i, size_t j) const
- {
- return v_[i][j];
- }
-
- inline Interval& operator () (size_t i, size_t j)
- {
- return v_[i][j];
- }
-
- IMatrix3 operator + (const IMatrix3& m) const;
- IMatrix3& operator += (const IMatrix3& m);
-
- IMatrix3 operator - (const IMatrix3& m) const;
- IMatrix3& operator -= (const IMatrix3& m);
-
- IVector3 operator * (const Vec3f& v) const;
- IVector3 operator * (const IVector3& v) const;
- IMatrix3 operator * (const IMatrix3& m) const;
- IMatrix3 operator * (const Matrix3f& m) const;
-
- IMatrix3& operator *= (const IMatrix3& m);
- IMatrix3& operator *= (const Matrix3f& m);
-
- IMatrix3& rotationConstrain();
-
- void print() const;
-
-#ifdef FCL_CCD_INTERVALMATRIX_PLUGIN
-# include FCL_CCD_INTERVALMATRIX_PLUGIN
-#endif
-};
-
-IMatrix3 rotationConstrain(const IMatrix3& m);
-
-}
-
-#endif
diff --git a/include/hpp/fcl/ccd/interval_vector.h b/include/hpp/fcl/ccd/interval_vector.h
deleted file mode 100644
index 7c1216b901887c7f1853cdde55f874fb42d27bde..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/interval_vector.h
+++ /dev/null
@@ -1,171 +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.
- */
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
-/** \author Jia Pan */
-
-
-#ifndef FCL_CCD_INTERVAL_VECTOR_H
-#define FCL_CCD_INTERVAL_VECTOR_H
-
-#include <hpp/fcl/ccd/interval.h>
-#include <hpp/fcl/math/vec_3f.h>
-
-namespace fcl
-{
-
-struct IVector3
-{
- Interval i_[3];
-
- IVector3();
- IVector3(FCL_REAL v);
- IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z);
- IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu);
- IVector3(Interval v[3]);
- IVector3(FCL_REAL v[3][2]);
- IVector3(const Interval& v1, const Interval& v2, const Interval& v3);
- IVector3(const Vec3f& v);
-
- inline void setValue(FCL_REAL v)
- {
- i_[0].setValue(v);
- i_[1].setValue(v);
- i_[2].setValue(v);
- }
-
- inline void setValue(FCL_REAL x, FCL_REAL y, FCL_REAL z)
- {
- i_[0].setValue(x);
- i_[1].setValue(y);
- i_[2].setValue(z);
- }
-
- inline void setValue(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu)
- {
- i_[0].setValue(xl, xu);
- i_[1].setValue(yl, yu);
- i_[2].setValue(zl, zu);
- }
-
- inline void setValue(FCL_REAL v[3][2])
- {
- i_[0].setValue(v[0][0], v[0][1]);
- i_[1].setValue(v[1][0], v[1][1]);
- i_[2].setValue(v[2][0], v[2][1]);
- }
-
- inline void setValue(Interval v[3])
- {
- i_[0] = v[0];
- i_[1] = v[1];
- i_[2] = v[2];
- }
-
- inline void setValue(const Interval& v1, const Interval& v2, const Interval& v3)
- {
- i_[0] = v1;
- i_[1] = v2;
- i_[2] = v3;
- }
-
- inline void setValue(const Vec3f& v)
- {
- i_[0].setValue(v[0]);
- i_[1].setValue(v[1]);
- i_[2].setValue(v[2]);
- }
-
- inline void setValue(FCL_REAL v[3])
- {
- i_[0].setValue(v[0]);
- i_[1].setValue(v[1]);
- i_[2].setValue(v[2]);
- }
-
- IVector3 operator + (const IVector3& other) const;
- IVector3& operator += (const IVector3& other);
-
- IVector3 operator - (const IVector3& other) const;
- IVector3& operator -= (const IVector3& other);
-
- Interval dot(const IVector3& other) const;
- IVector3 cross(const IVector3& other) const;
-
- Interval dot(const Vec3f& other) const;
- IVector3 cross(const Vec3f& other) const;
-
- inline const Interval& operator [] (size_t i) const
- {
- return i_[i];
- }
-
- inline Interval& operator [] (size_t i)
- {
- return i_[i];
- }
-
- inline Vec3f getLow() const
- {
- return Vec3f(i_[0][0], i_[1][0], i_[2][0]);
- }
-
- inline Vec3f getHigh() const
- {
- return Vec3f(i_[0][1], i_[1][1], i_[2][1]);
- }
-
- void print() const;
- Vec3f center() const;
- FCL_REAL volumn() const;
- void setZero();
-
- void bound(const Vec3f& v);
- void bound(const IVector3& v);
-
- bool overlap(const IVector3& v) const;
- bool contain(const IVector3& v) const;
-
-#ifdef FCL_CCD_INTERVALVECTOR_PLUGIN
-# include FCL_CCD_INTERVALVECTOR_PLUGIN
-#endif
-};
-
-IVector3 bound(const IVector3& i, const Vec3f& v);
-
-IVector3 bound(const IVector3& i, const IVector3& v);
-
-}
-
-#endif
diff --git a/include/hpp/fcl/ccd/motion.h b/include/hpp/fcl/ccd/motion.h
deleted file mode 100644
index 7f2af4718d7bb8145a58f1d24dd52c3363a5594a..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/motion.h
+++ /dev/null
@@ -1,564 +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 Jia Pan */
-
-
-#ifndef FCL_CCD_MOTION_H
-#define FCL_CCD_MOTION_H
-
-#include <hpp/fcl/ccd/motion_base.h>
-#include <hpp/fcl/intersect.h>
-#include <iostream>
-#include <vector>
-
-namespace fcl
-{
-
-class TranslationMotion : public MotionBase
-{
-public:
- /// @brief Construct motion from intial and goal transform
- TranslationMotion(const Transform3f& tf1,
- const Transform3f& tf2) : MotionBase(),
- rot(tf1.getQuatRotation()),
- trans_start(tf1.getTranslation()),
- trans_range(tf2.getTranslation() - tf1.getTranslation())
- {
- }
-
- TranslationMotion(const Matrix3f& R, const Vec3f& T1, const Vec3f& T2) : MotionBase()
- {
- rot.fromRotation(R);
- trans_start = T1;
- trans_range = T2 - T1;
- }
-
- bool integrate(FCL_REAL dt) const
- {
- if(dt > 1) dt = 1;
- tf = Transform3f(rot, trans_start + trans_range * dt);
- return true;
- }
-
- FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
- {
- return mb_visitor.visit(*this);
- }
-
- FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
- {
- return mb_visitor.visit(*this);
- }
-
- void getCurrentTransform(Transform3f& tf_) const
- {
- tf_ = tf;
- }
-
- void getTaylorModel(TMatrix3& /*tm*/, TVector3& /*tv*/) const
- {
- }
-
- Vec3f getVelocity() const
- {
- return trans_range;
- }
-
- private:
- /// @brief initial and goal transforms
- Quaternion3f rot;
- Vec3f trans_start, trans_range;
-
- mutable Transform3f tf;
-};
-
-class SplineMotion : public MotionBase
-{
-public:
- /// @brief Construct motion from 4 deBoor points
- SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3,
- const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
-
- // @brief Construct motion from initial and goal transform
- SplineMotion(const Matrix3f& /*R1*/, const Vec3f& /*T1*/,
- const Matrix3f& /*R2*/, const Vec3f& /*T2*/) : MotionBase()
- {
- // TODO
- }
-
- SplineMotion(const Transform3f& /*tf1*/,
- const Transform3f& /*tf2*/) : MotionBase()
- {
- // TODO
- }
-
-
- /// @brief Integrate the motion from 0 to dt
- /// We compute the current transformation from zero point instead of from last integrate time, for precision.
- bool integrate(double dt) const;
-
- /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
- FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
- {
- return mb_visitor.visit(*this);
- }
-
- /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
- FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
- {
- return mb_visitor.visit(*this);
- }
-
- /// @brief Get the rotation and translation in current step
- void getCurrentTransform(Transform3f& tf_) const
- {
- tf_ = tf;
- }
-
- void getTaylorModel(TMatrix3& tm, TVector3& tv) const
- {
- // set tv
- Vec3f c[4];
- c[0] = (Td[0] + Td[1] * 4 + Td[2] + Td[3]) * (1/6.0);
- c[1] = (-Td[0] + Td[2]) * (1/2.0);
- c[2] = (Td[0] - Td[1] * 2 + Td[2]) * (1/2.0);
- c[3] = (-Td[0] + Td[1] * 3 - Td[2] * 3 + Td[3]) * (1/6.0);
- tv.setTimeInterval(getTimeInterval());
- for(std::size_t i = 0; i < 3; ++i)
- {
- for(std::size_t j = 0; j < 4; ++j)
- {
- tv[i].coeff(j) = c[j][i];
- }
- }
-
- // set tm
- Matrix3f I(1, 0, 0, 0, 1, 0, 0, 0, 1);
- // R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5
- /// 1. compute M(1/2)
- Vec3f Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0);
- FCL_REAL Rt0_len = Rt0.length();
- FCL_REAL inv_Rt0_len = 1.0 / Rt0_len;
- FCL_REAL inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len;
- FCL_REAL inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len;
- FCL_REAL theta0 = Rt0_len;
- FCL_REAL costheta0 = cos(theta0);
- FCL_REAL sintheta0 = sin(theta0);
-
- Vec3f Wt0 = Rt0 * inv_Rt0_len;
- Matrix3f hatWt0;
- hat(hatWt0, Wt0);
- Matrix3f hatWt0_sqr = hatWt0 * hatWt0;
- Matrix3f Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0);
-
- /// 2. compute M'(1/2)
- Vec3f dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0);
- FCL_REAL Rt0_dot_dRt0 = Rt0.dot(dRt0);
- FCL_REAL dtheta0 = Rt0_dot_dRt0 * inv_Rt0_len;
- Vec3f dWt0 = dRt0 * inv_Rt0_len - Rt0 * (Rt0_dot_dRt0 * inv_Rt0_len_3);
- Matrix3f hatdWt0;
- hat(hatdWt0, dWt0);
- Matrix3f dMt0 = hatdWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0) + hatWt0_sqr * (sintheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (1 - costheta0);
-
- /// 3.1. compute M''(1/2)
- Vec3f ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5;
- FCL_REAL Rt0_dot_ddRt0 = Rt0.dot(ddRt0);
- FCL_REAL dRt0_dot_dRt0 = dRt0.sqrLength();
- FCL_REAL ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3;
- Vec3f ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5;
- Matrix3f hatddWt0;
- hat(hatddWt0, ddWt0);
- Matrix3f ddMt0 =
- hatddWt0 * sintheta0 +
- hatWt0 * (costheta0 * dtheta0 - sintheta0 * dtheta0 * dtheta0 + costheta0 * ddtheta0) +
- hatdWt0 * (costheta0 * dtheta0) +
- (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (sintheta0 * dtheta0 * 2) +
- hatdWt0 * hatdWt0 * (2 * (1 - costheta0)) +
- hatWt0 * hatWt0 * (costheta0 * dtheta0 * dtheta0 + sintheta0 * ddtheta0) +
- (hatWt0 * hatddWt0 + hatddWt0 + hatWt0) * (1 - costheta0);
-
-
- tm.setTimeInterval(getTimeInterval());
- for(std::size_t i = 0; i < 3; ++i)
- {
- for(std::size_t j = 0; j < 3; ++j)
- {
- tm(i, j).coeff(0) = Mt0(i, j) - dMt0(i, j) * 0.5 + ddMt0(i, j) * 0.25 * 0.5;
- tm(i, j).coeff(1) = dMt0(i, j) - ddMt0(i, j) * 0.5;
- tm(i, j).coeff(2) = ddMt0(i, j) * 0.5;
- tm(i, j).coeff(3) = 0;
-
- tm(i, j).remainder() = Interval(-1/48.0, 1/48.0); /// not correct, should fix
- }
- }
- }
-
-protected:
- void computeSplineParameter()
- {
- }
-
- FCL_REAL getWeight0(FCL_REAL t) const;
- FCL_REAL getWeight1(FCL_REAL t) const;
- FCL_REAL getWeight2(FCL_REAL t) const;
- FCL_REAL getWeight3(FCL_REAL t) const;
-
- Vec3f Td[4];
- Vec3f Rd[4];
-
- Vec3f TA, TB, TC;
- Vec3f RA, RB, RC;
-
- FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3;
- //// @brief The transformation at current time t
- mutable Transform3f tf;
-
- /// @brief The time related with tf
- mutable FCL_REAL tf_t;
-
-public:
- FCL_REAL computeTBound(const Vec3f& n) const;
-
- FCL_REAL computeDWMax() const;
-
- FCL_REAL getCurrentTime() const
- {
- return tf_t;
- }
-
-};
-
-class ScrewMotion : public MotionBase
-{
-public:
- /// @brief Default transformations are all identities
- ScrewMotion() : MotionBase()
- {
- // Default angular velocity is zero
- axis.setValue(1, 0, 0);
- angular_vel = 0;
-
- // Default reference point is local zero point
-
- // Default linear velocity is zero
- linear_vel = 0;
- }
-
- /// @brief Construct motion from the initial rotation/translation and goal rotation/translation
- ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2) : MotionBase(),
- tf1(R1, T1),
- tf2(R2, T2),
- tf(tf1)
- {
- computeScrewParameter();
- }
-
- /// @brief Construct motion from the initial transform and goal transform
- ScrewMotion(const Transform3f& tf1_,
- const Transform3f& tf2_) : tf1(tf1_),
- tf2(tf2_),
- tf(tf1)
- {
- computeScrewParameter();
- }
-
- /// @brief Integrate the motion from 0 to dt
- /// We compute the current transformation from zero point instead of from last integrate time, for precision.
- bool integrate(double dt) const
- {
- if(dt > 1) dt = 1;
-
- tf.setQuatRotation(absoluteRotation(dt));
-
- Quaternion3f delta_rot = deltaRotation(dt);
- tf.setTranslation(p + axis * (dt * linear_vel) + delta_rot.transform(tf1.getTranslation() - p));
-
- return true;
- }
-
- /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
- FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
- {
- return mb_visitor.visit(*this);
- }
-
- /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
- FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
- {
- return mb_visitor.visit(*this);
- }
-
-
- /// @brief Get the rotation and translation in current step
- void getCurrentTransform(Transform3f& tf_) const
- {
- tf_ = tf;
- }
-
- void getTaylorModel(TMatrix3& tm, TVector3& tv) const
- {
- Matrix3f hat_axis;
- hat(hat_axis, axis);
-
- TaylorModel cos_model(getTimeInterval());
- generateTaylorModelForCosFunc(cos_model, angular_vel, 0);
-
- TaylorModel sin_model(getTimeInterval());
- generateTaylorModelForSinFunc(sin_model, angular_vel, 0);
-
- TMatrix3 delta_R = hat_axis * sin_model - hat_axis * hat_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
-
- TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval());
- generateTaylorModelForLinearFunc(a, 0, linear_vel * axis[0]);
- generateTaylorModelForLinearFunc(b, 0, linear_vel * axis[1]);
- generateTaylorModelForLinearFunc(c, 0, linear_vel * axis[2]);
- TVector3 delta_T = p - delta_R * p + TVector3(a, b, c);
-
- tm = delta_R * tf1.getRotation();
- tv = delta_R * tf1.getTranslation() + delta_T;
- }
-
-protected:
- void computeScrewParameter()
- {
- Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
- deltaq.toAxisAngle(axis, angular_vel);
- if(angular_vel < 0)
- {
- angular_vel = -angular_vel;
- axis = -axis;
- }
-
- if(angular_vel < 1e-10)
- {
- angular_vel = 0;
- axis = tf2.getTranslation() - tf1.getTranslation();
- linear_vel = axis.length();
- p = tf1.getTranslation();
- }
- else
- {
- Vec3f o = tf2.getTranslation() - tf1.getTranslation();
- p = (tf1.getTranslation() + tf2.getTranslation() + axis.cross(o) * (1.0 / tan(angular_vel / 2.0))) * 0.5;
- linear_vel = o.dot(axis);
- }
- }
-
- Quaternion3f deltaRotation(FCL_REAL dt) const
- {
- Quaternion3f res;
- res.fromAxisAngle(axis, (FCL_REAL)(dt * angular_vel));
- return res;
- }
-
- Quaternion3f absoluteRotation(FCL_REAL dt) const
- {
- Quaternion3f delta_t = deltaRotation(dt);
- return delta_t * tf1.getQuatRotation();
- }
-
- /// @brief The transformation at time 0
- Transform3f tf1;
-
- /// @brief The transformation at time 1
- Transform3f tf2;
-
- /// @brief The transformation at current time t
- mutable Transform3f tf;
-
- /// @brief screw axis
- Vec3f axis;
-
- /// @brief A point on the axis S
- Vec3f p;
-
- /// @brief linear velocity along the axis
- FCL_REAL linear_vel;
-
- /// @brief angular velocity
- FCL_REAL angular_vel;
-
-public:
-
- inline FCL_REAL getLinearVelocity() const
- {
- return linear_vel;
- }
-
- inline FCL_REAL getAngularVelocity() const
- {
- return angular_vel;
- }
-
- inline const Vec3f& getAxis() const
- {
- return axis;
- }
-
- inline const Vec3f& getAxisOrigin() const
- {
- return p;
- }
-};
-
-
-
-/// @brief Linear interpolation motion
-/// Each Motion is assumed to have constant linear velocity and angular velocity
-/// The motion is R(t)(p - p_ref) + p_ref + T(t)
-/// Therefore, R(0) = R0, R(1) = R1
-/// T(0) = T0 + R0 p_ref - p_ref
-/// T(1) = T1 + R1 p_ref - p_ref
-class InterpMotion : public MotionBase
-{
-public:
- /// @brief Default transformations are all identities
- InterpMotion();
-
- /// @brief Construct motion from the initial rotation/translation and goal rotation/translation
- InterpMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2);
-
- InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_);
-
- /// @brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center
- InterpMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2,
- const Vec3f& O);
-
- InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O);
-
- /// @brief Integrate the motion from 0 to dt
- /// We compute the current transformation from zero point instead of from last integrate time, for precision.
- bool integrate(double dt) const;
-
- /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
- FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
- {
- return mb_visitor.visit(*this);
- }
-
- /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
- FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
- {
- return mb_visitor.visit(*this);
- }
-
- /// @brief Get the rotation and translation in current step
- void getCurrentTransform(Transform3f& tf_) const
- {
- tf_ = tf;
- }
-
- void getTaylorModel(TMatrix3& tm, TVector3& tv) const
- {
- Matrix3f hat_angular_axis;
- hat(hat_angular_axis, angular_axis);
-
- TaylorModel cos_model(getTimeInterval());
- generateTaylorModelForCosFunc(cos_model, angular_vel, 0);
- TaylorModel sin_model(getTimeInterval());
- generateTaylorModelForSinFunc(sin_model, angular_vel, 0);
-
- TMatrix3 delta_R = hat_angular_axis * sin_model - hat_angular_axis * hat_angular_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
-
- TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval());
- generateTaylorModelForLinearFunc(a, 0, linear_vel[0]);
- generateTaylorModelForLinearFunc(b, 0, linear_vel[1]);
- generateTaylorModelForLinearFunc(c, 0, linear_vel[2]);
- TVector3 delta_T(a, b, c);
-
- tm = delta_R * tf1.getRotation();
- tv = tf1.transform(reference_p) + delta_T - delta_R * tf1.getQuatRotation().transform(reference_p);
- }
-
-protected:
-
- void computeVelocity();
-
- Quaternion3f deltaRotation(FCL_REAL dt) const;
-
- Quaternion3f absoluteRotation(FCL_REAL dt) const;
-
- /// @brief The transformation at time 0
- Transform3f tf1;
-
- /// @brief The transformation at time 1
- Transform3f tf2;
-
- /// @brief The transformation at current time t
- mutable Transform3f tf;
-
- /// @brief Linear velocity
- Vec3f linear_vel;
-
- /// @brief Angular speed
- FCL_REAL angular_vel;
-
- /// @brief Angular velocity axis
- Vec3f angular_axis;
-
- /// @brief Reference point for the motion (in the object's local frame)
- Vec3f reference_p;
-
-public:
- const Vec3f& getReferencePoint() const
- {
- return reference_p;
- }
-
- const Vec3f& getAngularAxis() const
- {
- return angular_axis;
- }
-
- FCL_REAL getAngularVelocity() const
- {
- return angular_vel;
- }
-
- const Vec3f& getLinearVelocity() const
- {
- return linear_vel;
- }
-};
-
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/ccd/motion_base.h b/include/hpp/fcl/ccd/motion_base.h
deleted file mode 100644
index 8f2656e77f70e3aafc5f70daa8b5806f3776f9be..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/motion_base.h
+++ /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 Jia Pan */
-
-
-#ifndef FCL_CCD_MOTION_BASE_H
-#define FCL_CCD_MOTION_BASE_H
-
-
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/ccd/taylor_matrix.h>
-#include <hpp/fcl/ccd/taylor_vector.h>
-#include <hpp/fcl/BV/RSS.h>
-
-namespace fcl
-{
-
-class MotionBase;
-
-class SplineMotion;
-class ScrewMotion;
-class InterpMotion;
-class TranslationMotion;
-
-/// @brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
-class BVMotionBoundVisitor
-{
-public:
- virtual FCL_REAL visit(const MotionBase& motion) const = 0;
- virtual FCL_REAL visit(const SplineMotion& motion) const = 0;
- virtual FCL_REAL visit(const ScrewMotion& motion) const = 0;
- virtual FCL_REAL visit(const InterpMotion& motion) const = 0;
- virtual FCL_REAL visit(const TranslationMotion& motion) const = 0;
-};
-
-template<typename BV>
-class TBVMotionBoundVisitor : public BVMotionBoundVisitor
-{
-public:
- TBVMotionBoundVisitor(const BV& bv_, const Vec3f& n_) : bv(bv_), n(n_) {}
-
- virtual FCL_REAL visit(const MotionBase& /*motion*/) const { return 0; }
- virtual FCL_REAL visit(const SplineMotion& /*motion*/) const { return 0; }
- virtual FCL_REAL visit(const ScrewMotion& /*motion*/) const { return 0; }
- virtual FCL_REAL visit(const InterpMotion& /*motion*/) const { return 0; }
- virtual FCL_REAL visit(const TranslationMotion& /*motion*/) const { return 0; }
-
-protected:
- BV bv;
- Vec3f n;
-};
-
-template<>
-FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const SplineMotion& motion) const;
-
-template<>
-FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const;
-
-template<>
-FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const;
-
-template<>
-FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const TranslationMotion& motion) const;
-
-
-class TriangleMotionBoundVisitor
-{
-public:
- TriangleMotionBoundVisitor(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_, const Vec3f& n_) :
- a(a_), b(b_), c(c_), n(n_) {}
-
- virtual FCL_REAL visit(const MotionBase& /*motion*/) const { return 0; }
- virtual FCL_REAL visit(const SplineMotion& motion) const;
- virtual FCL_REAL visit(const ScrewMotion& motion) const;
- virtual FCL_REAL visit(const InterpMotion& motion) const;
- virtual FCL_REAL visit(const TranslationMotion& motion) const;
-
-protected:
- Vec3f a, b, c, n;
-};
-
-
-
-class MotionBase
-{
-public:
- MotionBase() : time_interval_(boost::shared_ptr<TimeInterval>(new TimeInterval(0, 1)))
- {
- }
-
- virtual ~MotionBase() {}
-
- /** \brief Integrate the motion from 0 to dt */
- virtual bool integrate(double dt) const = 0;
-
- /** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
- virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const = 0;
-
- /** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */
- virtual FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const = 0;
-
- /** \brief Get the rotation and translation in current step */
- void getCurrentTransform(Matrix3f& R, Vec3f& T) const
- {
- Transform3f tf;
- getCurrentTransform(tf);
- R = tf.getRotation();
- T = tf.getTranslation();
- }
-
- void getCurrentTransform(Quaternion3f& Q, Vec3f& T) const
- {
- Transform3f tf;
- getCurrentTransform(tf);
- Q = tf.getQuatRotation();
- T = tf.getTranslation();
- }
-
- void getCurrentRotation(Matrix3f& R) const
- {
- Transform3f tf;
- getCurrentTransform(tf);
- R = tf.getRotation();
- }
-
- void getCurrentRotation(Quaternion3f& Q) const
- {
- Transform3f tf;
- getCurrentTransform(tf);
- Q = tf.getQuatRotation();
- }
-
- void getCurrentTranslation(Vec3f& T) const
- {
- Transform3f tf;
- getCurrentTransform(tf);
- T = tf.getTranslation();
- }
-
- virtual void getCurrentTransform(Transform3f& tf) const = 0;
-
- virtual void getTaylorModel(TMatrix3& tm, TVector3& tv) const = 0;
-
- const boost::shared_ptr<TimeInterval>& getTimeInterval() const
- {
- return time_interval_;
- }
-protected:
-
- boost::shared_ptr<TimeInterval> time_interval_;
-
-};
-
-typedef boost::shared_ptr<MotionBase> MotionBasePtr;
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/ccd/taylor_matrix.h b/include/hpp/fcl/ccd/taylor_matrix.h
deleted file mode 100644
index 18819387fd9629bbcc3825392369a3a2292ed552..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/taylor_matrix.h
+++ /dev/null
@@ -1,121 +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.
- */
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
-/** \author Jia Pan */
-
-#ifndef FCL_CCD_TAYLOR_MATRIX_H
-#define FCL_CCD_TAYLOR_MATRIX_H
-
-
-#include <hpp/fcl/math/matrix_3f.h>
-#include <hpp/fcl/ccd/taylor_vector.h>
-#include <hpp/fcl/ccd/interval_matrix.h>
-
-namespace fcl
-{
-
-class TMatrix3
-{
- TVector3 v_[3];
-
-public:
- TMatrix3();
- TMatrix3(const boost::shared_ptr<TimeInterval>& time_interval);
- TMatrix3(TaylorModel m[3][3]);
- TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3);
- TMatrix3(const Matrix3f& m, const boost::shared_ptr<TimeInterval>& time_interval);
-
- TVector3 getColumn(size_t i) const;
- const TVector3& getRow(size_t i) const;
-
- const TaylorModel& operator () (size_t i, size_t j) const;
- TaylorModel& operator () (size_t i, size_t j);
-
- TVector3 operator * (const Vec3f& v) const;
- TVector3 operator * (const TVector3& v) const;
- TMatrix3 operator * (const Matrix3f& m) const;
- TMatrix3 operator * (const TMatrix3& m) const;
- TMatrix3 operator * (const TaylorModel& d) const;
- TMatrix3 operator * (FCL_REAL d) const;
-
- TMatrix3& operator *= (const Matrix3f& m);
- TMatrix3& operator *= (const TMatrix3& m);
- TMatrix3& operator *= (const TaylorModel& d);
- TMatrix3& operator *= (FCL_REAL d);
-
- TMatrix3 operator + (const TMatrix3& m) const;
- TMatrix3& operator += (const TMatrix3& m);
- TMatrix3 operator + (const Matrix3f& m) const;
- TMatrix3& operator += (const Matrix3f& m);
-
- TMatrix3 operator - (const TMatrix3& m) const;
- TMatrix3& operator -= (const TMatrix3& m);
- TMatrix3 operator - (const Matrix3f& m) const;
- TMatrix3& operator -= (const Matrix3f& m);
- TMatrix3 operator - () const;
-
- IMatrix3 getBound() const;
- IMatrix3 getBound(FCL_REAL l, FCL_REAL r) const;
- IMatrix3 getBound(FCL_REAL t) const;
-
- IMatrix3 getTightBound() const;
- IMatrix3 getTightBound(FCL_REAL l, FCL_REAL r) const;
-
-
- void print() const;
- void setIdentity();
- void setZero();
- FCL_REAL diameter() const;
-
- void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
- void setTimeInterval(FCL_REAL l, FCL_REAL r);
-
- const boost::shared_ptr<TimeInterval>& getTimeInterval() const;
-
- TMatrix3& rotationConstrain();
-};
-
-TMatrix3 rotationConstrain(const TMatrix3& m);
-
-TMatrix3 operator * (const Matrix3f& m, const TaylorModel& a);
-TMatrix3 operator * (const TaylorModel& a, const Matrix3f& m);
-TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m);
-TMatrix3 operator * (FCL_REAL d, const TMatrix3& m);
-TMatrix3 operator + (const Matrix3f& m1, const TMatrix3& m2);
-TMatrix3 operator - (const Matrix3f& m1, const TMatrix3& m2);
-
-}
-
-#endif
diff --git a/include/hpp/fcl/ccd/taylor_model.h b/include/hpp/fcl/ccd/taylor_model.h
deleted file mode 100644
index 418de73f2cc4b0c991bbf72b6c5b68251502ef54..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/taylor_model.h
+++ /dev/null
@@ -1,164 +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.
- */
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
-/** \author Jia Pan */
-
-#ifndef FCL_CCD_TAYLOR_MODEL_H
-#define FCL_CCD_TAYLOR_MODEL_H
-
-#include <hpp/fcl/ccd/interval.h>
-#include <boost/shared_ptr.hpp>
-
-namespace fcl
-{
-
-struct TimeInterval
-{
- /// @brief time interval and different powers
- Interval t_; // [t1, t2]
- Interval t2_; // [t1, t2]^2
- Interval t3_; // [t1, t2]^3
- Interval t4_; // [t1, t2]^4
- Interval t5_; // [t1, t2]^5
- Interval t6_; // [t1, t2]^6
-
- TimeInterval() {}
- TimeInterval(FCL_REAL l, FCL_REAL r)
- {
- setValue(l, r);
- }
-
- void setValue(FCL_REAL l, FCL_REAL r)
- {
- t_.setValue(l, r);
- t2_.setValue(l * t_[0], r * t_[1]);
- t3_.setValue(l * t2_[0], r * t2_[1]);
- t4_.setValue(l * t3_[0], r * t3_[1]);
- t5_.setValue(l * t4_[0], r * t4_[1]);
- t6_.setValue(l * t5_[0], r * t5_[1]);
- }
-};
-
-/// @brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function
-/// over a time interval, with an interval remainder.
-/// All the operations on two Taylor models assume their time intervals are the same.
-class TaylorModel
-{
- /// @brief time interval
- boost::shared_ptr<TimeInterval> time_interval_;
-
- /// @brief Coefficients of the cubic polynomial approximation
- FCL_REAL coeffs_[4];
-
- /// @brief interval remainder
- Interval r_;
-
-public:
-
- void setTimeInterval(FCL_REAL l, FCL_REAL r)
- {
- time_interval_->setValue(l, r);
- }
-
- void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
- {
- time_interval_ = time_interval;
- }
-
- const boost::shared_ptr<TimeInterval>& getTimeInterval() const
- {
- return time_interval_;
- }
-
- FCL_REAL coeff(std::size_t i) const { return coeffs_[i]; }
- FCL_REAL& coeff(std::size_t i) { return coeffs_[i]; }
- const Interval& remainder() const { return r_; }
- Interval& remainder() { return r_; }
-
-
- TaylorModel();
- TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval);
- TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval);
- TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval);
- TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval);
-
- TaylorModel operator + (const TaylorModel& other) const;
- TaylorModel& operator += (const TaylorModel& other);
-
- TaylorModel operator - (const TaylorModel& other) const;
- TaylorModel& operator -= (const TaylorModel& other);
-
- TaylorModel operator + (FCL_REAL d) const;
- TaylorModel& operator += (FCL_REAL d);
-
- TaylorModel operator - (FCL_REAL d) const;
- TaylorModel& operator -= (FCL_REAL d);
-
- TaylorModel operator * (const TaylorModel& other) const;
- TaylorModel operator * (FCL_REAL d) const;
- TaylorModel& operator *= (const TaylorModel& other);
- TaylorModel& operator *= (FCL_REAL d);
-
- TaylorModel operator - () const;
-
- void print() const;
-
- Interval getBound() const;
- Interval getBound(FCL_REAL l, FCL_REAL r) const;
-
- Interval getTightBound() const;
- Interval getTightBound(FCL_REAL l, FCL_REAL r) const;
-
- Interval getBound(FCL_REAL t) const;
-
- void setZero();
-};
-
-TaylorModel operator * (FCL_REAL d, const TaylorModel& a);
-TaylorModel operator + (FCL_REAL d, const TaylorModel& a);
-TaylorModel operator - (FCL_REAL d, const TaylorModel& a);
-
-/// @brief Generate Taylor model for cos(w t + q0)
-void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
-
-/// @brief Generate Taylor model for sin(w t + q0)
-void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
-
-/// @brief Generate Taylor model for p + v t
-void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v);
-
-}
-
-#endif
diff --git a/include/hpp/fcl/ccd/taylor_vector.h b/include/hpp/fcl/ccd/taylor_vector.h
deleted file mode 100644
index be570a38c55501fc975dd1565ac10516ac352333..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/ccd/taylor_vector.h
+++ /dev/null
@@ -1,118 +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.
- */
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
-/** \author Jia Pan */
-
-#ifndef FCL_CCD_TAYLOR_VECTOR_H
-#define FCL_CCD_TAYLOR_VECTOR_H
-
-#include <hpp/fcl/ccd/interval_vector.h>
-#include <hpp/fcl/ccd/taylor_model.h>
-
-#if FCL_HAVE_EIGEN
-#include <hpp/fcl/eigen/taylor_operator.h>
-#endif
-
-namespace fcl
-{
-
-class TVector3
-{
- TaylorModel i_[3];
-
-public:
-
- TVector3();
- TVector3(const boost::shared_ptr<TimeInterval>& time_interval);
- TVector3(TaylorModel v[3]);
- TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2);
- TVector3(const Vec3f& v, const boost::shared_ptr<TimeInterval>& time_interval);
-
- TVector3 operator + (const TVector3& other) const;
- TVector3& operator += (const TVector3& other);
-
- TVector3 operator + (const Vec3f& other) const;
- TVector3& operator += (const Vec3f& other);
-
- TVector3 operator - (const TVector3& other) const;
- TVector3& operator -= (const TVector3& other);
-
- TVector3 operator - (const Vec3f& other) const;
- TVector3& operator -= (const Vec3f& other);
-
- TVector3 operator - () const;
-
- TVector3 operator * (const TaylorModel& d) const;
- TVector3& operator *= (const TaylorModel& d);
- TVector3 operator * (FCL_REAL d) const;
- TVector3& operator *= (FCL_REAL d);
-
- const TaylorModel& operator [] (size_t i) const;
- TaylorModel& operator [] (size_t i);
-
- TaylorModel dot(const TVector3& other) const;
- TVector3 cross(const TVector3& other) const;
- TaylorModel dot(const Vec3f& other) const;
- TVector3 cross(const Vec3f& other) const;
-
- IVector3 getBound() const;
- IVector3 getBound(FCL_REAL l, FCL_REAL r) const;
- IVector3 getBound(FCL_REAL t) const;
-
- IVector3 getTightBound() const;
- IVector3 getTightBound(FCL_REAL l, FCL_REAL r) const;
-
- void print() const;
- FCL_REAL volumn() const;
- void setZero();
-
- TaylorModel squareLength() const;
-
- void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
- void setTimeInterval(FCL_REAL l, FCL_REAL r);
-
- const boost::shared_ptr<TimeInterval>& getTimeInterval() const;
-};
-
-void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity);
-
-
-TVector3 operator * (const Vec3f& v, const TaylorModel& a);
-TVector3 operator + (const Vec3f& v1, const TVector3& v2);
-TVector3 operator - (const Vec3f& v1, const TVector3& v2);
-
-}
-
-#endif
diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h
index 6f850d6911a1eed0112e2518aaedec37cc510bfc..f3edcb45af4dce89e99d84d5234627e8cc15b10b 100644
--- a/include/hpp/fcl/collision_data.h
+++ b/include/hpp/fcl/collision_data.h
@@ -470,58 +470,6 @@ public:
}
};
-
-enum CCDMotionType {CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE};
-enum CCDSolverType {CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER};
-
-
-struct ContinuousCollisionRequest
-{
- /// @brief maximum num of iterations
- std::size_t num_max_iterations;
-
- /// @brief error in first contact time
- FCL_REAL toc_err;
-
- /// @brief ccd motion type
- CCDMotionType ccd_motion_type;
-
- /// @brief gjk solver type
- GJKSolverType gjk_solver_type;
-
- /// @brief ccd solver type
- CCDSolverType ccd_solver_type;
-
- ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10,
- FCL_REAL toc_err_ = 0.0001,
- CCDMotionType ccd_motion_type_ = CCDM_TRANS,
- GJKSolverType gjk_solver_type_ = GST_LIBCCD,
- CCDSolverType ccd_solver_type_ = CCDC_NAIVE) : num_max_iterations(num_max_iterations_),
- toc_err(toc_err_),
- ccd_motion_type(ccd_motion_type_),
- gjk_solver_type(gjk_solver_type_),
- ccd_solver_type(ccd_solver_type_)
- {
- }
-
-};
-/// @brief continuous collision result
-struct ContinuousCollisionResult
-{
- /// @brief collision or not
- bool is_collide;
-
- /// @brief time of contact in [0, 1]
- FCL_REAL time_of_contact;
-
- Transform3f contact_tf1, contact_tf2;
-
- ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0)
- {
- }
-};
-
-
enum PenetrationDepthType {PDT_TRANSLATIONAL, PDT_GENERAL_EULER, PDT_GENERAL_QUAT, PDT_GENERAL_EULER_BALL, PDT_GENERAL_QUAT_BALL};
enum KNNSolverType {KNN_LINEAR, KNN_GNAT, KNN_SQRTAPPROX};
diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h
index fb140008088f4aae81f26cb2760311478dc4ce8e..28ebceecd26099293d8f934a70b1f6dd15dcfae7 100644
--- a/include/hpp/fcl/collision_object.h
+++ b/include/hpp/fcl/collision_object.h
@@ -42,7 +42,6 @@
#include <hpp/fcl/deprecated.h>
#include <hpp/fcl/BV/AABB.h>
#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/ccd/motion_base.h>
#include <boost/shared_ptr.hpp>
namespace fcl
@@ -360,126 +359,6 @@ protected:
void *user_data;
};
-
-/// @brief the object for continuous collision or distance computation, contains the geometry and the motion information
-class ContinuousCollisionObject
-{
-public:
- ContinuousCollisionObject(const boost::shared_ptr<CollisionGeometry>& cgeom_) :
- cgeom(cgeom_), cgeom_const(cgeom_)
- {
- }
-
- ContinuousCollisionObject(const boost::shared_ptr<CollisionGeometry>& cgeom_, const boost::shared_ptr<MotionBase>& motion_) :
- cgeom(cgeom_), cgeom_const(cgeom), motion(motion_)
- {
- }
-
- ~ContinuousCollisionObject() {}
-
- /// @brief get the type of the object
- OBJECT_TYPE getObjectType() const
- {
- return cgeom->getObjectType();
- }
-
- /// @brief get the node type
- NODE_TYPE getNodeType() const
- {
- return cgeom->getNodeType();
- }
-
- /// @brief get the AABB in the world space for the motion
- inline const AABB& getAABB() const
- {
- return aabb;
- }
-
- /// @brief compute the AABB in the world space for the motion
- inline void computeAABB()
- {
- IVector3 box;
- TMatrix3 R;
- TVector3 T;
- motion->getTaylorModel(R, T);
-
- Vec3f p = cgeom->aabb_local.min_;
- box = (R * p + T).getTightBound();
-
- p[2] = cgeom->aabb_local.max_[2];
- box = bound(box, (R * p + T).getTightBound());
-
- p[1] = cgeom->aabb_local.max_[1];
- p[2] = cgeom->aabb_local.min_[2];
- box = bound(box, (R * p + T).getTightBound());
-
- p[2] = cgeom->aabb_local.max_[2];
- box = bound(box, (R * p + T).getTightBound());
-
- p[0] = cgeom->aabb_local.max_[0];
- p[1] = cgeom->aabb_local.min_[1];
- p[2] = cgeom->aabb_local.min_[2];
- box = bound(box, (R * p + T).getTightBound());
-
- p[2] = cgeom->aabb_local.max_[2];
- box = bound(box, (R * p + T).getTightBound());
-
- p[1] = cgeom->aabb_local.max_[1];
- p[2] = cgeom->aabb_local.min_[2];
- box = bound(box, (R * p + T).getTightBound());
-
- p[2] = cgeom->aabb_local.max_[2];
- box = bound(box, (R * p + T).getTightBound());
-
- aabb.min_ = box.getLow();
- aabb.max_ = box.getHigh();
- }
-
- /// @brief get user data in object
- void* getUserData() const
- {
- return user_data;
- }
-
- /// @brief set user data in object
- void setUserData(void* data)
- {
- user_data = data;
- }
-
- /// @brief get motion from the object instance
- inline MotionBase* getMotion() const
- {
- return motion.get();
- }
-
- /// @brief get geometry from the object instance
- FCL_DEPRECATED
- inline const CollisionGeometry* getCollisionGeometry() const
- {
- return cgeom.get();
- }
-
- /// @brief get geometry from the object instance
- inline const boost::shared_ptr<const CollisionGeometry>& collisionGeometry() const
- {
- return cgeom_const;
- }
-
-protected:
-
- boost::shared_ptr<CollisionGeometry> cgeom;
- boost::shared_ptr<const CollisionGeometry> cgeom_const;
-
- boost::shared_ptr<MotionBase> motion;
-
- /// @brief AABB in the global coordinate for the motion
- mutable AABB aabb;
-
- /// @brief pointer to user defined data specific to this object
- void* user_data;
-};
-
}
#endif
diff --git a/include/hpp/fcl/continuous_collision.h b/include/hpp/fcl/continuous_collision.h
deleted file mode 100644
index 43dd73a58fe09368ab36e90bddde9bf1b9cc4fba..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/continuous_collision.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef FCL_CONTINUOUS_COLLISION_H
-#define FCL_CONTINUOUS_COLLISION_H
-
-#include <hpp/fcl/math/vec_3f.h>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision_data.h>
-
-namespace fcl
-{
-
-/// @brief continous collision checking between two objects
-FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
- const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result);
-
-FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3f& tf1_end,
- const CollisionObject* o2, const Transform3f& tf2_end,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result);
-
-FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result);
-
-}
-
-#endif
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 6c8717ad522ca2edd5c6b93e10694205ca1cdf00..db357b245200ac2004f220fb1304c0e6860af735 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -39,6 +39,8 @@
#ifndef FCL_GEOMETRIC_SHAPES_H
#define FCL_GEOMETRIC_SHAPES_H
+#include <boost/math/constants/constants.hpp>
+
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/math/vec_3f.h>
#include <string.h>
diff --git a/include/hpp/fcl/traversal/traversal_node_base.h b/include/hpp/fcl/traversal/traversal_node_base.h
index edaf35e340b4253fd3017c0ae6a94a009b963f97..fed49bb3301e77a1440ff6252d78ad45271efe7e 100644
--- a/include/hpp/fcl/traversal/traversal_node_base.h
+++ b/include/hpp/fcl/traversal/traversal_node_base.h
@@ -159,21 +159,6 @@ public:
/// @brief Whether stores statistics
bool enable_statistics;
};
-
-
-struct ConservativeAdvancementStackData
-{
- ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_)
- : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
-
- Vec3f P1;
- Vec3f P2;
- int c1;
- int c2;
- FCL_REAL d;
-};
-
-
}
#endif
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
index 119a549b40b3f11d3d4b4042d8c978c5b02bb493..e275ef1f05bff5fa788372479d4f4de838bd5bbb 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
@@ -1151,567 +1151,6 @@ public:
}
};
-
-
-/// @brief Traversal node for conservative advancement computation between BVH and shape
-template<typename BV, typename S, typename NarrowPhaseSolver>
-class MeshShapeConservativeAdvancementTraversalNode : public MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>
-{
-public:
- MeshShapeConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>()
- {
- delta_t = 1;
- toc = 0;
- t_err = (FCL_REAL)0.0001;
-
- w = w_;
-
- motion1 = NULL;
- motion2 = NULL;
- }
-
- /// @brief BV culling test in one BVTT node
- FCL_REAL BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = this->model2_bv.distance(this->model1->getBV(b1).bv, &P2, &P1);
-
- stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
- }
-
- /// @brief Conservative advancement testing between leaves (one triangle and one shape)
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node = this->model1->getBV(b1);
-
- int primitive_id = node.primitiveId();
-
- const Triangle& tri_id = this->tri_indices[primitive_id];
-
- const Vec3f& p1 = this->vertices[tri_id[0]];
- const Vec3f& p2 = this->vertices[tri_id[1]];
- const Vec3f& p3 = this->vertices[tri_id[2]];
-
- FCL_REAL d;
- Vec3f P1, P2;
- this->nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &P2, &P1);
-
- if(d < this->min_distance)
- {
- this->min_distance = d;
-
- closest_p1 = P1;
- closest_p2 = P2;
-
- last_tri_id = primitive_id;
- }
-
- Vec3f n = this->tf2.transform(P2) - P1; n.normalize();
- // here n should be in global frame
- TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n);
- TBVMotionBoundVisitor<BV> mb_visitor2(this->model2_bv, -n);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= d) cur_delta_t = 1;
- else cur_delta_t = d / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
- }
-
- /// @brief Whether the traversal process can stop early
- bool canStop(FCL_REAL c) const
- {
- if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
- {
- const ConservativeAdvancementStackData& data = stack.back();
-
- Vec3f n = this->tf2.transform(data.P2) - data.P1; n.normalize();
- int c1 = data.c1;
-
- TBVMotionBoundVisitor<BV> mb_visitor1(this->model1->getBV(c1).bv, n);
- TBVMotionBoundVisitor<BV> mb_visitor2(this->model2_bv, -n);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound < c) cur_delta_t = 1;
- else cur_delta_t = c / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- stack.pop_back();
-
- return false;
- }
- }
-
- mutable FCL_REAL min_distance;
-
- mutable Vec3f closest_p1, closest_p2;
-
- mutable int last_tri_id;
-
- /// @brief CA controlling variable: early stop for the early iterations of CA
- FCL_REAL w;
-
- /// @brief The time from beginning point
- FCL_REAL toc;
- FCL_REAL t_err;
-
- /// @brief The delta_t each step
- mutable FCL_REAL delta_t;
-
- /// @brief Motions for the two objects in query
- const MotionBase* motion1;
- const MotionBase* motion2;
-
- mutable std::vector<ConservativeAdvancementStackData> stack;
-};
-
-
-template<typename S, typename BV, typename NarrowPhaseSolver>
-class ShapeMeshConservativeAdvancementTraversalNode : public ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>
-{
-public:
- ShapeMeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>()
- {
- delta_t = 1;
- toc = 0;
- t_err = (FCL_REAL)0.0001;
-
- w = w_;
-
- motion1 = NULL;
- motion2 = NULL;
- }
-
- /// @brief BV culling test in one BVTT node
- FCL_REAL BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = this->model1_bv.distance(this->model2->getBV(b2).bv, &P1, &P2);
-
- stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
- }
-
- /// @brief Conservative advancement testing between leaves (one triangle and one shape)
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node = this->model2->getBV(b2);
-
- int primitive_id = node.primitiveId();
-
- const Triangle& tri_id = this->tri_indices[primitive_id];
-
- const Vec3f& p1 = this->vertices[tri_id[0]];
- const Vec3f& p2 = this->vertices[tri_id[1]];
- const Vec3f& p3 = this->vertices[tri_id[2]];
-
- FCL_REAL d;
- Vec3f P1, P2;
- this->nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d, &P1, &P2);
-
- if(d < this->min_distance)
- {
- this->min_distance = d;
-
- closest_p1 = P1;
- closest_p2 = P2;
-
- last_tri_id = primitive_id;
- }
-
- Vec3f n = P2 - this->tf1.transform(P1); n.normalize();
- // here n should be in global frame
- TBVMotionBoundVisitor<BV> mb_visitor1(this->model1_bv, n);
- TriangleMotionBoundVisitor mb_visitor2(p1, p2, p3, -n);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= d) cur_delta_t = 1;
- else cur_delta_t = d / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
- }
-
- /// @brief Whether the traversal process can stop early
- bool canStop(FCL_REAL c) const
- {
- if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
- {
- const ConservativeAdvancementStackData& data = stack.back();
-
- Vec3f n = data.P2 - this->tf1.transform(data.P1); n.normalize();
- int c2 = data.c2;
-
- TBVMotionBoundVisitor<BV> mb_visitor1(this->model1_bv, n);
- TBVMotionBoundVisitor<BV> mb_visitor2(this->model2->getBV(c2).bv, -n);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound < c) cur_delta_t = 1;
- else cur_delta_t = c / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- stack.pop_back();
-
- return false;
- }
- }
-
- mutable FCL_REAL min_distance;
-
- mutable Vec3f closest_p1, closest_p2;
-
- mutable int last_tri_id;
-
- /// @brief CA controlling variable: early stop for the early iterations of CA
- FCL_REAL w;
-
- /// @brief The time from beginning point
- FCL_REAL toc;
- FCL_REAL t_err;
-
- /// @brief The delta_t each step
- mutable FCL_REAL delta_t;
-
- /// @brief Motions for the two objects in query
- const MotionBase* motion1;
- const MotionBase* motion2;
-
- mutable std::vector<ConservativeAdvancementStackData> stack;
-};
-
-namespace details
-{
-template<typename BV, typename S, typename NarrowPhaseSolver>
-void meshShapeConservativeAdvancementOrientedNodeLeafTesting(int b1, int /* b2 */,
- const BVHModel<BV>* model1, const S& model2,
- const BV& model2_bv,
- Vec3f* vertices, Triangle* tri_indices,
- const Transform3f& tf1,
- const Transform3f& tf2,
- const MotionBase* motion1, const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- bool enable_statistics,
- FCL_REAL& min_distance,
- Vec3f& p1, Vec3f& p2,
- int& last_tri_id,
- FCL_REAL& delta_t,
- int& num_leaf_tests)
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<BV>& node = model1->getBV(b1);
- int primitive_id = node.primitiveId();
-
- const Triangle& tri_id = tri_indices[primitive_id];
- const Vec3f& t1 = vertices[tri_id[0]];
- const Vec3f& t2 = vertices[tri_id[1]];
- const Vec3f& t3 = vertices[tri_id[2]];
-
- FCL_REAL distance;
- Vec3f P1, P2;
- nsolver->shapeTriangleDistance(model2, tf2, t1, t2, t3, tf1, &distance, &P2, &P1);
-
- if(distance < min_distance)
- {
- min_distance = distance;
-
- p1 = P1;
- p2 = P2;
-
- last_tri_id = primitive_id;
- }
-
- // n is in global frame
- Vec3f n = P2 - P1; n.normalize();
-
- TriangleMotionBoundVisitor mb_visitor1(t1, t2, t3, n);
- TBVMotionBoundVisitor<BV> mb_visitor2(model2_bv, -n);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= distance) cur_delta_t = 1;
- else cur_delta_t = distance / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-}
-
-
-template<typename BV, typename S>
-bool meshShapeConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
- FCL_REAL min_distance,
- FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
- const BVHModel<BV>* model1, const S& model2,
- const BV& model2_bv,
- const MotionBase* motion1, const MotionBase* motion2,
- std::vector<ConservativeAdvancementStackData>& stack,
- FCL_REAL& delta_t)
-{
- if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
- {
- const ConservativeAdvancementStackData& data = stack.back();
- Vec3f n = data.P2 - data.P1; n.normalize();
- int c1 = data.c1;
-
- TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n);
- TBVMotionBoundVisitor<BV> mb_visitor2(model2_bv, -n);
-
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= c) cur_delta_t = 1;
- else cur_delta_t = c / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- stack.pop_back();
- return false;
- }
-}
-
-
-}
-
-template<typename S, typename NarrowPhaseSolver>
-class MeshShapeConservativeAdvancementTraversalNodeRSS : public MeshShapeConservativeAdvancementTraversalNode<RSS, S, NarrowPhaseSolver>
-{
-public:
- MeshShapeConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode<RSS, S, NarrowPhaseSolver>(w_)
- {
- }
-
- FCL_REAL BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2);
-
- this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
- }
-
- void leafTesting(int b1, int b2) const
- {
- details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2),
- this->model2_bv,
- this->vertices, this->tri_indices,
- this->tf1, this->tf2,
- this->motion1, this->motion2,
- this->nsolver,
- this->enable_statistics,
- this->min_distance,
- this->closest_p1, this->closest_p2,
- this->last_tri_id,
- this->delta_t,
- this->num_leaf_tests);
- }
-
- bool canStop(FCL_REAL c) const
- {
- return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
- this->abs_err, this->rel_err, this->w,
- this->model1, *(this->model2), this->model2_bv,
- this->motion1, this->motion2,
- this->stack, this->delta_t);
- }
-};
-
-
-template<typename S, typename NarrowPhaseSolver>
-class MeshShapeConservativeAdvancementTraversalNodeOBBRSS : public MeshShapeConservativeAdvancementTraversalNode<OBBRSS, S, NarrowPhaseSolver>
-{
-public:
- MeshShapeConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode<OBBRSS, S, NarrowPhaseSolver>(w_)
- {
- }
-
- FCL_REAL BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2);
-
- this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
- }
-
- void leafTesting(int b1, int b2) const
- {
- details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2),
- this->model2_bv,
- this->vertices, this->tri_indices,
- this->tf1, this->tf2,
- this->motion1, this->motion2,
- this->nsolver,
- this->enable_statistics,
- this->min_distance,
- this->closest_p1, this->closest_p2,
- this->last_tri_id,
- this->delta_t,
- this->num_leaf_tests);
- }
-
- bool canStop(FCL_REAL c) const
- {
- return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
- this->abs_err, this->rel_err, this->w,
- this->model1, *(this->model2), this->model2_bv,
- this->motion1, this->motion2,
- this->stack, this->delta_t);
- }
-};
-
-
-
-template<typename S, typename NarrowPhaseSolver>
-class ShapeMeshConservativeAdvancementTraversalNodeRSS : public ShapeMeshConservativeAdvancementTraversalNode<S, RSS, NarrowPhaseSolver>
-{
-public:
- ShapeMeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode<S, RSS, NarrowPhaseSolver>(w_)
- {
- }
-
- FCL_REAL BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1);
-
- this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
- }
-
- void leafTesting(int b1, int b2) const
- {
- details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1),
- this->model1_bv,
- this->vertices, this->tri_indices,
- this->tf2, this->tf1,
- this->motion2, this->motion1,
- this->nsolver,
- this->enable_statistics,
- this->min_distance,
- this->closest_p2, this->closest_p1,
- this->last_tri_id,
- this->delta_t,
- this->num_leaf_tests);
- }
-
- bool canStop(FCL_REAL c) const
- {
- return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
- this->abs_err, this->rel_err, this->w,
- this->model2, *(this->model1), this->model1_bv,
- this->motion2, this->motion1,
- this->stack, this->delta_t);
- }
-};
-
-
-template<typename S, typename NarrowPhaseSolver>
-class ShapeMeshConservativeAdvancementTraversalNodeOBBRSS : public ShapeMeshConservativeAdvancementTraversalNode<S, OBBRSS, NarrowPhaseSolver>
-{
-public:
- ShapeMeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode<S, OBBRSS, NarrowPhaseSolver>(w_)
- {
- }
-
- FCL_REAL BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1);
-
- this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
- }
-
- void leafTesting(int b1, int b2) const
- {
- details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1),
- this->model1_bv,
- this->vertices, this->tri_indices,
- this->tf2, this->tf1,
- this->motion2, this->motion1,
- this->nsolver,
- this->enable_statistics,
- this->min_distance,
- this->closest_p2, this->closest_p1,
- this->last_tri_id,
- this->delta_t,
- this->num_leaf_tests);
- }
-
- bool canStop(FCL_REAL c) const
- {
- return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
- this->abs_err, this->rel_err, this->w,
- this->model2, *(this->model1), this->model1_bv,
- this->motion2, this->motion1,
- this->stack, this->delta_t);
- }
-};
-
-
}
#endif
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index 9b3f4efd8b509f2e9206216fb66344e7560447c2..cfeec8dca79c71505ef4594ab561997513e29efc 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvhs.h
@@ -45,7 +45,6 @@
#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/intersect.h>
-#include <hpp/fcl/ccd/motion.h>
#include <boost/shared_array.hpp>
#include <boost/shared_ptr.hpp>
@@ -324,153 +323,6 @@ public:
Vec3f T;
};
-/// @brief Traversal node for continuous collision between BVH models
-struct BVHContinuousCollisionPair
-{
- BVHContinuousCollisionPair() {}
-
- BVHContinuousCollisionPair(int id1_, int id2_, FCL_REAL time) : id1(id1_), id2(id2_), collision_time(time) {}
-
- /// @brief The index of one in-collision primitive
- int id1;
-
- /// @brief The index of the other in-collision primitive
- int id2;
-
- /// @brief Collision time normalized in [0, 1]. The collision time out of [0, 1] means collision-free
- FCL_REAL collision_time;
-};
-
-/// @brief Traversal node for continuous collision between meshes
-template<typename BV>
-class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- MeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>(false)
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices1 = NULL;
- tri_indices2 = NULL;
- prev_vertices1 = NULL;
- prev_vertices2 = NULL;
-
- num_vf_tests = 0;
- num_ee_tests = 0;
- time_of_contact = 1;
- }
-
- /// @brief Intersection testing between leaves (two triangles)
- void leafTesting(int b1, int b2, FCL_REAL&) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- FCL_REAL collision_time = 2;
- Vec3f collision_pos;
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- Vec3f* S0[3];
- Vec3f* S1[3];
- Vec3f* T0[3];
- Vec3f* T1[3];
-
-
- for(int i = 0; i < 3; ++i)
- {
- S0[i] = prev_vertices1 + tri_id1[i];
- S1[i] = vertices1 + tri_id1[i];
- T0[i] = prev_vertices2 + tri_id2[i];
- T1[i] = vertices2 + tri_id2[i];
- }
-
- FCL_REAL tmp;
- Vec3f tmpv;
-
- // 6 VF checks
- for(int i = 0; i < 3; ++i)
- {
- if(this->enable_statistics) num_vf_tests++;
- if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *(T0[i]), *(S1[0]), *(S1[1]), *(S1[2]), *(T1[i]), &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
-
- if(this->enable_statistics) num_vf_tests++;
- if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *(S0[i]), *(T1[0]), *(T1[1]), *(T1[2]), *(S1[i]), &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
- }
-
- // 9 EE checks
- for(int i = 0; i < 3; ++i)
- {
- int S_id1 = i;
- int S_id2 = i + 1;
- if(S_id2 == 3) S_id2 = 0;
- for(int j = 0; j < 3; ++j)
- {
- int T_id1 = j;
- int T_id2 = j + 1;
- if(T_id2 == 3) T_id2 = 0;
-
- num_ee_tests++;
- if(Intersect::intersect_EE(*(S0[S_id1]), *(S0[S_id2]), *(T0[T_id1]), *(T0[T_id2]), *(S1[S_id1]), *(S1[S_id2]), *(T1[T_id1]), *(T1[T_id2]), &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
- }
- }
-
- if(!(collision_time > 1)) // collision happens
- {
- pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
- time_of_contact = std::min(time_of_contact, collision_time);
- }
- }
-
- /// @brief Whether the traversal process can stop early
- bool canStop() const
- {
- return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- Triangle* tri_indices1;
- Triangle* tri_indices2;
-
- Vec3f* prev_vertices1;
- Vec3f* prev_vertices2;
-
- mutable int num_vf_tests;
- mutable int num_ee_tests;
-
- mutable std::vector<BVHContinuousCollisionPair> pairs;
-
- mutable FCL_REAL time_of_contact;
-};
-
-
-
/// @brief Traversal node for distance computation between BVH models
template<typename BV>
class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
@@ -683,178 +535,6 @@ public:
Vec3f T;
};
-
-
-/// @brief continuous collision node using conservative advancement. when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
-template<typename BV>
-class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode<BV>
-{
-public:
- MeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshDistanceTraversalNode<BV>()
- {
- delta_t = 1;
- toc = 0;
- t_err = (FCL_REAL)0.00001;
-
- w = w_;
-
- motion1 = NULL;
- motion2 = NULL;
- }
-
- /// @brief BV culling test in one BVTT node
- FCL_REAL BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
-
- stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
- }
-
- /// @brief Conservative advancement testing between leaves (two triangles)
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = this->tri_indices1[primitive_id1];
- const Triangle& tri_id2 = this->tri_indices2[primitive_id2];
-
- const Vec3f& p1 = this->vertices1[tri_id1[0]];
- const Vec3f& p2 = this->vertices1[tri_id1[1]];
- const Vec3f& p3 = this->vertices1[tri_id1[2]];
-
- const Vec3f& q1 = this->vertices2[tri_id2[0]];
- const Vec3f& q2 = this->vertices2[tri_id2[1]];
- const Vec3f& q3 = this->vertices2[tri_id2[2]];
-
- // nearest point pair
- Vec3f P1, P2;
-
- FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
- (p1, p2, p3, q1, q2, q3, P1, P2));
-
- if(d < this->min_distance)
- {
- this->min_distance = d;
-
- closest_p1 = P1;
- closest_p2 = P2;
-
- last_tri_id1 = primitive_id1;
- last_tri_id2 = primitive_id2;
- }
-
-
- Vec3f n = P2 - P1;
- n.normalize();
- // here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
- TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n), mb_visitor2(q1, q2, q3, n);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= d) cur_delta_t = 1;
- else cur_delta_t = d / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
- }
-
- /// @brief Whether the traversal process can stop early
- bool canStop(FCL_REAL c) const
- {
- if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
- {
- const ConservativeAdvancementStackData& data = stack.back();
- FCL_REAL d = data.d;
- Vec3f n;
- int c1, c2;
-
- if(d > c)
- {
- const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
- d = data2.d;
- n = data2.P2 - data2.P1; n.normalize();
- c1 = data2.c1;
- c2 = data2.c2;
- stack[stack.size() - 2] = stack[stack.size() - 1];
- }
- else
- {
- n = data.P2 - data.P1; n.normalize();
- c1 = data.c1;
- c2 = data.c2;
- }
-
- assert(c == d);
-
- TBVMotionBoundVisitor<BV> mb_visitor1(this->model1->getBV(c1).bv, n), mb_visitor2(this->model2->getBV(c2).bv, n);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= c) cur_delta_t = 1;
- else cur_delta_t = c / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- const ConservativeAdvancementStackData& data = stack.back();
- FCL_REAL d = data.d;
-
- if(d > c)
- stack[stack.size() - 2] = stack[stack.size() - 1];
-
- stack.pop_back();
-
- return false;
- }
- }
-
- mutable FCL_REAL min_distance;
-
- mutable Vec3f closest_p1, closest_p2;
-
- mutable int last_tri_id1, last_tri_id2;
-
-
- /// @brief CA controlling variable: early stop for the early iterations of CA
- FCL_REAL w;
-
- /// @brief The time from beginning point
- FCL_REAL toc;
- FCL_REAL t_err;
-
- /// @brief The delta_t each step
- mutable FCL_REAL delta_t;
-
- /// @brief Motions for the two objects in query
- const MotionBase* motion1;
- const MotionBase* motion2;
-
- mutable std::vector<ConservativeAdvancementStackData> stack;
-};
-
-
/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to be transformed
namespace details
{
@@ -872,142 +552,8 @@ inline const Vec3f& getBVAxis<OBBRSS>(const OBBRSS& bv, int i)
}
-template<typename BV>
-bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c,
- FCL_REAL min_distance,
- FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- const MotionBase* motion1, const MotionBase* motion2,
- std::vector<ConservativeAdvancementStackData>& stack,
- FCL_REAL& delta_t)
-{
- if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
- {
- const ConservativeAdvancementStackData& data = stack.back();
- FCL_REAL d = data.d;
- Vec3f n;
- int c1, c2;
-
- if(d > c)
- {
- const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
- d = data2.d;
- n = data2.P2 - data2.P1; n.normalize();
- c1 = data2.c1;
- c2 = data2.c2;
- stack[stack.size() - 2] = stack[stack.size() - 1];
- }
- else
- {
- n = data.P2 - data.P1; n.normalize();
- c1 = data.c1;
- c2 = data.c2;
- }
-
- assert(c == d);
-
- Vec3f n_transformed =
- getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
- getBVAxis(model1->getBV(c1).bv, 1) * n[1] +
- getBVAxis(model1->getBV(c1).bv, 2) * n[2];
-
- TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, n_transformed);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= c) cur_delta_t = 1;
- else cur_delta_t = c / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- const ConservativeAdvancementStackData& data = stack.back();
- FCL_REAL d = data.d;
-
- if(d > c)
- stack[stack.size() - 2] = stack[stack.size() - 1];
-
- stack.pop_back();
-
- return false;
- }
-}
-
-}
-
-/// for OBB, RSS and OBBRSS, there is local coordinate of BV, so normal need to be transformed
-template<>
-inline bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
-{
- return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
- this->abs_err, this->rel_err, w,
- this->model1, this->model2,
- motion1, motion2,
- stack, delta_t);
}
-template<>
-inline bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
-{
- return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
- this->abs_err, this->rel_err, w,
- this->model1, this->model2,
- motion1, motion2,
- stack, delta_t);
}
-template<>
-inline bool MeshConservativeAdvancementTraversalNode<OBBRSS>::canStop(FCL_REAL c) const
-{
- return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
- this->abs_err, this->rel_err, w,
- this->model1, this->model2,
- motion1, motion2,
- stack, delta_t);
-}
-
-
-class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode<RSS>
-{
-public:
- MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1);
-
- FCL_REAL BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- bool canStop(FCL_REAL c) const;
-
- Matrix3f R;
- Vec3f T;
-};
-
-class MeshConservativeAdvancementTraversalNodeOBBRSS : public MeshConservativeAdvancementTraversalNode<OBBRSS>
-{
-public:
- MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1);
-
- FCL_REAL BVTesting(int b1, int b2) const;
-
- FCL_REAL BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
-
- void leafTesting(int b1, int b2) const;
-
- bool canStop(FCL_REAL c) const;
-
- Matrix3f R;
- Vec3f T;
-};
-}
-
-
#endif
diff --git a/include/hpp/fcl/traversal/traversal_node_setup.h b/include/hpp/fcl/traversal/traversal_node_setup.h
index 75271c307bfb994726daa609f647e78fb87a782e..b169fa8e7a549e5710ea999d2fe2dac8fc05ade7 100644
--- a/include/hpp/fcl/traversal/traversal_node_setup.h
+++ b/include/hpp/fcl/traversal/traversal_node_setup.h
@@ -1015,273 +1015,6 @@ bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
-
-
-/// @brief Initialize traversal node for continuous collision detection between two meshes
-template<typename BV>
-bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
- const BVHModel<BV>& model1, const Transform3f& tf1,
- const BVHModel<BV>& model2, const Transform3f& tf2,
- const CollisionRequest& request)
-{
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.prev_vertices1 = model1.prev_vertices;
- node.prev_vertices2 = model2.prev_vertices;
-
- node.request = request;
-
- return true;
-}
-
-/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms
-template<typename BV>
-bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
- BVHModel<BV>& model1, const Transform3f& tf1,
- BVHModel<BV>& model2, const Transform3f& tf2,
- FCL_REAL w = 1,
- bool use_refit = false, bool refit_bottomup = false)
-{
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = tf1.transform(p);
- vertices_transformed1[i] = new_v;
- }
-
-
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = tf2.transform(p);
- vertices_transformed2[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed2);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.w = w;
-
- return true;
-}
-
-
-/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS
-bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const Transform3f& tf1,
- const BVHModel<RSS>& model2, const Transform3f& tf2,
- FCL_REAL w = 1);
-
-bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node,
- const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
- const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- FCL_REAL w = 1);
-
-template<typename S1, typename S2, typename NarrowPhaseSolver>
-bool initialize(ShapeConservativeAdvancementTraversalNode<S1, S2, NarrowPhaseSolver>& node,
- const S1& shape1, const Transform3f& tf1,
- const S2& shape2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver)
-{
- node.model1 = &shape1;
- node.tf1 = tf1;
- node.model2 = &shape2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV<RSS, S1>(shape1, Transform3f(), node.model1_bv);
- computeBV<RSS, S2>(shape2, Transform3f(), node.model2_bv);
-
- return true;
-}
-
-template<typename BV, typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeConservativeAdvancementTraversalNode<BV, S, NarrowPhaseSolver>& node,
- BVHModel<BV>& model1, const Transform3f& tf1,
- const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- FCL_REAL w = 1,
- bool use_refit = false, bool refit_bottomup = false)
-{
- std::vector<Vec3f> vertices_transformed(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = tf1.transform(p);
- vertices_transformed[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
-
- node.tf1 = tf1;
- node.tf2 = tf2;
-
- node.nsolver = nsolver;
- node.w = w;
-
- computeBV<BV, S>(model2, Transform3f(), node.model2_bv);
-
- return true;
-}
-
-
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<RSS>& model1, const Transform3f& tf1,
- const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- FCL_REAL w = 1)
-{
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- node.w = w;
-
- computeBV<RSS, S>(model2, Transform3f(), node.model2_bv);
-
- return true;
-}
-
-
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
- const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- FCL_REAL w = 1)
-{
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- node.w = w;
-
- computeBV<OBBRSS, S>(model2, Transform3f(), node.model2_bv);
-
- return true;
-}
-
-
-template<typename S, typename BV, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshConservativeAdvancementTraversalNode<S, BV, NarrowPhaseSolver>& node,
- const S& model1, const Transform3f& tf1,
- BVHModel<BV>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- FCL_REAL w = 1,
- bool use_refit = false, bool refit_bottomup = false)
-{
- std::vector<Vec3f> vertices_transformed(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = tf2.transform(p);
- vertices_transformed[i] = new_v;
- }
-
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
-
- node.tf1 = tf1;
- node.tf2 = tf2;
-
- node.nsolver = nsolver;
- node.w = w;
-
- computeBV<BV, S>(model1, Transform3f(), node.model1_bv);
-
- return true;
-}
-
-
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const Transform3f& tf1,
- const BVHModel<RSS>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- FCL_REAL w = 1)
-{
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- node.w = w;
-
- computeBV<RSS, S>(model1, Transform3f(), node.model1_bv);
-
- return true;
-}
-
-
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const Transform3f& tf1,
- const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- FCL_REAL w = 1)
-{
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- node.w = w;
-
- computeBV<OBBRSS, S>(model1, Transform3f(), node.model1_bv);
-
- return true;
-}
-
-
-
-
}
#endif
diff --git a/include/hpp/fcl/traversal/traversal_node_shapes.h b/include/hpp/fcl/traversal/traversal_node_shapes.h
index fccdbadf4c7c7bc50d9bcc1a684bd1350708bc92..d1561b88e898b79c096e3035f230a3d29cdc9d1b 100644
--- a/include/hpp/fcl/traversal/traversal_node_shapes.h
+++ b/include/hpp/fcl/traversal/traversal_node_shapes.h
@@ -45,7 +45,6 @@
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/ccd/motion.h>
namespace fcl
{
@@ -168,60 +167,6 @@ public:
const NarrowPhaseSolver* nsolver;
};
-
-template<typename S1, typename S2, typename NarrowPhaseSolver>
-class ShapeConservativeAdvancementTraversalNode : public ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>
-{
-public:
- ShapeConservativeAdvancementTraversalNode() : ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>()
- {
- delta_t = 1;
- toc = 0;
- t_err = (FCL_REAL)0.0001;
-
- motion1 = NULL;
- motion2 = NULL;
- }
-
- void leafTesting(int, int) const
- {
- FCL_REAL distance;
- Vec3f closest_p1, closest_p2;
- this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2);
-
- Vec3f n = this->tf2.transform(closest_p2) - this->tf1.transform(closest_p1); n.normalize();
- TBVMotionBoundVisitor<RSS> mb_visitor1(model1_bv, n);
- TBVMotionBoundVisitor<RSS> mb_visitor2(model2_bv, -n);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= distance) cur_delta_t = 1;
- else cur_delta_t = distance / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
- }
-
- mutable FCL_REAL min_distance;
-
- /// @brief The time from beginning point
- FCL_REAL toc;
- FCL_REAL t_err;
-
- /// @brief The delta_t each step
- mutable FCL_REAL delta_t;
-
- /// @brief Motions for the two objects in query
- const MotionBase* motion1;
- const MotionBase* motion2;
-
- RSS model1_bv, model2_bv; // local bv for the two shapes
-};
-
-
}
#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3cfc6fdd99751a029d4d2536178d447be656da5c..d1596eb63f8600b9c2175f898c8a3e237a515af0 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -36,17 +36,6 @@ set(LIBRARY_NAME ${PROJECT_NAME})
set(${LIBRARY_NAME}_SOURCES
collision.cpp
distance_func_matrix.cpp
- ccd/taylor_matrix.cpp
- ccd/taylor_model.cpp
- ccd/interval.cpp
- ccd/interval_vector.cpp
- ccd/conservative_advancement.cpp
- ccd/taylor_vector.cpp
- ccd/interval_matrix.cpp
- ccd/interpolation/interpolation_linear.cpp
- ccd/interpolation/interpolation_factory.cpp
- ccd/interpolation/interpolation.cpp
- ccd/motion.cpp
collision_data.cpp
collision_node.cpp
BV/RSS.cpp
@@ -78,7 +67,6 @@ set(${LIBRARY_NAME}_SOURCES
traversal/traversal_recurse.cpp
traversal/traversal_node_base.cpp
profile.cpp
- continuous_collision.cpp
articulated_model/link.cpp
articulated_model/model.cpp
articulated_model/model_config.cpp
diff --git a/src/ccd/conservative_advancement.cpp b/src/ccd/conservative_advancement.cpp
deleted file mode 100644
index 146cecb90a8083be168ab8d1b2e0defe4c908930..0000000000000000000000000000000000000000
--- a/src/ccd/conservative_advancement.cpp
+++ /dev/null
@@ -1,963 +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 Jia Pan */
-
-#include <hpp/fcl/ccd/conservative_advancement.h>
-#include <hpp/fcl/ccd/motion.h>
-#include <hpp/fcl/collision_node.h>
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
-#include <hpp/fcl/traversal/traversal_node_setup.h>
-#include <hpp/fcl/traversal/traversal_recurse.h>
-#include <hpp/fcl/collision.h>
-
-
-namespace fcl
-{
-
-
-
-
-template<typename BV>
-bool conservativeAdvancement(const BVHModel<BV>& o1,
- const MotionBase* motion1,
- const BVHModel<BV>& o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
-
- // whether the first start configuration is in collision
- if(collide(&o1, tf1, &o2, tf2, request, result))
- {
- toc = 0;
- return true;
- }
-
-
- BVHModel<BV>* o1_tmp = new BVHModel<BV>(o1);
- BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
-
-
- MeshConservativeAdvancementTraversalNode<BV> node;
-
- node.motion1 = motion1;
- node.motion2 = motion2;
-
- do
- {
- // repeatedly update mesh to global coordinate, so time consuming
- initialize(node, *o1_tmp, tf1, *o2_tmp, tf2);
-
- node.delta_t = 1;
- node.min_distance = std::numeric_limits<FCL_REAL>::max();
-
- distanceRecurse(&node, 0, 0, NULL);
-
- if(node.delta_t <= node.t_err)
- {
- // std::cout << node.delta_t << " " << node.t_err << std::endl;
- break;
- }
-
- node.toc += node.delta_t;
- if(node.toc > 1)
- {
- node.toc = 1;
- break;
- }
-
- node.motion1->integrate(node.toc);
- node.motion2->integrate(node.toc);
-
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
- }
- while(1);
-
- delete o1_tmp;
- delete o2_tmp;
-
- toc = node.toc;
-
- if(node.toc < 1)
- return true;
-
- return false;
-}
-
-namespace details
-{
-
-template<typename BV, typename ConservativeAdvancementOrientedNode>
-bool conservativeAdvancementMeshOriented(const BVHModel<BV>& o1,
- const MotionBase* motion1,
- const BVHModel<BV>& o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
-
- // whether the first start configuration is in collision
- if(collide(&o1, tf1, &o2, tf2, request, result))
- {
- toc = 0;
- return true;
- }
-
-
- ConservativeAdvancementOrientedNode node;
-
- initialize(node, o1, tf1, o2, tf2);
-
- node.motion1 = motion1;
- node.motion2 = motion2;
-
- do
- {
- node.motion1->getCurrentTransform(tf1);
- node.motion2->getCurrentTransform(tf2);
-
- // compute the transformation from 1 to 2
- Transform3f tf;
- relativeTransform(tf1, tf2, tf);
- node.R = tf.getRotation();
- node.T = tf.getTranslation();
-
- node.delta_t = 1;
- node.min_distance = std::numeric_limits<FCL_REAL>::max();
-
- distanceRecurse(&node, 0, 0, NULL);
-
- if(node.delta_t <= node.t_err)
- {
- // std::cout << node.delta_t << " " << node.t_err << std::endl;
- break;
- }
-
- node.toc += node.delta_t;
- if(node.toc > 1)
- {
- node.toc = 1;
- break;
- }
-
- node.motion1->integrate(node.toc);
- node.motion2->integrate(node.toc);
- }
- while(1);
-
- toc = node.toc;
-
- if(node.toc < 1)
- return true;
-
- return false;
-}
-
-
-}
-
-template<>
-bool conservativeAdvancement(const BVHModel<RSS>& o1,
- const MotionBase* motion1,
- const BVHModel<RSS>& o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc);
-
-
-template<>
-bool conservativeAdvancement(const BVHModel<OBBRSS>& o1,
- const MotionBase* motion1,
- const BVHModel<OBBRSS>& o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc);
-
-template<typename S1, typename S2, typename NarrowPhaseSolver>
-bool conservativeAdvancement(const S1& o1,
- const MotionBase* motion1,
- const S2& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* solver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
-
- // whether the first start configuration is in collision
- if(collide(&o1, tf1, &o2, tf2, request, result))
- {
- toc = 0;
- return true;
- }
-
- ShapeConservativeAdvancementTraversalNode<S1, S2, NarrowPhaseSolver> node;
-
- initialize(node, o1, tf1, o2, tf2, solver);
-
- node.motion1 = motion1;
- node.motion2 = motion2;
-
- do
- {
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
- node.tf1 = tf1;
- node.tf2 = tf2;
-
- node.delta_t = 1;
- node.min_distance = std::numeric_limits<FCL_REAL>::max();
-
- distanceRecurse(&node, 0, 0, NULL);
-
- if(node.delta_t <= node.t_err)
- {
- // std::cout << node.delta_t << " " << node.t_err << std::endl;
- break;
- }
-
- node.toc += node.delta_t;
- if(node.toc > 1)
- {
- node.toc = 1;
- break;
- }
-
- node.motion1->integrate(node.toc);
- node.motion2->integrate(node.toc);
- }
- while(1);
-
- toc = node.toc;
-
- if(node.toc < 1)
- return true;
-
- return false;
-}
-
-template<typename BV, typename S, typename NarrowPhaseSolver>
-bool conservativeAdvancement(const BVHModel<BV>& o1,
- const MotionBase* motion1,
- const S& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
-
- if(collide(&o1, tf1, &o2, tf2, request, result))
- {
- toc = 0;
- return true;
- }
-
- BVHModel<BV>* o1_tmp = new BVHModel<BV>(o1);
-
- MeshShapeConservativeAdvancementTraversalNode<BV, S, NarrowPhaseSolver> node;
-
- node.motion1 = motion1;
- node.motion2 = motion2;
-
- do
- {
- // initialize update mesh to global coordinate, so time consuming
- initialize(node, *o1_tmp, tf1, o2, tf2, nsolver);
-
- node.delta_t = 1;
- node.min_distance = std::numeric_limits<FCL_REAL>::max();
-
- distanceRecurse(&node, 0, 0, NULL);
-
- if(node.delta_t <= node.t_err)
- {
- break;
- }
-
- node.toc += node.delta_t;
- if(node.toc > 1)
- {
- node.toc = 1;
- break;
- }
-
- node.motion1->integrate(node.toc);
- node.motion2->integrate(node.toc);
-
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
- }
- while(1);
-
- delete o1_tmp;
-
- toc = node.toc;
-
- if(node.toc < 1)
- return true;
-
- return false;
-}
-
-namespace details
-{
-
-template<typename BV, typename S, typename NarrowPhaseSolver, typename ConservativeAdvancementOrientedNode>
-bool conservativeAdvancementMeshShapeOriented(const BVHModel<BV>& o1,
- const MotionBase* motion1,
- const S& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
-
- if(collide(&o1, tf1, &o2, tf2, request, result))
- {
- toc = 0;
- return true;
- }
-
- ConservativeAdvancementOrientedNode node;
-
- initialize(node, o1, tf1, o2, tf2, nsolver);
-
- node.motion1 = motion1;
- node.motion2 = motion2;
-
- do
- {
- node.motion1->getCurrentTransform(tf1);
- node.motion2->getCurrentTransform(tf2);
-
- node.tf1 = tf1;
- node.tf2 = tf2;
-
- node.delta_t = 1;
- node.min_distance = std::numeric_limits<FCL_REAL>::max();
-
- distanceRecurse(&node, 0, 0, NULL);
-
- if(node.delta_t <= node.t_err)
- {
- break;
- }
-
- node.toc += node.delta_t;
- if(node.toc > 1)
- {
- node.toc = 1;
- break;
- }
-
- node.motion1->integrate(node.toc);
- node.motion2->integrate(node.toc);
- }
- while(1);
-
- toc = node.toc;
-
- if(node.toc < 1)
- return true;
-
- return false;
-}
-
-}
-
-
-template<typename S, typename NarrowPhaseSolver>
-bool conservativeAdvancement(const BVHModel<RSS>& o1,
- const MotionBase* motion1,
- const S& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- return details::conservativeAdvancementMeshShapeOriented<RSS, S, NarrowPhaseSolver, MeshShapeConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
-}
-
-template<typename S, typename NarrowPhaseSolver>
-bool conservativeAdvancement(const BVHModel<OBBRSS>& o1,
- const MotionBase* motion1,
- const S& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- return details::conservativeAdvancementMeshShapeOriented<OBBRSS, S, NarrowPhaseSolver, MeshShapeConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
-}
-
-template<typename S, typename BV, typename NarrowPhaseSolver>
-bool conservativeAdvancement(const S& o1,
- const MotionBase* motion1,
- const BVHModel<BV>& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
-
- if(collide(&o1, tf1, &o2, tf2, request, result))
- {
- toc = 0;
- return true;
- }
-
- BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
-
- ShapeMeshConservativeAdvancementTraversalNode<S, BV, NarrowPhaseSolver> node;
-
- node.motion1 = motion1;
- node.motion2 = motion2;
-
- do
- {
- // initialize update mesh to global coordinate, so time consuming
- initialize(node, o1, tf1, *o2_tmp, tf2, nsolver);
-
- node.delta_t = 1;
- node.min_distance = std::numeric_limits<FCL_REAL>::max();
-
- distanceRecurse(&node, 0, 0, NULL);
-
- if(node.delta_t <= node.t_err)
- {
- break;
- }
-
- node.toc += node.delta_t;
- if(node.toc > 1)
- {
- node.toc = 1;
- break;
- }
-
- node.motion1->integrate(node.toc);
- node.motion2->integrate(node.toc);
-
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
- }
- while(1);
-
- delete o2_tmp;
-
- toc = node.toc;
-
- if(node.toc < 1)
- return true;
-
- return false;
-}
-
-namespace details
-{
-
-template<typename S, typename BV, typename NarrowPhaseSolver, typename ConservativeAdvancementOrientedNode>
-bool conservativeAdvancementShapeMeshOriented(const S& o1,
- const MotionBase* motion1,
- const BVHModel<BV>& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
-
- if(collide(&o1, tf1, &o2, tf2, request, result))
- {
- toc = 0;
- return true;
- }
-
- ConservativeAdvancementOrientedNode node;
-
- initialize(node, o1, tf1, o2, tf2, nsolver);
-
- node.motion1 = motion1;
- node.motion2 = motion2;
-
- do
- {
- node.motion1->getCurrentTransform(tf1);
- node.motion2->getCurrentTransform(tf2);
-
- node.tf1 = tf1;
- node.tf2 = tf2;
-
- node.delta_t = 1;
- node.min_distance = std::numeric_limits<FCL_REAL>::max();
-
- distanceRecurse(&node, 0, 0, NULL);
-
- if(node.delta_t <= node.t_err)
- {
- break;
- }
-
- node.toc += node.delta_t;
- if(node.toc > 1)
- {
- node.toc = 1;
- break;
- }
-
- node.motion1->integrate(node.toc);
- node.motion2->integrate(node.toc);
- }
- while(1);
-
- toc = node.toc;
-
- if(node.toc < 1)
- return true;
-
- return false;
-}
-
-}
-
-template<typename S, typename NarrowPhaseSolver>
-bool conservativeAdvancement(const S& o1,
- const MotionBase* motion1,
- const BVHModel<RSS>& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- return details::conservativeAdvancementShapeMeshOriented<S, RSS, NarrowPhaseSolver, ShapeMeshConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
-}
-
-
-template<typename S, typename NarrowPhaseSolver>
-bool conservativeAdvancement(const S& o1,
- const MotionBase* motion1,
- const BVHModel<OBBRSS>& o2,
- const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- return details::conservativeAdvancementShapeMeshOriented<S, OBBRSS, NarrowPhaseSolver, ShapeMeshConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
-}
-
-
-
-template<>
-bool conservativeAdvancement(const BVHModel<RSS>& o1,
- const MotionBase* motion1,
- const BVHModel<RSS>& o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- return details::conservativeAdvancementMeshOriented<RSS, MeshConservativeAdvancementTraversalNodeRSS>(o1, motion1, o2, motion2, request, result, toc);
-}
-
-template<>
-bool conservativeAdvancement(const BVHModel<OBBRSS>& o1,
- const MotionBase* motion1,
- const BVHModel<OBBRSS>& o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
-{
- return details::conservativeAdvancementMeshOriented<OBBRSS, MeshConservativeAdvancementTraversalNodeOBBRSS>(o1, motion1, o2, motion2, request, result, toc);
-}
-
-
-template<typename BV, typename NarrowPhaseSolver>
-FCL_REAL BVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result)
-{
- const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
- const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
-
- CollisionRequest c_request;
- CollisionResult c_result;
- FCL_REAL toc;
- bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, c_request, c_result, toc);
-
- result.is_collide = is_collide;
- result.time_of_contact = toc;
-
- return toc;
-}
-
-template<typename S1, typename S2, typename NarrowPhaseSolver>
-FCL_REAL ShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result)
-{
- const S1* obj1 = static_cast<const S1*>(o1);
- const S2* obj2 = static_cast<const S2*>(o2);
-
- CollisionRequest c_request;
- CollisionResult c_result;
- FCL_REAL toc;
- bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
-
- result.is_collide = is_collide;
- result.time_of_contact = toc;
-
- return toc;
-}
-
-template<typename S, typename BV, typename NarrowPhaseSolver>
-FCL_REAL ShapeBVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result)
-{
- const S* obj1 = static_cast<const S*>(o1);
- const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
-
- CollisionRequest c_request;
- CollisionResult c_result;
- FCL_REAL toc;
-
- bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
-
- result.is_collide = is_collide;
- result.time_of_contact = toc;
-
- return toc;
-}
-
-template<typename BV, typename S, typename NarrowPhaseSolver>
-FCL_REAL BVHShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result)
-{
- const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
- const S* obj2 = static_cast<const S*>(o2);
-
- CollisionRequest c_request;
- CollisionResult c_result;
- FCL_REAL toc;
-
- bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
-
- result.is_collide = is_collide;
- result.time_of_contact = toc;
-
- return toc;
-}
-
-template<typename NarrowPhaseSolver>
-ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancementFunctionMatrix()
-{
- for(int i = 0; i < NODE_COUNT; ++i)
- {
- for(int j = 0; j < NODE_COUNT; ++j)
- conservative_advancement_matrix[i][j] = NULL;
- }
-
-
- conservative_advancement_matrix[GEOM_BOX][GEOM_BOX] = &ShapeConservativeAdvancement<Box, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeConservativeAdvancement<Box, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Box, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_BOX][GEOM_CONE] = &ShapeConservativeAdvancement<Box, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Box, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Box, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeConservativeAdvancement<Box, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Box, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeConservativeAdvancement<Sphere, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Sphere, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Sphere, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeConservativeAdvancement<Sphere, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Sphere, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Sphere, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeConservativeAdvancement<Sphere, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Sphere, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeConservativeAdvancement<Capsule, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Capsule, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Capsule, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeConservativeAdvancement<Capsule, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Capsule, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Capsule, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeConservativeAdvancement<Capsule, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Capsule, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_CONE][GEOM_BOX] = &ShapeConservativeAdvancement<Cone, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cone, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Cone, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][GEOM_CONE] = &ShapeConservativeAdvancement<Cone, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cone, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cone, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeConservativeAdvancement<Cone, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Cone, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeConservativeAdvancement<Cylinder, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cylinder, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Cylinder, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeConservativeAdvancement<Cylinder, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cylinder, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cylinder, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeConservativeAdvancement<Cylinder, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Cylinder, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeConservativeAdvancement<Convex, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeConservativeAdvancement<Convex, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Convex, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeConservativeAdvancement<Convex, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Convex, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Convex, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeConservativeAdvancement<Convex, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Convex, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeConservativeAdvancement<Plane, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Plane, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Plane, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeConservativeAdvancement<Plane, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Plane, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Plane, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeConservativeAdvancement<Plane, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Plane, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeConservativeAdvancement<Halfspace, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Halfspace, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Halfspace, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeConservativeAdvancement<Halfspace, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Halfspace, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Halfspace, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeConservativeAdvancement<Halfspace, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Halfspace, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_AABB][GEOM_BOX] = &BVHShapeConservativeAdvancement<AABB, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<AABB, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<AABB, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_AABB][GEOM_CONE] = &BVHShapeConservativeAdvancement<AABB, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<AABB, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<AABB, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<AABB, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<AABB, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_OBB][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBB, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBB, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<OBB, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBB][GEOM_CONE] = &BVHShapeConservativeAdvancement<OBB, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBB, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBB, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBB, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<OBB, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBBRSS, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBBRSS, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<OBBRSS, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeConservativeAdvancement<OBBRSS, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBBRSS, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBBRSS, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBBRSS, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<OBBRSS, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_RSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<RSS, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<RSS, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<RSS, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_RSS][GEOM_CONE] = &BVHShapeConservativeAdvancement<RSS, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<RSS, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<RSS, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<RSS, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<RSS, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<16>, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<16>, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<16>, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<16>, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<16>, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<16>, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<16>, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<16>, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<18>, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<18>, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<18>, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<18>, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<18>, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<18>, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<18>, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<18>, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<24>, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<24>, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<24>, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<24>, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<24>, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<24>, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<24>, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<24>, Halfspace, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeConservativeAdvancement<kIOS, Box, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<kIOS, Sphere, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<kIOS, Capsule, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeConservativeAdvancement<kIOS, Cone, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<kIOS, Cylinder, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<kIOS, Convex, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<kIOS, Plane, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<kIOS, Halfspace, NarrowPhaseSolver>;
-
-
- conservative_advancement_matrix[GEOM_BOX][BV_AABB] = &ShapeBVHConservativeAdvancement<Box, AABB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][BV_AABB] = &ShapeBVHConservativeAdvancement<Sphere, AABB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][BV_AABB] = &ShapeBVHConservativeAdvancement<Capsule, AABB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][BV_AABB] = &ShapeBVHConservativeAdvancement<Cone, AABB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][BV_AABB] = &ShapeBVHConservativeAdvancement<Cylinder, AABB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][BV_AABB] = &ShapeBVHConservativeAdvancement<Convex, AABB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][BV_AABB] = &ShapeBVHConservativeAdvancement<Plane, AABB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][BV_AABB] = &ShapeBVHConservativeAdvancement<Halfspace, AABB, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_BOX][BV_OBB] = &ShapeBVHConservativeAdvancement<Box, OBB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][BV_OBB] = &ShapeBVHConservativeAdvancement<Sphere, OBB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][BV_OBB] = &ShapeBVHConservativeAdvancement<Capsule, OBB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][BV_OBB] = &ShapeBVHConservativeAdvancement<Cone, OBB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][BV_OBB] = &ShapeBVHConservativeAdvancement<Cylinder, OBB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][BV_OBB] = &ShapeBVHConservativeAdvancement<Convex, OBB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][BV_OBB] = &ShapeBVHConservativeAdvancement<Plane, OBB, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBB] = &ShapeBVHConservativeAdvancement<Halfspace, OBB, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_BOX][BV_RSS] = &ShapeBVHConservativeAdvancement<Box, RSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][BV_RSS] = &ShapeBVHConservativeAdvancement<Sphere, RSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][BV_RSS] = &ShapeBVHConservativeAdvancement<Capsule, RSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][BV_RSS] = &ShapeBVHConservativeAdvancement<Cone, RSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][BV_RSS] = &ShapeBVHConservativeAdvancement<Cylinder, RSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][BV_RSS] = &ShapeBVHConservativeAdvancement<Convex, RSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][BV_RSS] = &ShapeBVHConservativeAdvancement<Plane, RSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][BV_RSS] = &ShapeBVHConservativeAdvancement<Halfspace, RSS, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_BOX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Box, OBBRSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Sphere, OBBRSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Capsule, OBBRSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Cone, OBBRSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Cylinder, OBBRSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Convex, OBBRSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Plane, OBBRSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Halfspace, OBBRSS, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_BOX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Box, KDOP<16>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<16>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Capsule, KDOP<16>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Cone, KDOP<16>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<16>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Convex, KDOP<16>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Plane, KDOP<16>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<16>, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_BOX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Box, KDOP<18>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<18>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Capsule, KDOP<18>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Cone, KDOP<18>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<18>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Convex, KDOP<18>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Plane, KDOP<18>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<18>, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_BOX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Box, KDOP<24>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<24>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Capsule, KDOP<24>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Cone, KDOP<24>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<24>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Convex, KDOP<24>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Plane, KDOP<24>, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<24>, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[GEOM_BOX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Box, kIOS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_SPHERE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Sphere, kIOS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CAPSULE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Capsule, kIOS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Cone, kIOS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CYLINDER][BV_kIOS] = &ShapeBVHConservativeAdvancement<Cylinder, kIOS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_CONVEX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Convex, kIOS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_PLANE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Plane, kIOS, NarrowPhaseSolver>;
- conservative_advancement_matrix[GEOM_HALFSPACE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Halfspace, kIOS, NarrowPhaseSolver>;
-
- conservative_advancement_matrix[BV_AABB][BV_AABB] = &BVHConservativeAdvancement<AABB, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBB][BV_OBB] = &BVHConservativeAdvancement<OBB, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_RSS][BV_RSS] = &BVHConservativeAdvancement<RSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHConservativeAdvancement<OBBRSS, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP16][BV_KDOP16] = &BVHConservativeAdvancement<KDOP<16>, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP18][BV_KDOP18] = &BVHConservativeAdvancement<KDOP<18>, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_KDOP24][BV_KDOP24] = &BVHConservativeAdvancement<KDOP<24>, NarrowPhaseSolver>;
- conservative_advancement_matrix[BV_kIOS][BV_kIOS] = &BVHConservativeAdvancement<kIOS, NarrowPhaseSolver>;
-
-}
-
-
-template struct ConservativeAdvancementFunctionMatrix<GJKSolver_libccd>;
-template struct ConservativeAdvancementFunctionMatrix<GJKSolver_indep>;
-
-
-
-
-
-
-
-
-
-}
-
-
diff --git a/src/ccd/interpolation/interpolation.cpp b/src/ccd/interpolation/interpolation.cpp
deleted file mode 100644
index f77a4f17df7666da513787fec862545e4cd35f1d..0000000000000000000000000000000000000000
--- a/src/ccd/interpolation/interpolation.cpp
+++ /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 */
-
-#include <hpp/fcl/ccd/interpolation/interpolation.h>
-
-namespace fcl
-{
-
-Interpolation::Interpolation() :
- value_0_(0.0),
- value_1_(1.0)
-{}
-
-Interpolation::Interpolation(FCL_REAL start_value, FCL_REAL end_value) :
- value_0_(start_value),
- value_1_(end_value)
-{}
-
-void Interpolation::setStartValue(FCL_REAL start_value)
-{
- value_0_ = start_value;
-}
-
-void Interpolation::setEndValue(FCL_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);
-}
-
-}
diff --git a/src/ccd/interpolation/interpolation_factory.cpp b/src/ccd/interpolation/interpolation_factory.cpp
deleted file mode 100644
index d09ece2ceb522fb9927e615285ac3dc85b84a4e6..0000000000000000000000000000000000000000
--- a/src/ccd/interpolation/interpolation_factory.cpp
+++ /dev/null
@@ -1,78 +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/ccd/interpolation/interpolation_factory.h>
-#include <hpp/fcl/ccd/interpolation/interpolation_linear.h>
-
-#include <boost/assert.hpp>
-
-// Define for boost version < 1.47
-#ifndef BOOST_ASSERT_MSG
-#define BOOST_ASSERT_MSG(expr, msg) ((void)0)
-#endif
-
-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;
-}
-
-boost::shared_ptr<Interpolation>
-InterpolationFactory::create(const InterpolationType type, const FCL_REAL start_value, const FCL_REAL end_value)
-{
- std::map<InterpolationType, CreateFunction>::const_iterator it = creation_map_.find(type);
-
- BOOST_ASSERT_MSG((it != creation_map_.end()), "CreateFunction wasn't found.");
-
- return (it->second)(start_value, end_value);
-}
-
-}
diff --git a/src/ccd/interpolation/interpolation_linear.cpp b/src/ccd/interpolation/interpolation_linear.cpp
deleted file mode 100644
index a9635dad0f15bcf00592cdac939981a6f0d18715..0000000000000000000000000000000000000000
--- a/src/ccd/interpolation/interpolation_linear.cpp
+++ /dev/null
@@ -1,92 +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/ccd/interpolation/interpolation_linear.h>
-#include <hpp/fcl/ccd/interpolation/interpolation_factory.h>
-
-namespace fcl
-{
-
-InterpolationType interpolation_linear_type = LINEAR;
-
-InterpolationLinear::InterpolationLinear() : Interpolation(0.0, 1.0)
-{}
-
-InterpolationLinear::InterpolationLinear(FCL_REAL start_value, FCL_REAL end_value) : Interpolation(start_value, end_value)
-{}
-
-FCL_REAL InterpolationLinear::getValue(FCL_REAL time) const
-{
- return value_0_ + (value_1_ - value_0_) * time;
-}
-
-FCL_REAL InterpolationLinear::getValueLowerBound() const
-{
- return value_0_;
-}
-
-FCL_REAL InterpolationLinear::getValueUpperBound() const
-{
- return value_1_;
-}
-
-InterpolationType InterpolationLinear::getType() const
-{
- return interpolation_linear_type;
-}
-
-boost::shared_ptr<Interpolation> InterpolationLinear::create(FCL_REAL start_value, FCL_REAL end_value)
-{
- return boost::shared_ptr<Interpolation>(new InterpolationLinear(start_value, end_value) );
-}
-
-void InterpolationLinear::registerToFactory()
-{
- InterpolationFactory::instance().registerClass(interpolation_linear_type, create);
-}
-
-FCL_REAL InterpolationLinear::getMovementLengthBound(FCL_REAL time) const
-{
- return getValueUpperBound() - getValue(time);
-}
-
-FCL_REAL InterpolationLinear::getVelocityBound(FCL_REAL /*time*/) const
-{
- return (value_1_ - value_0_);
-}
-
-}
diff --git a/src/ccd/interval.cpp b/src/ccd/interval.cpp
deleted file mode 100644
index 07bf68d43fc30102383cc90c624fcb4eb6aa811c..0000000000000000000000000000000000000000
--- a/src/ccd/interval.cpp
+++ /dev/null
@@ -1,204 +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 Jia Pan */
-
-#include <hpp/fcl/ccd/interval.h>
-#include <iostream>
-
-namespace fcl
-{
-
-Interval bound(const Interval& i, FCL_REAL v)
-{
- Interval res = i;
- if(v < res.i_[0]) res.i_[0] = v;
- if(v > res.i_[1]) res.i_[1] = v;
- return res;
-}
-
-Interval bound(const Interval& i, const Interval& other)
-{
- Interval res = i;
- if(other.i_[0] < res.i_[0]) res.i_[0] = other.i_[0];
- if(other.i_[1] > res.i_[1]) res.i_[1] = other.i_[1];
- return res;
-}
-
-Interval Interval::operator * (const Interval& other) const
-{
- if(other.i_[0] >= 0)
- {
- if(i_[0] >= 0) return Interval(i_[0] * other.i_[0], i_[1] * other.i_[1]);
- if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[1] * other.i_[0]);
- return Interval(i_[0] * other.i_[1], i_[1] * other.i_[1]);
- }
- if(other.i_[1] <= 0)
- {
- if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[0] * other.i_[1]);
- if(i_[1] <= 0) return Interval(i_[1] * other.i_[1], i_[0] * other.i_[0]);
- return Interval(i_[1] * other.i_[0], i_[0] * other.i_[0]);
- }
-
- if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[1] * other.i_[1]);
- if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[0] * other.i_[0]);
-
- FCL_REAL v00 = i_[0] * other.i_[0];
- FCL_REAL v11 = i_[1] * other.i_[1];
- if(v00 <= v11)
- {
- FCL_REAL v01 = i_[0] * other.i_[1];
- FCL_REAL v10 = i_[1] * other.i_[0];
- if(v01 < v10) return Interval(v01, v11);
- return Interval(v10, v11);
- }
-
- FCL_REAL v01 = i_[0] * other.i_[1];
- FCL_REAL v10 = i_[1] * other.i_[0];
- if(v01 < v10) return Interval(v01, v00);
- return Interval(v10, v00);
-}
-
-Interval& Interval::operator *= (const Interval& other)
-{
- if(other.i_[0] >= 0)
- {
- if(i_[0] >= 0)
- {
- i_[0] *= other.i_[0];
- i_[1] *= other.i_[1];
- }
- else if(i_[1] <= 0)
- {
- i_[0] *= other.i_[1];
- i_[1] *= other.i_[0];
- }
- else
- {
- i_[0] *= other.i_[1];
- i_[1] *= other.i_[1];
- }
- return *this;
- }
-
- if(other.i_[1] <= 0)
- {
- if(i_[0] >= 0)
- {
- FCL_REAL tmp = i_[0];
- i_[0] = i_[1] * other.i_[0];
- i_[1] = tmp * other.i_[1];
- }
- else if(i_[1] <= 0)
- {
- FCL_REAL tmp = i_[0];
- i_[0] = i_[1] * other.i_[1];
- i_[1] = tmp * other.i_[0];
- }
- else
- {
- FCL_REAL tmp = i_[0];
- i_[0] = i_[1] * other.i_[0];
- i_[1] = tmp * other.i_[0];
- }
- return *this;
- }
-
- if(i_[0] >= 0)
- {
- i_[0] = i_[1] * other.i_[0];
- i_[1] *= other.i_[1];
- return *this;
- }
-
- if(i_[1] <= 0)
- {
- i_[1] = i_[0] * other.i_[0];
- i_[0] *= other.i_[1];
- return *this;
- }
-
- FCL_REAL v00 = i_[0] * other.i_[0];
- FCL_REAL v11 = i_[1] * other.i_[1];
- if(v00 <= v11)
- {
- FCL_REAL v01 = i_[0] * other.i_[1];
- FCL_REAL v10 = i_[1] * other.i_[0];
- if(v01 < v10)
- {
- i_[0] = v01;
- i_[1] = v11;
- }
- else
- {
- i_[0] = v10;
- i_[1] = v11;
- }
- return *this;
- }
-
- FCL_REAL v01 = i_[0] * other.i_[1];
- FCL_REAL v10 = i_[1] * other.i_[0];
- if(v01 < v10)
- {
- i_[0] = v01;
- i_[1] = v00;
- }
- else
- {
- i_[0] = v10;
- i_[1] = v00;
- }
-
- return *this;
-}
-
-Interval Interval::operator / (const Interval& other) const
-{
- return *this * Interval(1.0 / other.i_[1], 1.0 / other.i_[0]);
-}
-
-Interval& Interval::operator /= (const Interval& other)
-{
- *this *= Interval(1.0 / other.i_[1], 1.0 / other.i_[0]);
- return *this;
-}
-
-void Interval::print() const
-{
- std::cout << "[" << i_[0] << ", " << i_[1] << "]" << std::endl;
-}
-
-}
diff --git a/src/ccd/interval_matrix.cpp b/src/ccd/interval_matrix.cpp
deleted file mode 100644
index 0c03f3bf24454781171eb2c58aed92cabfaabc00..0000000000000000000000000000000000000000
--- a/src/ccd/interval_matrix.cpp
+++ /dev/null
@@ -1,264 +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 Jia Pan */
-
-#include <hpp/fcl/ccd/interval_matrix.h>
-#include <iostream>
-
-namespace fcl
-{
-
-IMatrix3::IMatrix3() {}
-
-IMatrix3::IMatrix3(FCL_REAL v)
-{
- v_[0].setValue(v);
- v_[1].setValue(v);
- v_[2].setValue(v);
-}
-
-IMatrix3::IMatrix3(const Matrix3f& m)
-{
- v_[0] = m.getRow(0);
- v_[1] = m.getRow(1);
- v_[2] = m.getRow(2);
-}
-
-IMatrix3::IMatrix3(FCL_REAL m[3][3][2])
-{
- v_[0].setValue(m[0]);
- v_[1].setValue(m[1]);
- v_[2].setValue(m[2]);
-}
-
-IMatrix3::IMatrix3(FCL_REAL m[3][3])
-{
- v_[0].setValue(m[0]);
- v_[1].setValue(m[1]);
- v_[2].setValue(m[2]);
-}
-
-IMatrix3::IMatrix3(Interval m[3][3])
-{
- v_[0].setValue(m[0]);
- v_[1].setValue(m[1]);
- v_[2].setValue(m[2]);
-}
-
-IMatrix3::IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3)
-{
- v_[0] = v1;
- v_[1] = v2;
- v_[2] = v3;
-}
-
-void IMatrix3::setIdentity()
-{
- v_[0].setValue(1, 0, 0);
- v_[1].setValue(0, 1, 0);
- v_[2].setValue(0, 0, 1);
-}
-
-IVector3 IMatrix3::getColumn(size_t i) const
-{
- return IVector3(v_[0][i], v_[1][i], v_[2][i]);
-}
-
-const IVector3& IMatrix3::getRow(size_t i) const
-{
- return v_[i];
-}
-
-Vec3f IMatrix3::getColumnLow(size_t i) const
-{
- return Vec3f(v_[0][i][0], v_[1][i][0], v_[2][i][0]);
-}
-
-Vec3f IMatrix3::getRowLow(size_t i) const
-{
- return Vec3f(v_[i][0][0], v_[i][1][0], v_[i][2][0]);
-}
-
-Vec3f IMatrix3::getColumnHigh(size_t i) const
-{
- return Vec3f(v_[0][i][1], v_[1][i][1], v_[2][i][1]);
-}
-
-Vec3f IMatrix3::getRowHigh(size_t i) const
-{
- return Vec3f(v_[i][0][1], v_[i][1][1], v_[i][2][1]);
-}
-
-Matrix3f IMatrix3::getLow() const
-{
- return Matrix3f(v_[0][0][0], v_[0][1][0], v_[0][2][0],
- v_[1][0][0], v_[1][1][0], v_[1][2][0],
- v_[2][0][0], v_[2][1][0], v_[2][2][0]);
-}
-
-Matrix3f IMatrix3::getHigh() const
-{
- return Matrix3f(v_[0][0][1], v_[0][1][1], v_[0][2][1],
- v_[1][0][1], v_[1][1][1], v_[1][2][1],
- v_[2][0][1], v_[2][1][1], v_[2][2][1]);
-}
-
-IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
-{
- const Vec3f& mc0 = m.getColumn(0);
- const Vec3f& mc1 = m.getColumn(1);
- const Vec3f& mc2 = m.getColumn(2);
-
- return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
- IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
- IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
-}
-
-IVector3 IMatrix3::operator * (const Vec3f& v) const
-{
- return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
-}
-
-IVector3 IMatrix3::operator * (const IVector3& v) const
-{
- return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
-}
-
-IMatrix3 IMatrix3::operator * (const IMatrix3& m) const
-{
- const IVector3& mc0 = m.getColumn(0);
- const IVector3& mc1 = m.getColumn(1);
- const IVector3& mc2 = m.getColumn(2);
-
- return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
- IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
- IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
-}
-
-IMatrix3& IMatrix3::operator *= (const Matrix3f& m)
-{
- const Vec3f& mc0 = m.getColumn(0);
- const Vec3f& mc1 = m.getColumn(1);
- const Vec3f& mc2 = m.getColumn(2);
-
- v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
- v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
- v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
- return *this;
-}
-
-
-IMatrix3& IMatrix3::operator *= (const IMatrix3& m)
-{
- const IVector3& mc0 = m.getColumn(0);
- const IVector3& mc1 = m.getColumn(1);
- const IVector3& mc2 = m.getColumn(2);
-
- v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
- v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
- v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
- return *this;
-}
-
-IMatrix3 IMatrix3::operator + (const IMatrix3& m) const
-{
- return IMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]);
-}
-
-IMatrix3& IMatrix3::operator += (const IMatrix3& m)
-{
- v_[0] += m.v_[0];
- v_[1] += m.v_[1];
- v_[2] += m.v_[2];
- return *this;
-}
-
-IMatrix3 IMatrix3::operator - (const IMatrix3& m) const
-{
- return IMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]);
-}
-
-IMatrix3& IMatrix3::operator -= (const IMatrix3& m)
-{
- v_[0] -= m.v_[0];
- v_[1] -= m.v_[1];
- v_[2] -= m.v_[2];
- return *this;
-}
-
-IMatrix3& IMatrix3::rotationConstrain()
-{
- for(std::size_t i = 0; i < 3; ++i)
- {
- for(std::size_t j = 0; j < 3; ++j)
- {
- if(v_[i][j][0] < -1) v_[i][j][0] = -1;
- else if(v_[i][j][0] > 1) v_[i][j][0] = 1;
-
- if(v_[i][j][1] < -1) v_[i][j][1] = -1;
- else if(v_[i][j][1] > 1) v_[i][j][1] = 1;
- }
- }
-
- return *this;
-}
-
-void IMatrix3::print() const
-{
- std::cout << "[" << v_[0][0][0] << "," << v_[0][0][1] << "]" << " [" << v_[0][1][0] << "," << v_[0][1][1] << "]" << " [" << v_[0][2][0] << "," << v_[0][2][1] << "]" << std::endl;
- std::cout << "[" << v_[1][0][0] << "," << v_[1][0][1] << "]" << " [" << v_[1][1][0] << "," << v_[1][1][1] << "]" << " [" << v_[1][2][0] << "," << v_[1][2][1] << "]" << std::endl;
- std::cout << "[" << v_[2][0][0] << "," << v_[2][0][1] << "]" << " [" << v_[2][1][0] << "," << v_[2][1][1] << "]" << " [" << v_[2][2][0] << "," << v_[2][2][1] << "]" << std::endl;
-}
-
-IMatrix3 rotationConstrain(const IMatrix3& m)
-{
- IMatrix3 res;
- for(std::size_t i = 0; i < 3; ++i)
- {
- for(std::size_t j = 0; j < 3; ++j)
- {
- if(m(i, j)[0] < -1) res(i, j)[0] = -1;
- else if(m(i, j)[0] > 1) res(i, j)[0] = 1;
-
- if(m(i, j)[1] < -1) res(i, j)[1] = -1;
- else if(m(i, j)[1] > 1) res(i, j)[1] = 1;
- }
- }
-
- return res;
-}
-
-}
diff --git a/src/ccd/interval_vector.cpp b/src/ccd/interval_vector.cpp
deleted file mode 100644
index b0c8e650174e13139acc5b4019ef2efb0e7c1384..0000000000000000000000000000000000000000
--- a/src/ccd/interval_vector.cpp
+++ /dev/null
@@ -1,212 +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 Jia Pan */
-
-#include <hpp/fcl/ccd/interval_vector.h>
-#include <iostream>
-
-namespace fcl
-{
-
-
-IVector3::IVector3() {}
-
-IVector3::IVector3(FCL_REAL v) { setValue(v); }
-
-IVector3::IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z) { setValue(x, y, z); }
-
-IVector3::IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu)
-{
- setValue(xl, xu, yl, yu, zl, zu);
-}
-
-IVector3::IVector3(FCL_REAL v[3][2]) { setValue(v); }
-
-IVector3::IVector3(Interval v[3]) { setValue(v); }
-
-IVector3::IVector3(const Interval& v1, const Interval& v2, const Interval& v3) { setValue(v1, v2, v3); }
-
-IVector3::IVector3(const Vec3f& v) { setValue(v); }
-
-void IVector3::setZero() { setValue((FCL_REAL)0.0); }
-
-IVector3 IVector3::operator + (const IVector3& other) const
-{
- return IVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]);
-}
-
-IVector3& IVector3::operator += (const IVector3& other)
-{
- i_[0] += other[0];
- i_[1] += other[1];
- i_[2] += other[2];
- return *this;
-}
-
-IVector3 IVector3::operator - (const IVector3& other) const
-{
- return IVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]);
-}
-
-IVector3& IVector3::operator -= (const IVector3& other)
-{
- i_[0] -= other[0];
- i_[1] -= other[1];
- i_[2] -= other[2];
- return *this;
-}
-
-Interval IVector3::dot(const IVector3& other) const
-{
- return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2];
-}
-
-IVector3 IVector3::cross(const IVector3& other) const
-{
- return IVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1],
- i_[2] * other.i_[0] - i_[0] * other.i_[2],
- i_[0] * other.i_[1] - i_[1] * other.i_[0]);
-}
-
-Interval IVector3::dot(const Vec3f& other) const
-{
- return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
-}
-
-IVector3 IVector3::cross(const Vec3f& other) const
-{
- return IVector3(i_[1] * other[2] - i_[2] * other[1],
- i_[2] * other[0] - i_[0] * other[2],
- i_[0] * other[1] - i_[1] * other[0]);
-}
-
-FCL_REAL IVector3::volumn() const
-{
- return i_[0].diameter() * i_[1].diameter() * i_[2].diameter();
-}
-
-void IVector3::print() const
-{
- std::cout << "[" << i_[0][0] << "," << i_[0][1] << "]" << std::endl;
- std::cout << "[" << i_[1][0] << "," << i_[1][1] << "]" << std::endl;
- std::cout << "[" << i_[2][0] << "," << i_[2][1] << "]" << std::endl;
-}
-
-Vec3f IVector3::center() const
-{
- return Vec3f(i_[0].center(), i_[1].center(), i_[2].center());
-}
-
-void IVector3::bound(const IVector3& v)
-{
- if(v[0][0] < i_[0][0]) i_[0][0] = v[0][0];
- if(v[1][0] < i_[1][0]) i_[1][0] = v[1][0];
- if(v[2][0] < i_[2][0]) i_[2][0] = v[2][0];
-
- if(v[0][1] > i_[0][1]) i_[0][1] = v[0][1];
- if(v[1][1] > i_[1][1]) i_[1][1] = v[1][1];
- if(v[2][1] > i_[2][1]) i_[2][1] = v[2][1];
-}
-
-void IVector3::bound(const Vec3f& v)
-{
- if(v[0] < i_[0][0]) i_[0][0] = v[0];
- if(v[1] < i_[1][0]) i_[1][0] = v[1];
- if(v[2] < i_[2][0]) i_[2][0] = v[2];
-
- if(v[0] > i_[0][1]) i_[0][1] = v[0];
- if(v[1] > i_[1][1]) i_[1][1] = v[1];
- if(v[2] > i_[2][1]) i_[2][1] = v[2];
-}
-
-
-IVector3 bound(const IVector3& i, const IVector3& v)
-{
- IVector3 res(i);
- if(v[0][0] < res.i_[0][0]) res.i_[0][0] = v[0][0];
- if(v[1][0] < res.i_[1][0]) res.i_[1][0] = v[1][0];
- if(v[2][0] < res.i_[2][0]) res.i_[2][0] = v[2][0];
-
- if(v[0][1] > res.i_[0][1]) res.i_[0][1] = v[0][1];
- if(v[1][1] > res.i_[1][1]) res.i_[1][1] = v[1][1];
- if(v[2][1] > res.i_[2][1]) res.i_[2][1] = v[2][1];
-
- return res;
-}
-
-IVector3 bound(const IVector3& i, const Vec3f& v)
-{
- IVector3 res(i);
- if(v[0] < res.i_[0][0]) res.i_[0][0] = v[0];
- if(v[1] < res.i_[1][0]) res.i_[1][0] = v[1];
- if(v[2] < res.i_[2][0]) res.i_[2][0] = v[2];
-
- if(v[0] > res.i_[0][1]) res.i_[0][1] = v[0];
- if(v[1] > res.i_[1][1]) res.i_[1][1] = v[1];
- if(v[2] > res.i_[2][1]) res.i_[2][1] = v[2];
-
- return res;
-}
-
-bool IVector3::overlap(const IVector3& v) const
-{
- if(v[0][1] < i_[0][0]) return false;
- if(v[1][1] < i_[1][0]) return false;
- if(v[2][1] < i_[2][0]) return false;
-
- if(v[0][0] > i_[0][1]) return false;
- if(v[1][0] > i_[1][1]) return false;
- if(v[2][0] > i_[2][1]) return false;
-
- return true;
-}
-
-bool IVector3::contain(const IVector3& v) const
-{
- if(v[0][0] < i_[0][0]) return false;
- if(v[1][0] < i_[1][0]) return false;
- if(v[2][0] < i_[2][0]) return false;
-
- if(v[0][1] > i_[0][1]) return false;
- if(v[1][1] > i_[1][1]) return false;
- if(v[2][1] > i_[2][1]) return false;
-
- return true;
-}
-
-
-
-}
diff --git a/src/ccd/motion.cpp b/src/ccd/motion.cpp
deleted file mode 100644
index a0fb85369a29b00815075db8e1f7c54e30d9947a..0000000000000000000000000000000000000000
--- a/src/ccd/motion.cpp
+++ /dev/null
@@ -1,545 +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 Jia Pan */
-
-
-#include <hpp/fcl/ccd/motion.h>
-
-namespace fcl
-{
-
-template<>
-FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const SplineMotion& motion) const
-{
- FCL_REAL T_bound = motion.computeTBound(n);
- FCL_REAL tf_t = motion.getCurrentTime();
-
- Vec3f c1 = bv.Tr;
- Vec3f c2 = bv.Tr + bv.axis[0] * bv.l[0];
- Vec3f c3 = bv.Tr + bv.axis[1] * bv.l[1];
- Vec3f c4 = bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1];
-
- FCL_REAL tmp;
- // max_i |c_i * n|
- FCL_REAL cn_max = std::fabs(c1.dot(n));
- tmp = std::fabs(c2.dot(n));
- if(tmp > cn_max) cn_max = tmp;
- tmp = std::fabs(c3.dot(n));
- if(tmp > cn_max) cn_max = tmp;
- tmp = std::fabs(c4.dot(n));
- if(tmp > cn_max) cn_max = tmp;
-
- // max_i ||c_i||
- FCL_REAL cmax = c1.sqrLength();
- tmp = c2.sqrLength();
- if(tmp > cmax) cmax = tmp;
- tmp = c3.sqrLength();
- if(tmp > cmax) cmax = tmp;
- tmp = c4.sqrLength();
- if(tmp > cmax) cmax = tmp;
- cmax = sqrt(cmax);
-
- // max_i ||c_i x n||
- FCL_REAL cxn_max = (c1.cross(n)).sqrLength();
- tmp = (c2.cross(n)).sqrLength();
- if(tmp > cxn_max) cxn_max = tmp;
- tmp = (c3.cross(n)).sqrLength();
- if(tmp > cxn_max) cxn_max = tmp;
- tmp = (c4.cross(n)).sqrLength();
- if(tmp > cxn_max) cxn_max = tmp;
- cxn_max = sqrt(cxn_max);
-
- FCL_REAL dWdW_max = motion.computeDWMax();
- FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
-
- FCL_REAL R_bound = 2 * (cn_max + cmax + cxn_max + 3 * bv.r) * ratio;
-
-
- // std::cout << R_bound << " " << T_bound << std::endl;
-
- return R_bound + T_bound;
-}
-
-
-FCL_REAL TriangleMotionBoundVisitor::visit(const SplineMotion& motion) const
-{
- FCL_REAL T_bound = motion.computeTBound(n);
- FCL_REAL tf_t = motion.getCurrentTime();
-
- FCL_REAL R_bound = std::fabs(a.dot(n)) + a.length() + (a.cross(n)).length();
- FCL_REAL R_bound_tmp = std::fabs(b.dot(n)) + b.length() + (b.cross(n)).length();
- if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
- R_bound_tmp = std::fabs(c.dot(n)) + c.length() + (c.cross(n)).length();
- if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
-
- FCL_REAL dWdW_max = motion.computeDWMax();
- FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
-
- R_bound *= 2 * ratio;
-
- // std::cout << R_bound << " " << T_bound << std::endl;
-
- return R_bound + T_bound;
-}
-
-SplineMotion::SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3,
- const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3) : MotionBase()
-{
- Td[0] = Td0;
- Td[1] = Td1;
- Td[2] = Td2;
- Td[3] = Td3;
-
- Rd[0] = Rd0;
- Rd[1] = Rd1;
- Rd[2] = Rd2;
- Rd[3] = Rd3;
-
- Rd0Rd0 = Rd[0].dot(Rd[0]);
- Rd0Rd1 = Rd[0].dot(Rd[1]);
- Rd0Rd2 = Rd[0].dot(Rd[2]);
- Rd0Rd3 = Rd[0].dot(Rd[3]);
- Rd1Rd1 = Rd[1].dot(Rd[1]);
- Rd1Rd2 = Rd[1].dot(Rd[2]);
- Rd1Rd3 = Rd[1].dot(Rd[3]);
- Rd2Rd2 = Rd[2].dot(Rd[2]);
- Rd2Rd3 = Rd[2].dot(Rd[3]);
- Rd3Rd3 = Rd[3].dot(Rd[3]);
-
- TA = Td[1] * 3 - Td[2] * 3 + Td[3] - Td[0];
- TB = (Td[0] - Td[1] * 2 + Td[2]) * 3;
- TC = (Td[2] - Td[0]) * 3;
-
- RA = Rd[1] * 3 - Rd[2] * 3 + Rd[3] - Rd[0];
- RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3;
- RC = (Rd[2] - Rd[0]) * 3;
-
- integrate(0.0);
-}
-
-bool SplineMotion::integrate(double dt) const
-{
- if(dt > 1) dt = 1;
-
- Vec3f cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
- Vec3f cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
- FCL_REAL cur_angle = cur_w.length();
- cur_w.normalize();
-
- Quaternion3f cur_q;
- cur_q.fromAxisAngle(cur_w, cur_angle);
-
- tf.setTransform(cur_q, cur_T);
-
- tf_t = dt;
-
- return true;
-}
-
-
-FCL_REAL SplineMotion::computeTBound(const Vec3f& n) const
-{
- FCL_REAL Ta = TA.dot(n);
- FCL_REAL Tb = TB.dot(n);
- FCL_REAL Tc = TC.dot(n);
-
- std::vector<FCL_REAL> T_potential;
- T_potential.push_back(tf_t);
- T_potential.push_back(1);
- if(Tb * Tb - 3 * Ta * Tc >= 0)
- {
- if(Ta == 0)
- {
- if(Tb != 0)
- {
- FCL_REAL tmp = -Tc / (2 * Tb);
- if(tmp < 1 && tmp > tf_t)
- T_potential.push_back(tmp);
- }
- }
- else
- {
- FCL_REAL tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc);
- FCL_REAL tmp1 = (-Tb + tmp_delta) / (3 * Ta);
- FCL_REAL tmp2 = (-Tb - tmp_delta) / (3 * Ta);
- if(tmp1 < 1 && tmp1 > tf_t)
- T_potential.push_back(tmp1);
- if(tmp2 < 1 && tmp2 > tf_t)
- T_potential.push_back(tmp2);
- }
- }
-
- FCL_REAL T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0];
- for(unsigned int i = 1; i < T_potential.size(); ++i)
- {
- FCL_REAL T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i];
- if(T_bound_tmp > T_bound) T_bound = T_bound_tmp;
- }
-
-
- FCL_REAL cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t;
-
- T_bound -= cur_delta;
- T_bound /= 6.0;
-
- return T_bound;
-}
-
-
-FCL_REAL SplineMotion::computeDWMax() const
-{
- // first compute ||w'||
- int a00[5] = {1,-4,6,-4,1};
- int a01[5] = {-3,10,-11,4,0};
- int a02[5] = {3,-8,6,0,-1};
- int a03[5] = {-1,2,-1,0,0};
- int a11[5] = {9,-24,16,0,0};
- int a12[5] = {-9,18,-5,-4,0};
- int a13[5] = {3,-4,0,0,0};
- int a22[5] = {9,-12,-2,4,1};
- int a23[5] = {-3,2,1,0,0};
- int a33[5] = {1,0,0,0,0};
-
- FCL_REAL a[5];
-
- for(int i = 0; i < 5; ++i)
- {
- a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i]
- + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i]
- + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i]
- + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i];
- a[i] /= 4.0;
- }
-
- // compute polynomial for ||w'||'
- int da00[4] = {4,-12,12,-4};
- int da01[4] = {-12,30,-22,4};
- int da02[4] = {12,-24,12,0};
- int da03[4] = {-4,6,-2,0};
- int da11[4] = {36,-72,32,0};
- int da12[4] = {-36,54,-10,-4};
- int da13[4] = {12,-12,0,0};
- int da22[4] = {36,-36,-4,4};
- int da23[4] = {-12,6,2,0};
- int da33[4] = {4,0,0,0};
-
- FCL_REAL da[4];
- for(int i = 0; i < 4; ++i)
- {
- da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i]
- + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i]
- + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i]
- + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i];
- da[i] /= 4.0;
- }
-
- FCL_REAL roots[3];
-
- int root_num = PolySolver::solveCubic(da, roots);
-
- FCL_REAL dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4];
- FCL_REAL dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
- if(dWdW_max < dWdW_1) dWdW_max = dWdW_1;
- for(int i = 0; i < root_num; ++i)
- {
- FCL_REAL v = roots[i];
-
- if(v >= tf_t && v <= 1)
- {
- FCL_REAL value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4];
- if(value > dWdW_max) dWdW_max = value;
- }
- }
-
- return sqrt(dWdW_max);
-}
-
-FCL_REAL SplineMotion::getWeight0(FCL_REAL t) const
-{
- return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0;
-}
-
-FCL_REAL SplineMotion::getWeight1(FCL_REAL t) const
-{
- return (4 - 6 * t * t + 3 * t * t * t) / 6.0;
-}
-
-FCL_REAL SplineMotion::getWeight2(FCL_REAL t) const
-{
- return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0;
-}
-
-FCL_REAL SplineMotion::getWeight3(FCL_REAL t) const
-{
- return t * t * t / 6.0;
-}
-
-
-
-
-/// @brief Compute the motion bound for a bounding volume along a given direction n
-/// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized)
-/// and ci are the endpoints of the generator primitives of RSS.
-/// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
-template<>
-FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const
-{
- Transform3f tf;
- motion.getCurrentTransform(tf);
-
- const Vec3f& axis = motion.getAxis();
- FCL_REAL linear_vel = motion.getLinearVelocity();
- FCL_REAL angular_vel = motion.getAngularVelocity();
- const Vec3f& p = motion.getAxisOrigin();
-
- FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
- FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
-
- c_proj_max = sqrt(c_proj_max);
-
- FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
- FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
- FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();
-
- FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
-
- return mu;
-}
-
-FCL_REAL TriangleMotionBoundVisitor::visit(const ScrewMotion& motion) const
-{
- Transform3f tf;
- motion.getCurrentTransform(tf);
-
- const Vec3f& axis = motion.getAxis();
- FCL_REAL linear_vel = motion.getLinearVelocity();
- FCL_REAL angular_vel = motion.getAngularVelocity();
- const Vec3f& p = motion.getAxisOrigin();
-
- FCL_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
- FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength();
- if(tmp > proj_max) proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength();
- if(tmp > proj_max) proj_max = tmp;
-
- proj_max = std::sqrt(proj_max);
-
- FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
- FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
- FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
-
- return mu;
-}
-
-
-
-
-
-/// @brief Compute the motion bound for a bounding volume along a given direction n
-/// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized)
-/// and ci are the endpoints of the generator primitives of RSS.
-/// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
-template<>
-FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const
-{
- Transform3f tf;
- motion.getCurrentTransform(tf);
-
- const Vec3f& reference_p = motion.getReferencePoint();
- const Vec3f& angular_axis = motion.getAngularAxis();
- FCL_REAL angular_vel = motion.getAngularVelocity();
- const Vec3f& linear_vel = motion.getLinearVelocity();
-
- FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
- FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
-
- c_proj_max = std::sqrt(c_proj_max);
-
- FCL_REAL v_dot_n = linear_vel.dot(n);
- FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
-
- return mu;
-}
-
-/// @brief Compute the motion bound for a triangle along a given direction n
-/// according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity
-/// and ci are the triangle vertex coordinates.
-/// Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
-FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const
-{
- Transform3f tf;
- motion.getCurrentTransform(tf);
-
- const Vec3f& reference_p = motion.getReferencePoint();
- const Vec3f& angular_axis = motion.getAngularAxis();
- FCL_REAL angular_vel = motion.getAngularVelocity();
- const Vec3f& linear_vel = motion.getLinearVelocity();
-
- FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
- FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > proj_max) proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
- if(tmp > proj_max) proj_max = tmp;
-
- proj_max = std::sqrt(proj_max);
-
- FCL_REAL v_dot_n = linear_vel.dot(n);
- FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
-
- return mu;
-}
-
-InterpMotion::InterpMotion() : MotionBase()
-{
- // Default angular velocity is zero
- angular_axis.setValue(1, 0, 0);
- angular_vel = 0;
-
- // Default reference point is local zero point
-
- // Default linear velocity is zero
-}
-
-InterpMotion::InterpMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2) : MotionBase(),
- tf1(R1, T1),
- tf2(R2, T2),
- tf(tf1)
-{
- // Compute the velocities for the motion
- computeVelocity();
-}
-
-
-InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) : MotionBase(),
- tf1(tf1_),
- tf2(tf2_),
- tf(tf1)
-{
- // Compute the velocities for the motion
- computeVelocity();
-}
-
-InterpMotion::InterpMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2,
- const Vec3f& O) : MotionBase(),
- tf1(R1, T1),
- tf2(R2, T2),
- tf(tf1),
- reference_p(O)
-{
- // Compute the velocities for the motion
- computeVelocity();
-}
-
-InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O) : MotionBase(),
- tf1(tf1_),
- tf2(tf2_),
- tf(tf1),
- reference_p(O)
-{
-}
-
-bool InterpMotion::integrate(double dt) const
-{
- if(dt > 1) dt = 1;
-
- tf.setQuatRotation(absoluteRotation(dt));
- tf.setTranslation(linear_vel * dt + tf1.transform(reference_p) - tf.getQuatRotation().transform(reference_p));
-
- return true;
-}
-
-
-void InterpMotion::computeVelocity()
-{
- linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p);
- Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
- deltaq.toAxisAngle(angular_axis, angular_vel);
- if(angular_vel < 0)
- {
- angular_vel = -angular_vel;
- angular_axis = -angular_axis;
- }
-}
-
-
-Quaternion3f InterpMotion::deltaRotation(FCL_REAL dt) const
-{
- Quaternion3f res;
- res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel));
- return res;
-}
-
-Quaternion3f InterpMotion::absoluteRotation(FCL_REAL dt) const
-{
- Quaternion3f delta_t = deltaRotation(dt);
- return delta_t * tf1.getQuatRotation();
-}
-
-
-/// @brief Compute the motion bound for a bounding volume along a given direction n
-template<>
-FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const TranslationMotion& motion) const
-{
- return motion.getVelocity().dot(n);
-}
-
-/// @brief Compute the motion bound for a triangle along a given direction n
-FCL_REAL TriangleMotionBoundVisitor::visit(const TranslationMotion& motion) const
-{
- return motion.getVelocity().dot(n);
-}
-
-template class TBVMotionBoundVisitor<RSS>;
-
-}
diff --git a/src/ccd/taylor_matrix.cpp b/src/ccd/taylor_matrix.cpp
deleted file mode 100644
index ecde6afd13a2a52c9436f389c4e9f2ea7c617951..0000000000000000000000000000000000000000
--- a/src/ccd/taylor_matrix.cpp
+++ /dev/null
@@ -1,439 +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 Jia Pan */
-
-#include <hpp/fcl/ccd/taylor_matrix.h>
-
-namespace fcl
-{
-
-TMatrix3::TMatrix3()
-{
-}
-
-TMatrix3::TMatrix3(const boost::shared_ptr<TimeInterval>& time_interval)
-{
- setTimeInterval(time_interval);
-}
-
-TMatrix3::TMatrix3(TaylorModel m[3][3])
-{
- v_[0] = TVector3(m[0]);
- v_[1] = TVector3(m[1]);
- v_[2] = TVector3(m[2]);
-}
-
-TMatrix3::TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3)
-{
- v_[0] = v1;
- v_[1] = v2;
- v_[2] = v3;
-}
-
-
-TMatrix3::TMatrix3(const Matrix3f& m, const boost::shared_ptr<TimeInterval>& time_interval)
-{
- v_[0] = TVector3(m.getRow(0), time_interval);
- v_[1] = TVector3(m.getRow(1), time_interval);
- v_[2] = TVector3(m.getRow(2), time_interval);
-}
-
-void TMatrix3::setIdentity()
-{
- setZero();
- v_[0][0].coeff(0) = 1;
- v_[1][1].coeff(0) = 1;
- v_[2][2].coeff(0) = 1;
-
-}
-
-void TMatrix3::setZero()
-{
- v_[0].setZero();
- v_[1].setZero();
- v_[2].setZero();
-}
-
-TVector3 TMatrix3::getColumn(size_t i) const
-{
- return TVector3(v_[0][i], v_[1][i], v_[2][i]);
-}
-
-const TVector3& TMatrix3::getRow(size_t i) const
-{
- return v_[i];
-}
-
-const TaylorModel& TMatrix3::operator () (size_t i, size_t j) const
-{
- return v_[i][j];
-}
-
-TaylorModel& TMatrix3::operator () (size_t i, size_t j)
-{
- return v_[i][j];
-}
-
-TMatrix3 TMatrix3::operator * (const Matrix3f& m) const
-{
- const Vec3f& mc0 = m.getColumn(0);
- const Vec3f& mc1 = m.getColumn(1);
- const Vec3f& mc2 = m.getColumn(2);
-
- return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
- TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
- TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
-}
-
-
-TVector3 TMatrix3::operator * (const Vec3f& v) const
-{
- return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
-}
-
-TVector3 TMatrix3::operator * (const TVector3& v) const
-{
- return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
-}
-
-TMatrix3 TMatrix3::operator * (const TMatrix3& m) const
-{
- const TVector3& mc0 = m.getColumn(0);
- const TVector3& mc1 = m.getColumn(1);
- const TVector3& mc2 = m.getColumn(2);
-
- return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
- TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
- TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
-}
-
-TMatrix3 TMatrix3::operator * (const TaylorModel& d) const
-{
- return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d);
-}
-
-TMatrix3 TMatrix3::operator * (FCL_REAL d) const
-{
- return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d);
-}
-
-
-TMatrix3& TMatrix3::operator *= (const Matrix3f& m)
-{
- const Vec3f& mc0 = m.getColumn(0);
- const Vec3f& mc1 = m.getColumn(1);
- const Vec3f& mc2 = m.getColumn(2);
-
- v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
- v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
- v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
- return *this;
-}
-
-TMatrix3& TMatrix3::operator *= (const TMatrix3& m)
-{
- const TVector3& mc0 = m.getColumn(0);
- const TVector3& mc1 = m.getColumn(1);
- const TVector3& mc2 = m.getColumn(2);
-
- v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
- v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
- v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
- return *this;
-}
-
-TMatrix3& TMatrix3::operator *= (const TaylorModel& d)
-{
- v_[0] *= d;
- v_[1] *= d;
- v_[2] *= d;
- return *this;
-}
-
-TMatrix3& TMatrix3::operator *= (FCL_REAL d)
-{
- v_[0] *= d;
- v_[1] *= d;
- v_[2] *= d;
- return *this;
-}
-
-TMatrix3 TMatrix3::operator + (const TMatrix3& m) const
-{
- return TMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]);
-}
-
-TMatrix3& TMatrix3::operator += (const TMatrix3& m)
-{
- v_[0] += m.v_[0];
- v_[1] += m.v_[1];
- v_[2] += m.v_[2];
- return *this;
-}
-
-TMatrix3 TMatrix3::operator + (const Matrix3f& m) const
-{
- TMatrix3 res = *this;
- res += m;
- return res;
-}
-
-TMatrix3& TMatrix3::operator += (const Matrix3f& m)
-{
- for(std::size_t i = 0; i < 3; ++i)
- {
- for(std::size_t j = 0; j < 3; ++j)
- v_[i][j] += m(i, j);
- }
-
- return *this;
-}
-
-TMatrix3 TMatrix3::operator - (const TMatrix3& m) const
-{
- return TMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]);
-}
-
-TMatrix3& TMatrix3::operator -= (const TMatrix3& m)
-{
- v_[0] -= m.v_[0];
- v_[1] -= m.v_[1];
- v_[2] -= m.v_[2];
- return *this;
-}
-
-TMatrix3 TMatrix3::operator - (const Matrix3f& m) const
-{
- TMatrix3 res = *this;
- res -= m;
- return res;
-}
-
-TMatrix3& TMatrix3::operator -= (const Matrix3f& m)
-{
- for(std::size_t i = 0; i < 3; ++i)
- {
- for(std::size_t j = 0; j < 3; ++j)
- v_[i][j] -= m(i, j);
- }
-
- return *this;
-}
-
-TMatrix3 TMatrix3::operator - () const
-{
- return TMatrix3(-v_[0], -v_[1], -v_[2]);
-}
-
-
-void TMatrix3::print() const
-{
- getColumn(0).print();
- getColumn(1).print();
- getColumn(2).print();
-}
-
-IMatrix3 TMatrix3::getBound() const
-{
- return IMatrix3(v_[0].getBound(), v_[1].getBound(), v_[2].getBound());
-}
-
-IMatrix3 TMatrix3::getBound(FCL_REAL l, FCL_REAL r) const
-{
- return IMatrix3(v_[0].getBound(l, r), v_[1].getBound(l, r), v_[2].getBound(l, r));
-}
-
-IMatrix3 TMatrix3::getBound(FCL_REAL t) const
-{
- return IMatrix3(v_[0].getBound(t), v_[1].getBound(t), v_[2].getBound(t));
-}
-
-IMatrix3 TMatrix3::getTightBound() const
-{
- return IMatrix3(v_[0].getTightBound(), v_[1].getTightBound(), v_[2].getTightBound());
-}
-
-IMatrix3 TMatrix3::getTightBound(FCL_REAL l, FCL_REAL r) const
-{
- return IMatrix3(v_[0].getTightBound(l, r), v_[1].getTightBound(l, r), v_[2].getTightBound(l, r));
-}
-
-
-FCL_REAL TMatrix3::diameter() const
-{
- FCL_REAL d = 0;
-
- FCL_REAL tmp = v_[0][0].remainder().diameter();
- if(tmp > d) d = tmp;
- tmp = v_[0][1].remainder().diameter();
- if(tmp > d) d = tmp;
- tmp = v_[0][2].remainder().diameter();
- if(tmp > d) d = tmp;
-
- tmp = v_[1][0].remainder().diameter();
- if(tmp > d) d = tmp;
- tmp = v_[1][1].remainder().diameter();
- if(tmp > d) d = tmp;
- tmp = v_[1][2].remainder().diameter();
- if(tmp > d) d = tmp;
-
- tmp = v_[2][0].remainder().diameter();
- if(tmp > d) d = tmp;
- tmp = v_[2][1].remainder().diameter();
- if(tmp > d) d = tmp;
- tmp = v_[2][2].remainder().diameter();
- if(tmp > d) d = tmp;
-
- return d;
-}
-
-void TMatrix3::setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
-{
- v_[0].setTimeInterval(time_interval);
- v_[1].setTimeInterval(time_interval);
- v_[2].setTimeInterval(time_interval);
-}
-
-void TMatrix3::setTimeInterval(FCL_REAL l, FCL_REAL r)
-{
- v_[0].setTimeInterval(l, r);
- v_[1].setTimeInterval(l, r);
- v_[2].setTimeInterval(l, r);
-}
-
-const boost::shared_ptr<TimeInterval>& TMatrix3::getTimeInterval() const
-{
- return v_[0].getTimeInterval();
-}
-
-TMatrix3& TMatrix3::rotationConstrain()
-{
- for(std::size_t i = 0; i < 3; ++i)
- {
- for(std::size_t j = 0; j < 3; ++j)
- {
- if(v_[i][j].remainder()[0] < -1) v_[i][j].remainder()[0] = -1;
- else if(v_[i][j].remainder()[0] > 1) v_[i][j].remainder()[0] = 1;
-
- if(v_[i][j].remainder()[1] < -1) v_[i][j].remainder()[1] = -1;
- else if(v_[i][j].remainder()[1] > 1) v_[i][j].remainder()[1] = 1;
-
- if((v_[i][j].remainder()[0] == -1) && (v_[i][j].remainder()[1] == 1))
- {
- v_[i][j].coeff(0) = 0;
- v_[i][j].coeff(1) = 0;
- v_[i][j].coeff(2) = 0;
- v_[i][j].coeff(3) = 0;
- }
- }
- }
-
- return *this;
-}
-
-
-TMatrix3 rotationConstrain(const TMatrix3& m)
-{
- TMatrix3 res;
-
- for(std::size_t i = 0; i < 3; ++i)
- {
- for(std::size_t j = 0; j < 3; ++j)
- {
- if(m(i, j).remainder()[0] < -1) res(i, j).remainder()[0] = -1;
- else if(m(i, j).remainder()[0] > 1) res(i, j).remainder()[0] = 1;
-
- if(m(i, j).remainder()[1] < -1) res(i, j).remainder()[1] = -1;
- else if(m(i, j).remainder()[1] > 1) res(i, j).remainder()[1] = 1;
-
- if((m(i, j).remainder()[0] == -1) && (m(i, j).remainder()[1] == 1))
- {
- res(i, j).coeff(0) = 0;
- res(i, j).coeff(1) = 0;
- res(i, j).coeff(2) = 0;
- res(i, j).coeff(3) = 0;
- }
- }
- }
-
- return res;
-}
-
-TMatrix3 operator * (const Matrix3f& m, const TaylorModel& a)
-{
- TMatrix3 res(a.getTimeInterval());
- res(0, 0) = a * m(0, 0);
- res(0, 1) = a * m(0, 1);
- res(0, 1) = a * m(0, 2);
-
- res(1, 0) = a * m(1, 0);
- res(1, 1) = a * m(1, 1);
- res(1, 1) = a * m(1, 2);
-
- res(2, 0) = a * m(2, 0);
- res(2, 1) = a * m(2, 1);
- res(2, 1) = a * m(2, 2);
-
- return res;
-}
-
-TMatrix3 operator * (const TaylorModel& a, const Matrix3f& m)
-{
- return m * a;
-}
-
-TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m)
-{
- return m * a;
-}
-
-TMatrix3 operator * (FCL_REAL d, const TMatrix3& m)
-{
- return m * d;
-}
-
-TMatrix3 operator + (const Matrix3f& m1, const TMatrix3& m2)
-{
- return m2 + m1;
-}
-
-TMatrix3 operator - (const Matrix3f& m1, const TMatrix3& m2)
-{
- return -m2 + m1;
-}
-
-
-}
diff --git a/src/ccd/taylor_model.cpp b/src/ccd/taylor_model.cpp
deleted file mode 100644
index b1b64468bffd010cd4c707eafee6fd9d2d70155b..0000000000000000000000000000000000000000
--- a/src/ccd/taylor_model.cpp
+++ /dev/null
@@ -1,506 +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 Jia Pan */
-
-#include <hpp/fcl/ccd/taylor_model.h>
-#include <cassert>
-#include <iostream>
-#include <cmath>
-#include <boost/math/constants/constants.hpp>
-
-
-namespace fcl
-{
-
-TaylorModel::TaylorModel()
-{
- coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
-}
-
-TaylorModel::TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
-{
- coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
-}
-
-TaylorModel::TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
-{
- coeffs_[0] = coeff;
- coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0;
-}
-
-TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
-{
- coeffs_[0] = coeffs[0];
- coeffs_[1] = coeffs[1];
- coeffs_[2] = coeffs[2];
- coeffs_[3] = coeffs[3];
-
- r_ = r;
-}
-
-TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
-{
- coeffs_[0] = c0;
- coeffs_[1] = c1;
- coeffs_[2] = c2;
- coeffs_[3] = c3;
-
- r_ = r;
-}
-
-TaylorModel TaylorModel::operator + (FCL_REAL d) const
-{
- return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
-}
-
-TaylorModel& TaylorModel::operator += (FCL_REAL d)
-{
- coeffs_[0] += d;
- return *this;
-}
-
-TaylorModel TaylorModel::operator - (FCL_REAL d) const
-{
- return TaylorModel(coeffs_[0] - d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
-}
-
-TaylorModel& TaylorModel::operator -= (FCL_REAL d)
-{
- coeffs_[0] -= d;
- return *this;
-}
-
-
-TaylorModel TaylorModel::operator + (const TaylorModel& other) const
-{
- assert(other.time_interval_ == time_interval_);
- return TaylorModel(coeffs_[0] + other.coeffs_[0], coeffs_[1] + other.coeffs_[1], coeffs_[2] + other.coeffs_[2], coeffs_[3] + other.coeffs_[3], r_ + other.r_, time_interval_);
-}
-
-TaylorModel TaylorModel::operator - (const TaylorModel& other) const
-{
- assert(other.time_interval_ == time_interval_);
- return TaylorModel(coeffs_[0] - other.coeffs_[0], coeffs_[1] - other.coeffs_[1], coeffs_[2] - other.coeffs_[2], coeffs_[3] - other.coeffs_[3], r_ - other.r_, time_interval_);
-}
-TaylorModel& TaylorModel::operator += (const TaylorModel& other)
-{
- assert(other.time_interval_ == time_interval_);
- coeffs_[0] += other.coeffs_[0];
- coeffs_[1] += other.coeffs_[1];
- coeffs_[2] += other.coeffs_[2];
- coeffs_[3] += other.coeffs_[3];
- r_ += other.r_;
- return *this;
-}
-
-TaylorModel& TaylorModel::operator -= (const TaylorModel& other)
-{
- assert(other.time_interval_ == time_interval_);
- coeffs_[0] -= other.coeffs_[0];
- coeffs_[1] -= other.coeffs_[1];
- coeffs_[2] -= other.coeffs_[2];
- coeffs_[3] -= other.coeffs_[3];
- r_ -= other.r_;
- return *this;
-}
-
-/// @brief Taylor model multiplication:
-/// f(t) = c0+c1*t+c2*t^2+c3*t^3+[a,b]
-/// g(t) = c0'+c1'*t+c2'*t^2+c3'*t^2+[c,d]
-/// f(t)g(t)= c0c0'+
-/// (c0c1'+c1c0')t+
-/// (c0c2'+c1c1'+c2c0')t^2+
-/// (c0c3'+c1c2'+c2c1'+c3c0')t^3+
-/// [a,b][c,d]+
-/// (c1c3'+c2c2'+c3c1')t^4+
-/// (c2c3'+c3c2')t^5+
-/// (c3c3')t^6+
-/// (c0+c1*t+c2*t^2+c3*t^3)[c,d]+
-/// (c0'+c1'*t+c2'*t^2+c3'*c^3)[a,b]
-TaylorModel TaylorModel::operator * (const TaylorModel& other) const
-{
- TaylorModel res(*this);
- res *= other;
- return res;
-}
-
-TaylorModel TaylorModel::operator * (FCL_REAL d) const
-{
- return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d, time_interval_);
-}
-
-TaylorModel& TaylorModel::operator *= (const TaylorModel& other)
-{
- assert(other.time_interval_ == time_interval_);
- register FCL_REAL c0, c1, c2, c3;
- register FCL_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3];
-
- const Interval& rb = other.r_;
-
- c0 = coeffs_[0] * c0b;
- c1 = coeffs_[0] * c1b + coeffs_[1] * c0b;
- c2 = coeffs_[0] * c2b + coeffs_[1] * c1b + coeffs_[2] * c0b;
- c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b;
-
- Interval remainder(r_ * rb);
- register FCL_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
- remainder += time_interval_->t4_ * tempVal;
-
- tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b;
- remainder += time_interval_->t5_ * tempVal;
-
- tempVal = coeffs_[3] * c3b;
- remainder += time_interval_->t6_ * tempVal;
-
- remainder += ((Interval(coeffs_[0]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]) * rb +
- (Interval(c0b) + time_interval_->t_ * c1b + time_interval_->t2_ * c2b + time_interval_->t3_ * c3b) * r_);
-
- coeffs_[0] = c0;
- coeffs_[1] = c1;
- coeffs_[2] = c2;
- coeffs_[3] = c3;
-
- r_ = remainder;
-
- return *this;
-}
-
-TaylorModel& TaylorModel::operator *= (FCL_REAL d)
-{
- coeffs_[0] *= d;
- coeffs_[1] *= d;
- coeffs_[2] *= d;
- coeffs_[3] *= d;
- r_ *= d;
- return *this;
-}
-
-
-TaylorModel TaylorModel::operator - () const
-{
- return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_);
-}
-
-void TaylorModel::print() const
-{
- std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl;
-}
-
-Interval TaylorModel::getBound(FCL_REAL t) const
-{
- return Interval(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_;
-}
-
-Interval TaylorModel::getBound(FCL_REAL t0, FCL_REAL t1) const
-{
- Interval t(t0, t1);
- Interval t2(t0 * t0, t1 * t1);
- Interval t3(t0 * t2[0], t1 * t2[1]);
-
- return Interval(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_;
-}
-
-Interval TaylorModel::getBound() const
-{
- return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3];
-}
-
-Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const
-{
- if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0];
- if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1];
-
- if(coeffs_[3] == 0)
- {
- register FCL_REAL a = -coeffs_[1] / (2 * coeffs_[2]);
- Interval polybounds;
- if(a <= t1 && a >= t0)
- {
- FCL_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
- register FCL_REAL t = t0;
- FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
- t = t1;
- FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
-
- FCL_REAL minQ = LQ, maxQ = RQ;
- if(LQ > RQ)
- {
- minQ = RQ;
- maxQ = LQ;
- }
-
- if(minQ > AQ) minQ = AQ;
- if(maxQ < AQ) maxQ = AQ;
-
- polybounds.setValue(minQ, maxQ);
- }
- else
- {
- register FCL_REAL t = t0;
- FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
- t = t1;
- FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
-
- if(LQ > RQ) polybounds.setValue(RQ, LQ);
- else polybounds.setValue(LQ, RQ);
- }
-
- return polybounds + r_;
- }
- else
- {
- register FCL_REAL t = t0;
- FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
- t = t1;
- FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
-
- if(LQ > RQ)
- {
- FCL_REAL tmp = LQ;
- LQ = RQ;
- RQ = tmp;
- }
-
- // derivative: c1+2*c2*t+3*c3*t^2
-
- FCL_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
- if(delta < 0)
- return Interval(LQ, RQ) + r_;
-
- FCL_REAL r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
- FCL_REAL r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
-
- if(r1 <= t1 && r1 >= t0)
- {
- FCL_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
- if(Q < LQ) LQ = Q;
- else if(Q > RQ) RQ = Q;
- }
-
- if(r2 <= t1 && r2 >= t0)
- {
- FCL_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
- if(Q < LQ) LQ = Q;
- else if(Q > RQ) RQ = Q;
- }
-
- return Interval(LQ, RQ) + r_;
- }
-}
-
-Interval TaylorModel::getTightBound() const
-{
- return getTightBound(time_interval_->t_[0], time_interval_->t_[1]);
-}
-
-void TaylorModel::setZero()
-{
- coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
- r_.setValue(0);
-}
-
-TaylorModel operator * (FCL_REAL d, const TaylorModel& a)
-{
- TaylorModel res(a);
- res.coeff(0) *= d;
- res.coeff(1) *= d;
- res.coeff(2) *= d;
- res.coeff(3) *= d;
- res.remainder() *= d;
- return res;
-}
-
-TaylorModel operator + (FCL_REAL d, const TaylorModel& a)
-{
- return a + d;
-}
-
-TaylorModel operator - (FCL_REAL d, const TaylorModel& a)
-{
- return -a + d;
-}
-
-
-void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
-{
- FCL_REAL a = tm.getTimeInterval()->t_.center();
- FCL_REAL t = w * a + q0;
- FCL_REAL w2 = w * w;
- FCL_REAL fa = cos(t);
- FCL_REAL fda = -w*sin(t);
- FCL_REAL fdda = -w2*fa;
- FCL_REAL fddda = -w2*fda;
-
- tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
- tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda;
- tm.coeff(2) = 0.5*(fdda-a*fddda);
- tm.coeff(3) = 1.0/6.0*fddda;
-
- // compute bounds for w^3 cos(wt+q0)/16, t \in [t0, t1]
- Interval fddddBounds;
- if(w == 0) fddddBounds.setValue(0);
- else
- {
- FCL_REAL cosQL = cos(tm.getTimeInterval()->t_[0] * w + q0);
- FCL_REAL cosQR = cos(tm.getTimeInterval()->t_[1] * w + q0);
-
- if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR);
- else fddddBounds.setValue(cosQR, cosQL);
-
- // enlarge to handle round-off errors
- fddddBounds[0] -= 1e-15;
- fddddBounds[1] += 1e-15;
-
- // cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi];
- // cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi]
-
- FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>());
- FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>());
-
-
- if(w > 0)
- {
- if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
- k1 -= 0.5;
- k2 -= 0.5;
- if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
- }
- else
- {
- if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
- k1 -= 0.5;
- k2 -= 0.5;
- if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
- }
- }
-
- FCL_REAL w4 = w2 * w2;
- fddddBounds *= w4;
-
- FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]);
- FCL_REAL midSize2 = midSize * midSize;
- FCL_REAL midSize4 = midSize2 * midSize2;
-
- // [0, midSize4] * fdddBounds
- if(fddddBounds[0] > 0)
- tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
- else if(fddddBounds[0] < 0)
- tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
- else
- tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
-}
-
-void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
-{
- FCL_REAL a = tm.getTimeInterval()->t_.center();
- FCL_REAL t = w * a + q0;
- FCL_REAL w2 = w * w;
- FCL_REAL fa = sin(t);
- FCL_REAL fda = w*cos(t);
- FCL_REAL fdda = -w2*fa;
- FCL_REAL fddda = -w2*fda;
-
- tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
- tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda;
- tm.coeff(2) = 0.5*(fdda-a*fddda);
- tm.coeff(3) = 1.0/6.0*fddda;
-
- // compute bounds for w^3 sin(wt+q0)/16, t \in [t0, t1]
-
- Interval fddddBounds;
-
- if(w == 0) fddddBounds.setValue(0);
- else
- {
- FCL_REAL sinQL = sin(w * tm.getTimeInterval()->t_[0] + q0);
- FCL_REAL sinQR = sin(w * tm.getTimeInterval()->t_[1] + q0);
-
- if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR);
- else fddddBounds.setValue(sinQR, sinQL);
-
- // enlarge to handle round-off errors
- fddddBounds[0] -= 1e-15;
- fddddBounds[1] += 1e-15;
-
- // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi];
- // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi]
-
- FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25;
- FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25;
-
- if(w > 0)
- {
- if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
- k1 -= 0.5;
- k2 -= 0.5;
- if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
- }
- else
- {
- if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
- k1 -= 0.5;
- k2 -= 0.5;
- if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
- }
-
- FCL_REAL w4 = w2 * w2;
- fddddBounds *= w4;
-
- FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]);
- FCL_REAL midSize2 = midSize * midSize;
- FCL_REAL midSize4 = midSize2 * midSize2;
-
- // [0, midSize4] * fdddBounds
- if(fddddBounds[0] > 0)
- tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
- else if(fddddBounds[0] < 0)
- tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
- else
- tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
- }
-}
-
-void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v)
-{
- tm.coeff(0) = p;
- tm.coeff(1) = v;
- tm.coeff(2) = 0;
- tm.coeff(3) = 0;
- tm.remainder()[0] = 0;
- tm.remainder()[1] = 0;
-}
-
-}
diff --git a/src/ccd/taylor_vector.cpp b/src/ccd/taylor_vector.cpp
deleted file mode 100644
index de94c2dfdb50b1752e6787de4d3a6bfeedc872c3..0000000000000000000000000000000000000000
--- a/src/ccd/taylor_vector.cpp
+++ /dev/null
@@ -1,291 +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 Jia Pan */
-
-#include <hpp/fcl/ccd/taylor_vector.h>
-
-namespace fcl
-{
-
-TVector3::TVector3()
-{
-}
-
-TVector3::TVector3(const boost::shared_ptr<TimeInterval>& time_interval)
-{
- setTimeInterval(time_interval);
-}
-
-TVector3::TVector3(TaylorModel v[3])
-{
- i_[0] = v[0];
- i_[1] = v[1];
- i_[2] = v[2];
-}
-
-TVector3::TVector3(const TaylorModel& v1, const TaylorModel& v2, const TaylorModel& v3)
-{
- i_[0] = v1;
- i_[1] = v2;
- i_[2] = v3;
-}
-
-TVector3::TVector3(const Vec3f& v, const boost::shared_ptr<TimeInterval>& time_interval)
-{
- i_[0] = TaylorModel(v[0], time_interval);
- i_[1] = TaylorModel(v[1], time_interval);
- i_[2] = TaylorModel(v[2], time_interval);
-}
-
-void TVector3::setZero()
-{
- i_[0].setZero();
- i_[1].setZero();
- i_[2].setZero();
-}
-
-TVector3 TVector3::operator + (const TVector3& other) const
-{
- return TVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]);
-}
-
-TVector3 TVector3::operator - (const TVector3& other) const
-{
- return TVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]);
-}
-
-TVector3 TVector3::operator - () const
-{
- return TVector3(-i_[0], -i_[1], -i_[2]);
-}
-
-TVector3& TVector3::operator += (const TVector3& other)
-{
- i_[0] += other.i_[0];
- i_[1] += other.i_[1];
- i_[2] += other.i_[2];
- return *this;
-}
-
-TVector3& TVector3::operator -= (const TVector3& other)
-{
- i_[0] -= other.i_[0];
- i_[1] -= other.i_[1];
- i_[2] -= other.i_[2];
- return *this;
-}
-
-TVector3 TVector3::operator + (const Vec3f& other) const
-{
- return TVector3(i_[0] + other[0], i_[1] + other[1], i_[2] + other[2]);
-}
-
-TVector3& TVector3::operator += (const Vec3f& other)
-{
- i_[0] += other[0];
- i_[1] += other[1];
- i_[2] += other[2];
- return *this;
-}
-
-TVector3 TVector3::operator - (const Vec3f& other) const
-{
- return TVector3(i_[0] - other[0], i_[1] - other[1], i_[2] - other[2]);
-}
-
-TVector3& TVector3::operator -= (const Vec3f& other)
-{
- i_[0] -= other[0];
- i_[1] -= other[1];
- i_[2] -= other[2];
- return *this;
-}
-
-
-TVector3 TVector3::operator * (const TaylorModel& d) const
-{
- return TVector3(i_[0] * d, i_[1] * d, i_[2] * d);
-}
-
-TVector3& TVector3::operator *= (const TaylorModel& d)
-{
- i_[0] *= d;
- i_[1] *= d;
- i_[2] *= d;
- return *this;
-}
-
-TVector3 TVector3::operator * (FCL_REAL d) const
-{
- return TVector3(i_[0] * d, i_[1] * d, i_[2] * d);
-}
-
-TVector3& TVector3::operator *= (FCL_REAL d)
-{
- i_[0] *= d;
- i_[1] *= d;
- i_[2] *= d;
- return *this;
-}
-
-const TaylorModel& TVector3::operator [] (size_t i) const
-{
- return i_[i];
-}
-
-TaylorModel& TVector3::operator [] (size_t i)
-{
- return i_[i];
-}
-
-TaylorModel TVector3::dot(const TVector3& other) const
-{
- return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2];
-}
-
-TVector3 TVector3::cross(const TVector3& other) const
-{
- return TVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1],
- i_[2] * other.i_[0] - i_[0] * other.i_[2],
- i_[0] * other.i_[1] - i_[1] * other.i_[0]);
-}
-
-TaylorModel TVector3::dot(const Vec3f& other) const
-{
- return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
-}
-
-TVector3 TVector3::cross(const Vec3f& other) const
-{
- return TVector3(i_[1] * other[2] - i_[2] * other[1],
- i_[2] * other[0] - i_[0] * other[2],
- i_[0] * other[1] - i_[1] * other[0]);
-}
-
-FCL_REAL TVector3::volumn() const
-{
- return i_[0].getBound().diameter() * i_[1].getBound().diameter() * i_[2].getBound().diameter();
-}
-
-IVector3 TVector3::getBound() const
-{
- return IVector3(i_[0].getBound(), i_[1].getBound(), i_[2].getBound());
-}
-
-IVector3 TVector3::getBound(FCL_REAL l, FCL_REAL r) const
-{
- return IVector3(i_[0].getBound(l, r), i_[1].getBound(l, r), i_[2].getBound(l, r));
-}
-
-IVector3 TVector3::getBound(FCL_REAL t) const
-{
- return IVector3(i_[0].getBound(t), i_[1].getBound(t), i_[2].getBound(t));
-}
-
-IVector3 TVector3::getTightBound() const
-{
- return IVector3(i_[0].getTightBound(), i_[1].getTightBound(), i_[2].getTightBound());
-}
-
-IVector3 TVector3::getTightBound(FCL_REAL l, FCL_REAL r) const
-{
- return IVector3(i_[0].getTightBound(l, r), i_[1].getTightBound(l, r), i_[2].getTightBound(l, r));
-}
-
-
-void TVector3::print() const
-{
- i_[0].print();
- i_[1].print();
- i_[2].print();
-}
-
-
-TaylorModel TVector3::squareLength() const
-{
- return i_[0] * i_[0] + i_[1] * i_[1] + i_[2] * i_[2];
-}
-
-void TVector3::setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
-{
- i_[0].setTimeInterval(time_interval);
- i_[1].setTimeInterval(time_interval);
- i_[2].setTimeInterval(time_interval);
-}
-
-void TVector3::setTimeInterval(FCL_REAL l, FCL_REAL r)
-{
- i_[0].setTimeInterval(l, r);
- i_[1].setTimeInterval(l, r);
- i_[2].setTimeInterval(l, r);
-}
-
-const boost::shared_ptr<TimeInterval>& TVector3::getTimeInterval() const
-{
- return i_[0].getTimeInterval();
-}
-
-void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity)
-{
- generateTaylorModelForLinearFunc(v[0], position[0], velocity[0]);
- generateTaylorModelForLinearFunc(v[1], position[1], velocity[1]);
- generateTaylorModelForLinearFunc(v[2], position[2], velocity[2]);
-}
-
-
-TVector3 operator * (const Vec3f& v, const TaylorModel& a)
-{
- TVector3 res(a.getTimeInterval());
- res[0] = a * v[0];
- res[1] = a * v[1];
- res[2] = a * v[2];
-
- return res;
-}
-
-TVector3 operator + (const Vec3f& v1, const TVector3& v2)
-{
- return v2 + v1;
-}
-
-TVector3 operator - (const Vec3f& v1, const TVector3& v2)
-{
- return -v2 + v1;
-}
-
-
-
-}
diff --git a/src/continuous_collision.cpp b/src/continuous_collision.cpp
deleted file mode 100644
index c4896d59d1dca44ec2ae76922bdbd87efd05f76a..0000000000000000000000000000000000000000
--- a/src/continuous_collision.cpp
+++ /dev/null
@@ -1,335 +0,0 @@
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/continuous_collision.h>
-#include <hpp/fcl/ccd/motion.h>
-#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
-#include <hpp/fcl/traversal/traversal_node_setup.h>
-#include <hpp/fcl/collision_node.h>
-#include <hpp/fcl/ccd/conservative_advancement.h>
-#include <iostream>
-
-namespace fcl
-{
-
-template<typename GJKSolver>
-ConservativeAdvancementFunctionMatrix<GJKSolver>& getConservativeAdvancementFunctionLookTable()
-{
- static ConservativeAdvancementFunctionMatrix<GJKSolver> table;
- return table;
-}
-
-MotionBasePtr getMotionBase(const Transform3f& tf_beg, const Transform3f& tf_end, CCDMotionType motion_type)
-{
- switch(motion_type)
- {
- case CCDM_TRANS:
- return MotionBasePtr(new TranslationMotion(tf_beg, tf_end));
- break;
- case CCDM_LINEAR:
- return MotionBasePtr(new InterpMotion(tf_beg, tf_end));
- break;
- case CCDM_SCREW:
- return MotionBasePtr(new ScrewMotion(tf_beg, tf_end));
- break;
- case CCDM_SPLINE:
- return MotionBasePtr(new SplineMotion(tf_beg, tf_end));
- break;
- default:
- return MotionBasePtr();
- }
-}
-
-
-FCL_REAL continuousCollideNaive(const CollisionGeometry* o1, const MotionBase* motion1,
- const CollisionGeometry* o2, const MotionBase* motion2,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- std::size_t n_iter = std::min(request.num_max_iterations, (std::size_t)ceil(1 / request.toc_err));
- Transform3f cur_tf1, cur_tf2;
- for(std::size_t i = 0; i < n_iter; ++i)
- {
- FCL_REAL t = i / (FCL_REAL) (n_iter - 1);
- motion1->integrate(t);
- motion2->integrate(t);
-
- motion1->getCurrentTransform(cur_tf1);
- motion2->getCurrentTransform(cur_tf2);
-
- CollisionRequest c_request;
- CollisionResult c_result;
-
- if(collide(o1, cur_tf1, o2, cur_tf2, c_request, c_result))
- {
- result.is_collide = true;
- result.time_of_contact = t;
- result.contact_tf1 = cur_tf1;
- result.contact_tf2 = cur_tf2;
- return t;
- }
- }
-
- return 1.0;
-}
-
-namespace details
-{
-template<typename BV>
-FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1_, const TranslationMotion* motion1,
- const CollisionGeometry* o2_, const TranslationMotion* motion2,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- const BVHModel<BV>* o1__ = static_cast<const BVHModel<BV>*>(o1_);
- const BVHModel<BV>* o2__ = static_cast<const BVHModel<BV>*>(o2_);
-
- // ugly, but lets do it now.
- BVHModel<BV>* o1 = const_cast<BVHModel<BV>*>(o1__);
- BVHModel<BV>* o2 = const_cast<BVHModel<BV>*>(o2__);
- std::vector<Vec3f> new_v1(o1->num_vertices);
- std::vector<Vec3f> new_v2(o2->num_vertices);
-
- for(std::size_t i = 0; i < new_v1.size(); ++i)
- new_v1[i] = o1->vertices[i] + motion1->getVelocity();
- for(std::size_t i = 0; i < new_v2.size(); ++i)
- new_v2[i] = o2->vertices[i] + motion2->getVelocity();
-
- o1->beginUpdateModel();
- o1->updateSubModel(new_v1);
- o1->endUpdateModel(true, true);
-
- o2->beginUpdateModel();
- o2->updateSubModel(new_v2);
- o2->endUpdateModel(true, true);
-
- MeshContinuousCollisionTraversalNode<BV> node;
- CollisionRequest c_request;
-
- motion1->integrate(0);
- motion2->integrate(0);
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
- if(!initialize<BV>(node, *o1, tf1, *o2, tf2, c_request))
- return -1.0;
-
- FCL_REAL unused;
- collide(&node, unused);
-
- result.is_collide = (node.pairs.size() > 0);
- result.time_of_contact = node.time_of_contact;
-
- if(result.is_collide)
- {
- motion1->integrate(node.time_of_contact);
- motion2->integrate(node.time_of_contact);
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
- result.contact_tf1 = tf1;
- result.contact_tf2 = tf2;
- }
-
- return result.time_of_contact;
-}
-
-}
-
-FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1, const TranslationMotion* motion1,
- const CollisionGeometry* o2, const TranslationMotion* motion2,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- switch(o1->getNodeType())
- {
- case BV_AABB:
- if(o2->getNodeType() == BV_AABB)
- return details::continuousCollideBVHPolynomial<AABB>(o1, motion1, o2, motion2, request, result);
- break;
- case BV_OBB:
- if(o2->getNodeType() == BV_OBB)
- return details::continuousCollideBVHPolynomial<OBB>(o1, motion1, o2, motion2, request, result);
- break;
- case BV_RSS:
- if(o2->getNodeType() == BV_RSS)
- return details::continuousCollideBVHPolynomial<RSS>(o1, motion1, o2, motion2, request, result);
- break;
- case BV_kIOS:
- if(o2->getNodeType() == BV_kIOS)
- return details::continuousCollideBVHPolynomial<kIOS>(o1, motion1, o2, motion2, request, result);
- break;
- case BV_OBBRSS:
- if(o2->getNodeType() == BV_OBBRSS)
- return details::continuousCollideBVHPolynomial<OBBRSS>(o1, motion1, o2, motion2, request, result);
- break;
- case BV_KDOP16:
- if(o2->getNodeType() == BV_KDOP16)
- return details::continuousCollideBVHPolynomial<KDOP<16> >(o1, motion1, o2, motion2, request, result);
- break;
- case BV_KDOP18:
- if(o2->getNodeType() == BV_KDOP18)
- return details::continuousCollideBVHPolynomial<KDOP<18> >(o1, motion1, o2, motion2, request, result);
- break;
- case BV_KDOP24:
- if(o2->getNodeType() == BV_KDOP24)
- return details::continuousCollideBVHPolynomial<KDOP<24> >(o1, motion1, o2, motion2, request, result);
- break;
- default:
- ;
- }
-
- std::cerr << "Warning: BV type not supported by polynomial solver CCD" << std::endl;
-
- return -1;
-}
-
-namespace details
-{
-
-template<typename NarrowPhaseSolver>
-FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1,
- const CollisionGeometry* o2, const MotionBase* motion2,
- const NarrowPhaseSolver* nsolver_,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- const NarrowPhaseSolver* nsolver = nsolver_;
- if(!nsolver_)
- nsolver = new NarrowPhaseSolver();
-
- const ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>& looktable = getConservativeAdvancementFunctionLookTable<NarrowPhaseSolver>();
-
- NODE_TYPE node_type1 = o1->getNodeType();
- NODE_TYPE node_type2 = o2->getNodeType();
-
- FCL_REAL res = -1;
-
- if(!looktable.conservative_advancement_matrix[node_type1][node_type2])
- {
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- }
- else
- {
- res = looktable.conservative_advancement_matrix[node_type1][node_type2](o1, motion1, o2, motion2, nsolver, request, result);
- }
-
- if(!nsolver_)
- delete nsolver;
-
- if(result.is_collide)
- {
- motion1->integrate(result.time_of_contact);
- motion2->integrate(result.time_of_contact);
-
- Transform3f tf1, tf2;
- motion1->getCurrentTransform(tf1);
- motion2->getCurrentTransform(tf2);
- result.contact_tf1 = tf1;
- result.contact_tf2 = tf2;
- }
-
- return res;
-}
-
-}
-
-
-FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1,
- const CollisionGeometry* o2, const MotionBase* motion2,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- switch(request.gjk_solver_type)
- {
- case GST_LIBCCD:
- {
- GJKSolver_libccd solver;
- return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result);
- }
- case GST_INDEP:
- {
- GJKSolver_indep solver;
- return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result);
- }
- default:
- return -1;
- }
-}
-
-
-FCL_REAL continuousCollide(const CollisionGeometry* o1, const MotionBase* motion1,
- const CollisionGeometry* o2, const MotionBase* motion2,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- switch(request.ccd_solver_type)
- {
- case CCDC_NAIVE:
- return continuousCollideNaive(o1, motion1,
- o2, motion2,
- request,
- result);
- break;
- case CCDC_CONSERVATIVE_ADVANCEMENT:
- return continuousCollideConservativeAdvancement(o1, motion1,
- o2, motion2,
- request,
- result);
- break;
- case CCDC_RAY_SHOOTING:
- if(o1->getObjectType() == OT_GEOM && o2->getObjectType() == OT_GEOM && request.ccd_motion_type == CCDM_TRANS)
- {
-
- }
- else
- std::cerr << "Warning! Invalid continuous collision setting" << std::endl;
- break;
- case CCDC_POLYNOMIAL_SOLVER:
- if(o1->getObjectType() == OT_BVH && o2->getObjectType() == OT_BVH && request.ccd_motion_type == CCDM_TRANS)
- {
- return continuousCollideBVHPolynomial(o1, (const TranslationMotion*)motion1,
- o2, (const TranslationMotion*)motion2,
- request, result);
- }
- else
- std::cerr << "Warning! Invalid continuous collision checking" << std::endl;
- break;
- default:
- std::cerr << "Warning! Invalid continuous collision setting" << std::endl;
- }
-
- return -1;
-}
-
-FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
- const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- MotionBasePtr motion1 = getMotionBase(tf1_beg, tf1_end, request.ccd_motion_type);
- MotionBasePtr motion2 = getMotionBase(tf2_beg, tf2_end, request.ccd_motion_type);
-
- return continuousCollide(o1, motion1.get(), o2, motion2.get(), request, result);
-}
-
-
-FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3f& tf1_end,
- const CollisionObject* o2, const Transform3f& tf2_end,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- return continuousCollide(o1->collisionGeometry().get(), o1->getTransform(), tf1_end,
- o2->collisionGeometry().get(), o2->getTransform(), tf2_end,
- request, result);
-}
-
-
-FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
- const ContinuousCollisionRequest& request,
- ContinuousCollisionResult& result)
-{
- return continuousCollide(o1->collisionGeometry().get(), o1->getMotion(),
- o2->collisionGeometry().get(), o2->getMotion(),
- request, result);
-}
-
-}
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index edd316d3ebdddeb46c1e7b82c5884aaa7e334c83..dda013e2daf0cbf9dc26a841dc713f141ca2c5ad 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -445,260 +445,4 @@ void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
request, *result);
}
-
-
-
-
-
-namespace details
-{
-
-template<typename BV>
-bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
- FCL_REAL min_distance,
- FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- const MotionBase* motion1, const MotionBase* motion2,
- std::vector<ConservativeAdvancementStackData>& stack,
- FCL_REAL& delta_t)
-{
- if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
- {
- const ConservativeAdvancementStackData& data = stack.back();
- FCL_REAL d = data.d;
- Vec3f n;
- int c1, c2;
-
- if(d > c)
- {
- const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
- d = data2.d;
- n = data2.P2 - data2.P1; n.normalize();
- c1 = data2.c1;
- c2 = data2.c2;
- stack[stack.size() - 2] = stack[stack.size() - 1];
- }
- else
- {
- n = data.P2 - data.P1; n.normalize();
- c1 = data.c1;
- c2 = data.c2;
- }
-
- assert(c == d);
-
- // n is in local frame of c1, so we need to turn n into the global frame
- Vec3f n_transformed =
- getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
- getBVAxis(model1->getBV(c1).bv, 1) * n[2] +
- getBVAxis(model1->getBV(c1).bv, 2) * n[2];
- Quaternion3f R0;
- motion1->getCurrentRotation(R0);
- n_transformed = R0.transform(n_transformed);
- n_transformed.normalize();
-
- TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, -n_transformed);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= c) cur_delta_t = 1;
- else cur_delta_t = c / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- const ConservativeAdvancementStackData& data = stack.back();
- FCL_REAL d = data.d;
-
- if(d > c)
- stack[stack.size() - 2] = stack[stack.size() - 1];
-
- stack.pop_back();
-
- return false;
- }
-}
-
-template<typename BV>
-void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- const Triangle* tri_indices1, const Triangle* tri_indices2,
- const Vec3f* vertices1, const Vec3f* vertices2,
- const Matrix3f& R, const Vec3f& T,
- const MotionBase* motion1, const MotionBase* motion2,
- bool enable_statistics,
- FCL_REAL& min_distance,
- Vec3f& p1, Vec3f& p2,
- int& last_tri_id1, int& last_tri_id2,
- FCL_REAL& delta_t,
- int& num_leaf_tests)
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<BV>& node1 = model1->getBV(b1);
- const BVNode<BV>& node2 = model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& t11 = vertices1[tri_id1[0]];
- const Vec3f& t12 = vertices1[tri_id1[1]];
- const Vec3f& t13 = vertices1[tri_id1[2]];
-
- const Vec3f& t21 = vertices2[tri_id2[0]];
- const Vec3f& t22 = vertices2[tri_id2[1]];
- const Vec3f& t23 = vertices2[tri_id2[2]];
-
- // nearest point pair
- Vec3f P1, P2;
-
- FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
- (t11, t12, t13, t21, t22, t23, R, T, P1, P2));
-
- if(d < min_distance)
- {
- min_distance = d;
-
- p1 = P1;
- p2 = P2;
-
- last_tri_id1 = primitive_id1;
- last_tri_id2 = primitive_id2;
- }
-
-
- /// n is the local frame of object 1, pointing from object 1 to object2
- Vec3f n = P2 - P1;
- /// turn n into the global frame, pointing from object 1 to object 2
- Quaternion3f R0;
- motion1->getCurrentRotation(R0);
- Vec3f n_transformed = R0.transform(n);
- n_transformed.normalize(); // normalized here
-
- TriangleMotionBoundVisitor mb_visitor1(t11, t12, t13, n_transformed), mb_visitor2(t21, t22, t23, -n_transformed);
- FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
- FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
-
- FCL_REAL bound = bound1 + bound2;
-
- FCL_REAL cur_delta_t;
- if(bound <= d) cur_delta_t = 1;
- else cur_delta_t = d / bound;
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-}
-
-}
-
-
-MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_)
- : MeshConservativeAdvancementTraversalNode<RSS>(w_)
-{
- R.setIdentity();
-}
-
-FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
-
- stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
-}
-
-
-void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const
-{
- details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2,
- model1, model2,
- tri_indices1, tri_indices2,
- vertices1, vertices2,
- R, T,
- motion1, motion2,
- enable_statistics,
- min_distance,
- closest_p1, closest_p2,
- last_tri_id1, last_tri_id2,
- delta_t,
- num_leaf_tests);
-}
-
-bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const
-{
- return details::meshConservativeAdvancementOrientedNodeCanStop(c,
- min_distance,
- abs_err, rel_err, w,
- model1, model2,
- motion1, motion2,
- stack,
- delta_t);
-}
-
-
-
-
-MeshConservativeAdvancementTraversalNodeOBBRSS::MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_)
- : MeshConservativeAdvancementTraversalNode<OBBRSS>(w_)
-{
- R.setIdentity();
-}
-
-FCL_REAL MeshConservativeAdvancementTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
-
- stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
-}
-
-
-void MeshConservativeAdvancementTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
-{
- details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2,
- model1, model2,
- tri_indices1, tri_indices2,
- vertices1, vertices2,
- R, T,
- motion1, motion2,
- enable_statistics,
- min_distance,
- closest_p1, closest_p2,
- last_tri_id1, last_tri_id2,
- delta_t,
- num_leaf_tests);
-}
-
-bool MeshConservativeAdvancementTraversalNodeOBBRSS::canStop(FCL_REAL c) const
-{
- return details::meshConservativeAdvancementOrientedNodeCanStop(c,
- min_distance,
- abs_err, rel_err, w,
- model1, model2,
- motion1, motion2,
- stack,
- delta_t);
-}
-
-
-
-
-
}
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index 5f31d364904a1aa68b94e18262a5c9b11e275784..0525d5d6993a0f3a88826b6e307169dd513bfe96 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/src/traversal/traversal_node_setup.cpp
@@ -183,52 +183,7 @@ namespace details
{
-template<typename BV, typename OrientedDistanceNode>
-static inline bool setupMeshConservativeAdvancementOrientedDistanceNode(OrientedDistanceNode& node,
- const BVHModel<BV>& model1, const Transform3f& tf1,
- const BVHModel<BV>& model2, const Transform3f& tf2,
- FCL_REAL w)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.w = w;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
}
-}
-
-
-bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const Transform3f& tf1,
- const BVHModel<RSS>& model2, const Transform3f& tf2,
- FCL_REAL w)
-{
- return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w);
-}
-
-
-bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node,
- const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
- const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- FCL_REAL w)
-{
- return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w);
-}
-
-
-
}
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 046886618fe66854af67bc4ad89080ee9987301a..1fc0c9de9215be5f8b724045d8ca232635978c5d 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -44,7 +44,6 @@
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/collision.h>
#include "test_fcl_utility.h"
-#include <hpp/fcl/ccd/motion.h>
#include <iostream>
using namespace fcl;
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index 7f7f172327ae66c6f0690facb36e3532e1895ecd..1bc9feb98320b6105f6f57966b7e2811ef705bfe 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -1,6 +1,5 @@
#include "test_fcl_utility.h"
#include <hpp/fcl/collision.h>
-#include <hpp/fcl/continuous_collision.h>
#include <hpp/fcl/distance.h>
#include <cstdio>
#include <cstddef>
@@ -382,24 +381,6 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cda
return cdata->done;
}
-bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_)
-{
- ContinuousCollisionData* cdata = static_cast<ContinuousCollisionData*>(cdata_);
- const ContinuousCollisionRequest& request = cdata->request;
- ContinuousCollisionResult& result = cdata->result;
-
- if(cdata->done) return true;
-
- collide(o1, o2, request, result);
-
- return cdata->done;
-}
-
-bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist)
-{
- return true;
-}
-
std::string getNodeTypeName(NODE_TYPE node_type)
{
if (node_type == BV_UNKNOWN)
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index 65fd59ad19b63b3566e4aa9e73411e9a8a29e47b..f692baa6a1514d5e64da9ba5046eae9196e6bf4d 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -148,41 +148,12 @@ struct DistanceData
};
-/// @brief Continuous collision data stores the continuous collision request and result given the continuous collision algorithm.
-struct ContinuousCollisionData
-{
- ContinuousCollisionData()
- {
- done = false;
- }
-
- /// @brief Continuous collision request
- ContinuousCollisionRequest request;
-
- /// @brief Continuous collision result
- ContinuousCollisionResult result;
-
- /// @brief Whether the continuous collision iteration can stop
- bool done;
-};
-
-
-
/// @brief Default collision callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now.
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
/// @brief Default distance callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. also return dist, i.e. the bmin distance till now
bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
-
-
-
-
-bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_);
-
-bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist);
-
-
std::string getNodeTypeName(NODE_TYPE node_type);
std::string getGJKSolverName(GJKSolverType solver_type);