hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
37#ifndef HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H
38#define HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H
39
41
48#include <hpp/fcl/hfield.h>
50
51namespace hpp {
52namespace fcl {
53
56
57namespace details {
58template <typename BV>
59Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
60 const HeightField<BV>& model) {
61 const MatrixXf& heights = model.getHeights();
62 const VecXf& x_grid = model.getXGrid();
63 const VecXf& y_grid = model.getYGrid();
64
65 const FCL_REAL min_height = model.getMinHeight();
66
67 const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
68 y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
69 const Eigen::Block<const MatrixXf, 2, 2> cell =
70 heights.block<2, 2>(node.y_id, node.x_id);
71
72 assert(cell.maxCoeff() > min_height &&
73 "max_height is lower than min_height"); // Check whether the geometry
74 // is degenerated
75
76 Vec3f* pts = new Vec3f[8];
77 pts[0] = Vec3f(x0, y0, min_height);
78 pts[1] = Vec3f(x0, y1, min_height);
79 pts[2] = Vec3f(x1, y1, min_height);
80 pts[3] = Vec3f(x1, y0, min_height);
81 pts[4] = Vec3f(x0, y0, cell(0, 0));
82 pts[5] = Vec3f(x0, y1, cell(1, 0));
83 pts[6] = Vec3f(x1, y1, cell(1, 1));
84 pts[7] = Vec3f(x1, y0, cell(0, 1));
85
86 Quadrilateral* polygons = new Quadrilateral[6];
87 polygons[0].set(0, 3, 2, 1); // x+ side
88 polygons[1].set(0, 1, 5, 4); // y- side
89 polygons[2].set(1, 2, 6, 5); // x- side
90 polygons[3].set(2, 3, 7, 6); // y+ side
91 polygons[4].set(3, 0, 4, 7); // z- side
92 polygons[5].set(4, 5, 6, 7); // z+ side
93
94 return Convex<Quadrilateral>(true,
95 pts, // points
96 8, // num points
97 polygons,
98 6 // number of polygons
99 );
100}
101
102template <typename BV>
103void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
104 Convex<Triangle>& convex1,
105 Convex<Triangle>& convex2) {
106 const MatrixXf& heights = model.getHeights();
107 const VecXf& x_grid = model.getXGrid();
108 const VecXf& y_grid = model.getYGrid();
109
110 const FCL_REAL min_height = model.getMinHeight();
111
112 const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
113 y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
114 const FCL_REAL max_height = node.max_height;
115 const Eigen::Block<const MatrixXf, 2, 2> cell =
116 heights.block<2, 2>(node.y_id, node.x_id);
117
118 assert(max_height > min_height &&
119 "max_height is lower than min_height"); // Check whether the geometry
120 // is degenerated
121 HPP_FCL_UNUSED_VARIABLE(max_height);
122
123 {
124 Vec3f* pts = new Vec3f[8];
125 pts[0] = Vec3f(x0, y0, min_height);
126 pts[1] = Vec3f(x0, y1, min_height);
127 pts[2] = Vec3f(x1, y1, min_height);
128 pts[3] = Vec3f(x1, y0, min_height);
129 pts[4] = Vec3f(x0, y0, cell(0, 0));
130 pts[5] = Vec3f(x0, y1, cell(1, 0));
131 pts[6] = Vec3f(x1, y1, cell(1, 1));
132 pts[7] = Vec3f(x1, y0, cell(0, 1));
133
134 Triangle* triangles = new Triangle[8];
135 triangles[0].set(0, 1, 3); // bottom
136 triangles[1].set(4, 5, 7); // top
137 triangles[2].set(0, 1, 4);
138 triangles[3].set(4, 1, 5);
139 triangles[4].set(1, 7, 3);
140 triangles[5].set(1, 5, 7);
141 triangles[6].set(0, 3, 7);
142 triangles[7].set(7, 4, 0);
143
144 convex1.set(true,
145 pts, // points
146 8, // num points
147 triangles,
148 8 // number of polygons
149 );
150 }
151
152 {
153 Vec3f* pts = new Vec3f[8];
154 memcpy(pts, convex1.points, 8 * sizeof(Vec3f));
155
156 Triangle* triangles = new Triangle[8];
157 triangles[0].set(3, 2, 1); // top
158 triangles[1].set(5, 6, 7); // bottom
159 triangles[2].set(1, 2, 5);
160 triangles[3].set(5, 2, 6);
161 triangles[4].set(1, 3, 7);
162 triangles[5].set(1, 7, 5);
163 triangles[6].set(2, 3, 7);
164 triangles[7].set(6, 2, 3);
165
166 convex2.set(true,
167 pts, // points
168 8, // num points
169 triangles,
170 8 // number of polygons
171 );
172 }
173}
174} // namespace details
175
177template <typename BV, typename S,
178 int _Options = RelativeTransformationIsIdentity>
179class HeightFieldShapeCollisionTraversalNode
180 : public CollisionTraversalNodeBase {
181 public:
182 typedef CollisionTraversalNodeBase Base;
183 typedef Eigen::Array<FCL_REAL, 1, 2> Array2d;
184
185 enum {
186 Options = _Options,
187 RTIsIdentity = _Options & RelativeTransformationIsIdentity
188 };
189
190 HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request)
191 : CollisionTraversalNodeBase(request) {
192 model1 = NULL;
193 model2 = NULL;
194
195 num_bv_tests = 0;
196 num_leaf_tests = 0;
197 query_time_seconds = 0.0;
198
199 nsolver = NULL;
200 shape_inflation.setZero();
201 }
202
204 bool isFirstNodeLeaf(unsigned int b) const {
205 return model1->getBV(b).isLeaf();
206 }
207
209 int getFirstLeftChild(unsigned int b) const {
210 return static_cast<int>(model1->getBV(b).leftChild());
211 }
212
214 int getFirstRightChild(unsigned int b) const {
215 return static_cast<int>(model1->getBV(b).rightChild());
216 }
217
223 bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
224 FCL_REAL& sqrDistLowerBound) const {
225 if (this->enable_statistics) this->num_bv_tests++;
226
227 bool disjoint;
228 if (RTIsIdentity) {
229 assert(false && "must never happened");
230 disjoint = !this->model1->getBV(b1).bv.overlap(
231 this->model2_bv, this->request, sqrDistLowerBound);
232 } else {
233 disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
234 this->model1->getBV(b1).bv, this->model2_bv,
235 this->request, sqrDistLowerBound);
236 }
237
238 if (disjoint)
239 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
240 sqrDistLowerBound);
241
242 assert(!disjoint || sqrDistLowerBound > 0);
243 return disjoint;
244 }
245
246 template <typename Polygone>
247 bool shapeDistance(const Convex<Polygone>& convex1,
248 const Convex<Polygone>& convex2, const Transform3f& tf1,
249 const S& shape, const Transform3f& tf2, FCL_REAL& distance,
250 Vec3f& c1, Vec3f& c2, Vec3f& normal) const {
251 const Transform3f Id;
252 Vec3f contact2_1, contact2_2, normal2;
253 FCL_REAL distance2;
254 bool collision1, collision2;
255 if (RTIsIdentity)
256 collision1 = !nsolver->shapeDistance(convex1, Id, shape, tf2, distance,
257 c1, c2, normal);
258 else
259 collision1 = !nsolver->shapeDistance(convex1, tf1, shape, tf2, distance,
260 c1, c2, normal);
261
262 if (RTIsIdentity)
263 collision2 = !nsolver->shapeDistance(convex2, Id, shape, tf2, distance2,
264 c1, c2, normal);
265 else
266 collision2 = !nsolver->shapeDistance(convex2, tf1, shape, tf2, distance2,
267 contact2_1, contact2_2, normal2);
268
269 if (collision1 && collision2) {
270 if (distance > distance2) // switch values
271 {
272 distance = distance2;
273 c1 = contact2_1;
274 c2 = contact2_2;
275 normal = normal2;
276 }
277 return true;
278 } else if (collision1) {
279 return true;
280 } else if (collision2) {
281 distance = distance2;
282 c1 = contact2_1;
283 c2 = contact2_2;
284 normal = normal2;
285 return true;
286 }
287
288 return false;
289 }
290
291 template <typename Polygone>
292 bool shapeCollision(const Convex<Polygone>& convex1,
293 const Convex<Polygone>& convex2, const Transform3f& tf1,
294 const S& shape, const Transform3f& tf2,
295 FCL_REAL& distance_lower_bound, Vec3f& contact_point,
296 Vec3f& normal) const {
297 const Transform3f Id;
298 Vec3f contact_point2, normal2;
299 FCL_REAL distance_lower_bound2;
300 bool collision1, collision2;
301 if (RTIsIdentity)
302 collision1 =
303 nsolver->shapeIntersect(convex1, Id, shape, tf2, distance_lower_bound,
304 true, &contact_point, &normal);
305 else
306 collision1 = nsolver->shapeIntersect(convex1, tf1, shape, tf2,
307 distance_lower_bound, true,
308 &contact_point, &normal);
309
310 if (RTIsIdentity)
311 collision2 = nsolver->shapeIntersect(convex2, Id, shape, tf2,
312 distance_lower_bound2, true,
313 &contact_point2, &normal2);
314 else
315 collision2 = nsolver->shapeIntersect(convex2, tf1, shape, tf2,
316 distance_lower_bound2, true,
317 &contact_point2, &normal2);
318
319 if (collision1 && collision2) {
320 // In some case, EPA might returns something like
321 // -(std::numeric_limits<FCL_REAL>::max)().
322 if (distance_lower_bound != -(std::numeric_limits<FCL_REAL>::max)() &&
323 distance_lower_bound2 != -(std::numeric_limits<FCL_REAL>::max)()) {
324 if (distance_lower_bound > distance_lower_bound2) // switch values
325 {
326 distance_lower_bound = distance_lower_bound2;
327 contact_point = contact_point2;
328 normal = normal2;
329 }
330 } else if (distance_lower_bound2 !=
331 -(std::numeric_limits<FCL_REAL>::max)()) {
332 distance_lower_bound = distance_lower_bound2;
333 contact_point = contact_point2;
334 normal = normal2;
335 }
336 return true;
337 } else if (collision1) {
338 return true;
339 } else if (collision2) {
340 distance_lower_bound = distance_lower_bound2;
341 contact_point = contact_point2;
342 normal = normal2;
343 return true;
344 }
345
346 return false;
347 }
348
350 void leafCollides(unsigned int b1, unsigned int /*b2*/,
351 FCL_REAL& sqrDistLowerBound) const {
352 if (this->enable_statistics) this->num_leaf_tests++;
353 const HFNode<BV>& node = this->model1->getBV(b1);
354
355 // Split quadrilateral primitives into two convex shapes corresponding to
356 // polyhedron with triangular bases. This is essential to keep the convexity
357
358 // typedef Convex<Quadrilateral> ConvexQuadrilateral;
359 // const ConvexQuadrilateral convex =
360 // details::buildConvexQuadrilateral(node,*this->model1);
361
362 typedef Convex<Triangle> ConvexTriangle;
363 ConvexTriangle convex1, convex2;
364 details::buildConvexTriangles(node, *this->model1, convex1, convex2);
365
367 // Vec3f contact_point, normal;
368 Vec3f c1, c2, normal;
369
370 bool collision =
371 this->shapeDistance(convex1, convex2, this->tf1, *(this->model2),
372 this->tf2, distance, c1, c2, normal);
373
374 // this->shapeCollision(convex1, convex2, this->tf1, *(this->model2),
375 // this->tf2,
376 // distance, contact_point, normal);
377
378 FCL_REAL distToCollision = distance - this->request.security_margin;
379 if (distToCollision <= this->request.collision_distance_threshold) {
380 sqrDistLowerBound = 0;
381 if (this->request.num_max_contacts > this->result->numContacts()) {
382 this->result->addContact(Contact(this->model1, this->model2, (int)b1,
383 (int)Contact::NONE, .5 * (c1 + c2),
384 (c2 - c1).normalized(), -distance));
385 }
386 } else if (collision && this->request.security_margin >= 0) {
387 sqrDistLowerBound = 0;
388 if (this->request.num_max_contacts > this->result->numContacts()) {
389 this->result->addContact(Contact(this->model1, this->model2, (int)b1,
390 (int)Contact::NONE, c1, normal,
391 -distance));
392 assert(this->result->isCollision());
393 }
394 } else
395 sqrDistLowerBound = distToCollision * distToCollision;
396
397 // const Vec3f c1 = contact_point - distance * 0.5 * normal;
398 // const Vec3f c2 = contact_point + distance * 0.5 * normal;
399 internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
400 distToCollision, c1, c2);
401
402 assert(this->result->isCollision() || sqrDistLowerBound > 0);
403 }
404
405 const GJKSolver* nsolver;
406
407 const HeightField<BV>* model1;
408 const S* model2;
409 BV model2_bv;
410
411 Array2d shape_inflation;
412
413 mutable int num_bv_tests;
414 mutable int num_leaf_tests;
415 mutable FCL_REAL query_time_seconds;
416};
417
419
422
424template <typename BV, typename S,
425 int _Options = RelativeTransformationIsIdentity>
426class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
427 public:
428 typedef DistanceTraversalNodeBase Base;
429
430 enum {
431 Options = _Options,
432 RTIsIdentity = _Options & RelativeTransformationIsIdentity
433 };
434
435 HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
436 model1 = NULL;
437 model2 = NULL;
438
439 num_leaf_tests = 0;
440 query_time_seconds = 0.0;
441
442 rel_err = 0;
443 abs_err = 0;
444 nsolver = NULL;
445 }
446
448 bool isFirstNodeLeaf(unsigned int b) const {
449 return model1->getBV(b).isLeaf();
450 }
451
453 int getFirstLeftChild(unsigned int b) const {
454 return model1->getBV(b).leftChild();
455 }
456
458 int getFirstRightChild(unsigned int b) const {
459 return model1->getBV(b).rightChild();
460 }
461
463 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
464 return model1->getBV(b1).bv.distance(
465 model2_bv); // TODO(jcarpent): tf1 is not taken into account here.
466 }
467
469 void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
470 if (this->enable_statistics) this->num_leaf_tests++;
471
472 const BVNode<BV>& node = this->model1->getBV(b1);
473
474 typedef Convex<Quadrilateral> ConvexQuadrilateral;
475 const ConvexQuadrilateral convex =
476 details::buildConvexQuadrilateral(node, *this->model1);
477
478 FCL_REAL d;
479 Vec3f closest_p1, closest_p2, normal;
480
481 nsolver->shapeDistance(convex, this->tf1, *(this->model2), this->tf2, d,
482 closest_p1, closest_p2, normal);
483
484 this->result->update(d, this->model1, this->model2, b1,
485 DistanceResult::NONE, closest_p1, closest_p2, normal);
486 }
487
489 bool canStop(FCL_REAL c) const {
490 if ((c >= this->result->min_distance - abs_err) &&
491 (c * (1 + rel_err) >= this->result->min_distance))
492 return true;
493 return false;
494 }
495
496 FCL_REAL rel_err;
497 FCL_REAL abs_err;
498
499 const GJKSolver* nsolver;
500
501 const HeightField<BV>* model1;
502 const S* model2;
503 BV model2_bv;
504
505 mutable int num_bv_tests;
506 mutable int num_leaf_tests;
507 mutable FCL_REAL query_time_seconds;
508};
509
511
512} // namespace fcl
513} // namespace hpp
514
516
517#endif
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: fwd.hh:55
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, 1 > Vec3f
Definition: data_types.h:66
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, Eigen::Dynamic > MatrixXf
Definition: data_types.h:71
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 1 > VecXf
Definition: data_types.h:67
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44