FCL  0.6.0
Flexible Collision Library
broadphase_interval_tree.h
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 FCL_BROAD_PHASE_INTERVAL_TREE_H
39 #define FCL_BROAD_PHASE_INTERVAL_TREE_H
40 
41 #include <deque>
42 #include <map>
43 #include "fcl/broadphase/broadphase_collision_manager.h"
44 #include "fcl/broadphase/detail/interval_tree.h"
45 
46 namespace fcl
47 {
48 
50 template <typename S>
52 {
53 public:
55 
57 
60 
63 
65  void setup();
66 
68  void update();
69 
71  void update(CollisionObject<S>* updated_obj);
72 
74  void update(const std::vector<CollisionObject<S>*>& updated_objs);
75 
77  void clear();
78 
80  void getObjects(std::vector<CollisionObject<S>*>& objs) const;
81 
83  void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
84 
86  void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const;
87 
89  void collide(void* cdata, CollisionCallBack<S> callback) const;
90 
92  void distance(void* cdata, DistanceCallBack<S> callback) const;
93 
95  void collide(BroadPhaseCollisionManager<S>* other_manager, void* cdata, CollisionCallBack<S> callback) const;
96 
98  void distance(BroadPhaseCollisionManager<S>* other_manager, void* cdata, DistanceCallBack<S> callback) const;
99 
101  bool empty() const;
102 
104  size_t size() const;
105 
106 protected:
107 
109  struct EndPoint;
110 
112  struct SAPInterval;
113 
114  bool checkColl(
115  typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_start,
116  typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_end,
117  CollisionObject<S>* obj,
118  void* cdata,
119  CollisionCallBack<S> callback) const;
120 
121  bool checkDist(
122  typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_start,
123  typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_end,
124  CollisionObject<S>* obj,
125  void* cdata,
126  DistanceCallBack<S> callback,
127  S& min_dist) const;
128 
129  bool collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const;
130 
131  bool distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const;
132 
134  std::vector<EndPoint> endpoints[3];
135 
138 
139  std::map<CollisionObject<S>*, SAPInterval*> obj_interval_maps[3];
140 
142  bool setup_;
143 };
144 
147 
149 template <typename S>
151 {
154 
156  S value;
157 
159  char minmax;
160 
161  bool operator<(const EndPoint &p) const;
162 };
163 
165 template <typename S>
167 {
168  CollisionObject<S>* obj;
169 
170  SAPInterval(S low_, S high_, CollisionObject<S>* obj_);
171 };
172 
173 } // namespace fcl
174 
175 #include "fcl/broadphase/broadphase_interval_tree-inl.h"
176 
177 #endif
std::vector< EndPoint > endpoints[3]
vector stores all the end points
Definition: broadphase_interval_tree.h:134
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
void unregisterObject(CollisionObject< S > *obj)
add one object to the manager
Definition: broadphase_interval_tree-inl.h:52
bool setup_
tag for whether the interval tree is maintained suitably
Definition: broadphase_interval_tree.h:142
void getObjects(std::vector< CollisionObject< S > * > &objs) const
return the objects managed by the manager
Definition: broadphase_interval_tree-inl.h:358
bool empty() const
whether the manager is empty
Definition: broadphase_interval_tree-inl.h:646
SAP end point.
Definition: broadphase_interval_tree.h:150
detail::IntervalTree< S > * interval_trees[3]
interval tree manages the intervals
Definition: broadphase_interval_tree.h:137
bool(*)(CollisionObject< S > *o1, CollisionObject< S > *o2, void *cdata) CollisionCallBack
Callback for collision between two objects. Return value is whether can stop now. ...
Definition: broadphase_collision_manager.h:53
Interval trees implemented using red-black-trees as described in the book Introduction_To_Algorithms_...
Definition: simple_interval.h:50
CollisionObject< S > * obj
object related with the end point
Definition: broadphase_interval_tree.h:153
bool(*)(CollisionObject< S > *o1, CollisionObject< S > *o2, void *cdata, S &dist) DistanceCallBack
Callback for distance between two objects, Return value is whether can stop now, also return the mini...
Definition: broadphase_collision_manager.h:60
S value
end point value
Definition: broadphase_interval_tree.h:156
void clear()
clear the manager
Definition: broadphase_interval_tree-inl.h:331
Extention interval tree&#39;s interval to SAP interval, adding more information.
Definition: broadphase_interval_tree.h:166
void distance(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback) const
perform distance computation between one object and all the objects belonging to the manager ...
Definition: broadphase_interval_tree-inl.h:419
size_t size() const
the number of objects managed by the manager
Definition: broadphase_interval_tree-inl.h:653
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:51
void registerObject(CollisionObject< S > *obj)
remove one object from the manager
Definition: broadphase_interval_tree-inl.h:175
Collision manager based on interval tree.
Definition: broadphase_interval_tree.h:51
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_interval_tree-inl.h:202
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:66
void collide(CollisionObject< S > *obj, void *cdata, CollisionCallBack< S > callback) const
perform collision test between one object and all the objects belonging to the manager ...
Definition: broadphase_interval_tree-inl.h:373
Interval tree.
Definition: interval_tree.h:72
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:159
void update()
update the condition of manager
Definition: broadphase_interval_tree-inl.h:242