hpp-fcl: include/hpp/fcl/internal/traversal_node_hfield_shape.h Source File

hpp-fcl 1.8.0 HPP fork of FCL -- The Flexible Collision Library

traversal_node_hfield_shape.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
38#ifndef HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H
39#define HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H
40
42
43#include <hpp/fcl/collision_data.h>
44#include <hpp/fcl/shape/geometric_shapes.h>
45#include <hpp/fcl/narrowphase/narrowphase.h>
46#include <hpp/fcl/shape/geometric_shapes_utility.h>
47#include <hpp/fcl/internal/traversal_node_base.h>
48#include <hpp/fcl/internal/traversal.h>
49#include <hpp/fcl/hfield.h>
50#include <hpp/fcl/shape/convex.h>
51
52namespace hpp
53{
54namespace fcl
55{
56
59
60namespace details
61{
62 template<typename BV>
63 Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV> & node,
64 const HeightField<BV> & model)
65 {
66 const MatrixXf & heights = model.getHeights();
67 const VecXf & x_grid = model.getXGrid();
68 const VecXf & y_grid = model.getYGrid();
69
70 const FCL_REAL min_height = model.getMinHeight();
71
72 const FCL_REAL
73 x0 = x_grid[node.x_id],
74 x1 = x_grid[node.x_id+1],
75 y0 = y_grid[node.y_id],
76 y1 = y_grid[node.y_id+1];
77 const Eigen::Block<const MatrixXf,2,2> cell = heights.block<2,2>(node.y_id,node.x_id);
78
79 assert(cell.maxCoeff() > min_height && "max_height is lower than min_height"); // Check whether the geometry is degenerated
80
81 Vec3f* pts = new Vec3f[8];
82 pts[0] = Vec3f( x0, y0, min_height);
83 pts[1] = Vec3f( x0, y1, min_height);
84 pts[2] = Vec3f( x1, y1, min_height);
85 pts[3] = Vec3f( x1, y0, min_height);
86 pts[4] = Vec3f( x0, y0, cell(0,0));
87 pts[5] = Vec3f( x0, y1, cell(1,0));
88 pts[6] = Vec3f( x1, y1, cell(1,1));
89 pts[7] = Vec3f( x1, y0, cell(0,1));
90
91 Quadrilateral* polygons = new Quadrilateral[6];
92 polygons[0].set(0, 3, 2, 1); // x+ side
93 polygons[1].set(0, 1, 5, 4); // y- side
94 polygons[2].set(1, 2, 6, 5); // x- side
95 polygons[3].set(2, 3, 7, 6); // y+ side
96 polygons[4].set(3, 0, 4, 7); // z- side
97 polygons[5].set(4, 5, 6, 7); // z+ side
98
99 return Convex<Quadrilateral> (true,
100 pts, // points
101 8, // num points
102 polygons,
103 6 // number of polygons
104 );
105 }
106
107 template<typename BV>
108 void buildConvexTriangles(const HFNode<BV> & node,
109 const HeightField<BV> & model,
110 Convex<Triangle> & convex1, Convex<Triangle> & convex2)
111 {
112 const MatrixXf & heights = model.getHeights();
113 const VecXf & x_grid = model.getXGrid();
114 const VecXf & y_grid = model.getYGrid();
115
116 const FCL_REAL min_height = model.getMinHeight();
117
118 const FCL_REAL
119 x0 = x_grid[node.x_id],
120 x1 = x_grid[node.x_id+1],
121 y0 = y_grid[node.y_id],
122 y1 = y_grid[node.y_id+1];
123 const Eigen::Block<const MatrixXf,2,2> cell = heights.block<2,2>(node.y_id,node.x_id);
124 const FCL_REAL max_height = cell.maxCoeff();
125
126 assert(max_height > min_height && "max_height is lower than min_height"); // Check whether the geometry is degenerated
127 HPP_FCL_UNUSED_VARIABLE(max_height);
128
129 {
130 Vec3f* pts = new Vec3f[8];
131 pts[0] = Vec3f( x0, y0, min_height);
132 pts[1] = Vec3f( x0, y1, min_height);
133 pts[2] = Vec3f( x1, y1, min_height);
134 pts[3] = Vec3f( x1, y0, min_height);
135 pts[4] = Vec3f( x0, y0, cell(0,0));
136 pts[5] = Vec3f( x0, y1, cell(1,0));
137 pts[6] = Vec3f( x1, y1, cell(1,1));
138 pts[7] = Vec3f( x1, y0, cell(0,1));
139
140 Triangle* triangles = new Triangle[8];
141 triangles[0].set(0, 1, 3);
142 triangles[1].set(4, 5, 7);
143 triangles[2].set(0, 1, 4);
144 triangles[3].set(4, 1, 5);
145 triangles[4].set(1, 7, 3);
146 triangles[5].set(1, 5, 7);
147 triangles[6].set(0, 3, 7);
148 triangles[7].set(7, 4, 0);
149
150 convex1.set(true,
151 pts, // points
152 8, // num points
153 triangles,
154 8 // number of polygons
155 );
156 }
157
158 {
159 Vec3f* pts = new Vec3f[8];
160 pts[0] = Vec3f( x0, y0, min_height);
161 pts[1] = Vec3f( x0, y1, min_height);
162 pts[2] = Vec3f( x1, y1, min_height);
163 pts[3] = Vec3f( x1, y0, min_height);
164 pts[4] = Vec3f( x0, y0, cell(0,0));
165 pts[5] = Vec3f( x0, y1, cell(1,0));
166 pts[6] = Vec3f( x1, y1, cell(1,1));
167 pts[7] = Vec3f( x1, y0, cell(0,1));
168
169 Triangle* triangles = new Triangle[8];
170 triangles[0].set(3, 2, 1);
171 triangles[1].set(5, 6, 7);
172 triangles[2].set(1, 2, 5);
173 triangles[3].set(5, 2, 6);
174 triangles[4].set(1, 3, 7);
175 triangles[5].set(1, 7, 5);
176 triangles[6].set(2, 3, 7);
177 triangles[7].set(6, 2, 3);
178
179 convex2.set(true,
180 pts, // points
181 8, // num points
182 triangles,
183 8 // number of polygons
184 );
185 }
186
187 }
188}
189
191template<typename BV, typename S, int _Options = RelativeTransformationIsIdentity>
192class HeightFieldShapeCollisionTraversalNode
193: public CollisionTraversalNodeBase
194{
195public:
196
197 typedef CollisionTraversalNodeBase Base;
198
199 enum {
200 Options = _Options,
201 RTIsIdentity = _Options & RelativeTransformationIsIdentity
202 };
203
204 HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request)
205 : CollisionTraversalNodeBase(request)
206 {
207 model1 = NULL;
208 model2 = NULL;
209
210 num_bv_tests = 0;
211 num_leaf_tests = 0;
212 query_time_seconds = 0.0;
213
214 nsolver = NULL;
215 }
216
218 bool isFirstNodeLeaf(unsigned int b) const
219 {
220 return model1->getBV(b).isLeaf();
221 }
222
224 int getFirstLeftChild(unsigned int b) const
225 {
226 return model1->getBV(b).leftChild();
227 }
228
230 int getFirstRightChild(unsigned int b) const
231 {
232 return model1->getBV(b).rightChild();
233 }
234
236 bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const
237 {
238 std::cout << "\t BVDisjoints - 2" << std::endl;
239 if(this->enable_statistics) this->num_bv_tests++;
240 if (RTIsIdentity)
241 {
242 std::cout << "RTIsIdentity" << std::endl;
243 assert(false && "must never happened");
244 return !this->model1->getBV(b1).bv.overlap(this->model2_bv);
245 }
246 else
247 {
248 std::cout << "\t call !overlap(" << std::endl;
249 return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
250 this->model2_bv, this->model1->getBV(b1).bv);
251 }
252 }
253
259 bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const
260 {
261 if(this->enable_statistics) this->num_bv_tests++;
262 bool res;
263 if (RTIsIdentity)
264 {
265 assert(false && "must never happened");
266 res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound);
267 }
268 else
269 {
270 res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
271 this->model2_bv, this->model1->getBV(b1).bv,
272 this->request, sqrDistLowerBound);
273 }
274 assert (!res || sqrDistLowerBound > 0);
275 return res;
276 }
277
278 template<typename Polygone>
279 bool shapeDistance(const Convex<Polygone> & convex1, const Convex<Polygone> & convex2, const Transform3f & tf1,
280 const S & shape, const Transform3f & tf2,
281 FCL_REAL & distance, Vec3f& c1, Vec3f& c2,
282 Vec3f& normal) const
283 {
284 const Transform3f Id;
285 Vec3f contact2_1, contact2_2, normal2;
286 FCL_REAL distance2;
287 bool collision1, collision2;
288 if (RTIsIdentity)
289 collision1 = !nsolver->shapeDistance(convex1, Id, shape, tf2,
290 distance, c1, c2, normal);
291 else
292 collision1 = !nsolver->shapeDistance(convex1, tf1, shape, tf2,
293 distance, c1, c2, normal);
294
295 if (RTIsIdentity)
296 collision2 = !nsolver->shapeDistance(convex2, Id, shape, tf2,
297 distance2, c1, c2, normal);
298 else
299 collision2 = !nsolver->shapeDistance(convex2, tf1, shape, tf2,
300 distance2, contact2_1, contact2_2, normal2);
301
302 if(collision1 && collision2)
303 {
304 if(distance > distance2) // switch values
305 {
306 distance = distance2;
307 c1 = contact2_1; c2 = contact2_2;
308 normal = normal2;
309 }
310 return true;
311 }
312 else if(collision1)
313 {
314 return true;
315 }
316 else if(collision2)
317 {
318 distance = distance2;
319 c1 = contact2_1; c2 = contact2_2;
320 normal = normal2;
321 return true;
322 }
323
324 return false;
325 }
326
328 void leafCollides(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const
329 {
330 if(this->enable_statistics) this->num_leaf_tests++;
331 const HFNode<BV> & node = this->model1->getBV(b1);
332
333 // Split quadrilateral primitives into two convex shapes corresponding to polyhedron with triangular bases.
334 // This is essential to keep the convexity
335
336// typedef Convex<Quadrilateral> ConvexQuadrilateral;
337// const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model1);
338
339 typedef Convex<Triangle> ConvexTriangle;
340 ConvexTriangle convex1, convex2;
341 details::buildConvexTriangles(node,*this->model1,convex1,convex2);
342
343 FCL_REAL distance;
344 Vec3f c1, c2, normal;
345
346 bool collision = this->shapeDistance(convex1, convex2, this->tf1,
347 *(this->model2), this->tf2,
348 distance, c1, c2, normal);
349
350 if(collision) {
351 if(this->request.num_max_contacts > this->result->numContacts())
352 {
353 this->result->addContact(Contact(this->model1, this->model2,
354 b1, (int)Contact::NONE,
355 c1, normal, distance));
356 assert (this->result->isCollision());
357 return;
358 }
359 }
360 sqrDistLowerBound = distance * distance;
361// assert (distance > 0);
362 if ( this->request.security_margin > 0
363 && distance <= this->request.security_margin)
364 {
365 this->result->addContact(Contact(this->model1, this->model2,
366 b1, (int)Contact::NONE,
367 .5 * (c1+c2), (c2-c1).normalized (),
368 distance));
369 }
370 assert (!this->result->isCollision () || sqrDistLowerBound > 0);
371 }
372
373 const GJKSolver* nsolver;
374
375 const HeightField<BV>* model1;
376 const S* model2;
377 BV model2_bv;
378
379 mutable int num_bv_tests;
380 mutable int num_leaf_tests;
381 mutable FCL_REAL query_time_seconds;
382};
383
385template<typename S, typename BV, int _Options = RelativeTransformationIsIdentity>
386class ShapeHeightFieldCollisionTraversalNode
387: public CollisionTraversalNodeBase
388{
389public:
390
391 typedef CollisionTraversalNodeBase Base;
392
393 enum {
394 Options = _Options,
395 RTIsIdentity = _Options & RelativeTransformationIsIdentity
396 };
397
398 ShapeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
399 : CollisionTraversalNodeBase(request)
400 {
401 model1 = NULL;
402 model2 = NULL;
403
404 num_bv_tests = 0;
405 num_leaf_tests = 0;
406 query_time_seconds = 0.0;
407
408 nsolver = NULL;
409 }
410
412 bool firstOverSecond(unsigned int, unsigned int) const
413 {
414 return false;
415 }
416
418 bool isSecondNodeLeaf(unsigned int b) const
419 {
420 return model2->getBV(b).isLeaf();
421 }
422
424 int getSecondLeftChild(unsigned int b) const
425 {
426 return model2->getBV(b).leftChild();
427 }
428
430 int getSecondRightChild(unsigned int b) const
431 {
432 return model2->getBV(b).rightChild();
433 }
434
437 bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const
438 {
439 if(this->enable_statistics) this->num_bv_tests++;
440 if (RTIsIdentity)
441 return !this->model2->getBV(b2).bv.overlap(this->model1_bv);
442 else
443 return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
444 }
445
450 bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, FCL_REAL& sqrDistLowerBound) const
451 {
452 if(this->enable_statistics) this->num_bv_tests++;
453 bool res;
454 if (RTIsIdentity)
455 res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, sqrDistLowerBound);
456 else
457 res = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
458 this->model1_bv, this->model2->getBV(b2).bv,
459 sqrDistLowerBound);
460 assert (!res || sqrDistLowerBound > 0);
461 return res;
462 }
463
464 template<typename Polygone>
465 bool shapeDistance(const S & shape, const Transform3f & tf1,
466 const Convex<Polygone> & convex1, const Convex<Polygone> & convex2, const Transform3f & tf2,
467 FCL_REAL & distance, Vec3f& c1, Vec3f& c2,
468 Vec3f& normal) const
469 {
470 const Transform3f Id;
471 Vec3f contact2_1, contact2_2, normal2;
472 FCL_REAL distance2;
473 bool collision1, collision2;
474 if (RTIsIdentity)
475 collision1 = !nsolver->shapeDistance(shape, tf1, convex1, Id,
476 distance, c1, c2, normal);
477 else
478 collision1 = !nsolver->shapeDistance(shape, tf1, convex1, tf2,
479 distance, c1, c2, normal);
480
481 if (RTIsIdentity)
482 collision2 = !nsolver->shapeDistance(shape, tf1, convex2, Id,
483 distance2, c1, c2, normal);
484 else
485 collision2 = !nsolver->shapeDistance(shape, tf1, convex2, tf2,
486 distance2, contact2_1, contact2_2, normal2);
487
488 if(collision1 && collision2)
489 {
490 if(distance > distance2) // switch values
491 {
492 distance = distance2;
493 c1 = contact2_1; c2 = contact2_2;
494 normal = normal2;
495 }
496 return true;
497 }
498 else if(collision1)
499 {
500 return true;
501 }
502 else if(collision2)
503 {
504 distance = distance2;
505 c1 = contact2_1; c2 = contact2_2;
506 normal = normal2;
507 return true;
508 }
509
510 return false;
511 }
512
514 void leafCollides(unsigned int /*b1*/, unsigned int b2, FCL_REAL& sqrDistLowerBound) const
515 {
516 if(this->enable_statistics) this->num_leaf_tests++;
517 const HFNode<BV>& node = this->model2->getBV(b2);
518
519// typedef Convex<Quadrilateral> ConvexQuadrilateral;
520// const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model2);
521
522 typedef Convex<Triangle> ConvexTriangle;
523 ConvexTriangle convex1, convex2;
524 details::buildConvexTriangles(node,*this->model1,convex1,convex2);
525
526 FCL_REAL distance;
527 Vec3f normal;
528 Vec3f c1, c2; // closest points
529
530 bool collision = this->shapeDistance(*(this->model1), this->tf1,
531 convex1, convex2, this->tf2,
532 distance, c1, c2, normal);
533
534 if (collision) {
535 if(this->request.num_max_contacts > this->result->numContacts())
536 {
537 this->result->addContact (Contact(this->model1 , this->model2,
538 Contact::NONE, b2,
539 c1, normal, distance));
540 assert (this->result->isCollision ());
541 return;
542 }
543 }
544 sqrDistLowerBound = distance * distance;
545 if ( this->request.security_margin == 0
546 && distance <= this->request.security_margin)
547 {
548 this->result->addContact (Contact(this->model1 , this->model2,
549 Contact::NONE, b2,
550 .5 * (c1+c2), (c2-c1).normalized (),
551 distance));
552 }
553 assert (!this->result->isCollision () || sqrDistLowerBound > 0);
554 }
555
556 const GJKSolver* nsolver;
557
558 const S* model1;
559 const HeightField<BV>* model2;
560 BV model1_bv;
561
562 mutable int num_bv_tests;
563 mutable int num_leaf_tests;
564 mutable FCL_REAL query_time_seconds;
565};
566
568
571
573template<typename BV, typename S, int _Options = RelativeTransformationIsIdentity>
574class HeightFieldShapeDistanceTraversalNode
575: public DistanceTraversalNodeBase
576{
577public:
578
579 typedef DistanceTraversalNodeBase Base;
580
581 enum {
582 Options = _Options,
583 RTIsIdentity = _Options & RelativeTransformationIsIdentity
584 };
585
586 HeightFieldShapeDistanceTraversalNode()
587 : DistanceTraversalNodeBase()
588 {
589 model1 = NULL;
590 model2 = NULL;
591
592 num_leaf_tests = 0;
593 query_time_seconds = 0.0;
594
595 rel_err = 0;
596 abs_err = 0;
597 nsolver = NULL;
598 }
599
601 bool isFirstNodeLeaf(unsigned int b) const
602 {
603 return model1->getBV(b).isLeaf();
604 }
605
607 int getFirstLeftChild(unsigned int b) const
608 {
609 return model1->getBV(b).leftChild();
610 }
611
613 int getFirstRightChild(unsigned int b) const
614 {
615 return model1->getBV(b).rightChild();
616 }
617
619 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const
620 {
621 return model1->getBV(b1).bv.distance(model2_bv);
622 }
623
625 void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const
626 {
627 if(this->enable_statistics) this->num_leaf_tests++;
628
629 const BVNode<BV>& node = this->model1->getBV(b1);
630
631 typedef Convex<Quadrilateral> ConvexQuadrilateral;
632 const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model1);
633
634 FCL_REAL d;
635 Vec3f closest_p1, closest_p2, normal;
636
637 nsolver->shapeDistance(convex, this->tf1,
638 *(this->model2), this->tf2,
639 d, closest_p1, closest_p2, normal);
640
641 this->result->update(d, this->model1, this->model2, b1,
642 DistanceResult::NONE, closest_p1, closest_p2,
643 normal);
644 }
645
647 bool canStop(FCL_REAL c) const
648 {
649 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
650 return true;
651 return false;
652 }
653
654 FCL_REAL rel_err;
655 FCL_REAL abs_err;
656
657 const GJKSolver* nsolver;
658
659 const HeightField<BV>* model1;
660 const S* model2;
661 BV model2_bv;
662
663 mutable int num_bv_tests;
664 mutable int num_leaf_tests;
665 mutable FCL_REAL query_time_seconds;
666};
667
669template<typename S, typename BV, int _Options = RelativeTransformationIsIdentity>
670class ShapeHeightFieldDistanceTraversalNode
671: public DistanceTraversalNodeBase
672{
673public:
674
675 typedef DistanceTraversalNodeBase Base;
676
677 enum {
678 Options = _Options,
679 RTIsIdentity = _Options & RelativeTransformationIsIdentity
680 };
681
682 ShapeHeightFieldDistanceTraversalNode()
683 {
684 model1 = NULL;
685 model2 = NULL;
686
687 num_leaf_tests = 0;
688 query_time_seconds = 0.0;
689
690 rel_err = 0;
691 abs_err = 0;
692 nsolver = NULL;
693 }
694
696 bool firstOverSecond(unsigned int, unsigned int) const
697 {
698 return false;
699 }
700
702 bool isSecondNodeLeaf(unsigned int b) const
703 {
704 return model2->getBV(b).isLeaf();
705 }
706
708 int getSecondLeftChild(unsigned int b) const
709 {
710 return model2->getBV(b).leftChild();
711 }
712
714 int getSecondRightChild(unsigned int b) const
715 {
716 return model2->getBV(b).rightChild();
717 }
718
720 void leafComputeDistance(unsigned int /*b1*/, unsigned int b2) const
721 {
722 if(this->enable_statistics) this->num_leaf_tests++;
723
724 const BVNode<BV>& node = this->model2->getBV(b2);
725
726 typedef Convex<Quadrilateral> ConvexQuadrilateral;
727 const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model2);
728
729 FCL_REAL d;
730 Vec3f closest_p1, closest_p2, normal;
731
732 nsolver->shapeDistance(*(this->model1), this->tf1,
733 convex, this->tf2,
734 d, closest_p1, closest_p2, normal);
735
736 this->result->update(d, this->model1, this->model2,
737 DistanceResult::NONE, b2, closest_p1, closest_p2,
738 normal);
739 }
740
742 bool canStop(FCL_REAL c) const
743 {
744 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
745 return true;
746 return false;
747 }
748
749 FCL_REAL rel_err;
750 FCL_REAL abs_err;
751
752 const GJKSolver* nsolver;
753
754 const S* model1;
755 const HeightField<BV>* model2;
756 BV model1_bv;
757
758 mutable int num_leaf_tests;
759 mutable FCL_REAL query_time_seconds;
760};
761
763
764}} // namespace hpp::fcl
765
767
768#endif
collision_data.h
convex.h
HPP_FCL_UNUSED_VARIABLE
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: fwd.hh:51
geometric_shapes_utility.h
hpp::fcl::distance
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.
hpp::fcl::overlap
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.
hfield.h
details
Definition: traversal_node_setup.h:852
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::MatrixXf
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, Eigen::Dynamic > MatrixXf
Definition: data_types.h:72
hpp::fcl::VecXf
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 1 > VecXf
Definition: data_types.h:68
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
narrowphase.h
geometric_shapes.h
hpp::fcl::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:87
hpp::fcl::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:435
traversal.h
traversal_node_base.h