traversal_node_setup.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H
39 #define HPP_FCL_TRAVERSAL_NODE_SETUP_H
40 
42 
43 #include <hpp/fcl/internal/tools.h>
44 #include <hpp/fcl/internal/traversal_node_shapes.h>
45 
46 #include <hpp/fcl/internal/traversal_node_bvhs.h>
47 #include <hpp/fcl/internal/traversal_node_bvh_shape.h>
48 
49 // #include <hpp/fcl/internal/traversal_node_hfields.h>
50 #include <hpp/fcl/internal/traversal_node_hfield_shape.h>
51 
52 #ifdef HPP_FCL_HAS_OCTOMAP
53 #include <hpp/fcl/internal/traversal_node_octree.h>
54 #endif
55 
56 #include <hpp/fcl/BVH/BVH_utility.h>
57 
58 namespace hpp {
59 namespace fcl {
60 
61 #ifdef HPP_FCL_HAS_OCTOMAP
62 inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1,
65  const Transform3f& tf1, const OcTree& model2,
66  const Transform3f& tf2, const OcTreeSolver* otsolver,
67  CollisionResult& result) {
68  node.result = &result;
69 
70  node.model1 = &model1;
71  node.model2 = &model2;
72 
73  node.otsolver = otsolver;
74 
75  node.tf1 = tf1;
76  node.tf2 = tf2;
77 
78  return true;
79 }
80 
83 inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1,
84  const Transform3f& tf1, const OcTree& model2,
85  const Transform3f& tf2, const OcTreeSolver* otsolver,
86  const DistanceRequest& request, DistanceResult& result)
87 
88 {
89  node.request = request;
90  node.result = &result;
91 
92  node.model1 = &model1;
93  node.model2 = &model2;
94 
95  node.otsolver = otsolver;
96 
97  node.tf1 = tf1;
98  node.tf2 = tf2;
99 
100  return true;
101 }
102 
105 template <typename S>
106 bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node, const S& model1,
107  const Transform3f& tf1, const OcTree& model2,
108  const Transform3f& tf2, const OcTreeSolver* otsolver,
109  CollisionResult& result) {
110  node.result = &result;
111 
112  node.model1 = &model1;
113  node.model2 = &model2;
114 
115  node.otsolver = otsolver;
116 
117  node.tf1 = tf1;
118  node.tf2 = tf2;
119 
120  return true;
121 }
122 
125 template <typename S>
126 bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
127  const OcTree& model1, const Transform3f& tf1, const S& model2,
128  const Transform3f& tf2, const OcTreeSolver* otsolver,
129  CollisionResult& result) {
130  node.result = &result;
131 
132  node.model1 = &model1;
133  node.model2 = &model2;
134 
135  node.otsolver = otsolver;
136 
137  node.tf1 = tf1;
138  node.tf2 = tf2;
139 
140  return true;
141 }
142 
145 template <typename S>
146 bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node, const S& model1,
147  const Transform3f& tf1, const OcTree& model2,
148  const Transform3f& tf2, const OcTreeSolver* otsolver,
149  const DistanceRequest& request, DistanceResult& result) {
150  node.request = request;
151  node.result = &result;
152 
153  node.model1 = &model1;
154  node.model2 = &model2;
155 
156  node.otsolver = otsolver;
157 
158  node.tf1 = tf1;
159  node.tf2 = tf2;
160 
161  return true;
162 }
163 
166 template <typename S>
167 bool initialize(OcTreeShapeDistanceTraversalNode<S>& node, const OcTree& model1,
168  const Transform3f& tf1, const S& model2, const Transform3f& tf2,
169  const OcTreeSolver* otsolver, const DistanceRequest& request,
170  DistanceResult& result) {
171  node.request = request;
172  node.result = &result;
173 
174  node.model1 = &model1;
175  node.model2 = &model2;
176 
177  node.otsolver = otsolver;
178 
179  node.tf1 = tf1;
180  node.tf2 = tf2;
181 
182  return true;
183 }
184 
187 template <typename BV>
188 bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
189  const BVHModel<BV>& model1, const Transform3f& tf1,
190  const OcTree& model2, const Transform3f& tf2,
191  const OcTreeSolver* otsolver, CollisionResult& result) {
192  node.result = &result;
193 
194  node.model1 = &model1;
195  node.model2 = &model2;
196 
197  node.otsolver = otsolver;
198 
199  node.tf1 = tf1;
200  node.tf2 = tf2;
201 
202  return true;
203 }
204 
207 template <typename BV>
208 bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
209  const OcTree& model1, const Transform3f& tf1,
210  const BVHModel<BV>& model2, const Transform3f& tf2,
211  const OcTreeSolver* otsolver, CollisionResult& result) {
212  node.result = &result;
213 
214  node.model1 = &model1;
215  node.model2 = &model2;
216 
217  node.otsolver = otsolver;
218 
219  node.tf1 = tf1;
220  node.tf2 = tf2;
221 
222  return true;
223 }
224 
227 template <typename BV>
228 bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
229  const BVHModel<BV>& model1, const Transform3f& tf1,
230  const OcTree& model2, const Transform3f& tf2,
231  const OcTreeSolver* otsolver, const DistanceRequest& request,
232  DistanceResult& result) {
233  node.request = request;
234  node.result = &result;
235 
236  node.model1 = &model1;
237  node.model2 = &model2;
238 
239  node.otsolver = otsolver;
240 
241  node.tf1 = tf1;
242  node.tf2 = tf2;
243 
244  return true;
245 }
246 
249 template <typename BV>
250 bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node, const OcTree& model1,
251  const Transform3f& tf1, const BVHModel<BV>& model2,
252  const Transform3f& tf2, const OcTreeSolver* otsolver,
253  const DistanceRequest& request, DistanceResult& result) {
254  node.request = request;
255  node.result = &result;
256 
257  node.model1 = &model1;
258  node.model2 = &model2;
259 
260  node.otsolver = otsolver;
261 
262  node.tf1 = tf1;
263  node.tf2 = tf2;
264 
265  return true;
266 }
267 
268 #endif
269 
272 template <typename S1, typename S2>
273 bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1,
274  const Transform3f& tf1, const S2& shape2,
275  const Transform3f& tf2, const GJKSolver* nsolver,
276  CollisionResult& result) {
277  node.model1 = &shape1;
278  node.tf1 = tf1;
279  node.model2 = &shape2;
280  node.tf2 = tf2;
281  node.nsolver = nsolver;
282 
283  node.result = &result;
284 
285  return true;
286 }
287 
290 template <typename BV, typename S>
291 bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
292  BVHModel<BV>& model1, Transform3f& tf1, const S& model2,
293  const Transform3f& tf2, const GJKSolver* nsolver,
294  CollisionResult& result, bool use_refit = false,
295  bool refit_bottomup = false) {
296  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
297  HPP_FCL_THROW_PRETTY(
298  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
299  std::invalid_argument)
300 
301  if (!tf1.isIdentity()) // TODO(jcarpent): vectorized version
302  {
303  std::vector<Vec3f> vertices_transformed(model1.num_vertices);
304  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
305  const Vec3f& p = model1.vertices[i];
306  Vec3f new_v = tf1.transform(p);
307  vertices_transformed[i] = new_v;
308  }
309 
310  model1.beginReplaceModel();
311  model1.replaceSubModel(vertices_transformed);
312  model1.endReplaceModel(use_refit, refit_bottomup);
313 
314  tf1.setIdentity();
315  }
316 
317  node.model1 = &model1;
318  node.tf1 = tf1;
319  node.model2 = &model2;
320  node.tf2 = tf2;
321  node.nsolver = nsolver;
322 
323  computeBV(model2, tf2, node.model2_bv);
324 
325  node.vertices = model1.vertices;
326  node.tri_indices = model1.tri_indices;
327 
328  node.result = &result;
329 
330  return true;
331 }
332 
335 template <typename BV, typename S>
336 bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
337  const BVHModel<BV>& model1, const Transform3f& tf1,
338  const S& model2, const Transform3f& tf2,
339  const GJKSolver* nsolver, CollisionResult& result) {
340  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
341  HPP_FCL_THROW_PRETTY(
342  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
343  std::invalid_argument)
344 
345  node.model1 = &model1;
346  node.tf1 = tf1;
347  node.model2 = &model2;
348  node.tf2 = tf2;
349  node.nsolver = nsolver;
350 
351  computeBV(model2, tf2, node.model2_bv);
352 
353  node.vertices = model1.vertices;
354  node.tri_indices = model1.tri_indices;
355 
356  node.result = &result;
357 
358  return true;
359 }
360 
363 template <typename BV, typename S>
364 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
365  HeightField<BV>& model1, Transform3f& tf1, const S& model2,
366  const Transform3f& tf2, const GJKSolver* nsolver,
367  CollisionResult& result, bool use_refit = false,
368  bool refit_bottomup = false);
369 
372 template <typename BV, typename S>
373 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
374  const HeightField<BV>& model1, const Transform3f& tf1,
375  const S& model2, const Transform3f& tf2,
376  const GJKSolver* nsolver, CollisionResult& result) {
377  node.model1 = &model1;
378  node.tf1 = tf1;
379  node.model2 = &model2;
380  node.tf2 = tf2;
381  node.nsolver = nsolver;
382 
383  computeBV(model2, tf2, node.model2_bv);
384 
385  node.result = &result;
386 
387  return true;
388 }
389 
391 namespace details {
392 template <typename S, typename BV, template <typename> class OrientedNode>
393 static inline bool setupShapeMeshCollisionOrientedNode(
394  OrientedNode<S>& node, const S& model1, const Transform3f& tf1,
395  const BVHModel<BV>& model2, const Transform3f& tf2,
396  const GJKSolver* nsolver, CollisionResult& result) {
397  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
398  HPP_FCL_THROW_PRETTY(
399  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
400  std::invalid_argument)
401 
402  node.model1 = &model1;
403  node.tf1 = tf1;
404  node.model2 = &model2;
405  node.tf2 = tf2;
406  node.nsolver = nsolver;
407 
408  computeBV(model1, tf1, node.model1_bv);
409 
410  node.vertices = model2.vertices;
411  node.tri_indices = model2.tri_indices;
412 
413  node.result = &result;
414 
415  return true;
416 }
417 } // namespace details
419 
422 template <typename BV>
423 bool initialize(
424  MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
425  BVHModel<BV>& model1, Transform3f& tf1, BVHModel<BV>& model2,
426  Transform3f& tf2, CollisionResult& result, bool use_refit = false,
427  bool refit_bottomup = false) {
428  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
429  HPP_FCL_THROW_PRETTY(
430  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
431  std::invalid_argument)
432  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
433  HPP_FCL_THROW_PRETTY(
434  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
435  std::invalid_argument)
436 
437  if (!tf1.isIdentity()) {
438  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
439  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
440  Vec3f& p = model1.vertices[i];
441  Vec3f new_v = tf1.transform(p);
442  vertices_transformed1[i] = new_v;
443  }
444 
445  model1.beginReplaceModel();
446  model1.replaceSubModel(vertices_transformed1);
447  model1.endReplaceModel(use_refit, refit_bottomup);
448 
449  tf1.setIdentity();
450  }
451 
452  if (!tf2.isIdentity()) {
453  std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
454  for (unsigned int i = 0; i < model2.num_vertices; ++i) {
455  Vec3f& p = model2.vertices[i];
456  Vec3f new_v = tf2.transform(p);
457  vertices_transformed2[i] = new_v;
458  }
459 
460  model2.beginReplaceModel();
461  model2.replaceSubModel(vertices_transformed2);
462  model2.endReplaceModel(use_refit, refit_bottomup);
463 
464  tf2.setIdentity();
465  }
466 
467  node.model1 = &model1;
468  node.tf1 = tf1;
469  node.model2 = &model2;
470  node.tf2 = tf2;
471 
472  node.vertices1 = model1.vertices;
473  node.vertices2 = model2.vertices;
474 
475  node.tri_indices1 = model1.tri_indices;
476  node.tri_indices2 = model2.tri_indices;
477 
478  node.result = &result;
479 
480  return true;
481 }
482 
483 template <typename BV>
484 bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
485  const BVHModel<BV>& model1, const Transform3f& tf1,
486  const BVHModel<BV>& model2, const Transform3f& tf2,
487  CollisionResult& result) {
488  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
489  HPP_FCL_THROW_PRETTY(
490  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
491  std::invalid_argument)
492  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
493  HPP_FCL_THROW_PRETTY(
494  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
495  std::invalid_argument)
496 
497  node.vertices1 = model1.vertices;
498  node.vertices2 = model2.vertices;
499 
500  node.tri_indices1 = model1.tri_indices;
501  node.tri_indices2 = model2.tri_indices;
502 
503  node.model1 = &model1;
504  node.tf1 = tf1;
505  node.model2 = &model2;
506  node.tf2 = tf2;
507 
508  node.result = &result;
509 
510  node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
511  node.RT.T.noalias() = tf1.getRotation().transpose() *
512  (tf2.getTranslation() - tf1.getTranslation());
513 
514  return true;
515 }
516 
518 template <typename S1, typename S2>
519 bool initialize(ShapeDistanceTraversalNode<S1, S2>& node, const S1& shape1,
520  const Transform3f& tf1, const S2& shape2,
521  const Transform3f& tf2, const GJKSolver* nsolver,
522  const DistanceRequest& request, DistanceResult& result) {
523  node.request = request;
524  node.result = &result;
525 
526  node.model1 = &shape1;
527  node.tf1 = tf1;
528  node.model2 = &shape2;
529  node.tf2 = tf2;
530  node.nsolver = nsolver;
531 
532  return true;
533 }
534 
537 template <typename BV>
538 bool initialize(
539  MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
540  BVHModel<BV>& model1, Transform3f& tf1, BVHModel<BV>& model2,
541  Transform3f& tf2, const DistanceRequest& request, DistanceResult& result,
542  bool use_refit = false, bool refit_bottomup = false) {
543  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
544  HPP_FCL_THROW_PRETTY(
545  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
546  std::invalid_argument)
547  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
548  HPP_FCL_THROW_PRETTY(
549  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
550  std::invalid_argument)
551 
552  if (!tf1.isIdentity()) {
553  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
554  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
555  const Vec3f& p = model1.vertices[i];
556  Vec3f new_v = tf1.transform(p);
557  vertices_transformed1[i] = new_v;
558  }
559 
560  model1.beginReplaceModel();
561  model1.replaceSubModel(vertices_transformed1);
562  model1.endReplaceModel(use_refit, refit_bottomup);
563 
564  tf1.setIdentity();
565  }
566 
567  if (!tf2.isIdentity()) {
568  std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
569  for (unsigned int i = 0; i < model2.num_vertices; ++i) {
570  const Vec3f& p = model2.vertices[i];
571  Vec3f new_v = tf2.transform(p);
572  vertices_transformed2[i] = new_v;
573  }
574 
575  model2.beginReplaceModel();
576  model2.replaceSubModel(vertices_transformed2);
577  model2.endReplaceModel(use_refit, refit_bottomup);
578 
579  tf2.setIdentity();
580  }
581 
582  node.request = request;
583  node.result = &result;
584 
585  node.model1 = &model1;
586  node.tf1 = tf1;
587  node.model2 = &model2;
588  node.tf2 = tf2;
589 
590  node.vertices1 = model1.vertices;
591  node.vertices2 = model2.vertices;
592 
593  node.tri_indices1 = model1.tri_indices;
594  node.tri_indices2 = model2.tri_indices;
595 
596  return true;
597 }
598 
600 template <typename BV>
601 bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
602  const BVHModel<BV>& model1, const Transform3f& tf1,
603  const BVHModel<BV>& model2, const Transform3f& tf2,
604  const DistanceRequest& request, DistanceResult& result) {
605  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
606  HPP_FCL_THROW_PRETTY(
607  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
608  std::invalid_argument)
609  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
610  HPP_FCL_THROW_PRETTY(
611  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
612  std::invalid_argument)
613 
614  node.request = request;
615  node.result = &result;
616 
617  node.model1 = &model1;
618  node.tf1 = tf1;
619  node.model2 = &model2;
620  node.tf2 = tf2;
621 
622  node.vertices1 = model1.vertices;
623  node.vertices2 = model2.vertices;
624 
625  node.tri_indices1 = model1.tri_indices;
626  node.tri_indices2 = model2.tri_indices;
627 
628  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
629  tf2.getTranslation(), node.RT.R, node.RT.T);
630 
631  return true;
632 }
633 
636 template <typename BV, typename S>
637 bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
638  BVHModel<BV>& model1, Transform3f& tf1, const S& model2,
639  const Transform3f& tf2, const GJKSolver* nsolver,
640  const DistanceRequest& request, DistanceResult& result,
641  bool use_refit = false, bool refit_bottomup = false) {
642  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
643  HPP_FCL_THROW_PRETTY(
644  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
645  std::invalid_argument)
646 
647  if (!tf1.isIdentity()) {
648  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
649  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
650  const Vec3f& p = model1.vertices[i];
651  Vec3f new_v = tf1.transform(p);
652  vertices_transformed1[i] = new_v;
653  }
654 
655  model1.beginReplaceModel();
656  model1.replaceSubModel(vertices_transformed1);
657  model1.endReplaceModel(use_refit, refit_bottomup);
658 
659  tf1.setIdentity();
660  }
661 
662  node.request = request;
663  node.result = &result;
664 
665  node.model1 = &model1;
666  node.tf1 = tf1;
667  node.model2 = &model2;
668  node.tf2 = tf2;
669  node.nsolver = nsolver;
670 
671  node.vertices = model1.vertices;
672  node.tri_indices = model1.tri_indices;
673 
674  computeBV(model2, tf2, node.model2_bv);
675 
676  return true;
677 }
678 
681 template <typename S, typename BV>
682 bool initialize(ShapeMeshDistanceTraversalNode<S, BV>& node, const S& model1,
683  const Transform3f& tf1, BVHModel<BV>& model2, Transform3f& tf2,
684  const GJKSolver* nsolver, const DistanceRequest& request,
685  DistanceResult& result, bool use_refit = false,
686  bool refit_bottomup = false) {
687  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
688  HPP_FCL_THROW_PRETTY(
689  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
690  std::invalid_argument)
691 
692  if (!tf2.isIdentity()) {
693  std::vector<Vec3f> vertices_transformed(model2.num_vertices);
694  for (unsigned int i = 0; i < model2.num_vertices; ++i) {
695  const Vec3f& p = model2.vertices[i];
696  Vec3f new_v = tf2.transform(p);
697  vertices_transformed[i] = new_v;
698  }
699 
700  model2.beginReplaceModel();
701  model2.replaceSubModel(vertices_transformed);
702  model2.endReplaceModel(use_refit, refit_bottomup);
703 
704  tf2.setIdentity();
705  }
706 
707  node.request = request;
708  node.result = &result;
709 
710  node.model1 = &model1;
711  node.tf1 = tf1;
712  node.model2 = &model2;
713  node.tf2 = tf2;
714  node.nsolver = nsolver;
715 
716  node.vertices = model2.vertices;
717  node.tri_indices = model2.tri_indices;
718 
719  computeBV(model1, tf1, node.model1_bv);
720 
721  return true;
722 }
723 
725 namespace details {
726 
727 template <typename BV, typename S, template <typename> class OrientedNode>
728 static inline bool setupMeshShapeDistanceOrientedNode(
729  OrientedNode<S>& node, const BVHModel<BV>& model1, const Transform3f& tf1,
730  const S& model2, const Transform3f& tf2, const GJKSolver* nsolver,
731  const DistanceRequest& request, DistanceResult& result) {
732  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
733  HPP_FCL_THROW_PRETTY(
734  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
735  std::invalid_argument)
736 
737  node.request = request;
738  node.result = &result;
739 
740  node.model1 = &model1;
741  node.tf1 = tf1;
742  node.model2 = &model2;
743  node.tf2 = tf2;
744  node.nsolver = nsolver;
745 
746  computeBV(model2, tf2, node.model2_bv);
747 
748  node.vertices = model1.vertices;
749  node.tri_indices = model1.tri_indices;
750 
751  return true;
752 }
753 } // namespace details
755 
758 template <typename S>
759 bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& node,
760  const BVHModel<RSS>& model1, const Transform3f& tf1,
761  const S& model2, const Transform3f& tf2,
762  const GJKSolver* nsolver, const DistanceRequest& request,
763  DistanceResult& result) {
764  return details::setupMeshShapeDistanceOrientedNode(
765  node, model1, tf1, model2, tf2, nsolver, request, result);
766 }
767 
770 template <typename S>
771 bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& node,
772  const BVHModel<kIOS>& model1, const Transform3f& tf1,
773  const S& model2, const Transform3f& tf2,
774  const GJKSolver* nsolver, const DistanceRequest& request,
775  DistanceResult& result) {
776  return details::setupMeshShapeDistanceOrientedNode(
777  node, model1, tf1, model2, tf2, nsolver, request, result);
778 }
779 
782 template <typename S>
783 bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& node,
784  const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
785  const S& model2, const Transform3f& tf2,
786  const GJKSolver* nsolver, const DistanceRequest& request,
787  DistanceResult& result) {
788  return details::setupMeshShapeDistanceOrientedNode(
789  node, model1, tf1, model2, tf2, nsolver, request, result);
790 }
791 
792 namespace details {
793 template <typename S, typename BV, template <typename> class OrientedNode>
794 static inline bool setupShapeMeshDistanceOrientedNode(
795  OrientedNode<S>& node, const S& model1, const Transform3f& tf1,
796  const BVHModel<BV>& model2, const Transform3f& tf2,
797  const GJKSolver* nsolver, const DistanceRequest& request,
798  DistanceResult& result) {
799  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
800  HPP_FCL_THROW_PRETTY(
801  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
802  std::invalid_argument)
803 
804  node.request = request;
805  node.result = &result;
806 
807  node.model1 = &model1;
808  node.tf1 = tf1;
809  node.model2 = &model2;
810  node.tf2 = tf2;
811  node.nsolver = nsolver;
812 
813  computeBV(model1, tf1, node.model1_bv);
814 
815  node.vertices = model2.vertices;
816  node.tri_indices = model2.tri_indices;
817  node.R = tf2.getRotation();
818  node.T = tf2.getTranslation();
819 
820  return true;
821 }
822 } // namespace details
823 
826 template <typename S>
827 bool initialize(ShapeMeshDistanceTraversalNodeRSS<S>& node, const S& model1,
828  const Transform3f& tf1, const BVHModel<RSS>& model2,
829  const Transform3f& tf2, const GJKSolver* nsolver,
830  const DistanceRequest& request, DistanceResult& result) {
831  return details::setupShapeMeshDistanceOrientedNode(
832  node, model1, tf1, model2, tf2, nsolver, request, result);
833 }
834 
837 template <typename S>
838 bool initialize(ShapeMeshDistanceTraversalNodekIOS<S>& node, const S& model1,
839  const Transform3f& tf1, const BVHModel<kIOS>& model2,
840  const Transform3f& tf2, const GJKSolver* nsolver,
841  const DistanceRequest& request, DistanceResult& result) {
842  return details::setupShapeMeshDistanceOrientedNode(
843  node, model1, tf1, model2, tf2, nsolver, request, result);
844 }
845 
848 template <typename S>
849 bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S>& node, const S& model1,
850  const Transform3f& tf1, const BVHModel<OBBRSS>& model2,
851  const Transform3f& tf2, const GJKSolver* nsolver,
852  const DistanceRequest& request, DistanceResult& result) {
853  return details::setupShapeMeshDistanceOrientedNode(
854  node, model1, tf1, model2, tf2, nsolver, request, result);
855 }
856 
857 } // namespace fcl
858 
859 } // namespace hpp
860 
862 
863 #endif
hpp::fcl::computeBV
void computeBV(const S &s, const Transform3f &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: geometric_shapes_utility.h:74
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:37
initialize
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
Definition: traversal_node_setup.h:423
BVH_utility.h
hpp::fcl::BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:82
details::setupShapeMeshDistanceOrientedNode
static bool setupShapeMeshDistanceOrientedNode(OrientedNode< S > &node, const S &model1, const Transform3f &tf1, const BVHModel< BV > &model2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Definition: traversal_node_setup.h:794
traversal_node_bvh_shape.h
traversal_node_octree.h
traversal_node_bvhs.h
traversal_node_hfield_shape.h
ShapeMeshDistanceTraversalNode
Traversal node for distance between shape and mesh.
Definition: traversal_node_bvh_shape.h:631
ShapeMeshDistanceTraversalNodeRSS
Traversal node for distance between shape and mesh, when mesh BVH is one of the oriented node (RSS...
Definition: traversal_node_bvh_shape.h:689
hpp::fcl::relativeTransform
void relativeTransform(const Eigen::MatrixBase< Derived > &R1, const Eigen::MatrixBase< OtherDerived > &t1, const Eigen::MatrixBase< Derived > &R2, const Eigen::MatrixBase< OtherDerived > &t2, const Eigen::MatrixBase< Derived > &R, const Eigen::MatrixBase< OtherDerived > &t)
Definition: tools.h:92
traversal_node_shapes.h
MeshShapeDistanceTraversalNodeRSS
Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS...
Definition: traversal_node_bvh_shape.h:540
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:28
HPP_FCL_THROW_PRETTY
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: include/hpp/fcl/fwd.hh:57

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:02