broadphase_bruteforce.cpp
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 
39 
40 #include <iterator>
41 #include <algorithm>
42 
43 namespace hpp {
44 namespace fcl {
45 
46 //==============================================================================
48  // Do nothing
49 }
50 
51 //==============================================================================
53  const std::vector<CollisionObject*>& other_objs) {
54  std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
55 }
56 
57 //==============================================================================
59  objs.remove(obj);
60 }
61 
62 //==============================================================================
64  objs.push_back(obj);
65 }
66 
67 //==============================================================================
69  // Do nothing
70 }
71 
72 //==============================================================================
74  // Do nothing
75 }
76 
77 //==============================================================================
79 
80 //==============================================================================
82  std::vector<CollisionObject*>& objs_) const {
83  objs_.resize(objs.size());
84  std::copy(objs.begin(), objs.end(), objs_.begin());
85 }
86 
87 //==============================================================================
90  callback->init();
91  if (size() == 0) return;
92 
93  for (auto* obj2 : objs) {
94  if ((*callback)(obj, obj2)) return;
95  }
96 }
97 
98 //==============================================================================
101  callback->init();
102  if (size() == 0) return;
103 
104  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
105  for (auto* obj2 : objs) {
106  if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
107  if ((*callback)(obj, obj2, min_dist)) return;
108  }
109  }
110 }
111 
112 //==============================================================================
114  callback->init();
115  if (size() == 0) return;
116 
117  for (typename std::list<CollisionObject*>::const_iterator it1 = objs.begin(),
118  end = objs.end();
119  it1 != end; ++it1) {
120  typename std::list<CollisionObject*>::const_iterator it2 = it1;
121  it2++;
122  for (; it2 != end; ++it2) {
123  if ((*it1)->getAABB().overlap((*it2)->getAABB())) {
124  if ((*callback)(*it1, *it2)) return;
125  }
126  }
127  }
128 }
129 
130 //==============================================================================
132  callback->init();
133  if (size() == 0) return;
134 
135  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
136  for (typename std::list<CollisionObject*>::const_iterator it1 = objs.begin(),
137  end = objs.end();
138  it1 != end; ++it1) {
139  typename std::list<CollisionObject*>::const_iterator it2 = it1;
140  it2++;
141  for (; it2 != end; ++it2) {
142  if ((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) {
143  if ((*callback)(*it1, *it2, min_dist)) return;
144  }
145  }
146  }
147 }
148 
149 //==============================================================================
152  callback->init();
153  NaiveCollisionManager* other_manager =
154  static_cast<NaiveCollisionManager*>(other_manager_);
155 
156  if ((size() == 0) || (other_manager->size() == 0)) return;
157 
158  if (this == other_manager) {
159  collide(callback);
160  return;
161  }
162 
163  for (auto* obj1 : objs) {
164  for (auto* obj2 : other_manager->objs) {
165  if (obj1->getAABB().overlap(obj2->getAABB())) {
166  if ((*callback)(obj1, obj2)) return;
167  }
168  }
169  }
170 }
171 
172 //==============================================================================
175  callback->init();
176  NaiveCollisionManager* other_manager =
177  static_cast<NaiveCollisionManager*>(other_manager_);
178 
179  if ((size() == 0) || (other_manager->size() == 0)) return;
180 
181  if (this == other_manager) {
183  return;
184  }
185 
186  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
187  for (auto* obj1 : objs) {
188  for (auto* obj2 : other_manager->objs) {
189  if (obj1->getAABB().distance(obj2->getAABB()) < min_dist) {
190  if ((*callback)(obj1, obj2, min_dist)) return;
191  }
192  }
193  }
194 }
195 
196 //==============================================================================
197 bool NaiveCollisionManager::empty() const { return objs.empty(); }
198 
199 //==============================================================================
200 size_t NaiveCollisionManager::size() const { return objs.size(); }
201 
202 } // namespace fcl
203 } // namespace hpp
collision_manager.callback
callback
Definition: collision_manager.py:27
hpp::fcl::NaiveCollisionManager::NaiveCollisionManager
NaiveCollisionManager()
Definition: broadphase_bruteforce.cpp:47
hpp::fcl::BroadPhaseCollisionManager
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:54
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::DistanceCallBackBase
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
hpp::fcl::NaiveCollisionManager::objs
std::list< CollisionObject * > objs
objects belonging to the manager are stored in a list structure
Definition: broadphase_bruteforce.h:108
hpp::fcl::NaiveCollisionManager::update
virtual void update()
update the condition of manager
Definition: broadphase_bruteforce.cpp:73
hpp::fcl::NaiveCollisionManager::size
size_t size() const
the number of objects managed by the manager
Definition: broadphase_bruteforce.cpp:200
hpp::fcl::NaiveCollisionManager::collide
void collide(CollisionObject *obj, CollisionCallBackBase *callback) const
perform collision test between one object and all the objects belonging to the manager
Definition: broadphase_bruteforce.cpp:88
hpp::fcl::BroadPhaseCollisionManager::getObjects
virtual std::vector< CollisionObject * > getObjects() const
return the objects managed by the manager
Definition: broadphase_collision_manager.h:88
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
hpp::fcl::AABB::distance
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
hpp::fcl::NaiveCollisionManager::distance
void distance(CollisionObject *obj, DistanceCallBackBase *callback) const
perform distance computation between one object and all the objects belonging to the manager
Definition: broadphase_bruteforce.cpp:99
hpp::fcl::NaiveCollisionManager::registerObjects
void registerObjects(const std::vector< CollisionObject * > &other_objs)
add objects to the manager
Definition: broadphase_bruteforce.cpp:52
hpp::fcl::NaiveCollisionManager::setup
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_bruteforce.cpp:68
hpp::fcl::NaiveCollisionManager::clear
void clear()
clear the manager
Definition: broadphase_bruteforce.cpp:78
hpp::fcl::NaiveCollisionManager
Brute force N-body collision manager.
Definition: broadphase_bruteforce.h:48
hpp::fcl::CollisionObject::getAABB
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:253
hpp::fcl::CollisionCallBackBase
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
hpp::fcl::NaiveCollisionManager::unregisterObject
void unregisterObject(CollisionObject *obj)
remove one object from the manager
Definition: broadphase_bruteforce.cpp:58
hpp::fcl::NaiveCollisionManager::empty
bool empty() const
whether the manager is empty
Definition: broadphase_bruteforce.cpp:197
broadphase_bruteforce.h
hpp::fcl::NaiveCollisionManager::registerObject
void registerObject(CollisionObject *obj)
add one object to the manager
Definition: broadphase_bruteforce.cpp:63


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:12