38 #ifndef HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H 39 #define HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H 63 Convex<Quadrilateral> buildConvexQuadrilateral(
const HFNode<BV> & node,
64 const HeightField<BV> & model)
66 const MatrixXf & heights = model.getHeights();
67 const VecXf & x_grid = model.getXGrid();
68 const VecXf & y_grid = model.getYGrid();
70 const FCL_REAL min_height = model.getMinHeight();
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);
79 assert(cell.maxCoeff() > min_height &&
"max_height is lower than min_height");
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));
91 Quadrilateral* polygons =
new Quadrilateral[6];
92 polygons[0].set(0, 3, 2, 1);
93 polygons[1].set(0, 1, 5, 4);
94 polygons[2].set(1, 2, 6, 5);
95 polygons[3].set(2, 3, 7, 6);
96 polygons[4].set(3, 0, 4, 7);
97 polygons[5].set(4, 5, 6, 7);
99 return Convex<Quadrilateral> (
true,
107 template<
typename BV>
108 void buildConvexTriangles(
const HFNode<BV> & node,
109 const HeightField<BV> & model,
110 Convex<Triangle> & convex1, Convex<Triangle> & convex2)
112 const MatrixXf & heights = model.getHeights();
113 const VecXf & x_grid = model.getXGrid();
114 const VecXf & y_grid = model.getYGrid();
116 const FCL_REAL min_height = model.getMinHeight();
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();
126 assert(max_height > min_height &&
"max_height is lower than min_height");
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));
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);
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));
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);
191 template<
typename BV,
typename S,
int _Options = RelativeTransformationIsIdentity>
192 class HeightFieldShapeCollisionTraversalNode
193 :
public CollisionTraversalNodeBase
197 typedef CollisionTraversalNodeBase Base;
201 RTIsIdentity = _Options & RelativeTransformationIsIdentity
204 HeightFieldShapeCollisionTraversalNode(
const CollisionRequest& request)
205 : CollisionTraversalNodeBase(request)
212 query_time_seconds = 0.0;
218 bool isFirstNodeLeaf(
unsigned int b)
const 220 return model1->getBV(b).isLeaf();
224 int getFirstLeftChild(
unsigned int b)
const 226 return model1->getBV(b).leftChild();
230 int getFirstRightChild(
unsigned int b)
const 232 return model1->getBV(b).rightChild();
236 bool BVDisjoints(
unsigned int b1,
unsigned int )
const 238 std::cout <<
"\t BVDisjoints - 2" << std::endl;
239 if(this->enable_statistics) this->num_bv_tests++;
242 std::cout <<
"RTIsIdentity" << std::endl;
243 assert(
false &&
"must never happened");
244 return !this->model1->getBV(b1).bv.overlap(this->model2_bv);
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);
259 bool BVDisjoints(
unsigned int b1,
unsigned int ,
FCL_REAL& sqrDistLowerBound)
const 261 if(this->enable_statistics) this->num_bv_tests++;
265 assert(
false &&
"must never happened");
266 res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound);
270 res = !
overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
271 this->model2_bv, this->model1->getBV(b1).bv,
272 this->request, sqrDistLowerBound);
274 assert (!res || sqrDistLowerBound > 0);
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,
284 const Transform3f Id;
285 Vec3f contact2_1, contact2_2, normal2;
287 bool collision1, collision2;
289 collision1 = !nsolver->shapeDistance(convex1, Id, shape, tf2,
292 collision1 = !nsolver->shapeDistance(convex1, tf1, shape, tf2,
296 collision2 = !nsolver->shapeDistance(convex2, Id, shape, tf2,
297 distance2, c1, c2, normal);
299 collision2 = !nsolver->shapeDistance(convex2, tf1, shape, tf2,
300 distance2, contact2_1, contact2_2, normal2);
302 if(collision1 && collision2)
307 c1 = contact2_1; c2 = contact2_2;
319 c1 = contact2_1; c2 = contact2_2;
328 void leafCollides(
unsigned int b1,
unsigned int ,
FCL_REAL& sqrDistLowerBound)
const 330 if(this->enable_statistics) this->num_leaf_tests++;
331 const HFNode<BV> & node = this->model1->getBV(b1);
339 typedef Convex<Triangle> ConvexTriangle;
340 ConvexTriangle convex1, convex2;
341 details::buildConvexTriangles(node,*this->model1,convex1,convex2);
344 Vec3f c1, c2, normal;
346 bool collision = this->shapeDistance(convex1, convex2, this->tf1,
347 *(this->model2), this->tf2,
351 if(this->request.num_max_contacts > this->result->numContacts())
353 this->result->addContact(Contact(this->model1, this->model2,
356 assert (this->result->isCollision());
362 if ( this->request.security_margin > 0
363 && distance <= this->request.security_margin)
365 this->result->addContact(Contact(this->model1, this->model2,
367 .5 * (c1+c2), (c2-c1).normalized (),
370 assert (!this->result->isCollision () || sqrDistLowerBound > 0);
373 const GJKSolver* nsolver;
375 const HeightField<BV>* model1;
379 mutable int num_bv_tests;
380 mutable int num_leaf_tests;
381 mutable FCL_REAL query_time_seconds;
385 template<
typename S,
typename BV,
int _Options = RelativeTransformationIsIdentity>
386 class ShapeHeightFieldCollisionTraversalNode
387 :
public CollisionTraversalNodeBase
391 typedef CollisionTraversalNodeBase Base;
395 RTIsIdentity = _Options & RelativeTransformationIsIdentity
398 ShapeHeightFieldCollisionTraversalNode(
const CollisionRequest& request)
399 : CollisionTraversalNodeBase(request)
406 query_time_seconds = 0.0;
412 bool firstOverSecond(
unsigned int,
unsigned int)
const 418 bool isSecondNodeLeaf(
unsigned int b)
const 420 return model2->getBV(b).isLeaf();
424 int getSecondLeftChild(
unsigned int b)
const 426 return model2->getBV(b).leftChild();
430 int getSecondRightChild(
unsigned int b)
const 432 return model2->getBV(b).rightChild();
437 bool BVDisjoints(
unsigned int ,
unsigned int b2)
const 439 if(this->enable_statistics) this->num_bv_tests++;
441 return !this->model2->getBV(b2).bv.overlap(this->model1_bv);
443 return !
overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
450 bool BVDisjoints(
unsigned int ,
unsigned int b2,
FCL_REAL& sqrDistLowerBound)
const 452 if(this->enable_statistics) this->num_bv_tests++;
455 res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, sqrDistLowerBound);
457 res = !
overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
458 this->model1_bv, this->model2->getBV(b2).bv,
460 assert (!res || sqrDistLowerBound > 0);
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,
470 const Transform3f Id;
471 Vec3f contact2_1, contact2_2, normal2;
473 bool collision1, collision2;
475 collision1 = !nsolver->shapeDistance(shape, tf1, convex1, Id,
478 collision1 = !nsolver->shapeDistance(shape, tf1, convex1, tf2,
482 collision2 = !nsolver->shapeDistance(shape, tf1, convex2, Id,
483 distance2, c1, c2, normal);
485 collision2 = !nsolver->shapeDistance(shape, tf1, convex2, tf2,
486 distance2, contact2_1, contact2_2, normal2);
488 if(collision1 && collision2)
493 c1 = contact2_1; c2 = contact2_2;
505 c1 = contact2_1; c2 = contact2_2;
514 void leafCollides(
unsigned int ,
unsigned int b2,
FCL_REAL& sqrDistLowerBound)
const 516 if(this->enable_statistics) this->num_leaf_tests++;
517 const HFNode<BV>& node = this->model2->getBV(b2);
522 typedef Convex<Triangle> ConvexTriangle;
523 ConvexTriangle convex1, convex2;
524 details::buildConvexTriangles(node,*this->model1,convex1,convex2);
530 bool collision = this->shapeDistance(*(this->model1), this->tf1,
531 convex1, convex2, this->tf2,
535 if(this->request.num_max_contacts > this->result->numContacts())
537 this->result->addContact (Contact(this->model1 , this->model2,
540 assert (this->result->isCollision ());
545 if ( this->request.security_margin == 0
546 && distance <= this->request.security_margin)
548 this->result->addContact (Contact(this->model1 , this->model2,
550 .5 * (c1+c2), (c2-c1).normalized (),
553 assert (!this->result->isCollision () || sqrDistLowerBound > 0);
556 const GJKSolver* nsolver;
559 const HeightField<BV>* model2;
562 mutable int num_bv_tests;
563 mutable int num_leaf_tests;
564 mutable FCL_REAL query_time_seconds;
573 template<
typename BV,
typename S,
int _Options = RelativeTransformationIsIdentity>
574 class HeightFieldShapeDistanceTraversalNode
575 :
public DistanceTraversalNodeBase
579 typedef DistanceTraversalNodeBase Base;
583 RTIsIdentity = _Options & RelativeTransformationIsIdentity
586 HeightFieldShapeDistanceTraversalNode()
587 : DistanceTraversalNodeBase()
593 query_time_seconds = 0.0;
601 bool isFirstNodeLeaf(
unsigned int b)
const 603 return model1->getBV(b).isLeaf();
607 int getFirstLeftChild(
unsigned int b)
const 609 return model1->getBV(b).leftChild();
613 int getFirstRightChild(
unsigned int b)
const 615 return model1->getBV(b).rightChild();
619 FCL_REAL BVDistanceLowerBound(
unsigned int b1,
unsigned int )
const 621 return model1->getBV(b1).bv.distance(model2_bv);
625 void leafComputeDistance(
unsigned int b1,
unsigned int )
const 627 if(this->enable_statistics) this->num_leaf_tests++;
629 const BVNode<BV>& node = this->model1->getBV(b1);
631 typedef Convex<Quadrilateral> ConvexQuadrilateral;
632 const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model1);
635 Vec3f closest_p1, closest_p2, normal;
637 nsolver->shapeDistance(convex, this->tf1,
638 *(this->model2), this->tf2,
639 d, closest_p1, closest_p2, normal);
641 this->result->update(d, this->model1, this->model2, b1,
649 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
657 const GJKSolver* nsolver;
659 const HeightField<BV>* model1;
663 mutable int num_bv_tests;
664 mutable int num_leaf_tests;
665 mutable FCL_REAL query_time_seconds;
669 template<
typename S,
typename BV,
int _Options = RelativeTransformationIsIdentity>
670 class ShapeHeightFieldDistanceTraversalNode
671 :
public DistanceTraversalNodeBase
675 typedef DistanceTraversalNodeBase Base;
679 RTIsIdentity = _Options & RelativeTransformationIsIdentity
682 ShapeHeightFieldDistanceTraversalNode()
688 query_time_seconds = 0.0;
696 bool firstOverSecond(
unsigned int,
unsigned int)
const 702 bool isSecondNodeLeaf(
unsigned int b)
const 704 return model2->getBV(b).isLeaf();
708 int getSecondLeftChild(
unsigned int b)
const 710 return model2->getBV(b).leftChild();
714 int getSecondRightChild(
unsigned int b)
const 716 return model2->getBV(b).rightChild();
720 void leafComputeDistance(
unsigned int ,
unsigned int b2)
const 722 if(this->enable_statistics) this->num_leaf_tests++;
724 const BVNode<BV>& node = this->model2->getBV(b2);
726 typedef Convex<Quadrilateral> ConvexQuadrilateral;
727 const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model2);
730 Vec3f closest_p1, closest_p2, normal;
732 nsolver->shapeDistance(*(this->model1), this->tf1,
734 d, closest_p1, closest_p2, normal);
736 this->result->update(d, this->model1, this->model2,
744 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
752 const GJKSolver* nsolver;
755 const HeightField<BV>* model2;
758 mutable int num_leaf_tests;
759 mutable FCL_REAL query_time_seconds;
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
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