FCL  0.6.0
Flexible Collision Library
broadphase_bruteforce-inl.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_BRUTE_FORCE_INL_H
39 #define FCL_BROAD_PHASE_BRUTE_FORCE_INL_H
40 
41 #include "fcl/broadphase/broadphase_bruteforce.h"
42 
43 #include <iterator>
44 
45 namespace fcl {
46 
47 //==============================================================================
48 extern template
50 
51 //==============================================================================
52 template <typename S>
54 {
55  // Do nothing
56 }
57 
58 //==============================================================================
59 template <typename S>
61 {
62  std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
63 }
64 
65 //==============================================================================
66 template <typename S>
68 {
69  objs.remove(obj);
70 }
71 
72 //==============================================================================
73 template <typename S>
75 {
76  objs.push_back(obj);
77 }
78 
79 //==============================================================================
80 template <typename S>
82 {
83  // Do nothing
84 }
85 
86 //==============================================================================
87 template <typename S>
89 {
90  // Do nothing
91 }
92 
93 //==============================================================================
94 template <typename S>
96 {
97  objs.clear();
98 }
99 
100 //==============================================================================
101 template <typename S>
103 {
104  objs_.resize(objs.size());
105  std::copy(objs.begin(), objs.end(), objs_.begin());
106 }
107 
108 //==============================================================================
109 template <typename S>
111 {
112  if(size() == 0) return;
113 
114  for(auto* obj2 : objs)
115  {
116  if(callback(obj, obj2, cdata))
117  return;
118  }
119 }
120 
121 //==============================================================================
122 template <typename S>
124 {
125  if(size() == 0) return;
126 
127  S min_dist = std::numeric_limits<S>::max();
128  for(auto* obj2 : objs)
129  {
130  if(obj->getAABB().distance(obj2->getAABB()) < min_dist)
131  {
132  if(callback(obj, obj2, cdata, min_dist))
133  return;
134  }
135  }
136 }
137 
138 //==============================================================================
139 template <typename S>
141 {
142  if(size() == 0) return;
143 
144  for(typename std::list<CollisionObject<S>*>::const_iterator it1 = objs.begin(), end = objs.end();
145  it1 != end; ++it1)
146  {
147  typename std::list<CollisionObject<S>*>::const_iterator it2 = it1; it2++;
148  for(; it2 != end; ++it2)
149  {
150  if((*it1)->getAABB().overlap((*it2)->getAABB()))
151  {
152  if(callback(*it1, *it2, cdata))
153  return;
154  }
155  }
156  }
157 }
158 
159 //==============================================================================
160 template <typename S>
162 {
163  if(size() == 0) return;
164 
165  S min_dist = std::numeric_limits<S>::max();
166  for(typename std::list<CollisionObject<S>*>::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1)
167  {
168  typename std::list<CollisionObject<S>*>::const_iterator it2 = it1; it2++;
169  for(; it2 != end; ++it2)
170  {
171  if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)
172  {
173  if(callback(*it1, *it2, cdata, min_dist))
174  return;
175  }
176  }
177  }
178 }
179 
180 //==============================================================================
181 template <typename S>
183 {
184  NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
185 
186  if((size() == 0) || (other_manager->size() == 0)) return;
187 
188  if(this == other_manager)
189  {
190  collide(cdata, callback);
191  return;
192  }
193 
194  for(auto* obj1 : objs)
195  {
196  for(auto* obj2 : other_manager->objs)
197  {
198  if(obj1->getAABB().overlap(obj2->getAABB()))
199  {
200  if(callback(obj1, obj2, cdata))
201  return;
202  }
203  }
204  }
205 }
206 
207 //==============================================================================
208 template <typename S>
210 {
211  NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
212 
213  if((size() == 0) || (other_manager->size() == 0)) return;
214 
215  if(this == other_manager)
216  {
217  distance(cdata, callback);
218  return;
219  }
220 
221  S min_dist = std::numeric_limits<S>::max();
222  for(auto* obj1 : objs)
223  {
224  for(auto* obj2 : other_manager->objs)
225  {
226  if(obj1->getAABB().distance(obj2->getAABB()) < min_dist)
227  {
228  if(callback(obj1, obj2, cdata, min_dist))
229  return;
230  }
231  }
232  }
233 }
234 
235 //==============================================================================
236 template <typename S>
238 {
239  return objs.empty();
240 }
241 
242 //==============================================================================
243 template <typename S>
245 {
246  return objs.size();
247 }
248 
249 } // namespace fcl
250 
251 #endif
void registerObject(CollisionObject< S > *obj)
add one object to the manager
Definition: broadphase_bruteforce-inl.h:74
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
void getObjects(std::vector< CollisionObject< S > * > &objs) const
return the objects managed by the manager
Definition: broadphase_bruteforce-inl.h:102
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_bruteforce-inl.h:123
size_t size() const
the number of objects managed by the manager
Definition: broadphase_bruteforce-inl.h:244
void clear()
clear the manager
Definition: broadphase_bruteforce-inl.h:95
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
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
void update()
update the condition of manager
Definition: broadphase_bruteforce-inl.h:88
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_bruteforce-inl.h:110
std::list< CollisionObject< S > * > objs
objects belonging to the manager are stored in a list structure
Definition: broadphase_bruteforce.h:102
void registerObjects(const std::vector< CollisionObject< S > * > &other_objs)
add objects to the manager
Definition: broadphase_bruteforce-inl.h:60
bool empty() const
whether the manager is empty
Definition: broadphase_bruteforce-inl.h:237
const AABB< S > & getAABB() const
get the AABB in world space
Definition: collision_object-inl.h:111
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:51
Brute force N-body collision manager.
Definition: broadphase_bruteforce.h:49
void unregisterObject(CollisionObject< S > *obj)
remove one object from the manager
Definition: broadphase_bruteforce-inl.h:67
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
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:66
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_bruteforce-inl.h:81
S distance(const AABB< S > &other, Vector3< S > *P, Vector3< S > *Q) const
Distance between two AABBs; P and Q, should not be nullptr, return the nearest points.
Definition: AABB-inl.h:237