FCL  0.6.0
Flexible Collision Library
collision_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_COLLISION_FUNC_MATRIX_INL_H
39 #define FCL_COLLISION_FUNC_MATRIX_INL_H
40 
41 #include "fcl/narrowphase/detail/collision_func_matrix.h"
42 
43 #include "fcl/config.h"
44 
45 #include "fcl/narrowphase/collision_object.h"
46 
47 #include "fcl/geometry/shape/box.h"
48 #include "fcl/geometry/shape/capsule.h"
49 #include "fcl/geometry/shape/cone.h"
50 #include "fcl/geometry/shape/convex.h"
51 #include "fcl/geometry/shape/cylinder.h"
52 #include "fcl/geometry/shape/ellipsoid.h"
53 #include "fcl/geometry/shape/halfspace.h"
54 #include "fcl/geometry/shape/plane.h"
55 #include "fcl/geometry/shape/sphere.h"
56 #include "fcl/geometry/shape/triangle_p.h"
57 #include "fcl/geometry/shape/utility.h"
58 
59 #include "fcl/narrowphase/detail/traversal/collision_node.h"
60 
61 #include "fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h"
62 #include "fcl/narrowphase/detail/traversal/collision/bvh_shape_collision_traversal_node.h"
63 #include "fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h"
64 #include "fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h"
65 #include "fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.h"
66 #include "fcl/narrowphase/detail/traversal/collision/mesh_shape_collision_traversal_node.h"
67 #include "fcl/narrowphase/detail/traversal/collision/shape_bvh_collision_traversal_node.h"
68 #include "fcl/narrowphase/detail/traversal/collision/shape_collision_traversal_node.h"
69 #include "fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node.h"
70 
71 #if FCL_HAVE_OCTOMAP
72 
73 #include "fcl/narrowphase/detail/traversal/octree/collision/mesh_octree_collision_traversal_node.h"
74 #include "fcl/narrowphase/detail/traversal/octree/collision/octree_collision_traversal_node.h"
75 #include "fcl/narrowphase/detail/traversal/octree/collision/octree_mesh_collision_traversal_node.h"
76 #include "fcl/narrowphase/detail/traversal/octree/collision/octree_shape_collision_traversal_node.h"
77 #include "fcl/narrowphase/detail/traversal/octree/collision/shape_octree_collision_traversal_node.h"
78 
79 #endif // FCL_HAVE_OCTOMAP
80 
81 namespace fcl
82 {
83 
84 namespace detail
85 {
86 
87 #if FCL_HAVE_OCTOMAP
88 
89 //==============================================================================
90 template <typename Shape, typename NarrowPhaseSolver>
91 std::size_t ShapeOcTreeCollide(
92  const CollisionGeometry<typename Shape::S>* o1,
93  const Transform3<typename Shape::S>& tf1,
94  const CollisionGeometry<typename Shape::S>* o2,
95  const Transform3<typename Shape::S>& tf2,
96  const NarrowPhaseSolver* nsolver,
97  const CollisionRequest<typename Shape::S>& request,
98  CollisionResult<typename Shape::S>& result)
99 {
100  using S = typename Shape::S;
101 
102  if(request.isSatisfied(result)) return result.numContacts();
103 
104  ShapeOcTreeCollisionTraversalNode<Shape, NarrowPhaseSolver> node;
105  const Shape* obj1 = static_cast<const Shape*>(o1);
106  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
107  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
108 
109  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
110  collide(&node);
111 
112  return result.numContacts();
113 }
114 
115 //==============================================================================
116 template <typename Shape, typename NarrowPhaseSolver>
117 std::size_t OcTreeShapeCollide(
118  const CollisionGeometry<typename Shape::S>* o1,
119  const Transform3<typename Shape::S>& tf1,
120  const CollisionGeometry<typename Shape::S>* o2,
121  const Transform3<typename Shape::S>& tf2,
122  const NarrowPhaseSolver* nsolver,
123  const CollisionRequest<typename Shape::S>& request,
124  CollisionResult<typename Shape::S>& result)
125 {
126  using S = typename Shape::S;
127 
128  if(request.isSatisfied(result)) return result.numContacts();
129 
130  OcTreeShapeCollisionTraversalNode<Shape, NarrowPhaseSolver> node;
131  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
132  const Shape* obj2 = static_cast<const Shape*>(o2);
133  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
134 
135  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
136  collide(&node);
137 
138  return result.numContacts();
139 }
140 
141 //==============================================================================
142 template <typename NarrowPhaseSolver>
143 std::size_t OcTreeCollide(
144  const CollisionGeometry<typename NarrowPhaseSolver::S>* o1,
145  const Transform3<typename NarrowPhaseSolver::S>& tf1,
146  const CollisionGeometry<typename NarrowPhaseSolver::S>* o2,
147  const Transform3<typename NarrowPhaseSolver::S>& tf2,
148  const NarrowPhaseSolver* nsolver,
149  const CollisionRequest<typename NarrowPhaseSolver::S>& request,
150  CollisionResult<typename NarrowPhaseSolver::S>& result)
151 {
152  using S = typename NarrowPhaseSolver::S;
153 
154  if(request.isSatisfied(result)) return result.numContacts();
155 
156  OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
157  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
158  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
159  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
160 
161  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
162  collide(&node);
163 
164  return result.numContacts();
165 }
166 
167 //==============================================================================
168 template <typename BV, typename NarrowPhaseSolver>
169 std::size_t OcTreeBVHCollide(
170  const CollisionGeometry<typename BV::S>* o1,
171  const Transform3<typename BV::S>& tf1,
172  const CollisionGeometry<typename BV::S>* o2,
173  const Transform3<typename BV::S>& tf2,
174  const NarrowPhaseSolver* nsolver,
175  const CollisionRequest<typename BV::S>& request,
176  CollisionResult<typename BV::S>& result)
177 {
178  using S = typename BV::S;
179 
180  if(request.isSatisfied(result)) return result.numContacts();
181 
182  if(request.enable_cost && request.use_approximate_cost)
183  {
184  CollisionRequest<S> no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree
185  no_cost_request.enable_cost = false; // disable cost computation
186 
187  OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver> node;
188  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
189  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
190  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
191 
192  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
193  collide(&node);
194 
195  Box<S> box;
196  Transform3<S> box_tf;
197  constructBox(obj2->getBV(0).bv, tf2, box, box_tf); // compute the box for BVH's root node
198 
199  box.cost_density = obj2->cost_density;
200  box.threshold_occupied = obj2->threshold_occupied;
201  box.threshold_free = obj2->threshold_free;
202 
203  CollisionRequest<S> only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); // additional cost request, no contacts
204  OcTreeShapeCollide<Box<S>, NarrowPhaseSolver>(o1, tf1, &box, box_tf, nsolver, only_cost_request, result);
205  }
206  else
207  {
208  OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver> node;
209  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
210  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
211  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
212 
213  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
214  collide(&node);
215  }
216 
217  return result.numContacts();
218 }
219 
220 //==============================================================================
221 template <typename BV, typename NarrowPhaseSolver>
222 std::size_t BVHOcTreeCollide(
223  const CollisionGeometry<typename BV::S>* o1,
224  const Transform3<typename BV::S>& tf1,
225  const CollisionGeometry<typename BV::S>* o2,
226  const Transform3<typename BV::S>& tf2,
227  const NarrowPhaseSolver* nsolver,
228  const CollisionRequest<typename BV::S>& request,
229  CollisionResult<typename BV::S>& result)
230 {
231  using S = typename BV::S;
232 
233  if(request.isSatisfied(result)) return result.numContacts();
234 
235  if(request.enable_cost && request.use_approximate_cost)
236  {
237  CollisionRequest<S> no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree
238  no_cost_request.enable_cost = false; // disable cost computation
239 
240  MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver> node;
241  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
242  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
243  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
244 
245  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
246  collide(&node);
247 
248  Box<S> box;
249  Transform3<S> box_tf;
250  constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
251 
252  box.cost_density = obj1->cost_density;
253  box.threshold_occupied = obj1->threshold_occupied;
254  box.threshold_free = obj1->threshold_free;
255 
256  CollisionRequest<S> only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
257  ShapeOcTreeCollide<Box<S>, NarrowPhaseSolver>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
258  }
259  else
260  {
261  MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver> node;
262  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
263  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
264  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
265 
266  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
267  collide(&node);
268  }
269 
270  return result.numContacts();
271 }
272 
273 #endif
274 
275 //==============================================================================
276 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
277 std::size_t ShapeShapeCollide(
278  const CollisionGeometry<typename Shape1::S>* o1,
279  const Transform3<typename Shape1::S>& tf1,
280  const CollisionGeometry<typename Shape1::S>* o2,
281  const Transform3<typename Shape1::S>& tf2,
282  const NarrowPhaseSolver* nsolver,
283  const CollisionRequest<typename Shape1::S>& request,
284  CollisionResult<typename Shape1::S>& result)
285 {
286  if(request.isSatisfied(result)) return result.numContacts();
287 
288  ShapeCollisionTraversalNode<Shape1, Shape2, NarrowPhaseSolver> node;
289  const Shape1* obj1 = static_cast<const Shape1*>(o1);
290  const Shape2* obj2 = static_cast<const Shape2*>(o2);
291 
292  if(request.enable_cached_gjk_guess)
293  {
294  nsolver->enableCachedGuess(true);
295  nsolver->setCachedGuess(request.cached_gjk_guess);
296  }
297  else
298  {
299  nsolver->enableCachedGuess(true);
300  }
301 
302  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
303  collide(&node);
304 
305  if(request.enable_cached_gjk_guess)
306  result.cached_gjk_guess = nsolver->getCachedGuess();
307 
308  return result.numContacts();
309 }
310 
311 //==============================================================================
312 template <typename BV, typename Shape, typename NarrowPhaseSolver>
314 {
315  using S = typename BV::S;
316 
317  static std::size_t collide(
318  const CollisionGeometry<S>* o1,
319  const Transform3<S>& tf1,
320  const CollisionGeometry<S>* o2,
321  const Transform3<S>& tf2,
322  const NarrowPhaseSolver* nsolver,
323  const CollisionRequest<S>& request,
324  CollisionResult<S>& result)
325  {
326  if(request.isSatisfied(result)) return result.numContacts();
327 
328  if(request.enable_cost && request.use_approximate_cost)
329  {
330  CollisionRequest<S> no_cost_request(request);
331  no_cost_request.enable_cost = false;
332 
334  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
335  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
336  Transform3<S> tf1_tmp = tf1;
337  const Shape* obj2 = static_cast<const Shape*>(o2);
338 
339  initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result);
340  fcl::detail::collide(&node);
341 
342  delete obj1_tmp;
343 
344  Box<S> box;
345  Transform3<S> box_tf;
346  constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
347 
348  box.cost_density = obj1->cost_density;
349  box.threshold_occupied = obj1->threshold_occupied;
350  box.threshold_free = obj1->threshold_free;
351 
352  CollisionRequest<S> only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
353  ShapeShapeCollide<Box<S>, Shape>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
354  }
355  else
356  {
358  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
359  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
360  Transform3<S> tf1_tmp = tf1;
361  const Shape* obj2 = static_cast<const Shape*>(o2);
362 
363  initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
364  fcl::detail::collide(&node);
365 
366  delete obj1_tmp;
367  }
368 
369  return result.numContacts();
370  }
371 };
372 
373 //==============================================================================
374 template <typename OrientMeshShapeCollisionTraveralNode,
375  typename BV, typename Shape, typename NarrowPhaseSolver>
376 std::size_t orientedBVHShapeCollide(
378  const Transform3<typename BV::S>& tf1,
380  const Transform3<typename BV::S>& tf2,
381  const NarrowPhaseSolver* nsolver,
382  const CollisionRequest<typename BV::S>& request,
384 {
385  using S = typename BV::S;
386 
387  if(request.isSatisfied(result)) return result.numContacts();
388 
389  if(request.enable_cost && request.use_approximate_cost)
390  {
391  CollisionRequest<S> no_cost_request(request);
392  no_cost_request.enable_cost = false;
393 
394  OrientMeshShapeCollisionTraveralNode node;
395  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
396  const Shape* obj2 = static_cast<const Shape*>(o2);
397 
398  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, no_cost_request, result);
399  fcl::detail::collide(&node);
400 
401  Box<S> box;
402  Transform3<S> box_tf;
403  constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
404 
405  box.cost_density = obj1->cost_density;
406  box.threshold_occupied = obj1->threshold_occupied;
407  box.threshold_free = obj1->threshold_free;
408 
409  CollisionRequest<S> only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
410  ShapeShapeCollide<Box<S>, Shape>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
411  }
412  else
413  {
414  OrientMeshShapeCollisionTraveralNode node;
415  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
416  const Shape* obj2 = static_cast<const Shape*>(o2);
417 
418  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
419  fcl::detail::collide(&node);
420  }
421 
422  return result.numContacts();
423 }
424 
425 //==============================================================================
426 template <typename Shape, typename NarrowPhaseSolver>
428  OBB<typename Shape::S>, Shape, NarrowPhaseSolver>
429 {
430  using S = typename Shape::S;
431 
432  static std::size_t collide(
433  const CollisionGeometry<S>* o1,
434  const Transform3<S>& tf1,
435  const CollisionGeometry<S>* o2,
436  const Transform3<S>& tf2,
437  const NarrowPhaseSolver* nsolver,
438  const CollisionRequest<S>& request,
439  CollisionResult<S>& result)
440  {
441  return detail::orientedBVHShapeCollide<
443  OBB<S>,
444  Shape,
445  NarrowPhaseSolver>(
446  o1, tf1, o2, tf2, nsolver, request, result);
447  }
448 };
449 
450 //==============================================================================
451 template <typename Shape, typename NarrowPhaseSolver>
452 struct BVHShapeCollider<RSS<typename Shape::S>, Shape, NarrowPhaseSolver>
453 {
454  using S = typename Shape::S;
455 
456  static std::size_t collide(
457  const CollisionGeometry<S>* o1,
458  const Transform3<S>& tf1,
459  const CollisionGeometry<S>* o2,
460  const Transform3<S>& tf2,
461  const NarrowPhaseSolver* nsolver,
462  const CollisionRequest<S>& request,
463  CollisionResult<S>& result)
464  {
465  return detail::orientedBVHShapeCollide<
467  RSS<S>,
468  Shape,
469  NarrowPhaseSolver>(
470  o1, tf1, o2, tf2, nsolver, request, result);
471  }
472 };
473 
474 //==============================================================================
475 template <typename Shape, typename NarrowPhaseSolver>
476 struct BVHShapeCollider<kIOS<typename Shape::S>, Shape, NarrowPhaseSolver>
477 {
478  using S = typename Shape::S;
479 
480  static std::size_t collide(
481  const CollisionGeometry<S>* o1,
482  const Transform3<S>& tf1,
483  const CollisionGeometry<S>* o2,
484  const Transform3<S>& tf2,
485  const NarrowPhaseSolver* nsolver,
486  const CollisionRequest<S>& request,
487  CollisionResult<S>& result)
488  {
489  return detail::orientedBVHShapeCollide<
491  kIOS<S>,
492  Shape,
493  NarrowPhaseSolver>(
494  o1, tf1, o2, tf2, nsolver, request, result);
495  }
496 };
497 
498 //==============================================================================
499 template <typename Shape, typename NarrowPhaseSolver>
500 struct BVHShapeCollider<OBBRSS<typename Shape::S>, Shape, NarrowPhaseSolver>
501 {
502  using S = typename Shape::S;
503 
504  static std::size_t collide(
505  const CollisionGeometry<S>* o1,
506  const Transform3<S>& tf1,
507  const CollisionGeometry<S>* o2,
508  const Transform3<S>& tf2,
509  const NarrowPhaseSolver* nsolver,
510  const CollisionRequest<S>& request,
511  CollisionResult<S>& result)
512  {
513  return detail::orientedBVHShapeCollide<
515  OBBRSS<S>,
516  Shape,
517  NarrowPhaseSolver>(
518  o1, tf1, o2, tf2, nsolver, request, result);
519  }
520 };
521 
522 //==============================================================================
523 template <typename S, typename BV>
525 {
526  static std::size_t run(
527  const CollisionGeometry<S>* o1,
528  const Transform3<S>& tf1,
529  const CollisionGeometry<S>* o2,
530  const Transform3<S>& tf2,
531  const CollisionRequest<S>& request,
532  CollisionResult<S>& result)
533  {
534  if(request.isSatisfied(result)) return result.numContacts();
535 
537  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
538  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>* >(o2);
539  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
540  Transform3<S> tf1_tmp = tf1;
541  BVHModel<BV>* obj2_tmp = new BVHModel<BV>(*obj2);
542  Transform3<S> tf2_tmp = tf2;
543 
544  initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
545  collide(&node);
546 
547  delete obj1_tmp;
548  delete obj2_tmp;
549 
550  return result.numContacts();
551  }
552 };
553 
554 //==============================================================================
555 template <typename BV>
556 std::size_t BVHCollide(
558  const Transform3<typename BV::S>& tf1,
560  const Transform3<typename BV::S>& tf2,
561  const CollisionRequest<typename BV::S>& request,
563 {
565  o1, tf1, o2, tf2, request, result);
566 }
567 
568 //==============================================================================
569 template <typename OrientedMeshCollisionTraversalNode, typename BV>
570 std::size_t orientedMeshCollide(
572  const Transform3<typename BV::S>& tf1,
574  const Transform3<typename BV::S>& tf2,
575  const CollisionRequest<typename BV::S>& request,
577 {
578  if(request.isSatisfied(result)) return result.numContacts();
579 
580  OrientedMeshCollisionTraversalNode node;
581  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
582  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>* >(o2);
583 
584  initialize(node, *obj1, tf1, *obj2, tf2, request, result);
585  collide(&node);
586 
587  return result.numContacts();
588 }
589 
590 //==============================================================================
591 template <typename S>
592 struct BVHCollideImpl<S, OBB<S>>
593 {
594  static std::size_t run(
595  const CollisionGeometry<S>* o1,
596  const Transform3<S>& tf1,
597  const CollisionGeometry<S>* o2,
598  const Transform3<S>& tf2,
599  const CollisionRequest<S>& request,
600  CollisionResult<S>& result)
601  {
602  return detail::orientedMeshCollide<
604  o1, tf1, o2, tf2, request, result);
605  }
606 };
607 
608 //==============================================================================
609 template <typename S>
610 struct BVHCollideImpl<S, OBBRSS<S>>
611 {
612  static std::size_t run(
613  const CollisionGeometry<S>* o1,
614  const Transform3<S>& tf1,
615  const CollisionGeometry<S>* o2,
616  const Transform3<S>& tf2,
617  const CollisionRequest<S>& request,
618  CollisionResult<S>& result)
619  {
620  return detail::orientedMeshCollide<
622  o1, tf1, o2, tf2, request, result);
623  }
624 };
625 
626 //==============================================================================
627 template <typename S>
628 struct BVHCollideImpl<S, kIOS<S>>
629 {
630  static std::size_t run(
631  const CollisionGeometry<S>* o1,
632  const Transform3<S>& tf1,
633  const CollisionGeometry<S>* o2,
634  const Transform3<S>& tf2,
635  const CollisionRequest<S>& request,
636  CollisionResult<S>& result)
637  {
638  return detail::orientedMeshCollide<
640  o1, tf1, o2, tf2, request, result);
641  }
642 };
643 
644 //==============================================================================
645 template <typename BV, typename NarrowPhaseSolver>
646 std::size_t BVHCollide(
648  const Transform3<typename BV::S>& tf1,
650  const Transform3<typename BV::S>& tf2,
651  const NarrowPhaseSolver* nsolver,
652  const CollisionRequest<typename BV::S>& request,
654 {
655  return BVHCollide<BV>(o1, tf1, o2, tf2, request, result);
656 }
657 
658 //==============================================================================
659 template <typename NarrowPhaseSolver>
661 {
662  using S = typename NarrowPhaseSolver::S;
663 
664  for(int i = 0; i < NODE_COUNT; ++i)
665  {
666  for(int j = 0; j < NODE_COUNT; ++j)
667  collision_matrix[i][j] = nullptr;
668  }
669 
670  collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box<S>, Box<S>, NarrowPhaseSolver>;
671  collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box<S>, Sphere<S>, NarrowPhaseSolver>;
672  collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeCollide<Box<S>, Ellipsoid<S>, NarrowPhaseSolver>;
673  collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box<S>, Capsule<S>, NarrowPhaseSolver>;
674  collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box<S>, Cone<S>, NarrowPhaseSolver>;
675  collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box<S>, Cylinder<S>, NarrowPhaseSolver>;
676  collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box<S>, Convex<S>, NarrowPhaseSolver>;
677  collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box<S>, Plane<S>, NarrowPhaseSolver>;
678  collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide<Box<S>, Halfspace<S>, NarrowPhaseSolver>;
679 
680  collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere<S>, Box<S>, NarrowPhaseSolver>;
681  collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere<S>, Sphere<S>, NarrowPhaseSolver>;
682  collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Sphere<S>, Ellipsoid<S>, NarrowPhaseSolver>;
683  collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere<S>, Capsule<S>, NarrowPhaseSolver>;
684  collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere<S>, Cone<S>, NarrowPhaseSolver>;
685  collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere<S>, Cylinder<S>, NarrowPhaseSolver>;
686  collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere<S>, Convex<S>, NarrowPhaseSolver>;
687  collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere<S>, Plane<S>, NarrowPhaseSolver>;
688  collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide<Sphere<S>, Halfspace<S>, NarrowPhaseSolver>;
689 
690  collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeCollide<Ellipsoid<S>, Box<S>, NarrowPhaseSolver>;
691  collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeCollide<Ellipsoid<S>, Sphere<S>, NarrowPhaseSolver>;
692  collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeCollide<Ellipsoid<S>, Ellipsoid<S>, NarrowPhaseSolver>;
693  collision_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeCollide<Ellipsoid<S>, Capsule<S>, NarrowPhaseSolver>;
694  collision_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeCollide<Ellipsoid<S>, Cone<S>, NarrowPhaseSolver>;
695  collision_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeCollide<Ellipsoid<S>, Cylinder<S>, NarrowPhaseSolver>;
696  collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeCollide<Ellipsoid<S>, Convex<S>, NarrowPhaseSolver>;
697  collision_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeCollide<Ellipsoid<S>, Plane<S>, NarrowPhaseSolver>;
698  collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeCollide<Ellipsoid<S>, Halfspace<S>, NarrowPhaseSolver>;
699 
700  collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule<S>, Box<S>, NarrowPhaseSolver>;
701  collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule<S>, Sphere<S>, NarrowPhaseSolver>;
702  collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Capsule<S>, Ellipsoid<S>, NarrowPhaseSolver>;
703  collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule<S>, Capsule<S>, NarrowPhaseSolver>;
704  collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule<S>, Cone<S>, NarrowPhaseSolver>;
705  collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule<S>, Cylinder<S>, NarrowPhaseSolver>;
706  collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule<S>, Convex<S>, NarrowPhaseSolver>;
707  collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule<S>, Plane<S>, NarrowPhaseSolver>;
708  collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide<Capsule<S>, Halfspace<S>, NarrowPhaseSolver>;
709 
710  collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone<S>, Box<S>, NarrowPhaseSolver>;
711  collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone<S>, Sphere<S>, NarrowPhaseSolver>;
712  collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Cone<S>, Ellipsoid<S>, NarrowPhaseSolver>;
713  collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone<S>, Capsule<S>, NarrowPhaseSolver>;
714  collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone<S>, Cone<S>, NarrowPhaseSolver>;
715  collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone<S>, Cylinder<S>, NarrowPhaseSolver>;
716  collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone<S>, Convex<S>, NarrowPhaseSolver>;
717  collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone<S>, Plane<S>, NarrowPhaseSolver>;
718  collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide<Cone<S>, Halfspace<S>, NarrowPhaseSolver>;
719 
720  collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder<S>, Box<S>, NarrowPhaseSolver>;
721  collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder<S>, Sphere<S>, NarrowPhaseSolver>;
722  collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeCollide<Cylinder<S>, Ellipsoid<S>, NarrowPhaseSolver>;
723  collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder<S>, Capsule<S>, NarrowPhaseSolver>;
724  collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder<S>, Cone<S>, NarrowPhaseSolver>;
725  collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder<S>, Cylinder<S>, NarrowPhaseSolver>;
726  collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder<S>, Convex<S>, NarrowPhaseSolver>;
727  collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder<S>, Plane<S>, NarrowPhaseSolver>;
728  collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide<Cylinder<S>, Halfspace<S>, NarrowPhaseSolver>;
729 
730  collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex<S>, Box<S>, NarrowPhaseSolver>;
731  collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex<S>, Sphere<S>, NarrowPhaseSolver>;
732  collision_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeCollide<Convex<S>, Ellipsoid<S>, NarrowPhaseSolver>;
733  collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex<S>, Capsule<S>, NarrowPhaseSolver>;
734  collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex<S>, Cone<S>, NarrowPhaseSolver>;
735  collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex<S>, Cylinder<S>, NarrowPhaseSolver>;
736  collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex<S>, Convex<S>, NarrowPhaseSolver>;
737  collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex<S>, Plane<S>, NarrowPhaseSolver>;
738  collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide<Convex<S>, Halfspace<S>, NarrowPhaseSolver>;
739 
740  collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane<S>, Box<S>, NarrowPhaseSolver>;
741  collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane<S>, Sphere<S>, NarrowPhaseSolver>;
742  collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Plane<S>, Ellipsoid<S>, NarrowPhaseSolver>;
743  collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane<S>, Capsule<S>, NarrowPhaseSolver>;
744  collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane<S>, Cone<S>, NarrowPhaseSolver>;
745  collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane<S>, Cylinder<S>, NarrowPhaseSolver>;
746  collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane<S>, Convex<S>, NarrowPhaseSolver>;
747  collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane<S>, Plane<S>, NarrowPhaseSolver>;
748  collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide<Plane<S>, Halfspace<S>, NarrowPhaseSolver>;
749 
750  collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide<Halfspace<S>, Box<S>, NarrowPhaseSolver>;
751  collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide<Halfspace<S>, Sphere<S>, NarrowPhaseSolver>;
752  collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide<Halfspace<S>, Capsule<S>, NarrowPhaseSolver>;
753  collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide<Halfspace<S>, Cone<S>, NarrowPhaseSolver>;
754  collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide<Halfspace<S>, Cylinder<S>, NarrowPhaseSolver>;
755  collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide<Halfspace<S>, Convex<S>, NarrowPhaseSolver>;
756  collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide<Halfspace<S>, Plane<S>, NarrowPhaseSolver>;
757  collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide<Halfspace<S>, Halfspace<S>, NarrowPhaseSolver>;
758 
759  collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB<S>, Box<S>, NarrowPhaseSolver>::collide;
760  collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB<S>, Sphere<S>, NarrowPhaseSolver>::collide;
761  collision_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeCollider<AABB<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
762  collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB<S>, Capsule<S>, NarrowPhaseSolver>::collide;
763  collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB<S>, Cone<S>, NarrowPhaseSolver>::collide;
764  collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
765  collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB<S>, Convex<S>, NarrowPhaseSolver>::collide;
766  collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB<S>, Plane<S>, NarrowPhaseSolver>::collide;
767  collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider<AABB<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
768 
769  collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB<S>, Box<S>, NarrowPhaseSolver>::collide;
770  collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB<S>, Sphere<S>, NarrowPhaseSolver>::collide;
771  collision_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeCollider<OBB<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
772  collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB<S>, Capsule<S>, NarrowPhaseSolver>::collide;
773  collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB<S>, Cone<S>, NarrowPhaseSolver>::collide;
774  collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
775  collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB<S>, Convex<S>, NarrowPhaseSolver>::collide;
776  collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB<S>, Plane<S>, NarrowPhaseSolver>::collide;
777  collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider<OBB<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
778 
779  collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS<S>, Box<S>, NarrowPhaseSolver>::collide;
780  collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS<S>, Sphere<S>, NarrowPhaseSolver>::collide;
781  collision_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeCollider<RSS<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
782  collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS<S>, Capsule<S>, NarrowPhaseSolver>::collide;
783  collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS<S>, Cone<S>, NarrowPhaseSolver>::collide;
784  collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
785  collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS<S>, Convex<S>, NarrowPhaseSolver>::collide;
786  collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS<S>, Plane<S>, NarrowPhaseSolver>::collide;
787  collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider<RSS<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
788 
789  collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<S, 16>, Box<S>, NarrowPhaseSolver>::collide;
790  collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<S, 16>, Sphere<S>, NarrowPhaseSolver>::collide;
791  collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<S, 16>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
792  collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<S, 16>, Capsule<S>, NarrowPhaseSolver>::collide;
793  collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<S, 16>, Cone<S>, NarrowPhaseSolver>::collide;
794  collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<S, 16>, Cylinder<S>, NarrowPhaseSolver>::collide;
795  collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<S, 16>, Convex<S>, NarrowPhaseSolver>::collide;
796  collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<S, 16>, Plane<S>, NarrowPhaseSolver>::collide;
797  collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<S, 16>, Halfspace<S>, NarrowPhaseSolver>::collide;
798 
799  collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<S, 18>, Box<S>, NarrowPhaseSolver>::collide;
800  collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<S, 18>, Sphere<S>, NarrowPhaseSolver>::collide;
801  collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<S, 18>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
802  collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<S, 18>, Capsule<S>, NarrowPhaseSolver>::collide;
803  collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<S, 18>, Cone<S>, NarrowPhaseSolver>::collide;
804  collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<S, 18>, Cylinder<S>, NarrowPhaseSolver>::collide;
805  collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<S, 18>, Convex<S>, NarrowPhaseSolver>::collide;
806  collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<S, 18>, Plane<S>, NarrowPhaseSolver>::collide;
807  collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<S, 18>, Halfspace<S>, NarrowPhaseSolver>::collide;
808 
809  collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<S, 24>, Box<S>, NarrowPhaseSolver>::collide;
810  collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<S, 24>, Sphere<S>, NarrowPhaseSolver>::collide;
811  collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<S, 24>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
812  collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<S, 24>, Capsule<S>, NarrowPhaseSolver>::collide;
813  collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<S, 24>, Cone<S>, NarrowPhaseSolver>::collide;
814  collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<S, 24>, Cylinder<S>, NarrowPhaseSolver>::collide;
815  collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<S, 24>, Convex<S>, NarrowPhaseSolver>::collide;
816  collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<S, 24>, Plane<S>, NarrowPhaseSolver>::collide;
817  collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<S, 24>, Halfspace<S>, NarrowPhaseSolver>::collide;
818 
819  collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS<S>, Box<S>, NarrowPhaseSolver>::collide;
820  collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS<S>, Sphere<S>, NarrowPhaseSolver>::collide;
821  collision_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeCollider<kIOS<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
822  collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS<S>, Capsule<S>, NarrowPhaseSolver>::collide;
823  collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS<S>, Cone<S>, NarrowPhaseSolver>::collide;
824  collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
825  collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS<S>, Convex<S>, NarrowPhaseSolver>::collide;
826  collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS<S>, Plane<S>, NarrowPhaseSolver>::collide;
827  collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider<kIOS<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
828 
829  collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS<S>, Box<S>, NarrowPhaseSolver>::collide;
830  collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS<S>, Sphere<S>, NarrowPhaseSolver>::collide;
831  collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeCollider<OBBRSS<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
832  collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS<S>, Capsule<S>, NarrowPhaseSolver>::collide;
833  collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS<S>, Cone<S>, NarrowPhaseSolver>::collide;
834  collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
835  collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS<S>, Convex<S>, NarrowPhaseSolver>::collide;
836  collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS<S>, Plane<S>, NarrowPhaseSolver>::collide;
837  collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider<OBBRSS<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
838 
839  collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB<S>, NarrowPhaseSolver>;
840  collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB<S>, NarrowPhaseSolver>;
841  collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS<S>, NarrowPhaseSolver>;
842  collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<S, 16>, NarrowPhaseSolver>;
843  collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<S, 18>, NarrowPhaseSolver>;
844  collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<S, 24>, NarrowPhaseSolver>;
845  collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS<S>, NarrowPhaseSolver>;
846  collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS<S>, NarrowPhaseSolver>;
847 
848 #if FCL_HAVE_OCTOMAP
849  collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box<S>, NarrowPhaseSolver>;
850  collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere<S>, NarrowPhaseSolver>;
851  collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeCollide<Ellipsoid<S>, NarrowPhaseSolver>;
852  collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule<S>, NarrowPhaseSolver>;
853  collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone<S>, NarrowPhaseSolver>;
854  collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder<S>, NarrowPhaseSolver>;
855  collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<Convex<S>, NarrowPhaseSolver>;
856  collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane<S>, NarrowPhaseSolver>;
857  collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace<S>, NarrowPhaseSolver>;
858 
859  collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box<S>, NarrowPhaseSolver>;
860  collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere<S>, NarrowPhaseSolver>;
861  collision_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeCollide<Ellipsoid<S>, NarrowPhaseSolver>;
862  collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule<S>, NarrowPhaseSolver>;
863  collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone<S>, NarrowPhaseSolver>;
864  collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder<S>, NarrowPhaseSolver>;
865  collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<Convex<S>, NarrowPhaseSolver>;
866  collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane<S>, NarrowPhaseSolver>;
867  collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace<S>, NarrowPhaseSolver>;
868 
869  collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide<NarrowPhaseSolver>;
870 
871  collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB<S>, NarrowPhaseSolver>;
872  collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB<S>, NarrowPhaseSolver>;
873  collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS<S>, NarrowPhaseSolver>;
874  collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS<S>, NarrowPhaseSolver>;
875  collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS<S>, NarrowPhaseSolver>;
876  collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<S, 16>, NarrowPhaseSolver>;
877  collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<S, 18>, NarrowPhaseSolver>;
878  collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<S, 24>, NarrowPhaseSolver>;
879 
880  collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB<S>, NarrowPhaseSolver>;
881  collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB<S>, NarrowPhaseSolver>;
882  collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS<S>, NarrowPhaseSolver>;
883  collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS<S>, NarrowPhaseSolver>;
884  collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS<S>, NarrowPhaseSolver>;
885  collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<S, 16>, NarrowPhaseSolver>;
886  collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<S, 18>, NarrowPhaseSolver>;
887  collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<S, 24>, NarrowPhaseSolver>;
888 #endif
889 }
890 
891 } // namespace detail
892 } // namespace fcl
893 
894 #endif
Traversal node for collision between mesh and shape.
Definition: mesh_shape_collision_traversal_node.h:52
bool enable_cost
whether the cost sources will be computed
Definition: collision_request.h:64
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d; Po...
Definition: halfspace.h:55
Definition: mesh_collision_traversal_node.h:209
Definition: collision_func_matrix-inl.h:524
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
collision result
Definition: collision_request.h:48
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:49
Center at zero point ellipsoid.
Definition: ellipsoid.h:48
const BVNode< BV > & getBV(int id) const
We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some...
Definition: BVH_model-inl.h:160
size_t numContacts() const
number of contacts found
Definition: collision_result-inl.h:83
BV::S threshold_free
threshold for free (<= is free)
Definition: collision_geometry.h:109
bool use_approximate_cost
whether the cost computation is approximated
Definition: collision_request.h:67
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:59
Definition: mesh_collision_traversal_node.h:178
Definition: mesh_shape_collision_traversal_node.h:189
Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
Definition: mesh_shape_collision_traversal_node.h:108
request to the collision algorithm
Definition: collision_request.h:52
Center at zero point capsule.
Definition: capsule.h:48
size_t num_max_cost_sources
The maximum number of cost sources will return.
Definition: collision_request.h:61
Center at zero point, axis aligned box.
Definition: box.h:48
BV::S threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_geometry.h:106
Definition: collision_func_matrix-inl.h:313
Traversal node for collision between two meshes.
Definition: mesh_collision_traversal_node.h:58
Infinite plane.
Definition: plane.h:48
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
Center at zero cylinder.
Definition: cylinder.h:48
BV::S cost_density
collision cost for unit volume
Definition: collision_geometry.h:103
Center at zero cone.
Definition: cone.h:48
collision matrix stores the functions for collision between different types of objects and provides a...
Definition: collision_func_matrix.h:54
Center at zero point sphere.
Definition: sphere.h:48
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
Definition: mesh_shape_collision_traversal_node.h:162
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
Definition: mesh_shape_collision_traversal_node.h:135
Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB...
Definition: mesh_collision_traversal_node.h:98
Oriented bounding box class.
Definition: OBB.h:51