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 |
|
|
|
36 |
|
|
/** @author Jia Pan */ |
37 |
|
|
|
38 |
|
|
#include "hpp/fcl/broadphase/broadphase_bruteforce.h" |
39 |
|
|
|
40 |
|
|
#include <iterator> |
41 |
|
|
#include <algorithm> |
42 |
|
|
|
43 |
|
|
namespace hpp { |
44 |
|
|
namespace fcl { |
45 |
|
|
|
46 |
|
|
//============================================================================== |
47 |
|
59 |
NaiveCollisionManager::NaiveCollisionManager() { |
48 |
|
|
// Do nothing |
49 |
|
59 |
} |
50 |
|
|
|
51 |
|
|
//============================================================================== |
52 |
|
51 |
void NaiveCollisionManager::registerObjects( |
53 |
|
|
const std::vector<CollisionObject*>& other_objs) { |
54 |
|
51 |
std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs)); |
55 |
|
51 |
} |
56 |
|
|
|
57 |
|
|
//============================================================================== |
58 |
|
|
void NaiveCollisionManager::unregisterObject(CollisionObject* obj) { |
59 |
|
|
objs.remove(obj); |
60 |
|
|
} |
61 |
|
|
|
62 |
|
|
//============================================================================== |
63 |
|
7260 |
void NaiveCollisionManager::registerObject(CollisionObject* obj) { |
64 |
|
7260 |
objs.push_back(obj); |
65 |
|
7260 |
} |
66 |
|
|
|
67 |
|
|
//============================================================================== |
68 |
|
59 |
void NaiveCollisionManager::setup() { |
69 |
|
|
// Do nothing |
70 |
|
59 |
} |
71 |
|
|
|
72 |
|
|
//============================================================================== |
73 |
|
13 |
void NaiveCollisionManager::update() { |
74 |
|
|
// Do nothing |
75 |
|
13 |
} |
76 |
|
|
|
77 |
|
|
//============================================================================== |
78 |
|
|
void NaiveCollisionManager::clear() { objs.clear(); } |
79 |
|
|
|
80 |
|
|
//============================================================================== |
81 |
|
|
void NaiveCollisionManager::getObjects( |
82 |
|
|
std::vector<CollisionObject*>& objs_) const { |
83 |
|
|
objs_.resize(objs.size()); |
84 |
|
|
std::copy(objs.begin(), objs.end(), objs_.begin()); |
85 |
|
|
} |
86 |
|
|
|
87 |
|
|
//============================================================================== |
88 |
|
4646 |
void NaiveCollisionManager::collide(CollisionObject* obj, |
89 |
|
|
CollisionCallBackBase* callback) const { |
90 |
|
4646 |
callback->init(); |
91 |
✓✓ |
4646 |
if (size() == 0) return; |
92 |
|
|
|
93 |
✓✓ |
1225309 |
for (auto* obj2 : objs) { |
94 |
✓✗✓✓
|
1221467 |
if ((*callback)(obj, obj2)) return; |
95 |
|
|
} |
96 |
|
|
} |
97 |
|
|
|
98 |
|
|
//============================================================================== |
99 |
|
80 |
void NaiveCollisionManager::distance(CollisionObject* obj, |
100 |
|
|
DistanceCallBackBase* callback) const { |
101 |
✓✗ |
80 |
callback->init(); |
102 |
✓✗✗✓
|
80 |
if (size() == 0) return; |
103 |
|
|
|
104 |
|
80 |
FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)(); |
105 |
✓✓ |
72680 |
for (auto* obj2 : objs) { |
106 |
✓✗✓✓
|
72600 |
if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { |
107 |
✓✗✗✓
|
889 |
if ((*callback)(obj, obj2, min_dist)) return; |
108 |
|
|
} |
109 |
|
|
} |
110 |
|
|
} |
111 |
|
|
|
112 |
|
|
//============================================================================== |
113 |
|
37 |
void NaiveCollisionManager::collide(CollisionCallBackBase* callback) const { |
114 |
|
37 |
callback->init(); |
115 |
✓✓ |
37 |
if (size() == 0) return; |
116 |
|
|
|
117 |
|
2252 |
for (typename std::list<CollisionObject*>::const_iterator it1 = objs.begin(), |
118 |
|
29 |
end = objs.end(); |
119 |
✓✓ |
4475 |
it1 != end; ++it1) { |
120 |
|
2224 |
typename std::list<CollisionObject*>::const_iterator it2 = it1; |
121 |
|
2224 |
it2++; |
122 |
✓✓ |
300029 |
for (; it2 != end; ++it2) { |
123 |
✓✗✓✓
|
297806 |
if ((*it1)->getAABB().overlap((*it2)->getAABB())) { |
124 |
✓✗✓✓
|
6 |
if ((*callback)(*it1, *it2)) return; |
125 |
|
|
} |
126 |
|
|
} |
127 |
|
|
} |
128 |
|
|
} |
129 |
|
|
|
130 |
|
|
//============================================================================== |
131 |
|
6 |
void NaiveCollisionManager::distance(DistanceCallBackBase* callback) const { |
132 |
✓✗ |
6 |
callback->init(); |
133 |
✓✗✗✓
|
6 |
if (size() == 0) return; |
134 |
|
|
|
135 |
|
6 |
FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)(); |
136 |
|
2996 |
for (typename std::list<CollisionObject*>::const_iterator it1 = objs.begin(), |
137 |
|
6 |
end = objs.end(); |
138 |
✓✓ |
5986 |
it1 != end; ++it1) { |
139 |
|
2990 |
typename std::list<CollisionObject*>::const_iterator it2 = it1; |
140 |
|
2990 |
it2++; |
141 |
✓✓ |
1549828 |
for (; it2 != end; ++it2) { |
142 |
✓✗✓✓
|
1546838 |
if ((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) { |
143 |
✓✗✗✓
|
478 |
if ((*callback)(*it1, *it2, min_dist)) return; |
144 |
|
|
} |
145 |
|
|
} |
146 |
|
|
} |
147 |
|
|
} |
148 |
|
|
|
149 |
|
|
//============================================================================== |
150 |
|
|
void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, |
151 |
|
|
CollisionCallBackBase* callback) const { |
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 |
|
|
//============================================================================== |
173 |
|
|
void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, |
174 |
|
|
DistanceCallBackBase* callback) const { |
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) { |
182 |
|
|
distance(callback); |
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 |
|
4769 |
size_t NaiveCollisionManager::size() const { return objs.size(); } |
201 |
|
|
|
202 |
|
|
} // namespace fcl |
203 |
|
|
} // namespace hpp |