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