coal/internal/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 COAL_TRAVERSAL_NODE_SETUP_H
39 #define COAL_TRAVERSAL_NODE_SETUP_H
40 
42 
43 #include "coal/internal/tools.h"
44 #include "coal/internal/traversal_node_shapes.h"
45 
46 #include "coal/internal/traversal_node_bvhs.h"
47 #include "coal/internal/traversal_node_bvh_shape.h"
48 
49 // #include <hpp/fcl/internal/traversal_node_hfields.h>
50 #include "coal/internal/traversal_node_hfield_shape.h"
51 
52 #ifdef COAL_HAS_OCTOMAP
53 #include "coal/internal/traversal_node_octree.h"
54 #endif
55 
56 #include "coal/BVH/BVH_utility.h"
57 
58 namespace coal {
59 
60 #ifdef COAL_HAS_OCTOMAP
61 inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1,
64  const Transform3s& tf1, const OcTree& model2,
65  const Transform3s& tf2, const OcTreeSolver* otsolver,
66  CollisionResult& result) {
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 
82 inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1,
83  const Transform3s& tf1, const OcTree& model2,
84  const Transform3s& tf2, const OcTreeSolver* otsolver,
85  const DistanceRequest& request, DistanceResult& result)
86 
87 {
88  node.request = request;
89  node.result = &result;
90 
91  node.model1 = &model1;
92  node.model2 = &model2;
93 
94  node.otsolver = otsolver;
95 
96  node.tf1 = tf1;
97  node.tf2 = tf2;
98 
99  return true;
100 }
101 
104 template <typename S>
105 bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node, const S& model1,
106  const Transform3s& tf1, const OcTree& model2,
107  const Transform3s& tf2, const OcTreeSolver* otsolver,
108  CollisionResult& result) {
109  node.result = &result;
110 
111  node.model1 = &model1;
112  node.model2 = &model2;
113 
114  node.otsolver = otsolver;
115 
116  node.tf1 = tf1;
117  node.tf2 = tf2;
118 
119  return true;
120 }
121 
124 template <typename S>
125 bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
126  const OcTree& model1, const Transform3s& tf1, const S& model2,
127  const Transform3s& tf2, const OcTreeSolver* otsolver,
128  CollisionResult& result) {
129  node.result = &result;
130 
131  node.model1 = &model1;
132  node.model2 = &model2;
133 
134  node.otsolver = otsolver;
135 
136  node.tf1 = tf1;
137  node.tf2 = tf2;
138 
139  return true;
140 }
141 
144 template <typename S>
145 bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node, const S& model1,
146  const Transform3s& tf1, const OcTree& model2,
147  const Transform3s& tf2, const OcTreeSolver* otsolver,
148  const DistanceRequest& request, DistanceResult& result) {
149  node.request = request;
150  node.result = &result;
151 
152  node.model1 = &model1;
153  node.model2 = &model2;
154 
155  node.otsolver = otsolver;
156 
157  node.tf1 = tf1;
158  node.tf2 = tf2;
159 
160  return true;
161 }
162 
165 template <typename S>
166 bool initialize(OcTreeShapeDistanceTraversalNode<S>& node, const OcTree& model1,
167  const Transform3s& tf1, const S& model2, const Transform3s& tf2,
168  const OcTreeSolver* otsolver, const DistanceRequest& request,
169  DistanceResult& result) {
170  node.request = request;
171  node.result = &result;
172 
173  node.model1 = &model1;
174  node.model2 = &model2;
175 
176  node.otsolver = otsolver;
177 
178  node.tf1 = tf1;
179  node.tf2 = tf2;
180 
181  return true;
182 }
183 
186 template <typename BV>
187 bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
188  const BVHModel<BV>& model1, const Transform3s& tf1,
189  const OcTree& model2, const Transform3s& tf2,
190  const OcTreeSolver* otsolver, CollisionResult& result) {
191  node.result = &result;
192 
193  node.model1 = &model1;
194  node.model2 = &model2;
195 
196  node.otsolver = otsolver;
197 
198  node.tf1 = tf1;
199  node.tf2 = tf2;
200 
201  return true;
202 }
203 
206 template <typename BV>
207 bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
208  const OcTree& model1, const Transform3s& tf1,
209  const BVHModel<BV>& model2, const Transform3s& tf2,
210  const OcTreeSolver* otsolver, CollisionResult& result) {
211  node.result = &result;
212 
213  node.model1 = &model1;
214  node.model2 = &model2;
215 
216  node.otsolver = otsolver;
217 
218  node.tf1 = tf1;
219  node.tf2 = tf2;
220 
221  return true;
222 }
223 
226 template <typename BV>
227 bool initialize(OcTreeHeightFieldCollisionTraversalNode<BV>& node,
228  const OcTree& model1, const Transform3s& tf1,
229  const HeightField<BV>& model2, const Transform3s& tf2,
230  const OcTreeSolver* otsolver, CollisionResult& result) {
231  node.result = &result;
232 
233  node.model1 = &model1;
234  node.model2 = &model2;
235 
236  node.otsolver = otsolver;
237 
238  node.tf1 = tf1;
239  node.tf2 = tf2;
240 
241  return true;
242 }
243 
246 template <typename BV>
247 bool initialize(HeightFieldOcTreeCollisionTraversalNode<BV>& node,
248  const HeightField<BV>& model1, const Transform3s& tf1,
249  const OcTree& model2, const Transform3s& tf2,
250  const OcTreeSolver* otsolver, CollisionResult& result) {
251  node.result = &result;
252 
253  node.model1 = &model1;
254  node.model2 = &model2;
255 
256  node.otsolver = otsolver;
257 
258  node.tf1 = tf1;
259  node.tf2 = tf2;
260 
261  return true;
262 }
263 
266 template <typename BV>
267 bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
268  const BVHModel<BV>& model1, const Transform3s& tf1,
269  const OcTree& model2, const Transform3s& tf2,
270  const OcTreeSolver* otsolver, const DistanceRequest& request,
271  DistanceResult& result) {
272  node.request = request;
273  node.result = &result;
274 
275  node.model1 = &model1;
276  node.model2 = &model2;
277 
278  node.otsolver = otsolver;
279 
280  node.tf1 = tf1;
281  node.tf2 = tf2;
282 
283  return true;
284 }
285 
288 template <typename BV>
289 bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node, const OcTree& model1,
290  const Transform3s& tf1, const BVHModel<BV>& model2,
291  const Transform3s& tf2, const OcTreeSolver* otsolver,
292  const DistanceRequest& request, DistanceResult& result) {
293  node.request = request;
294  node.result = &result;
295 
296  node.model1 = &model1;
297  node.model2 = &model2;
298 
299  node.otsolver = otsolver;
300 
301  node.tf1 = tf1;
302  node.tf2 = tf2;
303 
304  return true;
305 }
306 
307 #endif
308 
311 template <typename S1, typename S2>
312 bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1,
313  const Transform3s& tf1, const S2& shape2,
314  const Transform3s& tf2, const GJKSolver* nsolver,
315  CollisionResult& result) {
316  node.model1 = &shape1;
317  node.tf1 = tf1;
318  node.model2 = &shape2;
319  node.tf2 = tf2;
320  node.nsolver = nsolver;
321 
322  node.result = &result;
323 
324  return true;
325 }
326 
329 template <typename BV, typename S>
330 bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
331  BVHModel<BV>& model1, Transform3s& tf1, const S& model2,
332  const Transform3s& tf2, const GJKSolver* nsolver,
333  CollisionResult& result, bool use_refit = false,
334  bool refit_bottomup = false) {
335  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
336  COAL_THROW_PRETTY(
337  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
338  std::invalid_argument)
339 
340  if (!tf1.isIdentity() &&
341  model1.vertices.get()) // TODO(jcarpent): vectorized version
342  {
343  std::vector<Vec3s> vertices_transformed(model1.num_vertices);
344  const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
345  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
346  const Vec3s& p = model1_vertices_[i];
347  Vec3s new_v = tf1.transform(p);
348  vertices_transformed[i] = new_v;
349  }
350 
351  model1.beginReplaceModel();
352  model1.replaceSubModel(vertices_transformed);
353  model1.endReplaceModel(use_refit, refit_bottomup);
354 
355  tf1.setIdentity();
356  }
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.get() ? model1.vertices->data() : NULL;
367  node.tri_indices =
368  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
369 
370  node.result = &result;
371 
372  return true;
373 }
374 
377 template <typename BV, typename S>
378 bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
379  const BVHModel<BV>& model1, const Transform3s& tf1,
380  const S& model2, const Transform3s& tf2,
381  const GJKSolver* nsolver, CollisionResult& result) {
382  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
383  COAL_THROW_PRETTY(
384  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
385  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(model2, tf2, node.model2_bv);
394 
395  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
396  node.tri_indices =
397  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
398 
399  node.result = &result;
400 
401  return true;
402 }
403 
406 template <typename BV, typename S>
407 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
408  HeightField<BV>& model1, Transform3s& tf1, const S& model2,
409  const Transform3s& tf2, const GJKSolver* nsolver,
410  CollisionResult& result, bool use_refit = false,
411  bool refit_bottomup = false);
412 
415 template <typename BV, typename S>
416 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
417  const HeightField<BV>& model1, const Transform3s& tf1,
418  const S& model2, const Transform3s& tf2,
419  const GJKSolver* nsolver, CollisionResult& result) {
420  node.model1 = &model1;
421  node.tf1 = tf1;
422  node.model2 = &model2;
423  node.tf2 = tf2;
424  node.nsolver = nsolver;
425 
426  computeBV(model2, tf2, node.model2_bv);
427 
428  node.result = &result;
429 
430  return true;
431 }
432 
434 namespace details {
435 template <typename S, typename BV, template <typename> class OrientedNode>
436 static inline bool setupShapeMeshCollisionOrientedNode(
437  OrientedNode<S>& node, const S& model1, const Transform3s& tf1,
438  const BVHModel<BV>& model2, const Transform3s& tf2,
439  const GJKSolver* nsolver, CollisionResult& result) {
440  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
441  COAL_THROW_PRETTY(
442  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
443  std::invalid_argument)
444 
445  node.model1 = &model1;
446  node.tf1 = tf1;
447  node.model2 = &model2;
448  node.tf2 = tf2;
449  node.nsolver = nsolver;
450 
451  computeBV(model1, tf1, node.model1_bv);
452 
453  node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
454  node.tri_indices =
455  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
456 
457  node.result = &result;
458 
459  return true;
460 }
461 } // namespace details
463 
466 template <typename BV>
467 bool initialize(
468  MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
469  BVHModel<BV>& model1, Transform3s& tf1, BVHModel<BV>& model2,
470  Transform3s& tf2, CollisionResult& result, bool use_refit = false,
471  bool refit_bottomup = false) {
472  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
473  COAL_THROW_PRETTY(
474  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
475  std::invalid_argument)
476  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
477  COAL_THROW_PRETTY(
478  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
479  std::invalid_argument)
480 
481  if (!tf1.isIdentity() && model1.vertices.get()) {
482  std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
483  const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
484  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
485  const Vec3s& p = model1_vertices_[i];
486  Vec3s new_v = tf1.transform(p);
487  vertices_transformed1[i] = new_v;
488  }
489 
490  model1.beginReplaceModel();
491  model1.replaceSubModel(vertices_transformed1);
492  model1.endReplaceModel(use_refit, refit_bottomup);
493 
494  tf1.setIdentity();
495  }
496 
497  if (!tf2.isIdentity() && model2.vertices.get()) {
498  std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
499  const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
500  for (unsigned int i = 0; i < model2.num_vertices; ++i) {
501  const Vec3s& p = model2_vertices_[i];
502  Vec3s new_v = tf2.transform(p);
503  vertices_transformed2[i] = new_v;
504  }
505 
506  model2.beginReplaceModel();
507  model2.replaceSubModel(vertices_transformed2);
508  model2.endReplaceModel(use_refit, refit_bottomup);
509 
510  tf2.setIdentity();
511  }
512 
513  node.model1 = &model1;
514  node.tf1 = tf1;
515  node.model2 = &model2;
516  node.tf2 = tf2;
517 
518  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
519  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
520 
521  node.tri_indices1 =
522  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
523  node.tri_indices2 =
524  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
525 
526  node.result = &result;
527 
528  return true;
529 }
530 
531 template <typename BV>
532 bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
533  const BVHModel<BV>& model1, const Transform3s& tf1,
534  const BVHModel<BV>& model2, const Transform3s& tf2,
535  CollisionResult& result) {
536  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
537  COAL_THROW_PRETTY(
538  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
539  std::invalid_argument)
540  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
541  COAL_THROW_PRETTY(
542  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
543  std::invalid_argument)
544 
545  node.vertices1 = model1.vertices ? model1.vertices->data() : NULL;
546  node.vertices2 = model2.vertices ? model2.vertices->data() : NULL;
547 
548  node.tri_indices1 =
549  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
550  node.tri_indices2 =
551  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
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() *
562  (tf2.getTranslation() - tf1.getTranslation());
563 
564  return true;
565 }
566 
568 template <typename S1, typename S2>
569 bool initialize(ShapeDistanceTraversalNode<S1, S2>& node, const S1& shape1,
570  const Transform3s& tf1, const S2& shape2,
571  const Transform3s& tf2, const GJKSolver* nsolver,
572  const DistanceRequest& request, DistanceResult& result) {
573  node.request = request;
574  node.result = &result;
575 
576  node.model1 = &shape1;
577  node.tf1 = tf1;
578  node.model2 = &shape2;
579  node.tf2 = tf2;
580  node.nsolver = nsolver;
581 
582  return true;
583 }
584 
587 template <typename BV>
588 bool initialize(
589  MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
590  BVHModel<BV>& model1, Transform3s& tf1, BVHModel<BV>& model2,
591  Transform3s& tf2, const DistanceRequest& request, DistanceResult& result,
592  bool use_refit = false, bool refit_bottomup = false) {
593  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
594  COAL_THROW_PRETTY(
595  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
596  std::invalid_argument)
597  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
598  COAL_THROW_PRETTY(
599  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
600  std::invalid_argument)
601 
602  if (!tf1.isIdentity() && model1.vertices.get()) {
603  std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
604  const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
605  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
606  const Vec3s& p = model1_vertices_[i];
607  Vec3s 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() && model2.vertices.get()) {
619  std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
620  const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
621  for (unsigned int i = 0; i < model2.num_vertices; ++i) {
622  const Vec3s& p = model2_vertices_[i];
623  Vec3s new_v = tf2.transform(p);
624  vertices_transformed2[i] = new_v;
625  }
626 
627  model2.beginReplaceModel();
628  model2.replaceSubModel(vertices_transformed2);
629  model2.endReplaceModel(use_refit, refit_bottomup);
630 
631  tf2.setIdentity();
632  }
633 
634  node.request = request;
635  node.result = &result;
636 
637  node.model1 = &model1;
638  node.tf1 = tf1;
639  node.model2 = &model2;
640  node.tf2 = tf2;
641 
642  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
643  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
644 
645  node.tri_indices1 =
646  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
647  node.tri_indices2 =
648  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
649 
650  return true;
651 }
652 
654 template <typename BV>
655 bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
656  const BVHModel<BV>& model1, const Transform3s& tf1,
657  const BVHModel<BV>& model2, const Transform3s& tf2,
658  const DistanceRequest& request, DistanceResult& result) {
659  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
660  COAL_THROW_PRETTY(
661  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
662  std::invalid_argument)
663  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
664  COAL_THROW_PRETTY(
665  "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
666  std::invalid_argument)
667 
668  node.request = request;
669  node.result = &result;
670 
671  node.model1 = &model1;
672  node.tf1 = tf1;
673  node.model2 = &model2;
674  node.tf2 = tf2;
675 
676  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
677  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
678 
679  node.tri_indices1 =
680  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
681  node.tri_indices2 =
682  model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
683 
684  COAL_COMPILER_DIAGNOSTIC_PUSH
685  COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
686 
687  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
688  tf2.getTranslation(), node.RT.R, node.RT.T);
689 
690  COAL_COMPILER_DIAGNOSTIC_POP
691 
692  return true;
693 }
694 
697 template <typename BV, typename S>
698 bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
699  BVHModel<BV>& model1, Transform3s& tf1, const S& model2,
700  const Transform3s& tf2, const GJKSolver* nsolver,
701  const DistanceRequest& request, DistanceResult& result,
702  bool use_refit = false, bool refit_bottomup = false) {
703  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
704  COAL_THROW_PRETTY(
705  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
706  std::invalid_argument)
707 
708  if (!tf1.isIdentity() && model1.vertices.get()) {
709  const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
710  std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
711  for (unsigned int i = 0; i < model1.num_vertices; ++i) {
712  const Vec3s& p = model1_vertices_[i];
713  Vec3s new_v = tf1.transform(p);
714  vertices_transformed1[i] = new_v;
715  }
716 
717  model1.beginReplaceModel();
718  model1.replaceSubModel(vertices_transformed1);
719  model1.endReplaceModel(use_refit, refit_bottomup);
720 
721  tf1.setIdentity();
722  }
723 
724  node.request = request;
725  node.result = &result;
726 
727  node.model1 = &model1;
728  node.tf1 = tf1;
729  node.model2 = &model2;
730  node.tf2 = tf2;
731  node.nsolver = nsolver;
732 
733  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
734  node.tri_indices =
735  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
736 
737  computeBV(model2, tf2, node.model2_bv);
738 
739  return true;
740 }
741 
743 namespace details {
744 
745 template <typename BV, typename S, template <typename> class OrientedNode>
746 static inline bool setupMeshShapeDistanceOrientedNode(
747  OrientedNode<S>& node, const BVHModel<BV>& model1, const Transform3s& tf1,
748  const S& model2, const Transform3s& tf2, const GJKSolver* nsolver,
749  const DistanceRequest& request, DistanceResult& result) {
750  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
751  COAL_THROW_PRETTY(
752  "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
753  std::invalid_argument)
754 
755  node.request = request;
756  node.result = &result;
757 
758  node.model1 = &model1;
759  node.tf1 = tf1;
760  node.model2 = &model2;
761  node.tf2 = tf2;
762  node.nsolver = nsolver;
763 
764  computeBV(model2, tf2, node.model2_bv);
765 
766  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
767  node.tri_indices =
768  model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
769 
770  return true;
771 }
772 } // namespace details
774 
777 template <typename S>
778 bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& node,
779  const BVHModel<RSS>& model1, const Transform3s& tf1,
780  const S& model2, const Transform3s& tf2,
781  const GJKSolver* nsolver, const DistanceRequest& request,
782  DistanceResult& result) {
783  return details::setupMeshShapeDistanceOrientedNode(
784  node, model1, tf1, model2, tf2, nsolver, request, result);
785 }
786 
789 template <typename S>
790 bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& node,
791  const BVHModel<kIOS>& model1, const Transform3s& tf1,
792  const S& model2, const Transform3s& tf2,
793  const GJKSolver* nsolver, const DistanceRequest& request,
794  DistanceResult& result) {
795  return details::setupMeshShapeDistanceOrientedNode(
796  node, model1, tf1, model2, tf2, nsolver, request, result);
797 }
798 
801 template <typename S>
802 bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& node,
803  const BVHModel<OBBRSS>& model1, const Transform3s& tf1,
804  const S& model2, const Transform3s& tf2,
805  const GJKSolver* nsolver, const DistanceRequest& request,
806  DistanceResult& result) {
807  return details::setupMeshShapeDistanceOrientedNode(
808  node, model1, tf1, model2, tf2, nsolver, request, result);
809 }
810 
811 } // namespace coal
812 
814 
815 #endif
MeshShapeDistanceTraversalNodeOBBRSS
Definition: coal/internal/traversal_node_bvh_shape.h:450
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
Definition: include/coal/fwd.hh:123
COAL_COMPILER_DIAGNOSTIC_PUSH
#define COAL_COMPILER_DIAGNOSTIC_PUSH
Definition: include/coal/fwd.hh:120
traversal_node_bvh_shape.h
traversal_node_hfield_shape.h
traversal_node_bvhs.h
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
coal::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: coal/internal/tools.h:90
traversal_node_octree.h
coal::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: coal/BVH/BVH_internal.h:81
MeshShapeDistanceTraversalNodeRSS
Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS,...
Definition: coal/internal/traversal_node_bvh_shape.h:390
COAL_THROW_PRETTY
#define COAL_THROW_PRETTY(message, exception)
Definition: include/coal/fwd.hh:64
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
tools.h
traversal_node_shapes.h
COAL_COMPILER_DIAGNOSTIC_POP
#define COAL_COMPILER_DIAGNOSTIC_POP
Definition: include/coal/fwd.hh:121
MeshShapeDistanceTraversalNodekIOS
Definition: coal/internal/traversal_node_bvh_shape.h:420
BVH_utility.h
coal::computeBV
void computeBV(const S &s, const Transform3s &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: coal/shape/geometric_shapes_utility.h:73
initialize
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3s &tf1, BVHModel< BV > &model2, Transform3s &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
Definition: coal/internal/traversal_node_setup.h:467

hpp-fcl
Author(s):
autogenerated on Fri Feb 14 2025 03:45:51