1 |
|
|
/* |
2 |
|
|
* Software License Agreement (BSD License) |
3 |
|
|
* |
4 |
|
|
* Copyright (c) 2011-2014, Willow Garage, Inc. |
5 |
|
|
* Copyright (c) 2014-2015, 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 |
|
|
#ifndef HPP_FCL_TRAVERSAL_NODE_OCTREE_H |
39 |
|
|
#define HPP_FCL_TRAVERSAL_NODE_OCTREE_H |
40 |
|
|
|
41 |
|
|
/// @cond INTERNAL |
42 |
|
|
|
43 |
|
|
#include <hpp/fcl/collision_data.h> |
44 |
|
|
#include <hpp/fcl/internal/traversal_node_base.h> |
45 |
|
|
#include <hpp/fcl/narrowphase/narrowphase.h> |
46 |
|
|
#include <hpp/fcl/octree.h> |
47 |
|
|
#include <hpp/fcl/BVH/BVH_model.h> |
48 |
|
|
#include <hpp/fcl/shape/geometric_shapes_utility.h> |
49 |
|
|
#include <hpp/fcl/internal/shape_shape_func.h> |
50 |
|
|
|
51 |
|
|
namespace hpp { |
52 |
|
|
namespace fcl { |
53 |
|
|
|
54 |
|
|
/// @brief Algorithms for collision related with octree |
55 |
|
|
class HPP_FCL_DLLAPI OcTreeSolver { |
56 |
|
|
private: |
57 |
|
|
const GJKSolver* solver; |
58 |
|
|
|
59 |
|
|
mutable const CollisionRequest* crequest; |
60 |
|
|
mutable const DistanceRequest* drequest; |
61 |
|
|
|
62 |
|
|
mutable CollisionResult* cresult; |
63 |
|
|
mutable DistanceResult* dresult; |
64 |
|
|
|
65 |
|
|
public: |
66 |
|
100 |
OcTreeSolver(const GJKSolver* solver_) |
67 |
|
100 |
: solver(solver_), |
68 |
|
|
crequest(NULL), |
69 |
|
|
drequest(NULL), |
70 |
|
|
cresult(NULL), |
71 |
|
100 |
dresult(NULL) {} |
72 |
|
|
|
73 |
|
|
/// @brief collision between two octrees |
74 |
|
|
void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, |
75 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
76 |
|
|
const CollisionRequest& request_, |
77 |
|
|
CollisionResult& result_) const { |
78 |
|
|
crequest = &request_; |
79 |
|
|
cresult = &result_; |
80 |
|
|
|
81 |
|
|
OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, |
82 |
|
|
tree2->getRoot(), tree2->getRootBV(), tf1, tf2); |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
/// @brief distance between two octrees |
86 |
|
|
void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, |
87 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
88 |
|
|
const DistanceRequest& request_, |
89 |
|
|
DistanceResult& result_) const { |
90 |
|
|
drequest = &request_; |
91 |
|
|
dresult = &result_; |
92 |
|
|
|
93 |
|
|
OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, |
94 |
|
|
tree2->getRoot(), tree2->getRootBV(), tf1, tf2); |
95 |
|
|
} |
96 |
|
|
|
97 |
|
|
/// @brief collision between octree and mesh |
98 |
|
|
template <typename BV> |
99 |
|
200 |
void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2, |
100 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
101 |
|
|
const CollisionRequest& request_, |
102 |
|
|
CollisionResult& result_) const { |
103 |
|
200 |
crequest = &request_; |
104 |
|
200 |
cresult = &result_; |
105 |
|
|
|
106 |
✓✗✓✗
|
200 |
OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), |
107 |
|
|
tree2, 0, tf1, tf2); |
108 |
|
|
} |
109 |
|
|
|
110 |
|
|
/// @brief distance between octree and mesh |
111 |
|
|
template <typename BV> |
112 |
|
|
void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2, |
113 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
114 |
|
|
const DistanceRequest& request_, |
115 |
|
|
DistanceResult& result_) const { |
116 |
|
|
drequest = &request_; |
117 |
|
|
dresult = &result_; |
118 |
|
|
|
119 |
|
|
OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), |
120 |
|
|
tree2, 0, tf1, tf2); |
121 |
|
|
} |
122 |
|
|
|
123 |
|
|
/// @brief collision between mesh and octree |
124 |
|
|
template <typename BV> |
125 |
|
|
void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2, |
126 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
127 |
|
|
const CollisionRequest& request_, |
128 |
|
|
CollisionResult& result_) const |
129 |
|
|
|
130 |
|
|
{ |
131 |
|
|
crequest = &request_; |
132 |
|
|
cresult = &result_; |
133 |
|
|
|
134 |
|
|
OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), |
135 |
|
|
tree1, 0, tf2, tf1); |
136 |
|
|
} |
137 |
|
|
|
138 |
|
|
/// @brief distance between mesh and octree |
139 |
|
|
template <typename BV> |
140 |
|
|
void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2, |
141 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
142 |
|
|
const DistanceRequest& request_, |
143 |
|
|
DistanceResult& result_) const { |
144 |
|
|
drequest = &request_; |
145 |
|
|
dresult = &result_; |
146 |
|
|
|
147 |
|
|
OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(), |
148 |
|
|
tree2->getRootBV(), tf1, tf2); |
149 |
|
|
} |
150 |
|
|
|
151 |
|
|
/// @brief collision between octree and shape |
152 |
|
|
template <typename S> |
153 |
|
|
void OcTreeShapeIntersect(const OcTree* tree, const S& s, |
154 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
155 |
|
|
const CollisionRequest& request_, |
156 |
|
|
CollisionResult& result_) const { |
157 |
|
|
crequest = &request_; |
158 |
|
|
cresult = &result_; |
159 |
|
|
|
160 |
|
|
AABB bv2; |
161 |
|
|
computeBV<AABB>(s, Transform3f(), bv2); |
162 |
|
|
OBB obb2; |
163 |
|
|
convertBV(bv2, tf2, obb2); |
164 |
|
|
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, |
165 |
|
|
obb2, tf1, tf2); |
166 |
|
|
} |
167 |
|
|
|
168 |
|
|
/// @brief collision between shape and octree |
169 |
|
|
template <typename S> |
170 |
|
|
void ShapeOcTreeIntersect(const S& s, const OcTree* tree, |
171 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
172 |
|
|
const CollisionRequest& request_, |
173 |
|
|
CollisionResult& result_) const { |
174 |
|
|
crequest = &request_; |
175 |
|
|
cresult = &result_; |
176 |
|
|
|
177 |
|
|
AABB bv1; |
178 |
|
|
computeBV<AABB>(s, Transform3f(), bv1); |
179 |
|
|
OBB obb1; |
180 |
|
|
convertBV(bv1, tf1, obb1); |
181 |
|
|
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, |
182 |
|
|
obb1, tf2, tf1); |
183 |
|
|
} |
184 |
|
|
|
185 |
|
|
/// @brief distance between octree and shape |
186 |
|
|
template <typename S> |
187 |
|
|
void OcTreeShapeDistance(const OcTree* tree, const S& s, |
188 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
189 |
|
|
const DistanceRequest& request_, |
190 |
|
|
DistanceResult& result_) const { |
191 |
|
|
drequest = &request_; |
192 |
|
|
dresult = &result_; |
193 |
|
|
|
194 |
|
|
AABB aabb2; |
195 |
|
|
computeBV<AABB>(s, tf2, aabb2); |
196 |
|
|
OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, |
197 |
|
|
aabb2, tf1, tf2); |
198 |
|
|
} |
199 |
|
|
|
200 |
|
|
/// @brief distance between shape and octree |
201 |
|
|
template <typename S> |
202 |
|
|
void ShapeOcTreeDistance(const S& s, const OcTree* tree, |
203 |
|
|
const Transform3f& tf1, const Transform3f& tf2, |
204 |
|
|
const DistanceRequest& request_, |
205 |
|
|
DistanceResult& result_) const { |
206 |
|
|
drequest = &request_; |
207 |
|
|
dresult = &result_; |
208 |
|
|
|
209 |
|
|
AABB aabb1; |
210 |
|
|
computeBV<AABB>(s, tf1, aabb1); |
211 |
|
|
OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, |
212 |
|
|
aabb1, tf2, tf1); |
213 |
|
|
} |
214 |
|
|
|
215 |
|
|
private: |
216 |
|
|
template <typename S> |
217 |
|
|
bool OcTreeShapeDistanceRecurse(const OcTree* tree1, |
218 |
|
|
const OcTree::OcTreeNode* root1, |
219 |
|
|
const AABB& bv1, const S& s, |
220 |
|
|
const AABB& aabb2, const Transform3f& tf1, |
221 |
|
|
const Transform3f& tf2) const { |
222 |
|
|
if (!tree1->nodeHasChildren(root1)) { |
223 |
|
|
if (tree1->isNodeOccupied(root1)) { |
224 |
|
|
Box box; |
225 |
|
|
Transform3f box_tf; |
226 |
|
|
constructBox(bv1, tf1, box, box_tf); |
227 |
|
|
|
228 |
|
|
FCL_REAL dist; |
229 |
|
|
Vec3f closest_p1, closest_p2, normal; |
230 |
|
|
solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1, closest_p2, |
231 |
|
|
normal); |
232 |
|
|
|
233 |
|
|
dresult->update(dist, tree1, &s, (int)(root1 - tree1->getRoot()), |
234 |
|
|
DistanceResult::NONE, closest_p1, closest_p2, normal); |
235 |
|
|
|
236 |
|
|
return drequest->isSatisfied(*dresult); |
237 |
|
|
} else |
238 |
|
|
return false; |
239 |
|
|
} |
240 |
|
|
|
241 |
|
|
if (!tree1->isNodeOccupied(root1)) return false; |
242 |
|
|
|
243 |
|
|
for (unsigned int i = 0; i < 8; ++i) { |
244 |
|
|
if (tree1->nodeChildExists(root1, i)) { |
245 |
|
|
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
246 |
|
|
AABB child_bv; |
247 |
|
|
computeChildBV(bv1, i, child_bv); |
248 |
|
|
|
249 |
|
|
AABB aabb1; |
250 |
|
|
convertBV(child_bv, tf1, aabb1); |
251 |
|
|
FCL_REAL d = aabb1.distance(aabb2); |
252 |
|
|
if (d < dresult->min_distance) { |
253 |
|
|
if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, |
254 |
|
|
tf2)) |
255 |
|
|
return true; |
256 |
|
|
} |
257 |
|
|
} |
258 |
|
|
} |
259 |
|
|
|
260 |
|
|
return false; |
261 |
|
|
} |
262 |
|
|
|
263 |
|
|
template <typename S> |
264 |
|
|
bool OcTreeShapeIntersectRecurse(const OcTree* tree1, |
265 |
|
|
const OcTree::OcTreeNode* root1, |
266 |
|
|
const AABB& bv1, const S& s, const OBB& obb2, |
267 |
|
|
const Transform3f& tf1, |
268 |
|
|
const Transform3f& tf2) const { |
269 |
|
|
// Empty OcTree is considered free. |
270 |
|
|
if (!root1) return false; |
271 |
|
|
|
272 |
|
|
/// stop when 1) bounding boxes of two objects not overlap; OR |
273 |
|
|
/// 2) at least of one the nodes is free; OR |
274 |
|
|
/// 2) (two uncertain nodes or one node occupied and one node |
275 |
|
|
/// uncertain) AND cost not required |
276 |
|
|
if (tree1->isNodeFree(root1)) |
277 |
|
|
return false; |
278 |
|
|
else if ((tree1->isNodeUncertain(root1) || s.isUncertain())) |
279 |
|
|
return false; |
280 |
|
|
else { |
281 |
|
|
OBB obb1; |
282 |
|
|
convertBV(bv1, tf1, obb1); |
283 |
|
|
FCL_REAL sqrDistLowerBound; |
284 |
|
|
if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { |
285 |
|
|
internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, |
286 |
|
|
sqrDistLowerBound); |
287 |
|
|
return false; |
288 |
|
|
} |
289 |
|
|
} |
290 |
|
|
|
291 |
|
|
if (!tree1->nodeHasChildren(root1)) { |
292 |
|
|
assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. |
293 |
|
|
|
294 |
|
|
Box box; |
295 |
|
|
Transform3f box_tf; |
296 |
|
|
constructBox(bv1, tf1, box, box_tf); |
297 |
|
|
|
298 |
|
|
bool contactNotAdded = |
299 |
|
|
(cresult->numContacts() >= crequest->num_max_contacts); |
300 |
|
|
std::size_t ncontact = ShapeShapeCollide<Box, S>( |
301 |
|
|
&box, box_tf, &s, tf2, solver, *crequest, *cresult); |
302 |
|
|
assert(ncontact == 0 || ncontact == 1); |
303 |
|
|
if (!contactNotAdded && ncontact == 1) { |
304 |
|
|
// Update contact information. |
305 |
|
|
const Contact& c = cresult->getContact(cresult->numContacts() - 1); |
306 |
|
|
cresult->setContact( |
307 |
|
|
cresult->numContacts() - 1, |
308 |
|
|
Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()), |
309 |
|
|
c.b2, c.pos, c.normal, c.penetration_depth)); |
310 |
|
|
} |
311 |
|
|
|
312 |
|
|
return crequest->isSatisfied(*cresult); |
313 |
|
|
} |
314 |
|
|
|
315 |
|
|
for (unsigned int i = 0; i < 8; ++i) { |
316 |
|
|
if (tree1->nodeChildExists(root1, i)) { |
317 |
|
|
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
318 |
|
|
AABB child_bv; |
319 |
|
|
computeChildBV(bv1, i, child_bv); |
320 |
|
|
|
321 |
|
|
if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, |
322 |
|
|
tf2)) |
323 |
|
|
return true; |
324 |
|
|
} |
325 |
|
|
} |
326 |
|
|
|
327 |
|
|
return false; |
328 |
|
|
} |
329 |
|
|
|
330 |
|
|
template <typename BV> |
331 |
|
|
bool OcTreeMeshDistanceRecurse(const OcTree* tree1, |
332 |
|
|
const OcTree::OcTreeNode* root1, |
333 |
|
|
const AABB& bv1, const BVHModel<BV>* tree2, |
334 |
|
|
unsigned int root2, const Transform3f& tf1, |
335 |
|
|
const Transform3f& tf2) const { |
336 |
|
|
if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { |
337 |
|
|
if (tree1->isNodeOccupied(root1)) { |
338 |
|
|
Box box; |
339 |
|
|
Transform3f box_tf; |
340 |
|
|
constructBox(bv1, tf1, box, box_tf); |
341 |
|
|
|
342 |
|
|
int primitive_id = tree2->getBV(root2).primitiveId(); |
343 |
|
|
const Triangle& tri_id = tree2->tri_indices[primitive_id]; |
344 |
|
|
const Vec3f& p1 = tree2->vertices[tri_id[0]]; |
345 |
|
|
const Vec3f& p2 = tree2->vertices[tri_id[1]]; |
346 |
|
|
const Vec3f& p3 = tree2->vertices[tri_id[2]]; |
347 |
|
|
|
348 |
|
|
FCL_REAL dist; |
349 |
|
|
Vec3f closest_p1, closest_p2, normal; |
350 |
|
|
solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, dist, |
351 |
|
|
closest_p1, closest_p2, normal); |
352 |
|
|
|
353 |
|
|
dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()), |
354 |
|
|
primitive_id, closest_p1, closest_p2, normal); |
355 |
|
|
|
356 |
|
|
return drequest->isSatisfied(*dresult); |
357 |
|
|
} else |
358 |
|
|
return false; |
359 |
|
|
} |
360 |
|
|
|
361 |
|
|
if (!tree1->isNodeOccupied(root1)) return false; |
362 |
|
|
|
363 |
|
|
if (tree2->getBV(root2).isLeaf() || |
364 |
|
|
(tree1->nodeHasChildren(root1) && |
365 |
|
|
(bv1.size() > tree2->getBV(root2).bv.size()))) { |
366 |
|
|
for (unsigned int i = 0; i < 8; ++i) { |
367 |
|
|
if (tree1->nodeChildExists(root1, i)) { |
368 |
|
|
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
369 |
|
|
AABB child_bv; |
370 |
|
|
computeChildBV(bv1, i, child_bv); |
371 |
|
|
|
372 |
|
|
FCL_REAL d; |
373 |
|
|
AABB aabb1, aabb2; |
374 |
|
|
convertBV(child_bv, tf1, aabb1); |
375 |
|
|
convertBV(tree2->getBV(root2).bv, tf2, aabb2); |
376 |
|
|
d = aabb1.distance(aabb2); |
377 |
|
|
|
378 |
|
|
if (d < dresult->min_distance) { |
379 |
|
|
if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, |
380 |
|
|
tf1, tf2)) |
381 |
|
|
return true; |
382 |
|
|
} |
383 |
|
|
} |
384 |
|
|
} |
385 |
|
|
} else { |
386 |
|
|
FCL_REAL d; |
387 |
|
|
AABB aabb1, aabb2; |
388 |
|
|
convertBV(bv1, tf1, aabb1); |
389 |
|
|
unsigned int child = (unsigned int)tree2->getBV(root2).leftChild(); |
390 |
|
|
convertBV(tree2->getBV(child).bv, tf2, aabb2); |
391 |
|
|
d = aabb1.distance(aabb2); |
392 |
|
|
|
393 |
|
|
if (d < dresult->min_distance) { |
394 |
|
|
if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, |
395 |
|
|
tf2)) |
396 |
|
|
return true; |
397 |
|
|
} |
398 |
|
|
|
399 |
|
|
child = (unsigned int)tree2->getBV(root2).rightChild(); |
400 |
|
|
convertBV(tree2->getBV(child).bv, tf2, aabb2); |
401 |
|
|
d = aabb1.distance(aabb2); |
402 |
|
|
|
403 |
|
|
if (d < dresult->min_distance) { |
404 |
|
|
if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, |
405 |
|
|
tf2)) |
406 |
|
|
return true; |
407 |
|
|
} |
408 |
|
|
} |
409 |
|
|
|
410 |
|
|
return false; |
411 |
|
|
} |
412 |
|
|
|
413 |
|
|
/// \return True if the request is satisfied. |
414 |
|
|
template <typename BV> |
415 |
|
57964 |
bool OcTreeMeshIntersectRecurse(const OcTree* tree1, |
416 |
|
|
const OcTree::OcTreeNode* root1, |
417 |
|
|
const AABB& bv1, const BVHModel<BV>* tree2, |
418 |
|
|
unsigned int root2, const Transform3f& tf1, |
419 |
|
|
const Transform3f& tf2) const { |
420 |
|
|
// FIXME(jmirabel) I do not understand why the BVHModel was traversed. The |
421 |
|
|
// code in this if(!root1) did not output anything so the empty OcTree is |
422 |
|
|
// considered free. Should an empty OcTree be considered free ? |
423 |
|
|
|
424 |
|
|
// Empty OcTree is considered free. |
425 |
✗✓ |
57964 |
if (!root1) return false; |
426 |
|
57964 |
BVNode<BV> const& bvn2 = tree2->getBV(root2); |
427 |
|
|
|
428 |
|
|
/// stop when 1) bounding boxes of two objects not overlap; OR |
429 |
|
|
/// 2) at least one of the nodes is free; OR |
430 |
|
|
/// 2) (two uncertain nodes OR one node occupied and one node |
431 |
|
|
/// uncertain) AND cost not required |
432 |
✗✓ |
57964 |
if (tree1->isNodeFree(root1)) |
433 |
|
|
return false; |
434 |
✓✗✗✓
✗✓ |
57964 |
else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) |
435 |
|
|
return false; |
436 |
|
|
else { |
437 |
✓✗✓✗
|
57964 |
OBB obb1, obb2; |
438 |
✓✗ |
57964 |
convertBV(bv1, tf1, obb1); |
439 |
✓✗ |
57964 |
convertBV(bvn2.bv, tf2, obb2); |
440 |
|
|
FCL_REAL sqrDistLowerBound; |
441 |
✓✗✓✓
|
57964 |
if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { |
442 |
|
34860 |
internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, |
443 |
|
|
sqrDistLowerBound); |
444 |
|
34860 |
return false; |
445 |
|
|
} |
446 |
|
|
} |
447 |
|
|
|
448 |
|
|
// Check if leaf collides. |
449 |
✓✓✓✗
✓✓ |
23104 |
if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { |
450 |
✗✓ |
1078 |
assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. |
451 |
✓✗ |
2156 |
Box box; |
452 |
✓✗ |
1078 |
Transform3f box_tf; |
453 |
✓✗ |
1078 |
constructBox(bv1, tf1, box, box_tf); |
454 |
|
|
|
455 |
|
1078 |
int primitive_id = bvn2.primitiveId(); |
456 |
|
1078 |
const Triangle& tri_id = tree2->tri_indices[primitive_id]; |
457 |
|
1078 |
const Vec3f& p1 = tree2->vertices[tri_id[0]]; |
458 |
|
1078 |
const Vec3f& p2 = tree2->vertices[tri_id[1]]; |
459 |
|
1078 |
const Vec3f& p3 = tree2->vertices[tri_id[2]]; |
460 |
|
|
|
461 |
✓✗✓✗
✓✗ |
1078 |
Vec3f c1, c2, normal; |
462 |
|
|
FCL_REAL distance; |
463 |
|
|
|
464 |
✓✗ |
1078 |
bool collision = solver->shapeTriangleInteraction( |
465 |
|
|
box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal); |
466 |
|
1078 |
FCL_REAL distToCollision = distance - crequest->security_margin; |
467 |
|
|
|
468 |
✓✗ |
1078 |
if (cresult->numContacts() < crequest->num_max_contacts) { |
469 |
✓✓ |
1078 |
if (collision) { |
470 |
✓✗ |
106 |
cresult->addContact(Contact(tree1, tree2, |
471 |
✓✗✓✗
|
106 |
(int)(root1 - tree1->getRoot()), |
472 |
|
|
primitive_id, c1, normal, -distance)); |
473 |
✗✓ |
972 |
} else if (distToCollision < 0) { |
474 |
|
|
cresult->addContact(Contact( |
475 |
|
|
tree1, tree2, (int)(root1 - tree1->getRoot()), primitive_id, |
476 |
|
|
.5 * (c1 + c2), (c2 - c1).normalized(), -distance)); |
477 |
|
|
} |
478 |
|
|
} |
479 |
✓✗ |
1078 |
internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult, |
480 |
|
|
distToCollision, c1, c2); |
481 |
|
|
|
482 |
✓✗ |
1078 |
return crequest->isSatisfied(*cresult); |
483 |
|
|
} |
484 |
|
|
|
485 |
|
|
// Determine which tree to traverse first. |
486 |
✓✓✓✓
|
39966 |
if (bvn2.isLeaf() || |
487 |
✓✗✓✓
|
17940 |
(tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { |
488 |
✓✓ |
95572 |
for (unsigned int i = 0; i < 8; ++i) { |
489 |
✓✓ |
85992 |
if (tree1->nodeChildExists(root1, i)) { |
490 |
✓✗ |
36610 |
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
491 |
✓✗ |
36610 |
AABB child_bv; |
492 |
✓✗ |
36610 |
computeChildBV(bv1, i, child_bv); |
493 |
|
|
|
494 |
✓✗✓✓
|
36610 |
if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, |
495 |
|
|
tf1, tf2)) |
496 |
|
1620 |
return true; |
497 |
|
|
} |
498 |
|
|
} |
499 |
|
|
} else { |
500 |
✓✓ |
10826 |
if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, |
501 |
|
10826 |
(unsigned int)bvn2.leftChild(), tf1, tf2)) |
502 |
|
498 |
return true; |
503 |
|
|
|
504 |
✓✓ |
10328 |
if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, |
505 |
|
10328 |
(unsigned int)bvn2.rightChild(), tf1, tf2)) |
506 |
|
294 |
return true; |
507 |
|
|
} |
508 |
|
|
|
509 |
|
19614 |
return false; |
510 |
|
|
} |
511 |
|
|
|
512 |
|
|
bool OcTreeDistanceRecurse(const OcTree* tree1, |
513 |
|
|
const OcTree::OcTreeNode* root1, const AABB& bv1, |
514 |
|
|
const OcTree* tree2, |
515 |
|
|
const OcTree::OcTreeNode* root2, const AABB& bv2, |
516 |
|
|
const Transform3f& tf1, |
517 |
|
|
const Transform3f& tf2) const { |
518 |
|
|
if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { |
519 |
|
|
if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { |
520 |
|
|
Box box1, box2; |
521 |
|
|
Transform3f box1_tf, box2_tf; |
522 |
|
|
constructBox(bv1, tf1, box1, box1_tf); |
523 |
|
|
constructBox(bv2, tf2, box2, box2_tf); |
524 |
|
|
|
525 |
|
|
FCL_REAL dist; |
526 |
|
|
Vec3f closest_p1, closest_p2, normal; |
527 |
|
|
solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist, closest_p1, |
528 |
|
|
closest_p2, normal); |
529 |
|
|
|
530 |
|
|
dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()), |
531 |
|
|
(int)(root2 - tree2->getRoot()), closest_p1, closest_p2, |
532 |
|
|
normal); |
533 |
|
|
|
534 |
|
|
return drequest->isSatisfied(*dresult); |
535 |
|
|
} else |
536 |
|
|
return false; |
537 |
|
|
} |
538 |
|
|
|
539 |
|
|
if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) |
540 |
|
|
return false; |
541 |
|
|
|
542 |
|
|
if (!tree2->nodeHasChildren(root2) || |
543 |
|
|
(tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { |
544 |
|
|
for (unsigned int i = 0; i < 8; ++i) { |
545 |
|
|
if (tree1->nodeChildExists(root1, i)) { |
546 |
|
|
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
547 |
|
|
AABB child_bv; |
548 |
|
|
computeChildBV(bv1, i, child_bv); |
549 |
|
|
|
550 |
|
|
FCL_REAL d; |
551 |
|
|
AABB aabb1, aabb2; |
552 |
|
|
convertBV(bv1, tf1, aabb1); |
553 |
|
|
convertBV(bv2, tf2, aabb2); |
554 |
|
|
d = aabb1.distance(aabb2); |
555 |
|
|
|
556 |
|
|
if (d < dresult->min_distance) { |
557 |
|
|
if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, |
558 |
|
|
tf1, tf2)) |
559 |
|
|
return true; |
560 |
|
|
} |
561 |
|
|
} |
562 |
|
|
} |
563 |
|
|
} else { |
564 |
|
|
for (unsigned int i = 0; i < 8; ++i) { |
565 |
|
|
if (tree2->nodeChildExists(root2, i)) { |
566 |
|
|
const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); |
567 |
|
|
AABB child_bv; |
568 |
|
|
computeChildBV(bv2, i, child_bv); |
569 |
|
|
|
570 |
|
|
FCL_REAL d; |
571 |
|
|
AABB aabb1, aabb2; |
572 |
|
|
convertBV(bv1, tf1, aabb1); |
573 |
|
|
convertBV(bv2, tf2, aabb2); |
574 |
|
|
d = aabb1.distance(aabb2); |
575 |
|
|
|
576 |
|
|
if (d < dresult->min_distance) { |
577 |
|
|
if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, |
578 |
|
|
tf1, tf2)) |
579 |
|
|
return true; |
580 |
|
|
} |
581 |
|
|
} |
582 |
|
|
} |
583 |
|
|
} |
584 |
|
|
|
585 |
|
|
return false; |
586 |
|
|
} |
587 |
|
|
|
588 |
|
|
bool OcTreeIntersectRecurse(const OcTree* tree1, |
589 |
|
|
const OcTree::OcTreeNode* root1, const AABB& bv1, |
590 |
|
|
const OcTree* tree2, |
591 |
|
|
const OcTree::OcTreeNode* root2, const AABB& bv2, |
592 |
|
|
const Transform3f& tf1, |
593 |
|
|
const Transform3f& tf2) const { |
594 |
|
|
// Empty OcTree is considered free. |
595 |
|
|
if (!root1) return false; |
596 |
|
|
if (!root2) return false; |
597 |
|
|
|
598 |
|
|
/// stop when 1) bounding boxes of two objects not overlap; OR |
599 |
|
|
/// 2) at least one of the nodes is free; OR |
600 |
|
|
/// 2) (two uncertain nodes OR one node occupied and one node |
601 |
|
|
/// uncertain) AND cost not required |
602 |
|
|
if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) |
603 |
|
|
return false; |
604 |
|
|
else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2))) |
605 |
|
|
return false; |
606 |
|
|
|
607 |
|
|
bool bothAreLeaves = |
608 |
|
|
(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)); |
609 |
|
|
if (!bothAreLeaves || !crequest->enable_contact) { |
610 |
|
|
OBB obb1, obb2; |
611 |
|
|
convertBV(bv1, tf1, obb1); |
612 |
|
|
convertBV(bv2, tf2, obb2); |
613 |
|
|
FCL_REAL sqrDistLowerBound; |
614 |
|
|
if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { |
615 |
|
|
if (cresult->distance_lower_bound > 0 && |
616 |
|
|
sqrDistLowerBound < |
617 |
|
|
cresult->distance_lower_bound * cresult->distance_lower_bound) |
618 |
|
|
cresult->distance_lower_bound = |
619 |
|
|
sqrt(sqrDistLowerBound) - crequest->security_margin; |
620 |
|
|
return false; |
621 |
|
|
} |
622 |
|
|
if (!crequest->enable_contact) { // Overlap |
623 |
|
|
if (cresult->numContacts() < crequest->num_max_contacts) |
624 |
|
|
cresult->addContact( |
625 |
|
|
Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), |
626 |
|
|
static_cast<int>(root2 - tree2->getRoot()))); |
627 |
|
|
return crequest->isSatisfied(*cresult); |
628 |
|
|
} |
629 |
|
|
} |
630 |
|
|
|
631 |
|
|
// Both node are leaves |
632 |
|
|
if (bothAreLeaves) { |
633 |
|
|
assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)); |
634 |
|
|
|
635 |
|
|
Box box1, box2; |
636 |
|
|
Transform3f box1_tf, box2_tf; |
637 |
|
|
constructBox(bv1, tf1, box1, box1_tf); |
638 |
|
|
constructBox(bv2, tf2, box2, box2_tf); |
639 |
|
|
|
640 |
|
|
FCL_REAL distance; |
641 |
|
|
Vec3f c1, c2, normal; |
642 |
|
|
bool collision = solver->shapeDistance(box1, box1_tf, box2, box2_tf, |
643 |
|
|
distance, c1, c2, normal); |
644 |
|
|
FCL_REAL distToCollision = distance - crequest->security_margin; |
645 |
|
|
|
646 |
|
|
if (cresult->numContacts() < crequest->num_max_contacts) { |
647 |
|
|
if (collision) |
648 |
|
|
cresult->addContact( |
649 |
|
|
Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), |
650 |
|
|
static_cast<int>(root2 - tree2->getRoot()), c1, normal, |
651 |
|
|
-distance)); |
652 |
|
|
else if (distToCollision <= 0) |
653 |
|
|
cresult->addContact( |
654 |
|
|
Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), |
655 |
|
|
static_cast<int>(root2 - tree2->getRoot()), |
656 |
|
|
.5 * (c1 + c2), (c2 - c1).normalized(), -distance)); |
657 |
|
|
} |
658 |
|
|
internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult, |
659 |
|
|
distToCollision, c1, c2); |
660 |
|
|
|
661 |
|
|
return crequest->isSatisfied(*cresult); |
662 |
|
|
} |
663 |
|
|
|
664 |
|
|
// Determine which tree to traverse first. |
665 |
|
|
if (!tree2->nodeHasChildren(root2) || |
666 |
|
|
(tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { |
667 |
|
|
for (unsigned int i = 0; i < 8; ++i) { |
668 |
|
|
if (tree1->nodeChildExists(root1, i)) { |
669 |
|
|
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
670 |
|
|
AABB child_bv; |
671 |
|
|
computeChildBV(bv1, i, child_bv); |
672 |
|
|
|
673 |
|
|
if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2, |
674 |
|
|
tf1, tf2)) |
675 |
|
|
return true; |
676 |
|
|
} |
677 |
|
|
} |
678 |
|
|
} else { |
679 |
|
|
for (unsigned int i = 0; i < 8; ++i) { |
680 |
|
|
if (tree2->nodeChildExists(root2, i)) { |
681 |
|
|
const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); |
682 |
|
|
AABB child_bv; |
683 |
|
|
computeChildBV(bv2, i, child_bv); |
684 |
|
|
|
685 |
|
|
if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv, |
686 |
|
|
tf1, tf2)) |
687 |
|
|
return true; |
688 |
|
|
} |
689 |
|
|
} |
690 |
|
|
} |
691 |
|
|
|
692 |
|
|
return false; |
693 |
|
|
} |
694 |
|
|
}; |
695 |
|
|
|
696 |
|
|
/// @addtogroup Traversal_For_Collision |
697 |
|
|
/// @{ |
698 |
|
|
|
699 |
|
|
/// @brief Traversal node for octree collision |
700 |
|
|
class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode |
701 |
|
|
: public CollisionTraversalNodeBase { |
702 |
|
|
public: |
703 |
|
|
OcTreeCollisionTraversalNode(const CollisionRequest& request) |
704 |
|
|
: CollisionTraversalNodeBase(request) { |
705 |
|
|
model1 = NULL; |
706 |
|
|
model2 = NULL; |
707 |
|
|
|
708 |
|
|
otsolver = NULL; |
709 |
|
|
} |
710 |
|
|
|
711 |
|
|
bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; } |
712 |
|
|
|
713 |
|
|
void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const { |
714 |
|
|
otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); |
715 |
|
|
sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); |
716 |
|
|
sqrDistLowerBound *= sqrDistLowerBound; |
717 |
|
|
} |
718 |
|
|
|
719 |
|
|
const OcTree* model1; |
720 |
|
|
const OcTree* model2; |
721 |
|
|
|
722 |
|
|
Transform3f tf1, tf2; |
723 |
|
|
|
724 |
|
|
const OcTreeSolver* otsolver; |
725 |
|
|
}; |
726 |
|
|
|
727 |
|
|
/// @brief Traversal node for shape-octree collision |
728 |
|
|
template <typename S> |
729 |
|
|
class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode |
730 |
|
|
: public CollisionTraversalNodeBase { |
731 |
|
|
public: |
732 |
|
|
ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) |
733 |
|
|
: CollisionTraversalNodeBase(request) { |
734 |
|
|
model1 = NULL; |
735 |
|
|
model2 = NULL; |
736 |
|
|
|
737 |
|
|
otsolver = NULL; |
738 |
|
|
} |
739 |
|
|
|
740 |
|
|
bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { |
741 |
|
|
return false; |
742 |
|
|
} |
743 |
|
|
|
744 |
|
|
void leafCollides(unsigned int, unsigned int, |
745 |
|
|
FCL_REAL& sqrDistLowerBound) const { |
746 |
|
|
otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); |
747 |
|
|
sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); |
748 |
|
|
sqrDistLowerBound *= sqrDistLowerBound; |
749 |
|
|
} |
750 |
|
|
|
751 |
|
|
const S* model1; |
752 |
|
|
const OcTree* model2; |
753 |
|
|
|
754 |
|
|
Transform3f tf1, tf2; |
755 |
|
|
|
756 |
|
|
const OcTreeSolver* otsolver; |
757 |
|
|
}; |
758 |
|
|
|
759 |
|
|
/// @brief Traversal node for octree-shape collision |
760 |
|
|
|
761 |
|
|
template <typename S> |
762 |
|
|
class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode |
763 |
|
|
: public CollisionTraversalNodeBase { |
764 |
|
|
public: |
765 |
|
|
OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) |
766 |
|
|
: CollisionTraversalNodeBase(request) { |
767 |
|
|
model1 = NULL; |
768 |
|
|
model2 = NULL; |
769 |
|
|
|
770 |
|
|
otsolver = NULL; |
771 |
|
|
} |
772 |
|
|
|
773 |
|
|
bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const { |
774 |
|
|
return false; |
775 |
|
|
} |
776 |
|
|
|
777 |
|
|
void leafCollides(unsigned int, unsigned int, |
778 |
|
|
FCL_REAL& sqrDistLowerBound) const { |
779 |
|
|
otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); |
780 |
|
|
sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); |
781 |
|
|
sqrDistLowerBound *= sqrDistLowerBound; |
782 |
|
|
} |
783 |
|
|
|
784 |
|
|
const OcTree* model1; |
785 |
|
|
const S* model2; |
786 |
|
|
|
787 |
|
|
Transform3f tf1, tf2; |
788 |
|
|
|
789 |
|
|
const OcTreeSolver* otsolver; |
790 |
|
|
}; |
791 |
|
|
|
792 |
|
|
/// @brief Traversal node for mesh-octree collision |
793 |
|
|
template <typename BV> |
794 |
|
|
class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode |
795 |
|
|
: public CollisionTraversalNodeBase { |
796 |
|
|
public: |
797 |
|
200 |
MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) |
798 |
✓✗✓✗
|
200 |
: CollisionTraversalNodeBase(request) { |
799 |
|
200 |
model1 = NULL; |
800 |
|
200 |
model2 = NULL; |
801 |
|
|
|
802 |
|
200 |
otsolver = NULL; |
803 |
|
|
} |
804 |
|
|
|
805 |
|
|
bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { |
806 |
|
|
return false; |
807 |
|
|
} |
808 |
|
|
|
809 |
|
200 |
void leafCollides(unsigned int, unsigned int, |
810 |
|
|
FCL_REAL& sqrDistLowerBound) const { |
811 |
|
200 |
otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); |
812 |
|
200 |
sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); |
813 |
|
200 |
sqrDistLowerBound *= sqrDistLowerBound; |
814 |
|
|
} |
815 |
|
|
|
816 |
|
|
const BVHModel<BV>* model1; |
817 |
|
|
const OcTree* model2; |
818 |
|
|
|
819 |
|
|
Transform3f tf1, tf2; |
820 |
|
|
|
821 |
|
|
const OcTreeSolver* otsolver; |
822 |
|
|
}; |
823 |
|
|
|
824 |
|
|
/// @brief Traversal node for octree-mesh collision |
825 |
|
|
template <typename BV> |
826 |
|
|
class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode |
827 |
|
|
: public CollisionTraversalNodeBase { |
828 |
|
|
public: |
829 |
|
|
OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) |
830 |
|
|
: CollisionTraversalNodeBase(request) { |
831 |
|
|
model1 = NULL; |
832 |
|
|
model2 = NULL; |
833 |
|
|
|
834 |
|
|
otsolver = NULL; |
835 |
|
|
} |
836 |
|
|
|
837 |
|
|
bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { |
838 |
|
|
return false; |
839 |
|
|
} |
840 |
|
|
|
841 |
|
|
void leafCollides(unsigned int, unsigned int, |
842 |
|
|
FCL_REAL& sqrDistLowerBound) const { |
843 |
|
|
std::cout << "leafCollides" << std::endl; |
844 |
|
|
otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); |
845 |
|
|
sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); |
846 |
|
|
sqrDistLowerBound *= sqrDistLowerBound; |
847 |
|
|
} |
848 |
|
|
|
849 |
|
|
const OcTree* model1; |
850 |
|
|
const BVHModel<BV>* model2; |
851 |
|
|
|
852 |
|
|
Transform3f tf1, tf2; |
853 |
|
|
|
854 |
|
|
const OcTreeSolver* otsolver; |
855 |
|
|
}; |
856 |
|
|
|
857 |
|
|
/// @} |
858 |
|
|
|
859 |
|
|
/// @addtogroup Traversal_For_Distance |
860 |
|
|
/// @{ |
861 |
|
|
|
862 |
|
|
/// @brief Traversal node for octree distance |
863 |
|
|
class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode |
864 |
|
|
: public DistanceTraversalNodeBase { |
865 |
|
|
public: |
866 |
|
|
OcTreeDistanceTraversalNode() { |
867 |
|
|
model1 = NULL; |
868 |
|
|
model2 = NULL; |
869 |
|
|
|
870 |
|
|
otsolver = NULL; |
871 |
|
|
} |
872 |
|
|
|
873 |
|
|
FCL_REAL BVDistanceLowerBound(unsigned, unsigned) const { return -1; } |
874 |
|
|
|
875 |
|
|
bool BVDistanceLowerBound(unsigned, unsigned, FCL_REAL&) const { |
876 |
|
|
return false; |
877 |
|
|
} |
878 |
|
|
|
879 |
|
|
void leafComputeDistance(unsigned, unsigned int) const { |
880 |
|
|
otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result); |
881 |
|
|
} |
882 |
|
|
|
883 |
|
|
const OcTree* model1; |
884 |
|
|
const OcTree* model2; |
885 |
|
|
|
886 |
|
|
const OcTreeSolver* otsolver; |
887 |
|
|
}; |
888 |
|
|
|
889 |
|
|
/// @brief Traversal node for shape-octree distance |
890 |
|
|
template <typename Shape> |
891 |
|
|
class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode |
892 |
|
|
: public DistanceTraversalNodeBase { |
893 |
|
|
public: |
894 |
|
|
ShapeOcTreeDistanceTraversalNode() { |
895 |
|
|
model1 = NULL; |
896 |
|
|
model2 = NULL; |
897 |
|
|
|
898 |
|
|
otsolver = NULL; |
899 |
|
|
} |
900 |
|
|
|
901 |
|
|
FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } |
902 |
|
|
|
903 |
|
|
void leafComputeDistance(unsigned int, unsigned int) const { |
904 |
|
|
otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); |
905 |
|
|
} |
906 |
|
|
|
907 |
|
|
const Shape* model1; |
908 |
|
|
const OcTree* model2; |
909 |
|
|
|
910 |
|
|
const OcTreeSolver* otsolver; |
911 |
|
|
}; |
912 |
|
|
|
913 |
|
|
/// @brief Traversal node for octree-shape distance |
914 |
|
|
template <typename Shape> |
915 |
|
|
class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode |
916 |
|
|
: public DistanceTraversalNodeBase { |
917 |
|
|
public: |
918 |
|
|
OcTreeShapeDistanceTraversalNode() { |
919 |
|
|
model1 = NULL; |
920 |
|
|
model2 = NULL; |
921 |
|
|
|
922 |
|
|
otsolver = NULL; |
923 |
|
|
} |
924 |
|
|
|
925 |
|
|
FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } |
926 |
|
|
|
927 |
|
|
void leafComputeDistance(unsigned int, unsigned int) const { |
928 |
|
|
otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); |
929 |
|
|
} |
930 |
|
|
|
931 |
|
|
const OcTree* model1; |
932 |
|
|
const Shape* model2; |
933 |
|
|
|
934 |
|
|
const OcTreeSolver* otsolver; |
935 |
|
|
}; |
936 |
|
|
|
937 |
|
|
/// @brief Traversal node for mesh-octree distance |
938 |
|
|
template <typename BV> |
939 |
|
|
class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode |
940 |
|
|
: public DistanceTraversalNodeBase { |
941 |
|
|
public: |
942 |
|
|
MeshOcTreeDistanceTraversalNode() { |
943 |
|
|
model1 = NULL; |
944 |
|
|
model2 = NULL; |
945 |
|
|
|
946 |
|
|
otsolver = NULL; |
947 |
|
|
} |
948 |
|
|
|
949 |
|
|
FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } |
950 |
|
|
|
951 |
|
|
void leafComputeDistance(unsigned int, unsigned int) const { |
952 |
|
|
otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); |
953 |
|
|
} |
954 |
|
|
|
955 |
|
|
const BVHModel<BV>* model1; |
956 |
|
|
const OcTree* model2; |
957 |
|
|
|
958 |
|
|
const OcTreeSolver* otsolver; |
959 |
|
|
}; |
960 |
|
|
|
961 |
|
|
/// @brief Traversal node for octree-mesh distance |
962 |
|
|
template <typename BV> |
963 |
|
|
class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode |
964 |
|
|
: public DistanceTraversalNodeBase { |
965 |
|
|
public: |
966 |
|
|
OcTreeMeshDistanceTraversalNode() { |
967 |
|
|
model1 = NULL; |
968 |
|
|
model2 = NULL; |
969 |
|
|
|
970 |
|
|
otsolver = NULL; |
971 |
|
|
} |
972 |
|
|
|
973 |
|
|
FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } |
974 |
|
|
|
975 |
|
|
void leafComputeDistance(unsigned int, unsigned int) const { |
976 |
|
|
otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); |
977 |
|
|
} |
978 |
|
|
|
979 |
|
|
const OcTree* model1; |
980 |
|
|
const BVHModel<BV>* model2; |
981 |
|
|
|
982 |
|
|
const OcTreeSolver* otsolver; |
983 |
|
|
}; |
984 |
|
|
|
985 |
|
|
/// @} |
986 |
|
|
|
987 |
|
|
} // namespace fcl |
988 |
|
|
|
989 |
|
|
} // namespace hpp |
990 |
|
|
|
991 |
|
|
/// @endcond |
992 |
|
|
|
993 |
|
|
#endif |