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