hpp-fcl  1.8.1
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>
49 #include <hpp/fcl/hfield.h>
50 #include <hpp/fcl/shape/convex.h>
51 
52 namespace hpp
53 {
54 namespace fcl
55 {
56 
59 
60 namespace 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 
191 template<typename BV, typename S, int _Options = RelativeTransformationIsIdentity>
192 class HeightFieldShapeCollisionTraversalNode
193 : public CollisionTraversalNodeBase
194 {
195 public:
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 
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 
385 template<typename S, typename BV, int _Options = RelativeTransformationIsIdentity>
386 class ShapeHeightFieldCollisionTraversalNode
387 : public CollisionTraversalNodeBase
388 {
389 public:
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 
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 
573 template<typename BV, typename S, int _Options = RelativeTransformationIsIdentity>
574 class HeightFieldShapeDistanceTraversalNode
575 : public DistanceTraversalNodeBase
576 {
577 public:
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 
669 template<typename S, typename BV, int _Options = RelativeTransformationIsIdentity>
670 class ShapeHeightFieldDistanceTraversalNode
671 : public DistanceTraversalNodeBase
672 {
673 public:
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
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, Eigen::Dynamic > MatrixXf
Definition: data_types.h:72
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: fwd.hh:51
Main namespace.
Definition: AABB.h:43
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 1 > VecXf
Definition: data_types.h:68
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.
double FCL_REAL
Definition: data_types.h:66
Definition: traversal_node_setup.h:851
static const int NONE
invalid contact primitive information
Definition: collision_data.h:87
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, 1 > Vec3f
Definition: data_types.h:67
static const int NONE
invalid contact primitive information
Definition: collision_data.h:435