hpp-fcl  1.4.4
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>
47 
48 #ifdef HPP_FCL_HAVE_OCTOMAP
50 #endif
51 
53 
54 namespace hpp
55 {
56 namespace fcl
57 {
58 
59 #ifdef HPP_FCL_HAVE_OCTOMAP
60 inline bool initialize(OcTreeCollisionTraversalNode& node,
62  const OcTree& model1, const Transform3f& tf1,
63  const OcTree& model2, const Transform3f& tf2,
64  const OcTreeSolver* otsolver,
65  CollisionResult& result)
66 {
67  node.result = &result;
68 
69  node.model1 = &model1;
70  node.model2 = &model2;
71 
72  node.otsolver = otsolver;
73 
74  node.tf1 = tf1;
75  node.tf2 = tf2;
76 
77  return true;
78 }
79 
81 inline bool initialize(OcTreeDistanceTraversalNode& node,
82  const OcTree& model1, const Transform3f& tf1,
83  const OcTree& model2, const Transform3f& tf2,
84  const OcTreeSolver* otsolver,
85  const DistanceRequest& request,
86  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 
104 template<typename S>
105 bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node,
106  const S& model1, const Transform3f& tf1,
107  const OcTree& model2, const Transform3f& tf2,
108  const OcTreeSolver* otsolver,
109  CollisionResult& result)
110 {
111  node.result = &result;
112 
113  node.model1 = &model1;
114  node.model2 = &model2;
115 
116  node.otsolver = otsolver;
117 
118  node.tf1 = tf1;
119  node.tf2 = tf2;
120 
121  return true;
122 }
123 
125 template<typename S>
126 bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
127  const OcTree& model1, const Transform3f& tf1,
128  const S& model2, const Transform3f& tf2,
129  const OcTreeSolver* otsolver,
130  CollisionResult& result)
131 {
132  node.result = &result;
133 
134  node.model1 = &model1;
135  node.model2 = &model2;
136 
137  node.otsolver = otsolver;
138 
139  node.tf1 = tf1;
140  node.tf2 = tf2;
141 
142  return true;
143 }
144 
146 template<typename S>
147 bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node,
148  const S& model1, const Transform3f& tf1,
149  const OcTree& model2, const Transform3f& tf2,
150  const OcTreeSolver* otsolver,
151  const DistanceRequest& request,
152  DistanceResult& result)
153 {
154  node.request = request;
155  node.result = &result;
156 
157  node.model1 = &model1;
158  node.model2 = &model2;
159 
160  node.otsolver = otsolver;
161 
162  node.tf1 = tf1;
163  node.tf2 = tf2;
164 
165  return true;
166 }
167 
169 template<typename S>
170 bool initialize(OcTreeShapeDistanceTraversalNode<S>& node,
171  const OcTree& model1, const Transform3f& tf1,
172  const S& model2, const Transform3f& tf2,
173  const OcTreeSolver* otsolver,
174  const DistanceRequest& request,
175  DistanceResult& result)
176 {
177  node.request = request;
178  node.result = &result;
179 
180  node.model1 = &model1;
181  node.model2 = &model2;
182 
183  node.otsolver = otsolver;
184 
185  node.tf1 = tf1;
186  node.tf2 = tf2;
187 
188  return true;
189 }
190 
192 template<typename BV>
193 bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
194  const BVHModel<BV>& model1, const Transform3f& tf1,
195  const OcTree& model2, const Transform3f& tf2,
196  const OcTreeSolver* otsolver,
197  CollisionResult& result)
198 {
199  node.result = &result;
200 
201  node.model1 = &model1;
202  node.model2 = &model2;
203 
204  node.otsolver = otsolver;
205 
206  node.tf1 = tf1;
207  node.tf2 = tf2;
208 
209  return true;
210 }
211 
213 template<typename BV>
214 bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
215  const OcTree& model1, const Transform3f& tf1,
216  const BVHModel<BV>& model2, const Transform3f& tf2,
217  const OcTreeSolver* otsolver,
218  CollisionResult& result)
219 {
220  node.result = &result;
221 
222  node.model1 = &model1;
223  node.model2 = &model2;
224 
225  node.otsolver = otsolver;
226 
227  node.tf1 = tf1;
228  node.tf2 = tf2;
229 
230  return true;
231 }
232 
234 template<typename BV>
235 bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
236  const BVHModel<BV>& model1, const Transform3f& tf1,
237  const OcTree& model2, const Transform3f& tf2,
238  const OcTreeSolver* otsolver,
239  const DistanceRequest& request,
240  DistanceResult& result)
241 {
242  node.request = request;
243  node.result = &result;
244 
245  node.model1 = &model1;
246  node.model2 = &model2;
247 
248  node.otsolver = otsolver;
249 
250  node.tf1 = tf1;
251  node.tf2 = tf2;
252 
253  return true;
254 }
255 
257 template<typename BV>
258 bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node,
259  const OcTree& model1, const Transform3f& tf1,
260  const BVHModel<BV>& model2, const Transform3f& tf2,
261  const OcTreeSolver* otsolver,
262  const DistanceRequest& request,
263  DistanceResult& result)
264 {
265  node.request = request;
266  node.result = &result;
267 
268  node.model1 = &model1;
269  node.model2 = &model2;
270 
271  node.otsolver = otsolver;
272 
273  node.tf1 = tf1;
274  node.tf2 = tf2;
275 
276  return true;
277 }
278 
279 #endif
280 
281 
283 template<typename S1, typename S2>
284 bool initialize(ShapeCollisionTraversalNode<S1, S2>& node,
285  const S1& shape1, const Transform3f& tf1,
286  const S2& shape2, const Transform3f& tf2,
287  const GJKSolver* nsolver,
288  CollisionResult& result)
289 {
290  node.model1 = &shape1;
291  node.tf1 = tf1;
292  node.model2 = &shape2;
293  node.tf2 = tf2;
294  node.nsolver = nsolver;
295 
296  node.result = &result;
297 
298  return true;
299 }
300 
302 template<typename BV, typename S>
303 bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
304  BVHModel<BV>& model1, Transform3f& tf1,
305  const S& model2, const Transform3f& tf2,
306  const GJKSolver* nsolver,
307  CollisionResult& result,
308  bool use_refit = false, bool refit_bottomup = false)
309 {
310  if(model1.getModelType() != BVH_MODEL_TRIANGLES)
311  return false;
312 
313  if(!tf1.isIdentity())
314  {
315  std::vector<Vec3f> vertices_transformed(model1.num_vertices);
316  for(int i = 0; i < model1.num_vertices; ++i)
317  {
318  Vec3f& p = model1.vertices[i];
319  Vec3f new_v = tf1.transform(p);
320  vertices_transformed[i] = new_v;
321  }
322 
323  model1.beginReplaceModel();
324  model1.replaceSubModel(vertices_transformed);
325  model1.endReplaceModel(use_refit, refit_bottomup);
326 
327  tf1.setIdentity();
328  }
329 
330  node.model1 = &model1;
331  node.tf1 = tf1;
332  node.model2 = &model2;
333  node.tf2 = tf2;
334  node.nsolver = nsolver;
335 
336  computeBV(model2, tf2, node.model2_bv);
337 
338  node.vertices = model1.vertices;
339  node.tri_indices = model1.tri_indices;
340 
341  node.result = &result;
342 
343  return true;
344 }
345 
347 template<typename BV, typename S>
348 bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
349  const BVHModel<BV>& model1, const Transform3f& tf1,
350  const S& model2, const Transform3f& tf2,
351  const GJKSolver* nsolver,
352  CollisionResult& result)
353 {
354  if(model1.getModelType() != BVH_MODEL_TRIANGLES)
355  return false;
356 
357  node.model1 = &model1;
358  node.tf1 = tf1;
359  node.model2 = &model2;
360  node.tf2 = tf2;
361  node.nsolver = nsolver;
362 
363  computeBV(model2, tf2, node.model2_bv);
364 
365  node.vertices = model1.vertices;
366  node.tri_indices = model1.tri_indices;
367 
368  node.result = &result;
369 
370  return true;
371 }
372 
374 namespace details
375 {
376 template<typename S, typename BV, template<typename> class OrientedNode>
377 static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S>& node,
378  const S& model1, const Transform3f& tf1,
379  const BVHModel<BV>& model2, const Transform3f& tf2,
380  const GJKSolver* nsolver,
381  CollisionResult& result)
382 {
383  if(model2.getModelType() != BVH_MODEL_TRIANGLES)
384  return false;
385 
386  node.model1 = &model1;
387  node.tf1 = tf1;
388  node.model2 = &model2;
389  node.tf2 = tf2;
390  node.nsolver = nsolver;
391 
392  computeBV(model1, tf1, node.model1_bv);
393 
394  node.vertices = model2.vertices;
395  node.tri_indices = model2.tri_indices;
396 
397  node.result = &result;
398 
399  return true;
400 }
401 }
403 
404 
406 template<typename BV>
407 bool initialize(MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
408  BVHModel<BV>& model1, Transform3f& tf1,
409  BVHModel<BV>& model2, Transform3f& tf2,
410  CollisionResult& result,
411  bool use_refit = false, bool refit_bottomup = false)
412 {
413  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
414  return false;
415 
416  if(!tf1.isIdentity())
417  {
418  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
419  for(int i = 0; i < model1.num_vertices; ++i)
420  {
421  Vec3f& p = model1.vertices[i];
422  Vec3f new_v = tf1.transform(p);
423  vertices_transformed1[i] = new_v;
424  }
425 
426  model1.beginReplaceModel();
427  model1.replaceSubModel(vertices_transformed1);
428  model1.endReplaceModel(use_refit, refit_bottomup);
429 
430  tf1.setIdentity();
431  }
432 
433  if(!tf2.isIdentity())
434  {
435  std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
436  for(int i = 0; i < model2.num_vertices; ++i)
437  {
438  Vec3f& p = model2.vertices[i];
439  Vec3f new_v = tf2.transform(p);
440  vertices_transformed2[i] = new_v;
441  }
442 
443  model2.beginReplaceModel();
444  model2.replaceSubModel(vertices_transformed2);
445  model2.endReplaceModel(use_refit, refit_bottomup);
446 
447  tf2.setIdentity();
448  }
449 
450  node.model1 = &model1;
451  node.tf1 = tf1;
452  node.model2 = &model2;
453  node.tf2 = tf2;
454 
455  node.vertices1 = model1.vertices;
456  node.vertices2 = model2.vertices;
457 
458  node.tri_indices1 = model1.tri_indices;
459  node.tri_indices2 = model2.tri_indices;
460 
461  node.result = &result;
462 
463  return true;
464 }
465 
466 template<typename BV>
467 bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
468  const BVHModel<BV>& model1, const Transform3f& tf1,
469  const BVHModel<BV>& model2, const Transform3f& tf2,
470  CollisionResult& result)
471 {
472  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
473  return false;
474 
475  node.vertices1 = model1.vertices;
476  node.vertices2 = model2.vertices;
477 
478  node.tri_indices1 = model1.tri_indices;
479  node.tri_indices2 = model2.tri_indices;
480 
481  node.model1 = &model1;
482  node.tf1 = tf1;
483  node.model2 = &model2;
484  node.tf2 = tf2;
485 
486  node.result = &result;
487 
488  node.RT.R = tf1.getRotation().transpose() * tf2.getRotation();
489  node.RT.T = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation());
490 
491  return true;
492 }
493 
495 template<typename S1, typename S2>
496 bool initialize(ShapeDistanceTraversalNode<S1, S2>& node,
497  const S1& shape1, const Transform3f& tf1,
498  const S2& shape2, const Transform3f& tf2,
499  const GJKSolver* nsolver,
500  const DistanceRequest& request,
501  DistanceResult& result)
502 {
503  node.request = request;
504  node.result = &result;
505 
506  node.model1 = &shape1;
507  node.tf1 = tf1;
508  node.model2 = &shape2;
509  node.tf2 = tf2;
510  node.nsolver = nsolver;
511 
512  return true;
513 }
514 
516 template<typename BV>
517 bool initialize(MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
518  BVHModel<BV>& model1, Transform3f& tf1,
519  BVHModel<BV>& model2, Transform3f& tf2,
520  const DistanceRequest& request,
521  DistanceResult& result,
522  bool use_refit = false, bool refit_bottomup = false)
523 {
524  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
525  return false;
526 
527  if(!tf1.isIdentity())
528  {
529  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
530  for(int i = 0; i < model1.num_vertices; ++i)
531  {
532  Vec3f& p = model1.vertices[i];
533  Vec3f new_v = tf1.transform(p);
534  vertices_transformed1[i] = new_v;
535  }
536 
537  model1.beginReplaceModel();
538  model1.replaceSubModel(vertices_transformed1);
539  model1.endReplaceModel(use_refit, refit_bottomup);
540 
541  tf1.setIdentity();
542  }
543 
544  if(!tf2.isIdentity())
545  {
546  std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
547  for(int i = 0; i < model2.num_vertices; ++i)
548  {
549  Vec3f& p = model2.vertices[i];
550  Vec3f new_v = tf2.transform(p);
551  vertices_transformed2[i] = new_v;
552  }
553 
554  model2.beginReplaceModel();
555  model2.replaceSubModel(vertices_transformed2);
556  model2.endReplaceModel(use_refit, refit_bottomup);
557 
558  tf2.setIdentity();
559  }
560 
561  node.request = request;
562  node.result = &result;
563 
564  node.model1 = &model1;
565  node.tf1 = tf1;
566  node.model2 = &model2;
567  node.tf2 = tf2;
568 
569  node.vertices1 = model1.vertices;
570  node.vertices2 = model2.vertices;
571 
572  node.tri_indices1 = model1.tri_indices;
573  node.tri_indices2 = model2.tri_indices;
574 
575  return true;
576 }
577 
579 template<typename BV>
580 bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
581  const BVHModel<BV>& model1, const Transform3f& tf1,
582  const BVHModel<BV>& model2, const Transform3f& tf2,
583  const DistanceRequest& request,
584  DistanceResult& result)
585 {
586  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
587  return false;
588 
589  node.request = request;
590  node.result = &result;
591 
592  node.model1 = &model1;
593  node.tf1 = tf1;
594  node.model2 = &model2;
595  node.tf2 = tf2;
596 
597  node.vertices1 = model1.vertices;
598  node.vertices2 = model2.vertices;
599 
600  node.tri_indices1 = model1.tri_indices;
601  node.tri_indices2 = model2.tri_indices;
602 
603  relativeTransform(tf1.getRotation(), tf1.getTranslation(),
604  tf2.getRotation(), tf2.getTranslation(),
605  node.RT.R, node.RT.T);
606 
607  return true;
608 }
609 
611 template<typename BV, typename S>
612 bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
613  BVHModel<BV>& model1, Transform3f& tf1,
614  const S& model2, const Transform3f& tf2,
615  const GJKSolver* nsolver,
616  const DistanceRequest& request,
617  DistanceResult& result,
618  bool use_refit = false, bool refit_bottomup = false)
619 {
620  if(model1.getModelType() != BVH_MODEL_TRIANGLES)
621  return false;
622 
623  if(!tf1.isIdentity())
624  {
625  std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
626  for(int i = 0; i < model1.num_vertices; ++i)
627  {
628  Vec3f& p = model1.vertices[i];
629  Vec3f new_v = tf1.transform(p);
630  vertices_transformed1[i] = new_v;
631  }
632 
633  model1.beginReplaceModel();
634  model1.replaceSubModel(vertices_transformed1);
635  model1.endReplaceModel(use_refit, refit_bottomup);
636 
637  tf1.setIdentity();
638  }
639 
640  node.request = request;
641  node.result = &result;
642 
643  node.model1 = &model1;
644  node.tf1 = tf1;
645  node.model2 = &model2;
646  node.tf2 = tf2;
647  node.nsolver = nsolver;
648 
649  node.vertices = model1.vertices;
650  node.tri_indices = model1.tri_indices;
651 
652  computeBV(model2, tf2, node.model2_bv);
653 
654  return true;
655 }
656 
658 template<typename S, typename BV>
660  const S& model1, const Transform3f& tf1,
661  BVHModel<BV>& model2, Transform3f& tf2,
662  const GJKSolver* nsolver,
663  const DistanceRequest& request,
664  DistanceResult& result,
665  bool use_refit = false, bool refit_bottomup = false)
666 {
667  if(model2.getModelType() != BVH_MODEL_TRIANGLES)
668  return false;
669 
670  if(!tf2.isIdentity())
671  {
672  std::vector<Vec3f> vertices_transformed(model2.num_vertices);
673  for(int i = 0; i < model2.num_vertices; ++i)
674  {
675  Vec3f& p = model2.vertices[i];
676  Vec3f new_v = tf2.transform(p);
677  vertices_transformed[i] = new_v;
678  }
679 
680  model2.beginReplaceModel();
681  model2.replaceSubModel(vertices_transformed);
682  model2.endReplaceModel(use_refit, refit_bottomup);
683 
684  tf2.setIdentity();
685  }
686 
687  node.request = request;
688  node.result = &result;
689 
690  node.model1 = &model1;
691  node.tf1 = tf1;
692  node.model2 = &model2;
693  node.tf2 = tf2;
694  node.nsolver = nsolver;
695 
696  node.vertices = model2.vertices;
697  node.tri_indices = model2.tri_indices;
698 
699  computeBV(model1, tf1, node.model1_bv);
700 
701  return true;
702 }
703 
705 namespace details
706 {
707 
708 template<typename BV, typename S, template<typename> class OrientedNode>
709 static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S>& node,
710  const BVHModel<BV>& model1, const Transform3f& tf1,
711  const S& model2, const Transform3f& tf2,
712  const GJKSolver* nsolver,
713  const DistanceRequest& request,
714  DistanceResult& result)
715 {
716  if(model1.getModelType() != BVH_MODEL_TRIANGLES)
717  return false;
718 
719  node.request = request;
720  node.result = &result;
721 
722  node.model1 = &model1;
723  node.tf1 = tf1;
724  node.model2 = &model2;
725  node.tf2 = tf2;
726  node.nsolver = nsolver;
727 
728  computeBV(model2, tf2, node.model2_bv);
729 
730  node.vertices = model1.vertices;
731  node.tri_indices = model1.tri_indices;
732 
733  return true;
734 }
735 }
737 
739 template<typename S>
741  const BVHModel<RSS>& model1, const Transform3f& tf1,
742  const S& model2, const Transform3f& tf2,
743  const GJKSolver* nsolver,
744  const DistanceRequest& request,
745  DistanceResult& result)
746 {
747  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
748 }
749 
751 template<typename S>
753  const BVHModel<kIOS>& model1, const Transform3f& tf1,
754  const S& model2, const Transform3f& tf2,
755  const GJKSolver* nsolver,
756  const DistanceRequest& request,
757  DistanceResult& result)
758 {
759  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
760 }
761 
763 template<typename S>
765  const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
766  const S& model2, const Transform3f& tf2,
767  const GJKSolver* nsolver,
768  const DistanceRequest& request,
769  DistanceResult& result)
770 {
771  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
772 }
773 
774 
775 namespace details
776 {
777 template<typename S, typename BV, template<typename> class OrientedNode>
778 static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S>& node,
779  const S& model1, const Transform3f& tf1,
780  const BVHModel<BV>& model2, const Transform3f& tf2,
781  const GJKSolver* nsolver,
782  const DistanceRequest& request,
783  DistanceResult& result)
784 {
785  if(model2.getModelType() != BVH_MODEL_TRIANGLES)
786  return false;
787 
788  node.request = request;
789  node.result = &result;
790 
791  node.model1 = &model1;
792  node.tf1 = tf1;
793  node.model2 = &model2;
794  node.tf2 = tf2;
795  node.nsolver = nsolver;
796 
797  computeBV(model1, tf1, node.model1_bv);
798 
799  node.vertices = model2.vertices;
800  node.tri_indices = model2.tri_indices;
801  node.R = tf2.getRotation();
802  node.T = tf2.getTranslation();
803 
804  return true;
805 }
806 }
807 
808 
810 template<typename S>
812  const S& model1, const Transform3f& tf1,
813  const BVHModel<RSS>& model2, const Transform3f& tf2,
814  const GJKSolver* nsolver,
815  const DistanceRequest& request,
816  DistanceResult& result)
817 {
818  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
819 }
820 
822 template<typename S>
824  const S& model1, const Transform3f& tf1,
825  const BVHModel<kIOS>& model2, const Transform3f& tf2,
826  const GJKSolver* nsolver,
827  const DistanceRequest& request,
828  DistanceResult& result)
829 {
830  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
831 }
832 
834 template<typename S>
836  const S& model1, const Transform3f& tf1,
837  const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
838  const GJKSolver* nsolver,
839  const DistanceRequest& request,
840  DistanceResult& result)
841 {
842  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
843 }
844 
845 }
846 
847 } // namespace hpp
848 
850 
851 #endif
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:69
Main namespace.
Definition: AABB.h:43
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:407
unknown model type
Definition: BVH_internal.h:80
Definition: traversal_node_setup.h:775
Triangle * tri_indices
Definition: traversal_node_bvh_shape.h:745
Definition: traversal_node_bvh_shape.h:787
Traversal node for distance between shape and mesh.
Definition: traversal_node_bvh_shape.h:696
Traversal node for distance between shape and mesh, when mesh BVH is one of the oriented node (RSS...
Definition: traversal_node_bvh_shape.h:755
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:103
Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS...
Definition: traversal_node_bvh_shape.h:598
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
Definition: traversal_node_bvh_shape.h:819
Definition: traversal_node_bvh_shape.h:630
Vec3f * vertices
Definition: traversal_node_bvh_shape.h:744
const GJKSolver * nsolver
Definition: traversal_node_bvh_shape.h:750
Definition: traversal_node_bvh_shape.h:662