hpp-fcl  3.0.0
HPP fork of FCL -- The Flexible Collision Library
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
64 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(OcTreeHeightFieldCollisionTraversalNode<BV>& node,
229  const OcTree& model1, const Transform3f& tf1,
230  const HeightField<BV>& model2, const Transform3f& tf2,
231  const OcTreeSolver* otsolver, CollisionResult& result) {
232  node.result = &result;
233 
234  node.model1 = &model1;
235  node.model2 = &model2;
236 
237  node.otsolver = otsolver;
238 
239  node.tf1 = tf1;
240  node.tf2 = tf2;
241 
242  return true;
243 }
244 
247 template <typename BV>
248 bool initialize(HeightFieldOcTreeCollisionTraversalNode<BV>& node,
249  const HeightField<BV>& model1, const Transform3f& tf1,
250  const OcTree& model2, const Transform3f& tf2,
251  const OcTreeSolver* otsolver, CollisionResult& result) {
252  node.result = &result;
253 
254  node.model1 = &model1;
255  node.model2 = &model2;
256 
257  node.otsolver = otsolver;
258 
259  node.tf1 = tf1;
260  node.tf2 = tf2;
261 
262  return true;
263 }
264 
267 template <typename BV>
268 bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
269  const BVHModel<BV>& model1, const Transform3f& tf1,
270  const OcTree& model2, const Transform3f& tf2,
271  const OcTreeSolver* otsolver, const DistanceRequest& request,
272  DistanceResult& result) {
273  node.request = request;
274  node.result = &result;
275 
276  node.model1 = &model1;
277  node.model2 = &model2;
278 
279  node.otsolver = otsolver;
280 
281  node.tf1 = tf1;
282  node.tf2 = tf2;
283 
284  return true;
285 }
286 
289 template <typename BV>
290 bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node, const OcTree& model1,
291  const Transform3f& tf1, const BVHModel<BV>& model2,
292  const Transform3f& tf2, const OcTreeSolver* otsolver,
293  const DistanceRequest& request, DistanceResult& result) {
294  node.request = request;
295  node.result = &result;
296 
297  node.model1 = &model1;
298  node.model2 = &model2;
299 
300  node.otsolver = otsolver;
301 
302  node.tf1 = tf1;
303  node.tf2 = tf2;
304 
305  return true;
306 }
307 
308 #endif
309 
312 template <typename S1, typename S2>
313 bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1,
314  const Transform3f& tf1, const S2& shape2,
315  const Transform3f& tf2, const GJKSolver* nsolver,
316  CollisionResult& result) {
317  node.model1 = &shape1;
318  node.tf1 = tf1;
319  node.model2 = &shape2;
320  node.tf2 = tf2;
321  node.nsolver = nsolver;
322 
323  node.result = &result;
324 
325  return true;
326 }
327 
330 template <typename BV, typename S>
331 bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
332  BVHModel<BV>& model1, Transform3f& tf1, const S& model2,
333  const Transform3f& tf2, const GJKSolver* nsolver,
334  CollisionResult& result, bool use_refit = false,
335  bool refit_bottomup = false) {
336  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
337  HPP_FCL_THROW_PRETTY(
338  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
339  std::invalid_argument)
340 
341  if (!tf1.isIdentity() &&
342  model1.vertices.get()) // TODO(jcarpent): vectorized version
343  {
344  std::vector<Vec3f> vertices_transformed(model1.num_vertices);
345  const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
346  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
347  const Vec3f& p = model1_vertices_[i];
348  Vec3f new_v = tf1.transform(p);
349  vertices_transformed[i] = new_v;
350  }
351 
352  model1.beginReplaceModel();
353  model1.replaceSubModel(vertices_transformed);
354  model1.endReplaceModel(use_refit, refit_bottomup);
355 
356  tf1.setIdentity();
357  }
358 
359  node.model1 = &model1;
360  node.tf1 = tf1;
361  node.model2 = &model2;
362  node.tf2 = tf2;
363  node.nsolver = nsolver;
364 
365  computeBV(model2, tf2, node.model2_bv);
366 
367  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
368  node.tri_indices =
369  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
370 
371  node.result = &result;
372 
373  return true;
374 }
375 
378 template <typename BV, typename S>
379 bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
380  const BVHModel<BV>& model1, const Transform3f& tf1,
381  const S& model2, const Transform3f& tf2,
382  const GJKSolver* nsolver, CollisionResult& result) {
383  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
384  HPP_FCL_THROW_PRETTY(
385  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
386  std::invalid_argument)
387 
388  node.model1 = &model1;
389  node.tf1 = tf1;
390  node.model2 = &model2;
391  node.tf2 = tf2;
392  node.nsolver = nsolver;
393 
394  computeBV(model2, tf2, node.model2_bv);
395 
396  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
397  node.tri_indices =
398  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
399 
400  node.result = &result;
401 
402  return true;
403 }
404 
407 template <typename BV, typename S>
408 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
409  HeightField<BV>& model1, Transform3f& tf1, const S& model2,
410  const Transform3f& tf2, const GJKSolver* nsolver,
411  CollisionResult& result, bool use_refit = false,
412  bool refit_bottomup = false);
413 
416 template <typename BV, typename S>
417 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
418  const HeightField<BV>& model1, const Transform3f& tf1,
419  const S& model2, const Transform3f& tf2,
420  const GJKSolver* nsolver, CollisionResult& result) {
421  node.model1 = &model1;
422  node.tf1 = tf1;
423  node.model2 = &model2;
424  node.tf2 = tf2;
425  node.nsolver = nsolver;
426 
427  computeBV(model2, tf2, node.model2_bv);
428 
429  node.result = &result;
430 
431  return true;
432 }
433 
435 namespace details {
436 template <typename S, typename BV, template <typename> class OrientedNode>
437 static inline bool setupShapeMeshCollisionOrientedNode(
438  OrientedNode<S>& node, const S& model1, const Transform3f& tf1,
439  const BVHModel<BV>& model2, const Transform3f& tf2,
440  const GJKSolver* nsolver, CollisionResult& result) {
441  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
442  HPP_FCL_THROW_PRETTY(
443  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
444  std::invalid_argument)
445 
446  node.model1 = &model1;
447  node.tf1 = tf1;
448  node.model2 = &model2;
449  node.tf2 = tf2;
450  node.nsolver = nsolver;
451 
452  computeBV(model1, tf1, node.model1_bv);
453 
454  node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
455  node.tri_indices =
456  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
457 
458  node.result = &result;
459 
460  return true;
461 }
462 } // namespace details
464 
467 template <typename BV>
468 bool initialize(
469  MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
470  BVHModel<BV>& model1, Transform3f& tf1, BVHModel<BV>& model2,
471  Transform3f& tf2, CollisionResult& result, bool use_refit = false,
472  bool refit_bottomup = false) {
473  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
474  HPP_FCL_THROW_PRETTY(
475  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
476  std::invalid_argument)
477  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
478  HPP_FCL_THROW_PRETTY(
479  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
480  std::invalid_argument)
481 
482  if (!tf1.isIdentity() && model1.vertices.get()) {
483  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
484  const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
485  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
486  const Vec3f& p = model1_vertices_[i];
487  Vec3f new_v = tf1.transform(p);
488  vertices_transformed1[i] = new_v;
489  }
490 
491  model1.beginReplaceModel();
492  model1.replaceSubModel(vertices_transformed1);
493  model1.endReplaceModel(use_refit, refit_bottomup);
494 
495  tf1.setIdentity();
496  }
497 
498  if (!tf2.isIdentity() && model2.vertices.get()) {
499  std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
500  const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
501  for (unsigned int i = 0; i < model2.num_vertices; ++i) {
502  const Vec3f& p = model2_vertices_[i];
503  Vec3f new_v = tf2.transform(p);
504  vertices_transformed2[i] = new_v;
505  }
506 
507  model2.beginReplaceModel();
508  model2.replaceSubModel(vertices_transformed2);
509  model2.endReplaceModel(use_refit, refit_bottomup);
510 
511  tf2.setIdentity();
512  }
513 
514  node.model1 = &model1;
515  node.tf1 = tf1;
516  node.model2 = &model2;
517  node.tf2 = tf2;
518 
519  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
520  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
521 
522  node.tri_indices1 =
523  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
524  node.tri_indices2 =
525  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
526 
527  node.result = &result;
528 
529  return true;
530 }
531 
532 template <typename BV>
533 bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
534  const BVHModel<BV>& model1, const Transform3f& tf1,
535  const BVHModel<BV>& model2, const Transform3f& tf2,
536  CollisionResult& result) {
537  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
538  HPP_FCL_THROW_PRETTY(
539  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
540  std::invalid_argument)
541  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
542  HPP_FCL_THROW_PRETTY(
543  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
544  std::invalid_argument)
545 
546  node.vertices1 = model1.vertices ? model1.vertices->data() : NULL;
547  node.vertices2 = model2.vertices ? model2.vertices->data() : NULL;
548 
549  node.tri_indices1 =
550  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
551  node.tri_indices2 =
552  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
553 
554  node.model1 = &model1;
555  node.tf1 = tf1;
556  node.model2 = &model2;
557  node.tf2 = tf2;
558 
559  node.result = &result;
560 
561  node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
562  node.RT.T.noalias() = tf1.getRotation().transpose() *
563  (tf2.getTranslation() - tf1.getTranslation());
564 
565  return true;
566 }
567 
569 template <typename S1, typename S2>
570 bool initialize(ShapeDistanceTraversalNode<S1, S2>& node, const S1& shape1,
571  const Transform3f& tf1, const S2& shape2,
572  const Transform3f& tf2, const GJKSolver* nsolver,
573  const DistanceRequest& request, DistanceResult& result) {
574  node.request = request;
575  node.result = &result;
576 
577  node.model1 = &shape1;
578  node.tf1 = tf1;
579  node.model2 = &shape2;
580  node.tf2 = tf2;
581  node.nsolver = nsolver;
582 
583  return true;
584 }
585 
588 template <typename BV>
589 bool initialize(
590  MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
591  BVHModel<BV>& model1, Transform3f& tf1, BVHModel<BV>& model2,
592  Transform3f& tf2, const DistanceRequest& request, DistanceResult& result,
593  bool use_refit = false, bool refit_bottomup = false) {
594  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
595  HPP_FCL_THROW_PRETTY(
596  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
597  std::invalid_argument)
598  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
599  HPP_FCL_THROW_PRETTY(
600  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
601  std::invalid_argument)
602 
603  if (!tf1.isIdentity() && model1.vertices.get()) {
604  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
605  const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
606  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
607  const Vec3f& p = model1_vertices_[i];
608  Vec3f new_v = tf1.transform(p);
609  vertices_transformed1[i] = new_v;
610  }
611 
612  model1.beginReplaceModel();
613  model1.replaceSubModel(vertices_transformed1);
614  model1.endReplaceModel(use_refit, refit_bottomup);
615 
616  tf1.setIdentity();
617  }
618 
619  if (!tf2.isIdentity() && model2.vertices.get()) {
620  std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
621  const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
622  for (unsigned int i = 0; i < model2.num_vertices; ++i) {
623  const Vec3f& p = model2_vertices_[i];
624  Vec3f new_v = tf2.transform(p);
625  vertices_transformed2[i] = new_v;
626  }
627 
628  model2.beginReplaceModel();
629  model2.replaceSubModel(vertices_transformed2);
630  model2.endReplaceModel(use_refit, refit_bottomup);
631 
632  tf2.setIdentity();
633  }
634 
635  node.request = request;
636  node.result = &result;
637 
638  node.model1 = &model1;
639  node.tf1 = tf1;
640  node.model2 = &model2;
641  node.tf2 = tf2;
642 
643  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
644  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
645 
646  node.tri_indices1 =
647  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
648  node.tri_indices2 =
649  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
650 
651  return true;
652 }
653 
655 template <typename BV>
656 bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
657  const BVHModel<BV>& model1, const Transform3f& tf1,
658  const BVHModel<BV>& model2, const Transform3f& tf2,
659  const DistanceRequest& request, DistanceResult& result) {
660  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
661  HPP_FCL_THROW_PRETTY(
662  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
663  std::invalid_argument)
664  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
665  HPP_FCL_THROW_PRETTY(
666  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
667  std::invalid_argument)
668 
669  node.request = request;
670  node.result = &result;
671 
672  node.model1 = &model1;
673  node.tf1 = tf1;
674  node.model2 = &model2;
675  node.tf2 = tf2;
676 
677  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
678  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
679 
680  node.tri_indices1 =
681  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
682  node.tri_indices2 =
683  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
684 
685  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
686  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
687 
688  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
689  tf2.getTranslation(), node.RT.R, node.RT.T);
690 
691  HPP_FCL_COMPILER_DIAGNOSTIC_POP
692 
693  return true;
694 }
695 
698 template <typename BV, typename S>
699 bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
700  BVHModel<BV>& model1, Transform3f& tf1, const S& model2,
701  const Transform3f& tf2, const GJKSolver* nsolver,
702  const DistanceRequest& request, DistanceResult& result,
703  bool use_refit = false, bool refit_bottomup = false) {
704  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
705  HPP_FCL_THROW_PRETTY(
706  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
707  std::invalid_argument)
708 
709  if (!tf1.isIdentity() && model1.vertices.get()) {
710  const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
711  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
712  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
713  const Vec3f& p = model1_vertices_[i];
714  Vec3f new_v = tf1.transform(p);
715  vertices_transformed1[i] = new_v;
716  }
717 
718  model1.beginReplaceModel();
719  model1.replaceSubModel(vertices_transformed1);
720  model1.endReplaceModel(use_refit, refit_bottomup);
721 
722  tf1.setIdentity();
723  }
724 
725  node.request = request;
726  node.result = &result;
727 
728  node.model1 = &model1;
729  node.tf1 = tf1;
730  node.model2 = &model2;
731  node.tf2 = tf2;
732  node.nsolver = nsolver;
733 
734  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
735  node.tri_indices =
736  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
737 
738  computeBV(model2, tf2, node.model2_bv);
739 
740  return true;
741 }
742 
744 namespace details {
745 
746 template <typename BV, typename S, template <typename> class OrientedNode>
747 static inline bool setupMeshShapeDistanceOrientedNode(
748  OrientedNode<S>& node, const BVHModel<BV>& model1, const Transform3f& tf1,
749  const S& model2, const Transform3f& tf2, const GJKSolver* nsolver,
750  const DistanceRequest& request, DistanceResult& result) {
751  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
752  HPP_FCL_THROW_PRETTY(
753  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
754  std::invalid_argument)
755 
756  node.request = request;
757  node.result = &result;
758 
759  node.model1 = &model1;
760  node.tf1 = tf1;
761  node.model2 = &model2;
762  node.tf2 = tf2;
763  node.nsolver = nsolver;
764 
765  computeBV(model2, tf2, node.model2_bv);
766 
767  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
768  node.tri_indices =
769  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
770 
771  return true;
772 }
773 } // namespace details
775 
778 template <typename S>
779 bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& node,
780  const BVHModel<RSS>& model1, const Transform3f& tf1,
781  const S& model2, const Transform3f& tf2,
782  const GJKSolver* nsolver, const DistanceRequest& request,
783  DistanceResult& result) {
784  return details::setupMeshShapeDistanceOrientedNode(
785  node, model1, tf1, model2, tf2, nsolver, request, result);
786 }
787 
790 template <typename S>
791 bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& node,
792  const BVHModel<kIOS>& model1, const Transform3f& tf1,
793  const S& model2, const Transform3f& tf2,
794  const GJKSolver* nsolver, const DistanceRequest& request,
795  DistanceResult& result) {
796  return details::setupMeshShapeDistanceOrientedNode(
797  node, model1, tf1, model2, tf2, nsolver, request, result);
798 }
799 
802 template <typename S>
803 bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& node,
804  const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
805  const S& model2, const Transform3f& tf2,
806  const GJKSolver* nsolver, const DistanceRequest& request,
807  DistanceResult& result) {
808  return details::setupMeshShapeDistanceOrientedNode(
809  node, model1, tf1, model2, tf2, nsolver, request, result);
810 }
811 
812 } // namespace fcl
813 } // namespace hpp
814 
816 
817 #endif
BVH_utility.h
MeshShapeDistanceTraversalNodeOBBRSS
Definition: traversal_node_bvh_shape.h:452
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:392
MeshShapeDistanceTraversalNodekIOS
Definition: traversal_node_bvh_shape.h:422
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
Definition: fwd.hh:120
HPP_FCL_COMPILER_DIAGNOSTIC_POP
#define HPP_FCL_COMPILER_DIAGNOSTIC_POP
Definition: fwd.hh:121
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
Definition: fwd.hh:123
HPP_FCL_THROW_PRETTY
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:64
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::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:82
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:91
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
traversal_node_bvh_shape.h
traversal_node_bvhs.h
traversal_node_hfield_shape.h
traversal_node_octree.h
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:468
traversal_node_shapes.h