hpp-fcl  2.4.4
HPP fork of FCL -- The Flexible Collision Library
broadphase_interval_tree.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef HPP_FCL_BROAD_PHASE_INTERVAL_TREE_H
39 #define HPP_FCL_BROAD_PHASE_INTERVAL_TREE_H
40 
41 #include <deque>
42 #include <map>
43 
46 
47 namespace hpp {
48 namespace fcl {
49 
53  public:
55  using Base::getObjects;
56 
58 
60 
62  void registerObject(CollisionObject* obj);
63 
65  void unregisterObject(CollisionObject* obj);
66 
68  void setup();
69 
71  virtual void update();
72 
74  void update(CollisionObject* updated_obj);
75 
77  void update(const std::vector<CollisionObject*>& updated_objs);
78 
80  void clear();
81 
83  void getObjects(std::vector<CollisionObject*>& objs) const;
84 
87  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
88 
91  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
92 
95  void collide(CollisionCallBackBase* callback) const;
96 
99  void distance(DistanceCallBackBase* callback) const;
100 
102  void collide(BroadPhaseCollisionManager* other_manager,
103  CollisionCallBackBase* callback) const;
104 
106  void distance(BroadPhaseCollisionManager* other_manager,
107  DistanceCallBackBase* callback) const;
108 
110  bool empty() const;
111 
113  size_t size() const;
114 
115  protected:
121 
124 
127  char minmax;
128 
129  bool operator<(const EndPoint& p) const;
130  };
131 
136 
137  SAPInterval(FCL_REAL low_, FCL_REAL high_, CollisionObject* obj_);
138  };
139 
140  bool checkColl(
141  typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
142  typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
143  CollisionObject* obj, CollisionCallBackBase* callback) const;
144 
145  bool checkDist(
146  typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
147  typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
148  CollisionObject* obj, DistanceCallBackBase* callback,
149  FCL_REAL& min_dist) const;
150 
151  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
152 
153  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
154  FCL_REAL& min_dist) const;
155 
157  std::vector<EndPoint> endpoints[3];
158 
160  detail::IntervalTree* interval_trees[3];
161 
162  std::map<CollisionObject*, SAPInterval*> obj_interval_maps[3];
163 
165  bool setup_;
166 };
167 
168 } // namespace fcl
169 
170 } // namespace hpp
171 
172 #endif
Extention interval tree&#39;s interval to SAP interval, adding more information.
Definition: broadphase_interval_tree.h:134
Interval trees implemented using red-black-trees as described in the book Introduction_To_Algorithms_...
Definition: simple_interval.h:50
Collision manager based on interval tree.
Definition: broadphase_interval_tree.h:51
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:54
Main namespace.
Definition: broadphase_bruteforce.h:44
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
FCL_REAL value
end point value
Definition: broadphase_interval_tree.h:123
BroadPhaseCollisionManager Base
Definition: broadphase_interval_tree.h:54
char minmax
tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi ...
Definition: broadphase_interval_tree.h:127
CollisionObject * obj
object related with the end point
Definition: broadphase_interval_tree.h:120
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
double FCL_REAL
Definition: data_types.h:65
CollisionObject * obj
Definition: broadphase_interval_tree.h:135
SAP end point.
Definition: broadphase_interval_tree.h:118
Interval tree.
Definition: interval_tree.h:64
bool setup_
tag for whether the interval tree is maintained suitably
Definition: broadphase_interval_tree.h:165
std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:215
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
#define HPP_FCL_DLLAPI
Definition: config.hh:88