hpp-fcl 2.3.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
traversal_node_bvhs.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-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
38#ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H
39#define HPP_FCL_TRAVERSAL_NODE_MESHES_H
40
42
45#include <hpp/fcl/BV/BV_node.h>
46#include <hpp/fcl/BV/BV.h>
52
53#include <limits>
54#include <vector>
55#include <cassert>
56
57namespace hpp {
58namespace fcl {
59
62
64template <typename BV>
65class BVHCollisionTraversalNode : public CollisionTraversalNodeBase {
66 public:
67 BVHCollisionTraversalNode(const CollisionRequest& request)
68 : CollisionTraversalNodeBase(request) {
69 model1 = NULL;
70 model2 = NULL;
71
72 num_bv_tests = 0;
73 num_leaf_tests = 0;
74 query_time_seconds = 0.0;
75 }
76
78 bool isFirstNodeLeaf(unsigned int b) const {
79 assert(model1 != NULL && "model1 is NULL");
80 return model1->getBV(b).isLeaf();
81 }
82
84 bool isSecondNodeLeaf(unsigned int b) const {
85 assert(model2 != NULL && "model2 is NULL");
86 return model2->getBV(b).isLeaf();
87 }
88
90 bool firstOverSecond(unsigned int b1, unsigned int b2) const {
91 FCL_REAL sz1 = model1->getBV(b1).bv.size();
92 FCL_REAL sz2 = model2->getBV(b2).bv.size();
93
94 bool l1 = model1->getBV(b1).isLeaf();
95 bool l2 = model2->getBV(b2).isLeaf();
96
97 if (l2 || (!l1 && (sz1 > sz2))) return true;
98 return false;
99 }
100
102 int getFirstLeftChild(unsigned int b) const {
103 return model1->getBV(b).leftChild();
104 }
105
107 int getFirstRightChild(unsigned int b) const {
108 return model1->getBV(b).rightChild();
109 }
110
112 int getSecondLeftChild(unsigned int b) const {
113 return model2->getBV(b).leftChild();
114 }
115
117 int getSecondRightChild(unsigned int b) const {
118 return model2->getBV(b).rightChild();
119 }
120
122 const BVHModel<BV>* model1;
124 const BVHModel<BV>* model2;
125
127 mutable int num_bv_tests;
128 mutable int num_leaf_tests;
129 mutable FCL_REAL query_time_seconds;
130};
131
133template <typename BV, int _Options = RelativeTransformationIsIdentity>
134class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
135 public:
136 enum {
137 Options = _Options,
138 RTIsIdentity = _Options & RelativeTransformationIsIdentity
139 };
140
141 MeshCollisionTraversalNode(const CollisionRequest& request)
142 : BVHCollisionTraversalNode<BV>(request) {
143 vertices1 = NULL;
144 vertices2 = NULL;
145 tri_indices1 = NULL;
146 tri_indices2 = NULL;
147 }
148
153 bool BVDisjoints(unsigned int b1, unsigned int b2,
154 FCL_REAL& sqrDistLowerBound) const {
155 if (this->enable_statistics) this->num_bv_tests++;
156 bool disjoint;
157 if (RTIsIdentity)
158 disjoint = !this->model1->getBV(b1).overlap(
159 this->model2->getBV(b2), this->request, sqrDistLowerBound);
160 else {
161 disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
162 this->model1->getBV(b1).bv, this->request,
163 sqrDistLowerBound);
164 }
165 if (disjoint)
166 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
167 sqrDistLowerBound);
168 return disjoint;
169 }
170
185 void leafCollides(unsigned int b1, unsigned int b2,
186 FCL_REAL& sqrDistLowerBound) const {
187 if (this->enable_statistics) this->num_leaf_tests++;
188
189 const BVNode<BV>& node1 = this->model1->getBV(b1);
190 const BVNode<BV>& node2 = this->model2->getBV(b2);
191
192 int primitive_id1 = node1.primitiveId();
193 int primitive_id2 = node2.primitiveId();
194
195 const Triangle& tri_id1 = tri_indices1[primitive_id1];
196 const Triangle& tri_id2 = tri_indices2[primitive_id2];
197
198 const Vec3f& P1 = vertices1[tri_id1[0]];
199 const Vec3f& P2 = vertices1[tri_id1[1]];
200 const Vec3f& P3 = vertices1[tri_id1[2]];
201 const Vec3f& Q1 = vertices2[tri_id2[0]];
202 const Vec3f& Q2 = vertices2[tri_id2[1]];
203 const Vec3f& Q3 = vertices2[tri_id2[2]];
204
205 TriangleP tri1(P1, P2, P3);
206 TriangleP tri2(Q1, Q2, Q3);
207 GJKSolver solver;
208 Vec3f p1,
209 p2; // closest points if no collision contact points if collision.
210 Vec3f normal;
212 solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
213 normal);
214
215 const FCL_REAL distToCollision = distance - this->request.security_margin;
216 if (distToCollision <=
217 this->request.collision_distance_threshold) { // collision
218 sqrDistLowerBound = 0;
219 Vec3f p(p1); // contact point
220 if (this->result->numContacts() < this->request.num_max_contacts) {
221 // How much (Q1, Q2, Q3) should be moved so that all vertices are
222 // above (P1, P2, P3).
223 if (distance > 0) {
224 normal = (p2 - p1).normalized();
225 p = .5 * (p1 + p2);
226 }
227 this->result->addContact(Contact(this->model1, this->model2,
228 primitive_id1, primitive_id2, p,
229 normal, -distance));
230 }
231 } else
232 sqrDistLowerBound = distToCollision * distToCollision;
233
234 internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
235 distToCollision, p1, p2);
236 }
237
238 Vec3f* vertices1;
239 Vec3f* vertices2;
240
241 Triangle* tri_indices1;
242 Triangle* tri_indices2;
243
244 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
245};
246
249typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB;
250typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS;
251typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS;
252typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
253
255
256namespace details {
257template <typename BV>
258struct DistanceTraversalBVDistanceLowerBound_impl {
259 static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
260 return b1.distance(b2);
261 }
262 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
263 const BVNode<BV>& b2) {
264 return distance(R, T, b1.bv, b2.bv);
265 }
266};
267
268template <>
269struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
270 static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
271 FCL_REAL sqrDistLowerBound;
272 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
273 // request.break_distance = ?
274 if (b1.overlap(b2, request, sqrDistLowerBound)) {
275 // TODO A penetration upper bound should be computed.
276 return -1;
277 }
278 return sqrt(sqrDistLowerBound);
279 }
280 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1,
281 const BVNode<OBB>& b2) {
282 FCL_REAL sqrDistLowerBound;
283 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
284 // request.break_distance = ?
285 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
286 // TODO A penetration upper bound should be computed.
287 return -1;
288 }
289 return sqrt(sqrDistLowerBound);
290 }
291};
292
293template <>
294struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
295 static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
296 FCL_REAL sqrDistLowerBound;
297 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
298 // request.break_distance = ?
299 if (b1.overlap(b2, request, sqrDistLowerBound)) {
300 // TODO A penetration upper bound should be computed.
301 return -1;
302 }
303 return sqrt(sqrDistLowerBound);
304 }
305 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1,
306 const BVNode<AABB>& b2) {
307 FCL_REAL sqrDistLowerBound;
308 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
309 // request.break_distance = ?
310 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
311 // TODO A penetration upper bound should be computed.
312 return -1;
313 }
314 return sqrt(sqrDistLowerBound);
315 }
316};
317} // namespace details
318
321
323template <typename BV>
324class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
325 public:
326 BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
327 model1 = NULL;
328 model2 = NULL;
329
330 num_bv_tests = 0;
331 num_leaf_tests = 0;
332 query_time_seconds = 0.0;
333 }
334
336 bool isFirstNodeLeaf(unsigned int b) const {
337 return model1->getBV(b).isLeaf();
338 }
339
341 bool isSecondNodeLeaf(unsigned int b) const {
342 return model2->getBV(b).isLeaf();
343 }
344
346 bool firstOverSecond(unsigned int b1, unsigned int b2) const {
347 FCL_REAL sz1 = model1->getBV(b1).bv.size();
348 FCL_REAL sz2 = model2->getBV(b2).bv.size();
349
350 bool l1 = model1->getBV(b1).isLeaf();
351 bool l2 = model2->getBV(b2).isLeaf();
352
353 if (l2 || (!l1 && (sz1 > sz2))) return true;
354 return false;
355 }
356
358 int getFirstLeftChild(unsigned int b) const {
359 return model1->getBV(b).leftChild();
360 }
361
363 int getFirstRightChild(unsigned int b) const {
364 return model1->getBV(b).rightChild();
365 }
366
368 int getSecondLeftChild(unsigned int b) const {
369 return model2->getBV(b).leftChild();
370 }
371
373 int getSecondRightChild(unsigned int b) const {
374 return model2->getBV(b).rightChild();
375 }
376
378 const BVHModel<BV>* model1;
380 const BVHModel<BV>* model2;
381
383 mutable int num_bv_tests;
384 mutable int num_leaf_tests;
385 mutable FCL_REAL query_time_seconds;
386};
387
389template <typename BV, int _Options = RelativeTransformationIsIdentity>
390class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
391 public:
392 enum {
393 Options = _Options,
394 RTIsIdentity = _Options & RelativeTransformationIsIdentity
395 };
396
397 using BVHDistanceTraversalNode<BV>::enable_statistics;
398 using BVHDistanceTraversalNode<BV>::request;
399 using BVHDistanceTraversalNode<BV>::result;
400 using BVHDistanceTraversalNode<BV>::tf1;
401 using BVHDistanceTraversalNode<BV>::model1;
402 using BVHDistanceTraversalNode<BV>::model2;
403 using BVHDistanceTraversalNode<BV>::num_bv_tests;
404 using BVHDistanceTraversalNode<BV>::num_leaf_tests;
405
406 MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
407 vertices1 = NULL;
408 vertices2 = NULL;
409 tri_indices1 = NULL;
410 tri_indices2 = NULL;
411
412 rel_err = this->request.rel_err;
413 abs_err = this->request.abs_err;
414 }
415
416 void preprocess() {
417 if (!RTIsIdentity) preprocessOrientedNode();
418 }
419
420 void postprocess() {
421 if (!RTIsIdentity) postprocessOrientedNode();
422 }
423
425 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
426 if (enable_statistics) num_bv_tests++;
427 if (RTIsIdentity)
428 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
429 model1->getBV(b1), model2->getBV(b2));
430 else
431 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
432 RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
433 }
434
436 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
437 if (this->enable_statistics) this->num_leaf_tests++;
438
439 const BVNode<BV>& node1 = this->model1->getBV(b1);
440 const BVNode<BV>& node2 = this->model2->getBV(b2);
441
442 int primitive_id1 = node1.primitiveId();
443 int primitive_id2 = node2.primitiveId();
444
445 const Triangle& tri_id1 = tri_indices1[primitive_id1];
446 const Triangle& tri_id2 = tri_indices2[primitive_id2];
447
448 const Vec3f& t11 = vertices1[tri_id1[0]];
449 const Vec3f& t12 = vertices1[tri_id1[1]];
450 const Vec3f& t13 = vertices1[tri_id1[2]];
451
452 const Vec3f& t21 = vertices2[tri_id2[0]];
453 const Vec3f& t22 = vertices2[tri_id2[1]];
454 const Vec3f& t23 = vertices2[tri_id2[2]];
455
456 // nearest point pair
457 Vec3f P1, P2, normal;
458
459 FCL_REAL d2;
460 if (RTIsIdentity)
461 d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
462 P2);
463 else
464 d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
465 RT._R(), RT._T(), P1, P2);
466 FCL_REAL d = sqrt(d2);
467
468 this->result->update(d, this->model1, this->model2, primitive_id1,
469 primitive_id2, P1, P2, normal);
470 }
471
473 bool canStop(FCL_REAL c) const {
474 if ((c >= this->result->min_distance - abs_err) &&
475 (c * (1 + rel_err) >= this->result->min_distance))
476 return true;
477 return false;
478 }
479
480 Vec3f* vertices1;
481 Vec3f* vertices2;
482
483 Triangle* tri_indices1;
484 Triangle* tri_indices2;
485
487 FCL_REAL rel_err;
488 FCL_REAL abs_err;
489
490 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
491
492 private:
493 void preprocessOrientedNode() {
494 const int init_tri_id1 = 0, init_tri_id2 = 0;
495 const Triangle& init_tri1 = tri_indices1[init_tri_id1];
496 const Triangle& init_tri2 = tri_indices2[init_tri_id2];
497
498 Vec3f init_tri1_points[3];
499 Vec3f init_tri2_points[3];
500
501 init_tri1_points[0] = vertices1[init_tri1[0]];
502 init_tri1_points[1] = vertices1[init_tri1[1]];
503 init_tri1_points[2] = vertices1[init_tri1[2]];
504
505 init_tri2_points[0] = vertices2[init_tri2[0]];
506 init_tri2_points[1] = vertices2[init_tri2[1]];
507 init_tri2_points[2] = vertices2[init_tri2[2]];
508
509 Vec3f p1, p2, normal;
510 FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance(
511 init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
512 init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
513 RT._T(), p1, p2));
514
515 result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
516 normal);
517 }
518 void postprocessOrientedNode() {
522 if (request.enable_nearest_points && (result->o1 == model1) &&
523 (result->o2 == model2)) {
524 result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
525 result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
526 }
527 }
528};
529
532typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
533typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
534typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
535
537
540namespace details {
541
542template <typename BV>
543inline const Matrix3f& getBVAxes(const BV& bv) {
544 return bv.axes;
545}
546
547template <>
548inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
549 return bv.obb.axes;
550}
551
552} // namespace details
553
554} // namespace fcl
555
556} // namespace hpp
557
559
560#endif
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
bool overlap(const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2)
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44