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 FCL_BROAD_PHASE_INTERVAL_TREE_H
39 #define FCL_BROAD_PHASE_INTERVAL_TREE_H
40 
41 #include <deque>
42 #include <map>
45 
46 namespace fcl
47 {
48 
50 template <typename S>
52 {
53 public:
55 
57 
59  void registerObject(CollisionObject<S>* obj);
60 
62  void unregisterObject(CollisionObject<S>* obj);
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,
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,
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 
137  detail::IntervalTree<S>* interval_trees[3];
138 
139  std::map<CollisionObject<S>*, SAPInterval*> obj_interval_maps[3];
140 
142  bool setup_;
143 };
144 
147 
149 template <typename S>
150 struct FCL_EXPORT IntervalTreeCollisionManager<S>::EndPoint
151 {
154 
156  S value;
157 
159  char minmax;
160 
161  bool operator<(const EndPoint &p) const;
162 };
163 
165 template <typename S>
166 struct FCL_EXPORT IntervalTreeCollisionManager<S>::SAPInterval : public detail::SimpleInterval<S>
167 {
169 
170  SAPInterval(S low_, S high_, CollisionObject<S>* obj_);
171 };
172 
173 } // namespace fcl
174 
176 
177 #endif
fcl::detail::SimpleInterval
Interval trees implemented using red-black-trees as described in the book Introduction_To_Algorithms_...
Definition: simple_interval.h:52
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
broadphase_collision_manager.h
fcl::IntervalTreeCollisionManager
Collision manager based on interval tree.
Definition: broadphase_interval_tree.h:51
fcl::IntervalTreeCollisionManager< S >::minmax
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
obj
CollisionObject< S > * obj
object
Definition: broadphase_SaP.h:164
fcl::DistanceCallBack
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
fcl::CollisionCallBack
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
fcl::IntervalTreeCollisionManager::setup_
bool setup_
tag for whether the interval tree is maintained suitably
Definition: broadphase_interval_tree.h:142
fcl::IntervalTreeCollisionManager< S >::obj
CollisionObject< S > * obj
object related with the end point
Definition: broadphase_interval_tree.h:153
broadphase_interval_tree-inl.h
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
interval_tree.h
fcl::detail::IntervalTree
Interval tree.
Definition: interval_tree.h:72
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::IntervalTreeCollisionManager< S >::value
S value
end point value
Definition: broadphase_interval_tree.h:156
fcl::BroadPhaseCollisionManager
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:66


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:49