hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
traversal_node_bvh_hfield.h
Go to the documentation of this file.
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021, INRIA
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Open Source Robotics Foundation nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 */
34
37#ifndef HPP_FCL_TRAVERSAL_NODE_BVH_HFIELD_H
38#define HPP_FCL_TRAVERSAL_NODE_BVH_HFIELD_H
39
41
45#include <hpp/fcl/BV/BV_node.h>
46#include <hpp/fcl/BV/BV.h>
48#include <hpp/fcl/hfield.h>
53
54#include <limits>
55#include <vector>
56#include <cassert>
57
58namespace hpp {
59namespace fcl {
60
63
66template <typename BV1, typename BV2,
67 int _Options = RelativeTransformationIsIdentity>
68class MeshHeightFieldCollisionTraversalNode
69 : public CollisionTraversalNodeBase {
70 public:
71 enum {
72 Options = _Options,
73 RTIsIdentity = _Options & RelativeTransformationIsIdentity
74 };
75
76 MeshHeightFieldCollisionTraversalNode(const CollisionRequest& request)
77 : CollisionTraversalNodeBase(request) {
78 model1 = NULL;
79 model2 = NULL;
80
81 num_bv_tests = 0;
82 num_leaf_tests = 0;
83 query_time_seconds = 0.0;
84
85 vertices1 = NULL;
86 tri_indices1 = NULL;
87 }
88
90 bool isFirstNodeLeaf(unsigned int b) const {
91 assert(model1 != NULL && "model1 is NULL");
92 return model1->getBV(b).isLeaf();
93 }
94
96 bool isSecondNodeLeaf(unsigned int b) const {
97 assert(model2 != NULL && "model2 is NULL");
98 return model2->getBV(b).isLeaf();
99 }
100
102 bool firstOverSecond(unsigned int b1, unsigned int b2) const {
103 FCL_REAL sz1 = model1->getBV(b1).bv.size();
104 FCL_REAL sz2 = model2->getBV(b2).bv.size();
105
106 bool l1 = model1->getBV(b1).isLeaf();
107 bool l2 = model2->getBV(b2).isLeaf();
108
109 if (l2 || (!l1 && (sz1 > sz2))) return true;
110 return false;
111 }
112
114 int getFirstLeftChild(unsigned int b) const {
115 return model1->getBV(b).leftChild();
116 }
117
119 int getFirstRightChild(unsigned int b) const {
120 return model1->getBV(b).rightChild();
121 }
122
124 int getSecondLeftChild(unsigned int b) const {
125 return model2->getBV(b).leftChild();
126 }
127
129 int getSecondRightChild(unsigned int b) const {
130 return model2->getBV(b).rightChild();
131 }
132
134 bool BVDisjoints(unsigned int b1, unsigned int b2) const {
135 if (this->enable_statistics) this->num_bv_tests++;
136 if (RTIsIdentity)
137 return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
138 else
139 return !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv,
140 this->model2->getBV(b2).bv);
141 }
142
147 bool BVDisjoints(unsigned int b1, unsigned int b2,
148 FCL_REAL& sqrDistLowerBound) const {
149 if (this->enable_statistics) this->num_bv_tests++;
150 if (RTIsIdentity)
151 return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
152 this->request, sqrDistLowerBound);
153 else {
154 bool res = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv,
155 this->model2->getBV(b2).bv, this->request,
156 sqrDistLowerBound);
157 assert(!res || sqrDistLowerBound > 0);
158 return res;
159 }
160 }
161
176 void leafCollides(unsigned int b1, unsigned int b2,
177 FCL_REAL& sqrDistLowerBound) const {
178 if (this->enable_statistics) this->num_leaf_tests++;
179
180 const BVNode<BV1>& node1 = this->model1->getBV(b1);
181 const HeightFieldNode<BV2>& node2 = this->model2->getBV(b2);
182
183 int primitive_id1 = node1.primitiveId();
184 const Triangle& tri_id1 = tri_indices1[primitive_id1];
185
186 const Vec3f& P1 = vertices1[tri_id1[0]];
187 const Vec3f& P2 = vertices1[tri_id1[1]];
188 const Vec3f& P3 = vertices1[tri_id1[2]];
189
190 TriangleP tri1(P1, P2, P3);
191
192 typedef Convex<Triangle> ConvexTriangle;
193 ConvexTriangle convex1, convex2;
194 details::buildConvexTriangles(node2, *this->model2, convex2, convex2);
195
196 GJKSolver solver;
197 Vec3f p1,
198 p2; // closest points if no collision contact points if collision.
199 Vec3f normal;
201 solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
202 normal);
203 FCL_REAL distToCollision = distance - this->request.security_margin;
204 sqrDistLowerBound = distance * distance;
205 if (distToCollision <= 0) { // collision
206 Vec3f p(p1); // contact point
207 FCL_REAL penetrationDepth(0);
208 if (this->result->numContacts() < this->request.num_max_contacts) {
209 // How much (Q1, Q2, Q3) should be moved so that all vertices are
210 // above (P1, P2, P3).
211 penetrationDepth = -distance;
212 if (distance > 0) {
213 normal = (p2 - p1).normalized();
214 p = .5 * (p1 + p2);
215 }
216 this->result->addContact(Contact(this->model1, this->model2,
217 primitive_id1, primitive_id2, p,
218 normal, penetrationDepth));
219 }
220 }
221 }
222
224 const BVHModel<BV1>* model1;
226 const HeightField<BV2>* model2;
227
229 mutable int num_bv_tests;
230 mutable int num_leaf_tests;
231 mutable FCL_REAL query_time_seconds;
232
233 Vec3f* vertices1 Triangle* tri_indices1;
234
235 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
236};
237
240typedef MeshHeightFieldCollisionTraversalNode<OBB, 0>
241 MeshHeightFieldCollisionTraversalNodeOBB;
242typedef MeshHeightFieldCollisionTraversalNode<RSS, 0>
243 MeshHeightFieldCollisionTraversalNodeRSS;
244typedef MeshHeightFieldCollisionTraversalNode<kIOS, 0>
245 MeshHeightFieldCollisionTraversalNodekIOS;
246typedef MeshHeightFieldCollisionTraversalNode<OBBRSS, 0>
247 MeshHeightFieldCollisionTraversalNodeOBBRSS;
248
250
251namespace details {
252template <typename BV>
253struct DistanceTraversalBVDistanceLowerBound_impl {
254 static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
255 return b1.distance(b2);
256 }
257 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
258 const BVNode<BV>& b2) {
259 return distance(R, T, b1.bv, b2.bv);
260 }
261};
262
263template <>
264struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
265 static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
266 FCL_REAL sqrDistLowerBound;
267 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
268 // request.break_distance = ?
269 if (b1.overlap(b2, request, sqrDistLowerBound)) {
270 // TODO A penetration upper bound should be computed.
271 return -1;
272 }
273 return sqrt(sqrDistLowerBound);
274 }
275 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1,
276 const BVNode<OBB>& b2) {
277 FCL_REAL sqrDistLowerBound;
278 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
279 // request.break_distance = ?
280 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
281 // TODO A penetration upper bound should be computed.
282 return -1;
283 }
284 return sqrt(sqrDistLowerBound);
285 }
286};
287
288template <>
289struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
290 static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
291 FCL_REAL sqrDistLowerBound;
292 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
293 // request.break_distance = ?
294 if (b1.overlap(b2, request, sqrDistLowerBound)) {
295 // TODO A penetration upper bound should be computed.
296 return -1;
297 }
298 return sqrt(sqrDistLowerBound);
299 }
300 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1,
301 const BVNode<AABB>& b2) {
302 FCL_REAL sqrDistLowerBound;
303 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
304 // request.break_distance = ?
305 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
306 // TODO A penetration upper bound should be computed.
307 return -1;
308 }
309 return sqrt(sqrDistLowerBound);
310 }
311};
312} // namespace details
313
316
318template <typename BV>
319class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
320 public:
321 BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
322 model1 = NULL;
323 model2 = NULL;
324
325 num_bv_tests = 0;
326 num_leaf_tests = 0;
327 query_time_seconds = 0.0;
328 }
329
331 bool isFirstNodeLeaf(unsigned int b) const {
332 return model1->getBV(b).isLeaf();
333 }
334
336 bool isSecondNodeLeaf(unsigned int b) const {
337 return model2->getBV(b).isLeaf();
338 }
339
341 bool firstOverSecond(unsigned int b1, unsigned int b2) const {
342 FCL_REAL sz1 = model1->getBV(b1).bv.size();
343 FCL_REAL sz2 = model2->getBV(b2).bv.size();
344
345 bool l1 = model1->getBV(b1).isLeaf();
346 bool l2 = model2->getBV(b2).isLeaf();
347
348 if (l2 || (!l1 && (sz1 > sz2))) return true;
349 return false;
350 }
351
353 int getFirstLeftChild(unsigned int b) const {
354 return model1->getBV(b).leftChild();
355 }
356
358 int getFirstRightChild(unsigned int b) const {
359 return model1->getBV(b).rightChild();
360 }
361
363 int getSecondLeftChild(unsigned int b) const {
364 return model2->getBV(b).leftChild();
365 }
366
368 int getSecondRightChild(unsigned int b) const {
369 return model2->getBV(b).rightChild();
370 }
371
373 const BVHModel<BV>* model1;
375 const BVHModel<BV>* model2;
376
378 mutable int num_bv_tests;
379 mutable int num_leaf_tests;
380 mutable FCL_REAL query_time_seconds;
381};
382
384template <typename BV, int _Options = RelativeTransformationIsIdentity>
385class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
386 public:
387 enum {
388 Options = _Options,
389 RTIsIdentity = _Options & RelativeTransformationIsIdentity
390 };
391
392 using BVHDistanceTraversalNode<BV>::enable_statistics;
393 using BVHDistanceTraversalNode<BV>::request;
394 using BVHDistanceTraversalNode<BV>::result;
395 using BVHDistanceTraversalNode<BV>::tf1;
396 using BVHDistanceTraversalNode<BV>::model1;
397 using BVHDistanceTraversalNode<BV>::model2;
398 using BVHDistanceTraversalNode<BV>::num_bv_tests;
399 using BVHDistanceTraversalNode<BV>::num_leaf_tests;
400
401 MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
402 vertices1 = NULL;
403 vertices2 = NULL;
404 tri_indices1 = NULL;
405 tri_indices2 = NULL;
406
407 rel_err = this->request.rel_err;
408 abs_err = this->request.abs_err;
409 }
410
411 void preprocess() {
412 if (!RTIsIdentity) preprocessOrientedNode();
413 }
414
415 void postprocess() {
416 if (!RTIsIdentity) postprocessOrientedNode();
417 }
418
420 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
421 if (enable_statistics) num_bv_tests++;
422 if (RTIsIdentity)
423 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
424 model1->getBV(b1), model2->getBV(b2));
425 else
426 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
427 RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
428 }
429
431 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
432 if (this->enable_statistics) this->num_leaf_tests++;
433
434 const BVNode<BV>& node1 = this->model1->getBV(b1);
435 const BVNode<BV>& node2 = this->model2->getBV(b2);
436
437 int primitive_id1 = node1.primitiveId();
438 int primitive_id2 = node2.primitiveId();
439
440 const Triangle& tri_id1 = tri_indices1[primitive_id1];
441 const Triangle& tri_id2 = tri_indices2[primitive_id2];
442
443 const Vec3f& t11 = vertices1[tri_id1[0]];
444 const Vec3f& t12 = vertices1[tri_id1[1]];
445 const Vec3f& t13 = vertices1[tri_id1[2]];
446
447 const Vec3f& t21 = vertices2[tri_id2[0]];
448 const Vec3f& t22 = vertices2[tri_id2[1]];
449 const Vec3f& t23 = vertices2[tri_id2[2]];
450
451 // nearest point pair
452 Vec3f P1, P2, normal;
453
454 FCL_REAL d2;
455 if (RTIsIdentity)
456 d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
457 P2);
458 else
459 d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
460 RT._R(), RT._T(), P1, P2);
461 FCL_REAL d = sqrt(d2);
462
463 this->result->update(d, this->model1, this->model2, primitive_id1,
464 primitive_id2, P1, P2, normal);
465 }
466
468 bool canStop(FCL_REAL c) const {
469 if ((c >= this->result->min_distance - abs_err) &&
470 (c * (1 + rel_err) >= this->result->min_distance))
471 return true;
472 return false;
473 }
474
475 Vec3f* vertices1;
476 Vec3f* vertices2;
477
478 Triangle* tri_indices1;
479 Triangle* tri_indices2;
480
482 FCL_REAL rel_err;
483 FCL_REAL abs_err;
484
485 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
486
487 private:
488 void preprocessOrientedNode() {
489 const int init_tri_id1 = 0, init_tri_id2 = 0;
490 const Triangle& init_tri1 = tri_indices1[init_tri_id1];
491 const Triangle& init_tri2 = tri_indices2[init_tri_id2];
492
493 Vec3f init_tri1_points[3];
494 Vec3f init_tri2_points[3];
495
496 init_tri1_points[0] = vertices1[init_tri1[0]];
497 init_tri1_points[1] = vertices1[init_tri1[1]];
498 init_tri1_points[2] = vertices1[init_tri1[2]];
499
500 init_tri2_points[0] = vertices2[init_tri2[0]];
501 init_tri2_points[1] = vertices2[init_tri2[1]];
502 init_tri2_points[2] = vertices2[init_tri2[2]];
503
504 Vec3f p1, p2, normal;
505 FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance(
506 init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
507 init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
508 RT._T(), p1, p2));
509
510 result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
511 normal);
512 }
513 void postprocessOrientedNode() {
517 if (request.enable_nearest_points && (result->o1 == model1) &&
518 (result->o2 == model2)) {
519 result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
520 result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
521 }
522 }
523};
524
527typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
528typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
529typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
530
532
535namespace details {
536
537template <typename BV>
538inline const Matrix3f& getBVAxes(const BV& bv) {
539 return bv.axes;
540}
541
542template <>
543inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
544 return bv.obb.axes;
545}
546
547} // namespace details
548
549} // namespace fcl
550
551} // namespace hpp
552
554
555#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