FCL  0.6.0
Flexible Collision Library
mesh_conservative_advancement_traversal_node-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, 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 FCL_TRAVERSAL_MESHCONSERVATIVEADVANCEMENTTRAVERSALNODE_INL_H
39 #define FCL_TRAVERSAL_MESHCONSERVATIVEADVANCEMENTTRAVERSALNODE_INL_H
40 
41 #include "fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h"
42 
43 #include "fcl/math/bv/RSS.h"
44 #include "fcl/math/motion/triangle_motion_bound_visitor.h"
45 
46 namespace fcl
47 {
48 
49 namespace detail
50 {
51 
52 //==============================================================================
53 extern template
54 class MeshConservativeAdvancementTraversalNodeRSS<double>;
55 
56 //==============================================================================
57 extern template
58 bool initialize(
59  MeshConservativeAdvancementTraversalNodeRSS<double>& node,
60  const BVHModel<RSS<double>>& model1,
61  const Transform3<double>& tf1,
62  const BVHModel<RSS<double>>& model2,
63  const Transform3<double>& tf2,
64  double w);
65 
66 //==============================================================================
67 extern template
68 class MeshConservativeAdvancementTraversalNodeOBBRSS<double>;
69 
70 //==============================================================================
71 extern template
72 bool initialize(
73  MeshConservativeAdvancementTraversalNodeOBBRSS<double>& node,
74  const BVHModel<OBBRSS<double>>& model1,
75  const Transform3<double>& tf1,
76  const BVHModel<OBBRSS<double>>& model2,
77  const Transform3<double>& tf2,
78  double w);
79 
80 //==============================================================================
81 template <typename BV>
82 MeshConservativeAdvancementTraversalNode<BV>::
83 MeshConservativeAdvancementTraversalNode(typename BV::S w_)
84  : MeshDistanceTraversalNode<BV>()
85 {
86  delta_t = 1;
87  toc = 0;
88  t_err = (S)0.00001;
89 
90  w = w_;
91 
92  motion1 = nullptr;
93  motion2 = nullptr;
94 }
95 
96 //==============================================================================
97 template <typename BV>
98 typename BV::S
100 {
101  if(this->enable_statistics) this->num_bv_tests++;
102  Vector3<S> P1, P2;
103  S d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
104 
105  stack.emplace_back(P1, P2, b1, b2, d);
106 
107  return d;
108 }
109 
110 //==============================================================================
111 template <typename BV>
113 {
114  if(this->enable_statistics) this->num_leaf_tests++;
115 
116  const BVNode<BV>& node1 = this->model1->getBV(b1);
117  const BVNode<BV>& node2 = this->model2->getBV(b2);
118 
119  int primitive_id1 = node1.primitiveId();
120  int primitive_id2 = node2.primitiveId();
121 
122  const Triangle& tri_id1 = this->tri_indices1[primitive_id1];
123  const Triangle& tri_id2 = this->tri_indices2[primitive_id2];
124 
125  const Vector3<S>& p1 = this->vertices1[tri_id1[0]];
126  const Vector3<S>& p2 = this->vertices1[tri_id1[1]];
127  const Vector3<S>& p3 = this->vertices1[tri_id1[2]];
128 
129  const Vector3<S>& q1 = this->vertices2[tri_id2[0]];
130  const Vector3<S>& q2 = this->vertices2[tri_id2[1]];
131  const Vector3<S>& q3 = this->vertices2[tri_id2[2]];
132 
133  // nearest point pair
134  Vector3<S> P1, P2;
135 
136  S d = TriangleDistance<S>::triDistance(p1, p2, p3, q1, q2, q3,
137  P1, P2);
138 
139  if(d < this->min_distance)
140  {
141  this->min_distance = d;
142 
143  closest_p1 = P1;
144  closest_p2 = P2;
145 
146  last_tri_id1 = primitive_id1;
147  last_tri_id2 = primitive_id2;
148  }
149 
150  Vector3<S> n = P2 - P1;
151  n.normalize();
152  // here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
153  TriangleMotionBoundVisitor<S> mb_visitor1(p1, p2, p3, n), mb_visitor2(q1, q2, q3, n);
154  S bound1 = motion1->computeMotionBound(mb_visitor1);
155  S bound2 = motion2->computeMotionBound(mb_visitor2);
156 
157  S bound = bound1 + bound2;
158 
159  S cur_delta_t;
160  if(bound <= d) cur_delta_t = 1;
161  else cur_delta_t = d / bound;
162 
163  if(cur_delta_t < delta_t)
164  delta_t = cur_delta_t;
165 }
166 
167 //==============================================================================
168 template <typename S, typename BV>
170 {
171  static bool run(
173  {
174  if((c >= node.w * (node.min_distance - node.abs_err))
175  && (c * (1 + node.rel_err) >= node.w * node.min_distance))
176  {
177  const ConservativeAdvancementStackData<S>& data = node.stack.back();
178  S d = data.d;
179  Vector3<S> n;
180  int c1, c2;
181 
182  if(d > c)
183  {
184  const ConservativeAdvancementStackData<S>& data2 = node.stack[node.stack.size() - 2];
185  d = data2.d;
186  n = data2.P2 - data2.P1; n.normalize();
187  c1 = data2.c1;
188  c2 = data2.c2;
189  node.stack[node.stack.size() - 2] = node.stack[node.stack.size() - 1];
190  }
191  else
192  {
193  n = data.P2 - data.P1; n.normalize();
194  c1 = data.c1;
195  c2 = data.c2;
196  }
197 
198  assert(c == d);
199 
200  TBVMotionBoundVisitor<BV> mb_visitor1(node.model1->getBV(c1).bv, n), mb_visitor2(node.model2->getBV(c2).bv, n);
201  S bound1 = node.motion1->computeMotionBound(mb_visitor1);
202  S bound2 = node.motion2->computeMotionBound(mb_visitor2);
203 
204  S bound = bound1 + bound2;
205 
206  S cur_delta_t;
207  if(bound <= c) cur_delta_t = 1;
208  else cur_delta_t = c / bound;
209 
210  if(cur_delta_t < node.delta_t)
211  node.delta_t = cur_delta_t;
212 
213  node.stack.pop_back();
214 
215  return true;
216  }
217  else
218  {
219  const ConservativeAdvancementStackData<S>& data = node.stack.back();
220  S d = data.d;
221 
222  if(d > c)
223  node.stack[node.stack.size() - 2] = node.stack[node.stack.size() - 1];
224 
225  node.stack.pop_back();
226 
227  return false;
228  }
229  }
230 };
231 
232 //==============================================================================
233 template <typename BV>
235  typename BV::S c) const
236 {
237  return CanStopImpl<typename BV::S, BV>::run(*this, c);
238 }
239 
240 //==============================================================================
241 template <typename S>
243 {
244  static bool run(
246  S c)
247  {
248  return detail::meshConservativeAdvancementTraversalNodeCanStop(
249  c,
250  node.min_distance,
251  node.abs_err,
252  node.rel_err,
253  node.w,
254  node.model1,
255  node.model2,
256  node.motion1,
257  node.motion2,
258  node.stack,
259  node.delta_t);
260  }
261 };
262 
263 //==============================================================================
264 template <typename S>
265 struct CanStopImpl<S, RSS<S>>
266 {
267  static bool run(
269  S c)
270  {
271  return detail::meshConservativeAdvancementTraversalNodeCanStop(
272  c,
273  node.min_distance,
274  node.abs_err,
275  node.rel_err,
276  node.w,
277  node.model1,
278  node.model2,
279  node.motion1,
280  node.motion2,
281  node.stack,
282  node.delta_t);
283  }
284 };
285 
286 //==============================================================================
287 template <typename S>
288 struct CanStopImpl<S, OBBRSS<S>>
289 {
290  static bool run(
292  S c)
293  {
294  return detail::meshConservativeAdvancementTraversalNodeCanStop(
295  c,
296  node.min_distance,
297  node.abs_err,
298  node.rel_err,
299  node.w,
300  node.model1,
301  node.model2,
302  node.motion1,
303  node.motion2,
304  node.stack,
305  node.delta_t);
306  }
307 };
308 
309 //==============================================================================
310 template <typename BV>
311 bool initialize(
313  BVHModel<BV>& model1,
314  const Transform3<typename BV::S>& tf1,
315  BVHModel<BV>& model2,
316  const Transform3<typename BV::S>& tf2,
317  typename BV::S w,
318  bool use_refit,
319  bool refit_bottomup)
320 {
321  using S = typename BV::S;
322 
323  std::vector<Vector3<S>> vertices_transformed1(model1.num_vertices);
324  for(int i = 0; i < model1.num_vertices; ++i)
325  {
326  Vector3<S>& p = model1.vertices[i];
327  Vector3<S> new_v = tf1 * p;
328  vertices_transformed1[i] = new_v;
329  }
330 
331  std::vector<Vector3<S>> vertices_transformed2(model2.num_vertices);
332  for(int i = 0; i < model2.num_vertices; ++i)
333  {
334  Vector3<S>& p = model2.vertices[i];
335  Vector3<S> new_v = tf2 * p;
336  vertices_transformed2[i] = new_v;
337  }
338 
339  model1.beginReplaceModel();
340  model1.replaceSubModel(vertices_transformed1);
341  model1.endReplaceModel(use_refit, refit_bottomup);
342 
343  model2.beginReplaceModel();
344  model2.replaceSubModel(vertices_transformed2);
345  model2.endReplaceModel(use_refit, refit_bottomup);
346 
347  node.model1 = &model1;
348  node.model2 = &model2;
349 
350  node.vertices1 = model1.vertices;
351  node.vertices2 = model2.vertices;
352 
353  node.tri_indices1 = model1.tri_indices;
354  node.tri_indices2 = model2.tri_indices;
355 
356  node.w = w;
357 
358  return true;
359 }
360 
361 //==============================================================================
362 template <typename S>
366 {
367  R.setIdentity();
368 }
369 
370 //==============================================================================
371 template <typename S>
373 leafTesting(int b1, int b2) const
374 {
375  detail::meshConservativeAdvancementOrientedNodeLeafTesting(
376  b1,
377  b2,
378  this->model1,
379  this->model2,
380  this->tri_indices1,
381  this->tri_indices2,
382  this->vertices1,
383  this->vertices2,
384  R,
385  T,
386  this->motion1,
387  this->motion2,
388  this->enable_statistics,
389  this->min_distance,
390  this->closest_p1,
391  this->closest_p2,
392  this->last_tri_id1,
393  this->last_tri_id2,
394  this->delta_t,
395  this->num_leaf_tests);
396 }
397 
398 //==============================================================================
399 template <typename S>
401 {
402  return detail::meshConservativeAdvancementOrientedNodeCanStop(
403  c,
404  this->min_distance,
405  this->abs_err,
406  this->rel_err,
407  this->w,
408  this->model1,
409  this->model2,
410  this->motion1,
411  this->motion2,
412  this->stack,
413  this->delta_t);
414 }
415 
416 //==============================================================================
417 template <typename S>
421 {
422  R.setIdentity();
423 }
424 
425 //==============================================================================
426 template <typename S>
428 leafTesting(int b1, int b2) const
429 {
430  detail::meshConservativeAdvancementOrientedNodeLeafTesting(
431  b1,
432  b2,
433  this->model1,
434  this->model2,
435  this->tri_indices1,
436  this->tri_indices2,
437  this->vertices1,
438  this->vertices2,
439  this->R,
440  this->T,
441  this->motion1,
442  this->motion2,
443  this->enable_statistics,
444  this->min_distance,
445  this->closest_p1,
446  this->closest_p2,
447  this->last_tri_id1,
448  this->last_tri_id2,
449  this->delta_t,
450  this->num_leaf_tests);
451 }
452 
453 //==============================================================================
454 template <typename S>
456 {
457  return detail::meshConservativeAdvancementOrientedNodeCanStop(
458  c,
459  this->min_distance,
460  this->abs_err,
461  this->rel_err,
462  this->w,
463  this->model1,
464  this->model2,
465  this->motion1,
466  this->motion2,
467  this->stack,
468  this->delta_t);
469 }
470 
472 
473 //==============================================================================
474 template <typename S, typename BV>
476 {
477  const Vector3<S> operator()(const BV& bv, int i)
478  {
479  return bv.axis.col(i);
480  }
481 };
482 
483 //==============================================================================
484 template <typename BV>
485 const Vector3<typename BV::S> getBVAxis(const BV& bv, int i)
486 {
487  GetBVAxisImpl<typename BV::S, BV> getBVAxisImpl;
488  return getBVAxisImpl(bv, i);
489 }
490 
491 //==============================================================================
492 template <typename S>
493 struct GetBVAxisImpl<S, OBBRSS<S>>
494 {
495  const Vector3<S> operator()(const OBBRSS<S>& bv, int i)
496  {
497  return bv.obb.axis.col(i);
498  }
499 };
500 
501 //==============================================================================
502 template <typename BV>
503 bool meshConservativeAdvancementTraversalNodeCanStop(
504  typename BV::S c,
505  typename BV::S min_distance,
506  typename BV::S abs_err,
507  typename BV::S rel_err,
508  typename BV::S w,
509  const BVHModel<BV>* model1,
510  const BVHModel<BV>* model2,
511  const MotionBase<typename BV::S>* motion1,
512  const MotionBase<typename BV::S>* motion2,
514  typename BV::S& delta_t)
515 {
516  using S = typename BV::S;
517 
518  if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
519  {
520  const ConservativeAdvancementStackData<S>& data = stack.back();
521  S d = data.d;
522  Vector3<S> n;
523  int c1, c2;
524 
525  if(d > c)
526  {
527  const ConservativeAdvancementStackData<S>& data2 = stack[stack.size() - 2];
528  d = data2.d;
529  n = data2.P2 - data2.P1; n.normalize();
530  c1 = data2.c1;
531  c2 = data2.c2;
532  stack[stack.size() - 2] = stack[stack.size() - 1];
533  }
534  else
535  {
536  n = data.P2 - data.P1; n.normalize();
537  c1 = data.c1;
538  c2 = data.c2;
539  }
540 
541  assert(c == d);
542 
543  Vector3<S> n_transformed =
544  getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
545  getBVAxis(model1->getBV(c1).bv, 1) * n[1] +
546  getBVAxis(model1->getBV(c1).bv, 2) * n[2];
547 
548  TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, n_transformed);
549  S bound1 = motion1->computeMotionBound(mb_visitor1);
550  S bound2 = motion2->computeMotionBound(mb_visitor2);
551 
552  S bound = bound1 + bound2;
553 
554  S cur_delta_t;
555  if(bound <= c) cur_delta_t = 1;
556  else cur_delta_t = c / bound;
557 
558  if(cur_delta_t < delta_t)
559  delta_t = cur_delta_t;
560 
561  stack.pop_back();
562 
563  return true;
564  }
565  else
566  {
567  const ConservativeAdvancementStackData<S>& data = stack.back();
568  S d = data.d;
569 
570  if(d > c)
571  stack[stack.size() - 2] = stack[stack.size() - 1];
572 
573  stack.pop_back();
574 
575  return false;
576  }
577 }
578 
579 //==============================================================================
580 template <typename BV>
581 bool meshConservativeAdvancementOrientedNodeCanStop(
582  typename BV::S c,
583  typename BV::S min_distance,
584  typename BV::S abs_err,
585  typename BV::S rel_err,
586  typename BV::S w,
587  const BVHModel<BV>* model1,
588  const BVHModel<BV>* model2,
589  const MotionBase<typename BV::S>* motion1,
590  const MotionBase<typename BV::S>* motion2,
592  typename BV::S& delta_t)
593 {
594  using S = typename BV::S;
595 
596  if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
597  {
598  const ConservativeAdvancementStackData<S>& data = stack.back();
599  S d = data.d;
600  Vector3<S> n;
601  int c1, c2;
602 
603  if(d > c)
604  {
605  const ConservativeAdvancementStackData<S>& data2 = stack[stack.size() - 2];
606  d = data2.d;
607  n = data2.P2 - data2.P1; n.normalize();
608  c1 = data2.c1;
609  c2 = data2.c2;
610  stack[stack.size() - 2] = stack[stack.size() - 1];
611  }
612  else
613  {
614  n = data.P2 - data.P1; n.normalize();
615  c1 = data.c1;
616  c2 = data.c2;
617  }
618 
619  assert(c == d);
620 
621  // n is in local frame of c1, so we need to turn n into the global frame
622  Vector3<S> n_transformed =
623  getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
624  getBVAxis(model1->getBV(c1).bv, 1) * n[2] + // TODO(JS): not n[1]?
625  getBVAxis(model1->getBV(c1).bv, 2) * n[2];
626  Quaternion<S> R0;
627  motion1->getCurrentRotation(R0);
628  n_transformed = R0 * n_transformed;
629  n_transformed.normalize();
630 
631  TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, -n_transformed);
632  S bound1 = motion1->computeMotionBound(mb_visitor1);
633  S bound2 = motion2->computeMotionBound(mb_visitor2);
634 
635  S bound = bound1 + bound2;
636 
637  S cur_delta_t;
638  if(bound <= c) cur_delta_t = 1;
639  else cur_delta_t = c / bound;
640 
641  if(cur_delta_t < delta_t)
642  delta_t = cur_delta_t;
643 
644  stack.pop_back();
645 
646  return true;
647  }
648  else
649  {
650  const ConservativeAdvancementStackData<S>& data = stack.back();
651  S d = data.d;
652 
653  if(d > c)
654  stack[stack.size() - 2] = stack[stack.size() - 1];
655 
656  stack.pop_back();
657 
658  return false;
659  }
660 }
661 
662 //==============================================================================
663 template <typename BV>
664 void meshConservativeAdvancementOrientedNodeLeafTesting(
665  int b1,
666  int b2,
667  const BVHModel<BV>* model1,
668  const BVHModel<BV>* model2,
669  const Triangle* tri_indices1,
670  const Triangle* tri_indices2,
671  const Vector3<typename BV::S>* vertices1,
672  const Vector3<typename BV::S>* vertices2,
673  const Matrix3<typename BV::S>& R,
674  const Vector3<typename BV::S>& T,
675  const MotionBase<typename BV::S>* motion1,
676  const MotionBase<typename BV::S>* motion2,
677  bool enable_statistics,
678  typename BV::S& min_distance,
679  Vector3<typename BV::S>& p1,
680  Vector3<typename BV::S>& p2,
681  int& last_tri_id1,
682  int& last_tri_id2,
683  typename BV::S& delta_t,
684  int& num_leaf_tests)
685 {
686  using S = typename BV::S;
687 
688  if(enable_statistics) num_leaf_tests++;
689 
690  const BVNode<BV>& node1 = model1->getBV(b1);
691  const BVNode<BV>& node2 = model2->getBV(b2);
692 
693  int primitive_id1 = node1.primitiveId();
694  int primitive_id2 = node2.primitiveId();
695 
696  const Triangle& tri_id1 = tri_indices1[primitive_id1];
697  const Triangle& tri_id2 = tri_indices2[primitive_id2];
698 
699  const Vector3<S>& t11 = vertices1[tri_id1[0]];
700  const Vector3<S>& t12 = vertices1[tri_id1[1]];
701  const Vector3<S>& t13 = vertices1[tri_id1[2]];
702 
703  const Vector3<S>& t21 = vertices2[tri_id2[0]];
704  const Vector3<S>& t22 = vertices2[tri_id2[1]];
705  const Vector3<S>& t23 = vertices2[tri_id2[2]];
706 
707  // nearest point pair
708  Vector3<S> P1, P2;
709 
710  S d = TriangleDistance<S>::triDistance(t11, t12, t13, t21, t22, t23,
711  R, T,
712  P1, P2);
713 
714  if(d < min_distance)
715  {
716  min_distance = d;
717 
718  p1 = P1;
719  p2 = P2;
720 
721  last_tri_id1 = primitive_id1;
722  last_tri_id2 = primitive_id2;
723  }
724 
725 
727  Vector3<S> n = P2 - P1;
729  Quaternion<S> R0;
730  motion1->getCurrentRotation(R0);
731  Vector3<S> n_transformed = R0 * n;
732  n_transformed.normalize(); // normalized here
733 
734  TriangleMotionBoundVisitor<S> mb_visitor1(t11, t12, t13, n_transformed), mb_visitor2(t21, t22, t23, -n_transformed);
735  S bound1 = motion1->computeMotionBound(mb_visitor1);
736  S bound2 = motion2->computeMotionBound(mb_visitor2);
737 
738  S bound = bound1 + bound2;
739 
740  S cur_delta_t;
741  if(bound <= d) cur_delta_t = 1;
742  else cur_delta_t = d / bound;
743 
744  if(cur_delta_t < delta_t)
745  delta_t = cur_delta_t;
746 }
747 
748 //==============================================================================
749 template <typename BV, typename OrientedDistanceNode>
750 bool setupMeshConservativeAdvancementOrientedDistanceNode(
751  OrientedDistanceNode& node,
752  const BVHModel<BV>& model1, const Transform3<typename BV::S>& tf1,
753  const BVHModel<BV>& model2, const Transform3<typename BV::S>& tf2,
754  typename BV::S w)
755 {
757  return false;
758 
759  node.model1 = &model1;
760  node.model2 = &model2;
761 
762  node.vertices1 = model1.vertices;
763  node.vertices2 = model2.vertices;
764 
765  node.tri_indices1 = model1.tri_indices;
766  node.tri_indices2 = model2.tri_indices;
767 
768  node.w = w;
769 
770  relativeTransform(tf1.linear(), tf1.translation(), tf2.linear(), tf2.translation(), node.R, node.T);
771 
772  return true;
773 }
774 
775 //==============================================================================
776 template <typename S>
777 bool initialize(
779  const BVHModel<RSS<S>>& model1,
780  const Transform3<S>& tf1,
781  const BVHModel<RSS<S>>& model2,
782  const Transform3<S>& tf2,
783  S w)
784 {
785  return detail::setupMeshConservativeAdvancementOrientedDistanceNode(
786  node, model1, tf1, model2, tf2, w);
787 }
788 
789 //==============================================================================
790 template <typename S>
791 bool initialize(
793  const BVHModel<OBBRSS<S>>& model1,
794  const Transform3<S>& tf1,
795  const BVHModel<OBBRSS<S>>& model2,
796  const Transform3<S>& tf2,
797  S w)
798 {
799  return detail::setupMeshConservativeAdvancementOrientedDistanceNode(
800  node, model1, tf1, model2, tf2, w);
801 }
802 
803 } // namespace detail
804 } // namespace fcl
805 
806 #endif
const BVHModel< BV > * model1
The first BVH model.
Definition: bvh_distance_traversal_node.h:87
Definition: motion_base.h:52
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
int replaceSubModel(const std::vector< Vector3< S >> &ps)
Replace a set of points in the old BVH model.
Definition: BVH_model-inl.h:559
S rel_err
relative and absolute error, default value is 0.01 for both terms
Definition: mesh_distance_traversal_node.h:76
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:49
Definition: mesh_conservative_advancement_traversal_node.h:161
const MotionBase< S > * motion1
Motions for the two objects in query.
Definition: mesh_conservative_advancement_traversal_node.h:88
virtual S computeMotionBound(const BVMotionBoundVisitor< S > &mb_visitor) const =0
Compute the motion bound for a bounding volume, given the closest direction n between two query objec...
int beginReplaceModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
Definition: BVH_model-inl.h:508
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model-inl.h:48
S w
CA controlling variable: early stop for the early iterations of CA.
Definition: mesh_conservative_advancement_traversal_node.h:78
const BVNode< BV > & getBV(int id) const
We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some...
Definition: BVH_model-inl.h:160
continuous collision node using conservative advancement. when using this default version...
Definition: mesh_conservative_advancement_traversal_node.h:53
Vector3< S > * vertices
Geometry point data.
Definition: BVH_model.h:162
int num_vertices
Number of points.
Definition: BVH_model.h:174
Definition: tbv_motion_bound_visitor.h:65
int primitiveId() const
Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices)...
Definition: BV_node_base.cpp:50
Triangle * tri_indices
Geometry triangle index data, will be nullptr for point clouds.
Definition: BVH_model.h:165
Triangle with 3 indices for points.
Definition: triangle.h:47
Definition: mesh_conservative_advancement_traversal_node.h:111
unknown model type
Definition: BVH_internal.h:78
S delta_t
The delta_t each step.
Definition: mesh_conservative_advancement_traversal_node.h:85
OBB< S > obb
OBB member, for rotation.
Definition: OBBRSS.h:57
for OBB and RSS, there is local coordinate of BV, so normal need to be transformed ...
Definition: mesh_conservative_advancement_traversal_node-inl.h:475
Definition: bv_motion_bound_visitor.h:45
Definition: conservative_advancement_stack_data.h:50
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
const BVHModel< BV > * model2
The second BVH model.
Definition: bvh_distance_traversal_node.h:89
Matrix3< S > axis
Orientation of OBB. The axes of the rotation matrix are the principle directions of the box...
Definition: OBB.h:62
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:52
Triangle distance functions.
Definition: triangle_distance.h:51
Definition: mesh_conservative_advancement_traversal_node-inl.h:169
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
int endReplaceModel(bool refit=true, bool bottomup=true)
End BVH model replacement, will also refit or rebuild the bounding volume hierarchy.
Definition: BVH_model-inl.h:577