conservative_advancement_func_matrix-inl.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-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_CONSERVATIVE_ADVANCEMENT_INL_H
39 #define FCL_CONSERVATIVE_ADVANCEMENT_INL_H
40 
41 #include "fcl/narrowphase/detail/conservative_advancement_func_matrix.h"
42 
43 #include "fcl/common/unused.h"
44 
45 #include "fcl/narrowphase/collision_object.h"
46 
47 #include "fcl/math/motion/motion_base.h"
48 
49 #include "fcl/geometry/bvh/BVH_model.h"
50 #include "fcl/geometry/shape/box.h"
51 #include "fcl/geometry/shape/capsule.h"
52 #include "fcl/geometry/shape/cone.h"
53 #include "fcl/geometry/shape/convex.h"
54 #include "fcl/geometry/shape/cylinder.h"
55 #include "fcl/geometry/shape/ellipsoid.h"
56 #include "fcl/geometry/shape/halfspace.h"
57 #include "fcl/geometry/shape/plane.h"
58 #include "fcl/geometry/shape/sphere.h"
59 #include "fcl/geometry/shape/triangle_p.h"
60 
61 #include "fcl/narrowphase/detail/traversal/traversal_recurse.h"
62 #include "fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h"
63 #include "fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node.h"
64 #include "fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h"
65 #include "fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h"
66 
67 namespace fcl
68 {
69 
70 namespace detail
71 {
72 
73 //==============================================================================
74 template<typename BV>
75 bool conservativeAdvancement(const BVHModel<BV>& o1,
76  const MotionBase<typename BV::S>* motion1,
77  const BVHModel<BV>& o2,
78  const MotionBase<typename BV::S>* motion2,
79  const CollisionRequest<typename BV::S>& request,
80  CollisionResult<typename BV::S>& result,
81  typename BV::S& toc)
82 {
83  using S = typename BV::S;
84 
85  Transform3<S> tf1;
86  Transform3<S> tf2;
87  motion1->getCurrentTransform(tf1);
88  motion2->getCurrentTransform(tf2);
89 
90  // whether the first start configuration is in collision
91  if(collide(&o1, tf1, &o2, tf2, request, result))
92  {
93  toc = 0;
94  return true;
95  }
96 
97 
98  BVHModel<BV>* o1_tmp = new BVHModel<BV>(o1);
99  BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
100 
101 
102  MeshConservativeAdvancementTraversalNode<BV> node;
103 
104  node.motion1 = motion1;
105  node.motion2 = motion2;
106 
107  do
108  {
109  // repeatedly update mesh to global coordinate, so time consuming
110  initialize(node, *o1_tmp, tf1, *o2_tmp, tf2);
111 
112  node.delta_t = 1;
113  node.min_distance = std::numeric_limits<S>::max();
114 
115  distanceRecurse<S>(&node, 0, 0, nullptr);
116 
117  if(node.delta_t <= node.t_err)
118  {
119  // std::cout << node.delta_t << " " << node.t_err << std::endl;
120  break;
121  }
122 
123  node.toc += node.delta_t;
124  if(node.toc > 1)
125  {
126  node.toc = 1;
127  break;
128  }
129 
130  node.motion1->integrate(node.toc);
131  node.motion2->integrate(node.toc);
132 
133  motion1->getCurrentTransform(tf1);
134  motion2->getCurrentTransform(tf2);
135  }
136  while(1);
137 
138  delete o1_tmp;
139  delete o2_tmp;
140 
141  toc = node.toc;
142 
143  if(node.toc < 1)
144  return true;
145 
146  return false;
147 }
148 
149 template<typename BV, typename ConservativeAdvancementOrientedNode>
150 bool conservativeAdvancementMeshOriented(const BVHModel<BV>& o1,
151  const MotionBase<typename BV::S>* motion1,
152  const BVHModel<BV>& o2,
153  const MotionBase<typename BV::S>* motion2,
154  const CollisionRequest<typename BV::S>& request,
155  CollisionResult<typename BV::S>& result,
156  typename BV::S& toc)
157 {
158  using S = typename BV::S;
159 
160  Transform3<S> tf1;
161  Transform3<S> tf2;
162  motion1->getCurrentTransform(tf1);
163  motion2->getCurrentTransform(tf2);
164 
165  // whether the first start configuration is in collision
166  if(collide(&o1, tf1, &o2, tf2, request, result))
167  {
168  toc = 0;
169  return true;
170  }
171 
172 
173  ConservativeAdvancementOrientedNode node;
174 
175  initialize(node, o1, tf1, o2, tf2);
176 
177  node.motion1 = motion1;
178  node.motion2 = motion2;
179 
180  do
181  {
182  node.motion1->getCurrentTransform(tf1);
183  node.motion2->getCurrentTransform(tf2);
184 
185  // compute the transformation from 1 to 2
186  Transform3<S> tf = tf1.inverse(Eigen::Isometry) * tf2;
187  node.R = tf.linear();
188  node.T = tf.translation();
189 
190  node.delta_t = 1;
191  node.min_distance = std::numeric_limits<S>::max();
192 
193  distanceRecurse(&node, 0, 0, nullptr);
194 
195  if(node.delta_t <= node.t_err)
196  {
197  // std::cout << node.delta_t << " " << node.t_err << std::endl;
198  break;
199  }
200 
201  node.toc += node.delta_t;
202  if(node.toc > 1)
203  {
204  node.toc = 1;
205  break;
206  }
207 
208  node.motion1->integrate(node.toc);
209  node.motion2->integrate(node.toc);
210  }
211  while(1);
212 
213  toc = node.toc;
214 
215  if(node.toc < 1)
216  return true;
217 
218  return false;
219 }
220 
221 template<typename Shape1, typename Shape2, typename NarrowPhaseSolver>
222 bool conservativeAdvancement(const Shape1& o1,
223  const MotionBase<typename Shape1::S>* motion1,
224  const Shape2& o2,
225  const MotionBase<typename Shape1::S>* motion2,
226  const NarrowPhaseSolver* solver,
227  const CollisionRequest<typename Shape1::S>& request,
228  CollisionResult<typename Shape1::S>& result,
229  typename Shape1::S& toc)
230 {
231  using S = typename Shape1::S;
232 
233  Transform3<S> tf1;
234  Transform3<S> tf2;
235  motion1->getCurrentTransform(tf1);
236  motion2->getCurrentTransform(tf2);
237 
238  // whether the first start configuration is in collision
239  if(collide(&o1, tf1, &o2, tf2, request, result))
240  {
241  toc = 0;
242  return true;
243  }
244 
245  ShapeConservativeAdvancementTraversalNode<Shape1, Shape2, NarrowPhaseSolver> node;
246 
247  initialize(node, o1, tf1, o2, tf2, solver);
248 
249  node.motion1 = motion1;
250  node.motion2 = motion2;
251 
252  do
253  {
254  motion1->getCurrentTransform(tf1);
255  motion2->getCurrentTransform(tf2);
256  node.tf1 = tf1;
257  node.tf2 = tf2;
258 
259  node.delta_t = 1;
260  node.min_distance = std::numeric_limits<S>::max();
261 
262  distanceRecurse(&node, 0, 0, nullptr);
263 
264  if(node.delta_t <= node.t_err)
265  {
266  // std::cout << node.delta_t << " " << node.t_err << std::endl;
267  break;
268  }
269 
270  node.toc += node.delta_t;
271  if(node.toc > 1)
272  {
273  node.toc = 1;
274  break;
275  }
276 
277  node.motion1->integrate(node.toc);
278  node.motion2->integrate(node.toc);
279  }
280  while(1);
281 
282  toc = node.toc;
283 
284  if(node.toc < 1)
285  return true;
286 
287  return false;
288 }
289 
290 template<typename BV, typename Shape, typename NarrowPhaseSolver>
291 bool conservativeAdvancement(const BVHModel<BV>& o1,
292  const MotionBase<typename BV::S>* motion1,
293  const Shape& o2,
294  const MotionBase<typename BV::S>* motion2,
295  const NarrowPhaseSolver* nsolver,
296  const CollisionRequest<typename BV::S>& request,
297  CollisionResult<typename BV::S>& result,
298  typename BV::S& toc)
299 {
300  using S = typename BV::S;
301 
302  Transform3<S> tf1;
303  Transform3<S> tf2;
304  motion1->getCurrentTransform(tf1);
305  motion2->getCurrentTransform(tf2);
306 
307  if(collide(&o1, tf1, &o2, tf2, request, result))
308  {
309  toc = 0;
310  return true;
311  }
312 
313  BVHModel<BV>* o1_tmp = new BVHModel<BV>(o1);
314 
315  MeshShapeConservativeAdvancementTraversalNode<BV, Shape, NarrowPhaseSolver> node;
316 
317  node.motion1 = motion1;
318  node.motion2 = motion2;
319 
320  do
321  {
322  // initialize update mesh to global coordinate, so time consuming
323  initialize(node, *o1_tmp, tf1, o2, tf2, nsolver);
324 
325  node.delta_t = 1;
326  node.min_distance = std::numeric_limits<S>::max();
327 
328  distanceRecurse<S>(&node, 0, 0, nullptr);
329 
330  if(node.delta_t <= node.t_err)
331  {
332  break;
333  }
334 
335  node.toc += node.delta_t;
336  if(node.toc > 1)
337  {
338  node.toc = 1;
339  break;
340  }
341 
342  node.motion1->integrate(node.toc);
343  node.motion2->integrate(node.toc);
344 
345  motion1->getCurrentTransform(tf1);
346  motion2->getCurrentTransform(tf2);
347  }
348  while(1);
349 
350  delete o1_tmp;
351 
352  toc = node.toc;
353 
354  if(node.toc < 1)
355  return true;
356 
357  return false;
358 }
359 
360 template<typename BV, typename Shape, typename NarrowPhaseSolver, typename ConservativeAdvancementOrientedNode>
361 bool conservativeAdvancementMeshShapeOriented(const BVHModel<BV>& o1,
362  const MotionBase<typename BV::S>* motion1,
363  const Shape& o2,
364  const MotionBase<typename BV::S>* motion2,
365  const NarrowPhaseSolver* nsolver,
366  const CollisionRequest<typename BV::S>& request,
367  CollisionResult<typename BV::S>& result,
368  typename BV::S& toc)
369 {
370  using S = typename BV::S;
371 
372  Transform3<S> tf1;
373  Transform3<S> tf2;
374  motion1->getCurrentTransform(tf1);
375  motion2->getCurrentTransform(tf2);
376 
377  if(collide(&o1, tf1, &o2, tf2, request, result))
378  {
379  toc = 0;
380  return true;
381  }
382 
383  ConservativeAdvancementOrientedNode node;
384 
385  initialize(node, o1, tf1, o2, tf2, nsolver);
386 
387  node.motion1 = motion1;
388  node.motion2 = motion2;
389 
390  do
391  {
392  node.motion1->getCurrentTransform(tf1);
393  node.motion2->getCurrentTransform(tf2);
394 
395  node.tf1 = tf1;
396  node.tf2 = tf2;
397 
398  node.delta_t = 1;
399  node.min_distance = std::numeric_limits<S>::max();
400 
401  distanceRecurse(&node, 0, 0, nullptr);
402 
403  if(node.delta_t <= node.t_err)
404  {
405  break;
406  }
407 
408  node.toc += node.delta_t;
409  if(node.toc > 1)
410  {
411  node.toc = 1;
412  break;
413  }
414 
415  node.motion1->integrate(node.toc);
416  node.motion2->integrate(node.toc);
417  }
418  while(1);
419 
420  toc = node.toc;
421 
422  if(node.toc < 1)
423  return true;
424 
425  return false;
426 }
427 
428 template<typename Shape, typename NarrowPhaseSolver>
429 bool conservativeAdvancement(const BVHModel<RSS<typename Shape::S>>& o1,
430  const MotionBase<typename Shape::S>* motion1,
431  const Shape& o2,
432  const MotionBase<typename Shape::S>* motion2,
433  const NarrowPhaseSolver* nsolver,
434  const CollisionRequest<typename Shape::S>& request,
435  CollisionResult<typename Shape::S>& result,
436  typename Shape::S& toc)
437 {
438  using S = typename Shape::S;
439 
440  return detail::conservativeAdvancementMeshShapeOriented<RSS<S>, Shape, NarrowPhaseSolver, MeshShapeConservativeAdvancementTraversalNodeRSS<Shape, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
441 }
442 
443 template<typename Shape, typename NarrowPhaseSolver>
444 bool conservativeAdvancement(const BVHModel<OBBRSS<typename Shape::S>>& o1,
445  const MotionBase<typename Shape::S>* motion1,
446  const Shape& o2,
447  const MotionBase<typename Shape::S>* motion2,
448  const NarrowPhaseSolver* nsolver,
449  const CollisionRequest<typename Shape::S>& request,
450  CollisionResult<typename Shape::S>& result,
451  typename Shape::S& toc)
452 {
453  using S = typename Shape::S;
454 
455  return detail::conservativeAdvancementMeshShapeOriented<OBBRSS<S>, Shape, NarrowPhaseSolver, MeshShapeConservativeAdvancementTraversalNodeOBBRSS<Shape, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
456 }
457 
458 template<typename Shape, typename BV, typename NarrowPhaseSolver>
459 bool conservativeAdvancement(const Shape& o1,
460  const MotionBase<typename BV::S>* motion1,
461  const BVHModel<BV>& o2,
462  const MotionBase<typename BV::S>* motion2,
463  const NarrowPhaseSolver* nsolver,
464  const CollisionRequest<typename BV::S>& request,
465  CollisionResult<typename BV::S>& result,
466  typename BV::S& toc)
467 {
468  using S = typename BV::S;
469 
470  Transform3<S> tf1;
471  Transform3<S> tf2;
472  motion1->getCurrentTransform(tf1);
473  motion2->getCurrentTransform(tf2);
474 
475  if(collide(&o1, tf1, &o2, tf2, request, result))
476  {
477  toc = 0;
478  return true;
479  }
480 
481  BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
482 
483  ShapeMeshConservativeAdvancementTraversalNode<Shape, BV, NarrowPhaseSolver> node;
484 
485  node.motion1 = motion1;
486  node.motion2 = motion2;
487 
488  do
489  {
490  // initialize update mesh to global coordinate, so time consuming
491  initialize(node, o1, tf1, *o2_tmp, tf2, nsolver);
492 
493  node.delta_t = 1;
494  node.min_distance = std::numeric_limits<S>::max();
495 
496  distanceRecurse(&node, 0, 0, nullptr);
497 
498  if(node.delta_t <= node.t_err)
499  {
500  break;
501  }
502 
503  node.toc += node.delta_t;
504  if(node.toc > 1)
505  {
506  node.toc = 1;
507  break;
508  }
509 
510  node.motion1->integrate(node.toc);
511  node.motion2->integrate(node.toc);
512 
513  motion1->getCurrentTransform(tf1);
514  motion2->getCurrentTransform(tf2);
515  }
516  while(1);
517 
518  delete o2_tmp;
519 
520  toc = node.toc;
521 
522  if(node.toc < 1)
523  return true;
524 
525  return false;
526 }
527 
528 template<typename Shape, typename BV, typename NarrowPhaseSolver, typename ConservativeAdvancementOrientedNode>
529 bool conservativeAdvancementShapeMeshOriented(const Shape& o1,
530  const MotionBase<typename BV::S>* motion1,
531  const BVHModel<BV>& o2,
532  const MotionBase<typename BV::S>* motion2,
533  const NarrowPhaseSolver* nsolver,
534  const CollisionRequest<typename BV::S>& request,
535  CollisionResult<typename BV::S>& result,
536  typename BV::S& toc)
537 {
538  using S = typename BV::S;
539  Transform3<S> tf1;
540  Transform3<S> tf2;
541  motion1->getCurrentTransform(tf1);
542  motion2->getCurrentTransform(tf2);
543 
544  if(collide(&o1, tf1, &o2, tf2, request, result))
545  {
546  toc = 0;
547  return true;
548  }
549 
550  ConservativeAdvancementOrientedNode node;
551 
552  initialize(node, o1, tf1, o2, tf2, nsolver);
553 
554  node.motion1 = motion1;
555  node.motion2 = motion2;
556 
557  do
558  {
559  node.motion1->getCurrentTransform(tf1);
560  node.motion2->getCurrentTransform(tf2);
561 
562  node.tf1 = tf1;
563  node.tf2 = tf2;
564 
565  node.delta_t = 1;
566  node.min_distance = std::numeric_limits<S>::max();
567 
568  distanceRecurse(&node, 0, 0, nullptr);
569 
570  if(node.delta_t <= node.t_err)
571  {
572  break;
573  }
574 
575  node.toc += node.delta_t;
576  if(node.toc > 1)
577  {
578  node.toc = 1;
579  break;
580  }
581 
582  node.motion1->integrate(node.toc);
583  node.motion2->integrate(node.toc);
584  }
585  while(1);
586 
587  toc = node.toc;
588 
589  if(node.toc < 1)
590  return true;
591 
592  return false;
593 }
594 
595 //==============================================================================
596 template <typename S, typename Shape, typename NarrowPhaseSolver>
597 struct ConservativeAdvancementImpl
598 {
599  static bool run(
600  const Shape& o1,
601  const MotionBase<S>* motion1,
602  const BVHModel<RSS<S>>& o2,
603  const MotionBase<S>* motion2,
604  const NarrowPhaseSolver* nsolver,
605  const CollisionRequest<S>& request,
606  CollisionResult<S>& result,
607  S& toc)
608  {
609  return detail::conservativeAdvancementShapeMeshOriented<Shape, RSS<S>, NarrowPhaseSolver, ShapeMeshConservativeAdvancementTraversalNodeRSS<Shape, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
610  }
611 
612  static bool run(
613  const Shape& o1,
614  const MotionBase<S>* motion1,
615  const BVHModel<OBBRSS<S>>& o2,
616  const MotionBase<S>* motion2,
617  const NarrowPhaseSolver* nsolver,
618  const CollisionRequest<S>& request,
619  CollisionResult<S>& result,
620  S& toc)
621  {
622  return detail::conservativeAdvancementShapeMeshOriented<Shape, OBBRSS<S>, NarrowPhaseSolver, ShapeMeshConservativeAdvancementTraversalNodeOBBRSS<Shape, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
623  }
624 };
625 
626 template<typename Shape, typename NarrowPhaseSolver>
627 bool conservativeAdvancement(const Shape& o1,
628  const MotionBase<typename Shape::S>* motion1,
629  const BVHModel<RSS<typename Shape::S>>& o2,
630  const MotionBase<typename Shape::S>* motion2,
631  const NarrowPhaseSolver* nsolver,
632  const CollisionRequest<typename Shape::S>& request,
633  CollisionResult<typename Shape::S>& result,
634  typename Shape::S& toc)
635 {
636  return ConservativeAdvancementImpl<
637  typename Shape::S, Shape, NarrowPhaseSolver>::run(
638  o1, motion1, o2, motion2, nsolver, request, result, toc);
639 }
640 
641 template<typename Shape, typename NarrowPhaseSolver>
642 bool conservativeAdvancement(const Shape& o1,
643  const MotionBase<typename Shape::S>* motion1,
644  const BVHModel<OBBRSS<typename Shape::S>>& o2,
645  const MotionBase<typename Shape::S>* motion2,
646  const NarrowPhaseSolver* nsolver,
647  const CollisionRequest<typename Shape::S>& request,
648  CollisionResult<typename Shape::S>& result,
649  typename Shape::S& toc)
650 {
651  return ConservativeAdvancementImpl<
652  typename Shape::S, Shape, NarrowPhaseSolver>::run(
653  o1, motion1, o2, motion2, nsolver, request, result, toc);
654 }
655 
656 //==============================================================================
657 template <typename S, typename NarrowPhaseSolver>
658 struct ConservativeAdvancementImpl<S, BVHModel<RSS<S>>, NarrowPhaseSolver>
659 {
660  static bool run(
661  const BVHModel<RSS<S>>& o1,
662  const MotionBase<S>* motion1,
663  const BVHModel<RSS<S>>& o2,
664  const MotionBase<S>* motion2,
665  const NarrowPhaseSolver* /*nsolver*/,
666  const CollisionRequest<S>& request,
667  CollisionResult<S>& result,
668  S& toc)
669  {
670  return detail::conservativeAdvancementMeshOriented<RSS<S>, MeshConservativeAdvancementTraversalNodeRSS<S>>(o1, motion1, o2, motion2, request, result, toc);
671  }
672 };
673 
674 //==============================================================================
675 template <typename S, typename NarrowPhaseSolver>
676 struct ConservativeAdvancementImpl<S, BVHModel<OBBRSS<S>>, NarrowPhaseSolver>
677 {
678  static bool run(
679  const BVHModel<OBBRSS<S>>& o1,
680  const MotionBase<S>* motion1,
681  const BVHModel<OBBRSS<S>>& o2,
682  const MotionBase<S>* motion2,
683  const NarrowPhaseSolver* /*nsolver*/,
684  const CollisionRequest<S>& request,
685  CollisionResult<S>& result,
686  S& toc)
687  {
688  return detail::conservativeAdvancementMeshOriented<OBBRSS<S>, MeshConservativeAdvancementTraversalNodeOBBRSS<S>>(o1, motion1, o2, motion2, request, result, toc);
689  }
690 };
691 
692 template<typename BV, typename NarrowPhaseSolver>
693 typename BV::S BVHConservativeAdvancement(const CollisionGeometry<typename BV::S>* o1, const MotionBase<typename BV::S>* motion1, const CollisionGeometry<typename BV::S>* o2, const MotionBase<typename BV::S>* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest<typename BV::S>& request, ContinuousCollisionResult<typename BV::S>& result)
694 {
695  FCL_UNUSED(nsolver);
696  FCL_UNUSED(request);
697 
698  using S = typename BV::S;
699 
700  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
701  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
702 
703  CollisionRequest<S> c_request;
704  CollisionResult<S> c_result;
705  S toc;
706  bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, c_request, c_result, toc);
707 
708  result.is_collide = is_collide;
709  result.time_of_contact = toc;
710 
711  return toc;
712 }
713 
714 template<typename Shape1, typename Shape2, typename NarrowPhaseSolver>
715 typename Shape1::S ShapeConservativeAdvancement(const CollisionGeometry<typename Shape1::S>* o1, const MotionBase<typename Shape1::S>* motion1, const CollisionGeometry<typename Shape1::S>* o2, const MotionBase<typename Shape1::S>* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest<typename Shape1::S>& request, ContinuousCollisionResult<typename Shape1::S>& result)
716 {
717  FCL_UNUSED(request);
718 
719  using S = typename Shape1::S;
720 
721  const Shape1* obj1 = static_cast<const Shape1*>(o1);
722  const Shape2* obj2 = static_cast<const Shape2*>(o2);
723 
724  CollisionRequest<S> c_request;
725  CollisionResult<S> c_result;
726  S toc;
727  bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
728 
729  result.is_collide = is_collide;
730  result.time_of_contact = toc;
731 
732  return toc;
733 }
734 
735 template<typename Shape, typename BV, typename NarrowPhaseSolver>
736 typename BV::S ShapeBVHConservativeAdvancement(const CollisionGeometry<typename BV::S>* o1, const MotionBase<typename BV::S>* motion1, const CollisionGeometry<typename BV::S>* o2, const MotionBase<typename BV::S>* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest<typename BV::S>& request, ContinuousCollisionResult<typename BV::S>& result)
737 {
738  FCL_UNUSED(request);
739 
740  using S = typename BV::S;
741 
742  const Shape* obj1 = static_cast<const Shape*>(o1);
743  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
744 
745  CollisionRequest<S> c_request;
746  CollisionResult<S> c_result;
747  S toc;
748 
749  bool is_collide = conservativeAdvancement<Shape, BV, NarrowPhaseSolver>(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
750 
751  result.is_collide = is_collide;
752  result.time_of_contact = toc;
753 
754  return toc;
755 }
756 
757 template<typename BV, typename Shape, typename NarrowPhaseSolver>
758 typename BV::S BVHShapeConservativeAdvancement(const CollisionGeometry<typename BV::S>* o1, const MotionBase<typename BV::S>* motion1, const CollisionGeometry<typename BV::S>* o2, const MotionBase<typename BV::S>* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest<typename BV::S>& request, ContinuousCollisionResult<typename BV::S>& result)
759 {
760  FCL_UNUSED(request);
761 
762  using S = typename BV::S;
763 
764  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
765  const Shape* obj2 = static_cast<const Shape*>(o2);
766 
767  CollisionRequest<S> c_request;
768  CollisionResult<S> c_result;
769  S toc;
770 
771  bool is_collide = conservativeAdvancement<BV, Shape, NarrowPhaseSolver>(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
772 
773  result.is_collide = is_collide;
774  result.time_of_contact = toc;
775 
776  return toc;
777 }
778 
779 template<typename NarrowPhaseSolver>
780 ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancementFunctionMatrix()
781 {
782  using S = typename NarrowPhaseSolver::S;
783 
784  for(int i = 0; i < NODE_COUNT; ++i)
785  {
786  for(int j = 0; j < NODE_COUNT; ++j)
787  conservative_advancement_matrix[i][j] = nullptr;
788  }
789 
790 
791  conservative_advancement_matrix[GEOM_BOX][GEOM_BOX] = &ShapeConservativeAdvancement<Box<S>, Box<S>, NarrowPhaseSolver>;
792  conservative_advancement_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeConservativeAdvancement<Box<S>, Sphere<S>, NarrowPhaseSolver>;
793  conservative_advancement_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Box<S>, Capsule<S>, NarrowPhaseSolver>;
794  conservative_advancement_matrix[GEOM_BOX][GEOM_CONE] = &ShapeConservativeAdvancement<Box<S>, Cone<S>, NarrowPhaseSolver>;
795  conservative_advancement_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Box<S>, Cylinder<S>, NarrowPhaseSolver>;
796  conservative_advancement_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Box<S>, Convex<S>, NarrowPhaseSolver>;
797  conservative_advancement_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeConservativeAdvancement<Box<S>, Plane<S>, NarrowPhaseSolver>;
798  conservative_advancement_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Box<S>, Halfspace<S>, NarrowPhaseSolver>;
799 
800  conservative_advancement_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeConservativeAdvancement<Sphere<S>, Box<S>, NarrowPhaseSolver>;
801  conservative_advancement_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Sphere<S>, Sphere<S>, NarrowPhaseSolver>;
802  conservative_advancement_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Sphere<S>, Capsule<S>, NarrowPhaseSolver>;
803  conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeConservativeAdvancement<Sphere<S>, Cone<S>, NarrowPhaseSolver>;
804  conservative_advancement_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Sphere<S>, Cylinder<S>, NarrowPhaseSolver>;
805  conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Sphere<S>, Convex<S>, NarrowPhaseSolver>;
806  conservative_advancement_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeConservativeAdvancement<Sphere<S>, Plane<S>, NarrowPhaseSolver>;
807  conservative_advancement_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Sphere<S>, Halfspace<S>, NarrowPhaseSolver>;
808 
809  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeConservativeAdvancement<Capsule<S>, Box<S>, NarrowPhaseSolver>;
810  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Capsule<S>, Sphere<S>, NarrowPhaseSolver>;
811  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Capsule<S>, Capsule<S>, NarrowPhaseSolver>;
812  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeConservativeAdvancement<Capsule<S>, Cone<S>, NarrowPhaseSolver>;
813  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Capsule<S>, Cylinder<S>, NarrowPhaseSolver>;
814  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Capsule<S>, Convex<S>, NarrowPhaseSolver>;
815  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeConservativeAdvancement<Capsule<S>, Plane<S>, NarrowPhaseSolver>;
816  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Capsule<S>, Halfspace<S>, NarrowPhaseSolver>;
817 
818  conservative_advancement_matrix[GEOM_CONE][GEOM_BOX] = &ShapeConservativeAdvancement<Cone<S>, Box<S>, NarrowPhaseSolver>;
819  conservative_advancement_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cone<S>, Sphere<S>, NarrowPhaseSolver>;
820  conservative_advancement_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Cone<S>, Capsule<S>, NarrowPhaseSolver>;
821  conservative_advancement_matrix[GEOM_CONE][GEOM_CONE] = &ShapeConservativeAdvancement<Cone<S>, Cone<S>, NarrowPhaseSolver>;
822  conservative_advancement_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cone<S>, Cylinder<S>, NarrowPhaseSolver>;
823  conservative_advancement_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cone<S>, Convex<S>, NarrowPhaseSolver>;
824  conservative_advancement_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeConservativeAdvancement<Cone<S>, Plane<S>, NarrowPhaseSolver>;
825  conservative_advancement_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Cone<S>, Halfspace<S>, NarrowPhaseSolver>;
826 
827  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeConservativeAdvancement<Cylinder<S>, Box<S>, NarrowPhaseSolver>;
828  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cylinder<S>, Sphere<S>, NarrowPhaseSolver>;
829  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Cylinder<S>, Capsule<S>, NarrowPhaseSolver>;
830  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeConservativeAdvancement<Cylinder<S>, Cone<S>, NarrowPhaseSolver>;
831  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cylinder<S>, Cylinder<S>, NarrowPhaseSolver>;
832  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cylinder<S>, Convex<S>, NarrowPhaseSolver>;
833  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeConservativeAdvancement<Cylinder<S>, Plane<S>, NarrowPhaseSolver>;
834  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Cylinder<S>, Halfspace<S>, NarrowPhaseSolver>;
835 
836  conservative_advancement_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeConservativeAdvancement<Convex<S>, Box<S>, NarrowPhaseSolver>;
837  conservative_advancement_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeConservativeAdvancement<Convex<S>, Sphere<S>, NarrowPhaseSolver>;
838  conservative_advancement_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Convex<S>, Capsule<S>, NarrowPhaseSolver>;
839  conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeConservativeAdvancement<Convex<S>, Cone<S>, NarrowPhaseSolver>;
840  conservative_advancement_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Convex<S>, Cylinder<S>, NarrowPhaseSolver>;
841  conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Convex<S>, Convex<S>, NarrowPhaseSolver>;
842  conservative_advancement_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeConservativeAdvancement<Convex<S>, Plane<S>, NarrowPhaseSolver>;
843  conservative_advancement_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Convex<S>, Halfspace<S>, NarrowPhaseSolver>;
844 
845  conservative_advancement_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeConservativeAdvancement<Plane<S>, Box<S>, NarrowPhaseSolver>;
846  conservative_advancement_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Plane<S>, Sphere<S>, NarrowPhaseSolver>;
847  conservative_advancement_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Plane<S>, Capsule<S>, NarrowPhaseSolver>;
848  conservative_advancement_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeConservativeAdvancement<Plane<S>, Cone<S>, NarrowPhaseSolver>;
849  conservative_advancement_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Plane<S>, Cylinder<S>, NarrowPhaseSolver>;
850  conservative_advancement_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Plane<S>, Convex<S>, NarrowPhaseSolver>;
851  conservative_advancement_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeConservativeAdvancement<Plane<S>, Plane<S>, NarrowPhaseSolver>;
852  conservative_advancement_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Plane<S>, Halfspace<S>, NarrowPhaseSolver>;
853 
854  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeConservativeAdvancement<Halfspace<S>, Box<S>, NarrowPhaseSolver>;
855  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Halfspace<S>, Sphere<S>, NarrowPhaseSolver>;
856  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Halfspace<S>, Capsule<S>, NarrowPhaseSolver>;
857  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeConservativeAdvancement<Halfspace<S>, Cone<S>, NarrowPhaseSolver>;
858  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Halfspace<S>, Cylinder<S>, NarrowPhaseSolver>;
859  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Halfspace<S>, Convex<S>, NarrowPhaseSolver>;
860  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeConservativeAdvancement<Halfspace<S>, Plane<S>, NarrowPhaseSolver>;
861  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Halfspace<S>, Halfspace<S>, NarrowPhaseSolver>;
862 
863  conservative_advancement_matrix[BV_AABB][GEOM_BOX] = &BVHShapeConservativeAdvancement<AABB<S>, Box<S>, NarrowPhaseSolver>;
864  conservative_advancement_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<AABB<S>, Sphere<S>, NarrowPhaseSolver>;
865  conservative_advancement_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<AABB<S>, Capsule<S>, NarrowPhaseSolver>;
866  conservative_advancement_matrix[BV_AABB][GEOM_CONE] = &BVHShapeConservativeAdvancement<AABB<S>, Cone<S>, NarrowPhaseSolver>;
867  conservative_advancement_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<AABB<S>, Cylinder<S>, NarrowPhaseSolver>;
868  conservative_advancement_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<AABB<S>, Convex<S>, NarrowPhaseSolver>;
869  conservative_advancement_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<AABB<S>, Plane<S>, NarrowPhaseSolver>;
870  conservative_advancement_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<AABB<S>, Halfspace<S>, NarrowPhaseSolver>;
871 
872  conservative_advancement_matrix[BV_OBB][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBB<S>, Box<S>, NarrowPhaseSolver>;
873  conservative_advancement_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBB<S>, Sphere<S>, NarrowPhaseSolver>;
874  conservative_advancement_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<OBB<S>, Capsule<S>, NarrowPhaseSolver>;
875  conservative_advancement_matrix[BV_OBB][GEOM_CONE] = &BVHShapeConservativeAdvancement<OBB<S>, Cone<S>, NarrowPhaseSolver>;
876  conservative_advancement_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBB<S>, Cylinder<S>, NarrowPhaseSolver>;
877  conservative_advancement_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBB<S>, Convex<S>, NarrowPhaseSolver>;
878  conservative_advancement_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBB<S>, Plane<S>, NarrowPhaseSolver>;
879  conservative_advancement_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<OBB<S>, Halfspace<S>, NarrowPhaseSolver>;
880 
881  conservative_advancement_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBBRSS<S>, Box<S>, NarrowPhaseSolver>;
882  conservative_advancement_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBBRSS<S>, Sphere<S>, NarrowPhaseSolver>;
883  conservative_advancement_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<OBBRSS<S>, Capsule<S>, NarrowPhaseSolver>;
884  conservative_advancement_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeConservativeAdvancement<OBBRSS<S>, Cone<S>, NarrowPhaseSolver>;
885  conservative_advancement_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBBRSS<S>, Cylinder<S>, NarrowPhaseSolver>;
886  conservative_advancement_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBBRSS<S>, Convex<S>, NarrowPhaseSolver>;
887  conservative_advancement_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBBRSS<S>, Plane<S>, NarrowPhaseSolver>;
888  conservative_advancement_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<OBBRSS<S>, Halfspace<S>, NarrowPhaseSolver>;
889 
890  conservative_advancement_matrix[BV_RSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<RSS<S>, Box<S>, NarrowPhaseSolver>;
891  conservative_advancement_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<RSS<S>, Sphere<S>, NarrowPhaseSolver>;
892  conservative_advancement_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<RSS<S>, Capsule<S>, NarrowPhaseSolver>;
893  conservative_advancement_matrix[BV_RSS][GEOM_CONE] = &BVHShapeConservativeAdvancement<RSS<S>, Cone<S>, NarrowPhaseSolver>;
894  conservative_advancement_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<RSS<S>, Cylinder<S>, NarrowPhaseSolver>;
895  conservative_advancement_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<RSS<S>, Convex<S>, NarrowPhaseSolver>;
896  conservative_advancement_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<RSS<S>, Plane<S>, NarrowPhaseSolver>;
897  conservative_advancement_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<RSS<S>, Halfspace<S>, NarrowPhaseSolver>;
898 
899  conservative_advancement_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<S, 16>, Box<S>, NarrowPhaseSolver>;
900  conservative_advancement_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<S, 16>, Sphere<S>, NarrowPhaseSolver>;
901  conservative_advancement_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<S, 16>, Capsule<S>, NarrowPhaseSolver>;
902  conservative_advancement_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<S, 16>, Cone<S>, NarrowPhaseSolver>;
903  conservative_advancement_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<S, 16>, Cylinder<S>, NarrowPhaseSolver>;
904  conservative_advancement_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<S, 16>, Convex<S>, NarrowPhaseSolver>;
905  conservative_advancement_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<S, 16>, Plane<S>, NarrowPhaseSolver>;
906  conservative_advancement_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<S, 16>, Halfspace<S>, NarrowPhaseSolver>;
907 
908  conservative_advancement_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<S, 18>, Box<S>, NarrowPhaseSolver>;
909  conservative_advancement_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<S, 18>, Sphere<S>, NarrowPhaseSolver>;
910  conservative_advancement_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<S, 18>, Capsule<S>, NarrowPhaseSolver>;
911  conservative_advancement_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<S, 18>, Cone<S>, NarrowPhaseSolver>;
912  conservative_advancement_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<S, 18>, Cylinder<S>, NarrowPhaseSolver>;
913  conservative_advancement_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<S, 18>, Convex<S>, NarrowPhaseSolver>;
914  conservative_advancement_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<S, 18>, Plane<S>, NarrowPhaseSolver>;
915  conservative_advancement_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<S, 18>, Halfspace<S>, NarrowPhaseSolver>;
916 
917  conservative_advancement_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<S, 24>, Box<S>, NarrowPhaseSolver>;
918  conservative_advancement_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<S, 24>, Sphere<S>, NarrowPhaseSolver>;
919  conservative_advancement_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<S, 24>, Capsule<S>, NarrowPhaseSolver>;
920  conservative_advancement_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<S, 24>, Cone<S>, NarrowPhaseSolver>;
921  conservative_advancement_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<S, 24>, Cylinder<S>, NarrowPhaseSolver>;
922  conservative_advancement_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<S, 24>, Convex<S>, NarrowPhaseSolver>;
923  conservative_advancement_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<S, 24>, Plane<S>, NarrowPhaseSolver>;
924  conservative_advancement_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<S, 24>, Halfspace<S>, NarrowPhaseSolver>;
925 
926  conservative_advancement_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeConservativeAdvancement<kIOS<S>, Box<S>, NarrowPhaseSolver>;
927  conservative_advancement_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<kIOS<S>, Sphere<S>, NarrowPhaseSolver>;
928  conservative_advancement_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<kIOS<S>, Capsule<S>, NarrowPhaseSolver>;
929  conservative_advancement_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeConservativeAdvancement<kIOS<S>, Cone<S>, NarrowPhaseSolver>;
930  conservative_advancement_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<kIOS<S>, Cylinder<S>, NarrowPhaseSolver>;
931  conservative_advancement_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<kIOS<S>, Convex<S>, NarrowPhaseSolver>;
932  conservative_advancement_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<kIOS<S>, Plane<S>, NarrowPhaseSolver>;
933  conservative_advancement_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<kIOS<S>, Halfspace<S>, NarrowPhaseSolver>;
934 
935 
936  conservative_advancement_matrix[GEOM_BOX][BV_AABB] = &ShapeBVHConservativeAdvancement<Box<S>, AABB<S>, NarrowPhaseSolver>;
937  conservative_advancement_matrix[GEOM_SPHERE][BV_AABB] = &ShapeBVHConservativeAdvancement<Sphere<S>, AABB<S>, NarrowPhaseSolver>;
938  conservative_advancement_matrix[GEOM_CAPSULE][BV_AABB] = &ShapeBVHConservativeAdvancement<Capsule<S>, AABB<S>, NarrowPhaseSolver>;
939  conservative_advancement_matrix[GEOM_CONE][BV_AABB] = &ShapeBVHConservativeAdvancement<Cone<S>, AABB<S>, NarrowPhaseSolver>;
940  conservative_advancement_matrix[GEOM_CYLINDER][BV_AABB] = &ShapeBVHConservativeAdvancement<Cylinder<S>, AABB<S>, NarrowPhaseSolver>;
941  conservative_advancement_matrix[GEOM_CONVEX][BV_AABB] = &ShapeBVHConservativeAdvancement<Convex<S>, AABB<S>, NarrowPhaseSolver>;
942  conservative_advancement_matrix[GEOM_PLANE][BV_AABB] = &ShapeBVHConservativeAdvancement<Plane<S>, AABB<S>, NarrowPhaseSolver>;
943  conservative_advancement_matrix[GEOM_HALFSPACE][BV_AABB] = &ShapeBVHConservativeAdvancement<Halfspace<S>, AABB<S>, NarrowPhaseSolver>;
944 
945  conservative_advancement_matrix[GEOM_BOX][BV_OBB] = &ShapeBVHConservativeAdvancement<Box<S>, OBB<S>, NarrowPhaseSolver>;
946  conservative_advancement_matrix[GEOM_SPHERE][BV_OBB] = &ShapeBVHConservativeAdvancement<Sphere<S>, OBB<S>, NarrowPhaseSolver>;
947  conservative_advancement_matrix[GEOM_CAPSULE][BV_OBB] = &ShapeBVHConservativeAdvancement<Capsule<S>, OBB<S>, NarrowPhaseSolver>;
948  conservative_advancement_matrix[GEOM_CONE][BV_OBB] = &ShapeBVHConservativeAdvancement<Cone<S>, OBB<S>, NarrowPhaseSolver>;
949  conservative_advancement_matrix[GEOM_CYLINDER][BV_OBB] = &ShapeBVHConservativeAdvancement<Cylinder<S>, OBB<S>, NarrowPhaseSolver>;
950  conservative_advancement_matrix[GEOM_CONVEX][BV_OBB] = &ShapeBVHConservativeAdvancement<Convex<S>, OBB<S>, NarrowPhaseSolver>;
951  conservative_advancement_matrix[GEOM_PLANE][BV_OBB] = &ShapeBVHConservativeAdvancement<Plane<S>, OBB<S>, NarrowPhaseSolver>;
952  conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBB] = &ShapeBVHConservativeAdvancement<Halfspace<S>, OBB<S>, NarrowPhaseSolver>;
953 
954  conservative_advancement_matrix[GEOM_BOX][BV_RSS] = &ShapeBVHConservativeAdvancement<Box<S>, RSS<S>, NarrowPhaseSolver>;
955  conservative_advancement_matrix[GEOM_SPHERE][BV_RSS] = &ShapeBVHConservativeAdvancement<Sphere<S>, RSS<S>, NarrowPhaseSolver>;
956  conservative_advancement_matrix[GEOM_CAPSULE][BV_RSS] = &ShapeBVHConservativeAdvancement<Capsule<S>, RSS<S>, NarrowPhaseSolver>;
957  conservative_advancement_matrix[GEOM_CONE][BV_RSS] = &ShapeBVHConservativeAdvancement<Cone<S>, RSS<S>, NarrowPhaseSolver>;
958  conservative_advancement_matrix[GEOM_CYLINDER][BV_RSS] = &ShapeBVHConservativeAdvancement<Cylinder<S>, RSS<S>, NarrowPhaseSolver>;
959  conservative_advancement_matrix[GEOM_CONVEX][BV_RSS] = &ShapeBVHConservativeAdvancement<Convex<S>, RSS<S>, NarrowPhaseSolver>;
960  conservative_advancement_matrix[GEOM_PLANE][BV_RSS] = &ShapeBVHConservativeAdvancement<Plane<S>, RSS<S>, NarrowPhaseSolver>;
961  conservative_advancement_matrix[GEOM_HALFSPACE][BV_RSS] = &ShapeBVHConservativeAdvancement<Halfspace<S>, RSS<S>, NarrowPhaseSolver>;
962 
963  conservative_advancement_matrix[GEOM_BOX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Box<S>, OBBRSS<S>, NarrowPhaseSolver>;
964  conservative_advancement_matrix[GEOM_SPHERE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Sphere<S>, OBBRSS<S>, NarrowPhaseSolver>;
965  conservative_advancement_matrix[GEOM_CAPSULE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Capsule<S>, OBBRSS<S>, NarrowPhaseSolver>;
966  conservative_advancement_matrix[GEOM_CONE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Cone<S>, OBBRSS<S>, NarrowPhaseSolver>;
967  conservative_advancement_matrix[GEOM_CYLINDER][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Cylinder<S>, OBBRSS<S>, NarrowPhaseSolver>;
968  conservative_advancement_matrix[GEOM_CONVEX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Convex<S>, OBBRSS<S>, NarrowPhaseSolver>;
969  conservative_advancement_matrix[GEOM_PLANE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Plane<S>, OBBRSS<S>, NarrowPhaseSolver>;
970  conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Halfspace<S>, OBBRSS<S>, NarrowPhaseSolver>;
971 
972  conservative_advancement_matrix[GEOM_BOX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Box<S>, KDOP<S, 16>, NarrowPhaseSolver>;
973  conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Sphere<S>, KDOP<S, 16>, NarrowPhaseSolver>;
974  conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Capsule<S>, KDOP<S, 16>, NarrowPhaseSolver>;
975  conservative_advancement_matrix[GEOM_CONE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Cone<S>, KDOP<S, 16>, NarrowPhaseSolver>;
976  conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Cylinder<S>, KDOP<S, 16>, NarrowPhaseSolver>;
977  conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Convex<S>, KDOP<S, 16>, NarrowPhaseSolver>;
978  conservative_advancement_matrix[GEOM_PLANE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Plane<S>, KDOP<S, 16>, NarrowPhaseSolver>;
979  conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Halfspace<S>, KDOP<S, 16>, NarrowPhaseSolver>;
980 
981  conservative_advancement_matrix[GEOM_BOX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Box<S>, KDOP<S, 18>, NarrowPhaseSolver>;
982  conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Sphere<S>, KDOP<S, 18>, NarrowPhaseSolver>;
983  conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Capsule<S>, KDOP<S, 18>, NarrowPhaseSolver>;
984  conservative_advancement_matrix[GEOM_CONE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Cone<S>, KDOP<S, 18>, NarrowPhaseSolver>;
985  conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Cylinder<S>, KDOP<S, 18>, NarrowPhaseSolver>;
986  conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Convex<S>, KDOP<S, 18>, NarrowPhaseSolver>;
987  conservative_advancement_matrix[GEOM_PLANE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Plane<S>, KDOP<S, 18>, NarrowPhaseSolver>;
988  conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Halfspace<S>, KDOP<S, 18>, NarrowPhaseSolver>;
989 
990  conservative_advancement_matrix[GEOM_BOX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Box<S>, KDOP<S, 24>, NarrowPhaseSolver>;
991  conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Sphere<S>, KDOP<S, 24>, NarrowPhaseSolver>;
992  conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Capsule<S>, KDOP<S, 24>, NarrowPhaseSolver>;
993  conservative_advancement_matrix[GEOM_CONE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Cone<S>, KDOP<S, 24>, NarrowPhaseSolver>;
994  conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Cylinder<S>, KDOP<S, 24>, NarrowPhaseSolver>;
995  conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Convex<S>, KDOP<S, 24>, NarrowPhaseSolver>;
996  conservative_advancement_matrix[GEOM_PLANE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Plane<S>, KDOP<S, 24>, NarrowPhaseSolver>;
997  conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Halfspace<S>, KDOP<S, 24>, NarrowPhaseSolver>;
998 
999  conservative_advancement_matrix[GEOM_BOX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Box<S>, kIOS<S>, NarrowPhaseSolver>;
1000  conservative_advancement_matrix[GEOM_SPHERE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Sphere<S>, kIOS<S>, NarrowPhaseSolver>;
1001  conservative_advancement_matrix[GEOM_CAPSULE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Capsule<S>, kIOS<S>, NarrowPhaseSolver>;
1002  conservative_advancement_matrix[GEOM_CONE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Cone<S>, kIOS<S>, NarrowPhaseSolver>;
1003  conservative_advancement_matrix[GEOM_CYLINDER][BV_kIOS] = &ShapeBVHConservativeAdvancement<Cylinder<S>, kIOS<S>, NarrowPhaseSolver>;
1004  conservative_advancement_matrix[GEOM_CONVEX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Convex<S>, kIOS<S>, NarrowPhaseSolver>;
1005  conservative_advancement_matrix[GEOM_PLANE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Plane<S>, kIOS<S>, NarrowPhaseSolver>;
1006  conservative_advancement_matrix[GEOM_HALFSPACE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Halfspace<S>, kIOS<S>, NarrowPhaseSolver>;
1007 
1008  conservative_advancement_matrix[BV_AABB][BV_AABB] = &BVHConservativeAdvancement<AABB<S>, NarrowPhaseSolver>;
1009  conservative_advancement_matrix[BV_OBB][BV_OBB] = &BVHConservativeAdvancement<OBB<S>, NarrowPhaseSolver>;
1010  conservative_advancement_matrix[BV_RSS][BV_RSS] = &BVHConservativeAdvancement<RSS<S>, NarrowPhaseSolver>;
1011  conservative_advancement_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHConservativeAdvancement<OBBRSS<S>, NarrowPhaseSolver>;
1012  conservative_advancement_matrix[BV_KDOP16][BV_KDOP16] = &BVHConservativeAdvancement<KDOP<S, 16>, NarrowPhaseSolver>;
1013  conservative_advancement_matrix[BV_KDOP18][BV_KDOP18] = &BVHConservativeAdvancement<KDOP<S, 18>, NarrowPhaseSolver>;
1014  conservative_advancement_matrix[BV_KDOP24][BV_KDOP24] = &BVHConservativeAdvancement<KDOP<S, 24>, NarrowPhaseSolver>;
1015  conservative_advancement_matrix[BV_kIOS][BV_kIOS] = &BVHConservativeAdvancement<kIOS<S>, NarrowPhaseSolver>;
1016 
1017 }
1018 
1019 } // namespace detail
1020 } // namespace fcl
1021 
1022 #endif
fcl::detail::conservativeAdvancementShapeMeshOriented
bool conservativeAdvancementShapeMeshOriented(const Shape &o1, const MotionBase< typename BV::S > *motion1, const BVHModel< BV > &o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc)
Definition: conservative_advancement_func_matrix-inl.h:529
fcl::detail::BVHConservativeAdvancement
BV::S BVHConservativeAdvancement(const CollisionGeometry< typename BV::S > *o1, const MotionBase< typename BV::S > *motion1, const CollisionGeometry< typename BV::S > *o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const ContinuousCollisionRequest< typename BV::S > &request, ContinuousCollisionResult< typename BV::S > &result)
Definition: conservative_advancement_func_matrix-inl.h:693
fcl::Halfspace
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d...
Definition: geometry/shape/halfspace.h:60
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::CollisionResult
collision result
Definition: collision_request.h:48
fcl::RSS
A class for rectangle swept sphere bounding volume.
Definition: RSS.h:58
fcl::detail::ConservativeAdvancementImpl::run
static bool run(const Shape &o1, const MotionBase< S > *motion1, const BVHModel< OBBRSS< S >> &o2, const MotionBase< S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< S > &request, CollisionResult< S > &result, S &toc)
Definition: conservative_advancement_func_matrix-inl.h:612
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
obj1
CollisionObject< S > * obj1
Definition: broadphase_SaP.h:210
triangle_p.h
conservative_advancement_func_matrix.h
fcl::ContinuousCollisionResult::time_of_contact
S time_of_contact
time of contact in [0, 1]
Definition: continuous_collision_result.h:54
fcl::detail::ShapeConservativeAdvancement
Shape1::S ShapeConservativeAdvancement(const CollisionGeometry< typename Shape1::S > *o1, const MotionBase< typename Shape1::S > *motion1, const CollisionGeometry< typename Shape1::S > *o2, const MotionBase< typename Shape1::S > *motion2, const NarrowPhaseSolver *nsolver, const ContinuousCollisionRequest< typename Shape1::S > &request, ContinuousCollisionResult< typename Shape1::S > &result)
Definition: conservative_advancement_func_matrix-inl.h:715
shape_conservative_advancement_traversal_node.h
fcl::detail::ConservativeAdvancementImpl::run
static bool run(const Shape &o1, const MotionBase< S > *motion1, const BVHModel< RSS< S >> &o2, const MotionBase< S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< S > &request, CollisionResult< S > &result, S &toc)
Definition: conservative_advancement_func_matrix-inl.h:599
fcl::detail::TraversalNodeBase< Shape1::S >::tf2
Transform3< Shape1::S > tf2
configuration of second object
Definition: traversal_node_base.h:88
fcl::detail::MeshConservativeAdvancementTraversalNode
continuous collision node using conservative advancement. when using this default version...
Definition: mesh_conservative_advancement_traversal_node.h:53
fcl::detail::MeshShapeConservativeAdvancementTraversalNode
Traversal node for conservative advancement computation between BVH and shape.
Definition: mesh_shape_conservative_advancement_traversal_node.h:52
fcl::detail::ConservativeAdvancementImpl< S, BVHModel< OBBRSS< S > >, NarrowPhaseSolver >::run
static bool run(const BVHModel< OBBRSS< S >> &o1, const MotionBase< S > *motion1, const BVHModel< OBBRSS< S >> &o2, const MotionBase< S > *motion2, const NarrowPhaseSolver *, const CollisionRequest< S > &request, CollisionResult< S > &result, S &toc)
Definition: conservative_advancement_func_matrix-inl.h:678
fcl::detail::conservativeAdvancementMeshShapeOriented
bool conservativeAdvancementMeshShapeOriented(const BVHModel< BV > &o1, const MotionBase< typename BV::S > *motion1, const Shape &o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc)
Definition: conservative_advancement_func_matrix-inl.h:361
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
fcl::detail::BVHShapeConservativeAdvancement
BV::S BVHShapeConservativeAdvancement(const CollisionGeometry< typename BV::S > *o1, const MotionBase< typename BV::S > *motion1, const CollisionGeometry< typename BV::S > *o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const ContinuousCollisionRequest< typename BV::S > &request, ContinuousCollisionResult< typename BV::S > &result)
Definition: conservative_advancement_func_matrix-inl.h:758
fcl::detail::conservativeAdvancement
bool conservativeAdvancement(const BVHModel< BV > &o1, const MotionBase< typename BV::S > *motion1, const BVHModel< BV > &o2, const MotionBase< typename BV::S > *motion2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc)
Definition: conservative_advancement_func_matrix-inl.h:75
shape_mesh_conservative_advancement_traversal_node.h
mesh_conservative_advancement_traversal_node.h
traversal_recurse.h
fcl::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 The KDOP structure is defined by some pairs of parallel planes defined by some axes. For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 8 (0,-1,0) and (0,1,0) -> indices 1 and 9 (0,0,-1) and (0,0,1) -> indices 2 and 10 (-1,-1,0) and (1,1,0) -> indices 3 and 11 (-1,0,-1) and (1,0,1) -> indices 4 and 12 (0,-1,-1) and (0,1,1) -> indices 5 and 13 (-1,1,0) and (1,-1,0) -> indices 6 and 14 (-1,0,1) and (1,0,-1) -> indices 7 and 15 For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 9 (0,-1,0) and (0,1,0) -> indices 1 and 10 (0,0,-1) and (0,0,1) -> indices 2 and 11 (-1,-1,0) and (1,1,0) -> indices 3 and 12 (-1,0,-1) and (1,0,1) -> indices 4 and 13 (0,-1,-1) and (0,1,1) -> indices 5 and 14 (-1,1,0) and (1,-1,0) -> indices 6 and 15 (-1,0,1) and (1,0,-1) -> indices 7 and 16 (0,-1,1) and (0,1,-1) -> indices 8 and 17 For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 12 (0,-1,0) and (0,1,0) -> indices 1 and 13 (0,0,-1) and (0,0,1) -> indices 2 and 14 (-1,-1,0) and (1,1,0) -> indices 3 and 15 (-1,0,-1) and (1,0,1) -> indices 4 and 16 (0,-1,-1) and (0,1,1) -> indices 5 and 17 (-1,1,0) and (1,-1,0) -> indices 6 and 18 (-1,0,1) and (1,0,-1) -> indices 7 and 19 (0,-1,1) and (0,1,-1) -> indices 8 and 20 (-1, -1, 1) and (1, 1, -1) –> indices 9 and 21 (-1, 1, -1) and (1, -1, 1) –> indices 10 and 22 (1, -1, -1) and (-1, 1, 1) –> indices 11 and 23.
Definition: kDOP.h:84
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
fcl::detail::collide
template void collide(CollisionTraversalNodeBase< double > *node, BVHFrontList *front_list)
fcl::detail::initialize
template bool initialize(MeshCollisionTraversalNodeOBB< double > &node, const BVHModel< OBB< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBB< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
max
static T max(T x, T y)
Definition: svm.cpp:52
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
BVH_model.h
fcl::Plane
Infinite plane.
Definition: geometry/shape/plane.h:51
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:51
fcl::detail::distanceRecurse
template void distanceRecurse(DistanceTraversalNodeBase< double > *node, int b1, int b2, BVHFrontList *front_list)
fcl::detail::ShapeConservativeAdvancementTraversalNode::motion1
const MotionBase< S > * motion1
Motions for the two objects in query.
Definition: shape_conservative_advancement_traversal_node.h:70
fcl::detail::ShapeBVHConservativeAdvancement
BV::S ShapeBVHConservativeAdvancement(const CollisionGeometry< typename BV::S > *o1, const MotionBase< typename BV::S > *motion1, const CollisionGeometry< typename BV::S > *o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const ContinuousCollisionRequest< typename BV::S > &request, ContinuousCollisionResult< typename BV::S > &result)
Definition: conservative_advancement_func_matrix-inl.h:736
fcl::Cone
Center at zero cone.
Definition: cone.h:51
obj2
CollisionObject< S > * obj2
Definition: broadphase_SaP.h:211
mesh_shape_conservative_advancement_traversal_node.h
fcl::detail::TraversalNodeBase< Shape1::S >::tf1
Transform3< Shape1::S > tf1
configuation of first object
Definition: traversal_node_base.h:85
fcl::detail::ConservativeAdvancementImpl< S, BVHModel< RSS< S > >, NarrowPhaseSolver >::run
static bool run(const BVHModel< RSS< S >> &o1, const MotionBase< S > *motion1, const BVHModel< RSS< S >> &o2, const MotionBase< S > *motion2, const NarrowPhaseSolver *, const CollisionRequest< S > &request, CollisionResult< S > &result, S &toc)
Definition: conservative_advancement_func_matrix-inl.h:660
collision_object.h
fcl::MotionBase::getCurrentTransform
void getCurrentTransform(Matrix3< S > &R, Vector3< S > &T) const
Get the rotation and translation in current step.
Definition: motion_base-inl.h:64
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
motion_base.h
fcl::detail::conservativeAdvancementMeshOriented
bool conservativeAdvancementMeshOriented(const BVHModel< BV > &o1, const MotionBase< typename BV::S > *motion1, const BVHModel< BV > &o2, const MotionBase< typename BV::S > *motion2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc)
Definition: conservative_advancement_func_matrix-inl.h:150

fcl_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:17