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