Commit 840b91a0 authored by Your Name's avatar Your Name Committed by Florent Lamiraux florent@laas.fr
Browse files

Remove continuous collision checking

  - this feature is implemented in a more general way in HPP.
parent 602e309a
......@@ -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
......
......@@ -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>
......
/*
* 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
/*
* 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
/*
* 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
/*
* 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
/*
* 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];