test_fcl_collision.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #include <gtest/gtest.h>
39 
40 #include "fcl/math/bv/utility.h"
41 #include "fcl/narrowphase/collision.h"
42 #include "fcl/narrowphase/detail/gjk_solver_indep.h"
43 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
44 #include "fcl/narrowphase/detail/traversal/collision_node.h"
45 #include "fcl/narrowphase/continuous_collision.h"
46 
47 #include "test_fcl_utility.h"
48 
49 #include "fcl_resources/config.h"
50 
51 using namespace fcl;
52 
53 template<typename BV>
54 bool collide_Test(const Transform3<typename BV::S>& tf,
55  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
56  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method, bool verbose = true);
57 
58 template<typename BV>
59 bool collide_Test2(const Transform3<typename BV::S>& tf,
60  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
61  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method, bool verbose = true);
62 
63 template<typename BV, typename TraversalNode>
64 bool collide_Test_Oriented(const Transform3<typename BV::S>& tf,
65  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
66  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method, bool verbose = true);
67 
68 
69 template<typename BV>
70 bool test_collide_func(const Transform3<typename BV::S>& tf,
71  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
72  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method);
73 
74 int num_max_contacts = std::numeric_limits<int>::max();
75 bool enable_contact = true;
76 
77 template<typename S>
78 std::vector<Contact<S>>& global_pairs()
79 {
80  static std::vector<Contact<S>> static_global_pairs;
81  return static_global_pairs;
82 }
83 
84 template<typename S>
85 std::vector<Contact<S>>& global_pairs_now()
86 {
87  static std::vector<Contact<S>> static_global_pairs_now;
88  return static_global_pairs_now;
89 }
90 
91 template <typename S>
92 void test_SplineMotion_rotated_spline_collide_test()
93 {
94  fcl::Vector3<S> t[4];
95  t[0] = fcl::Vector3<S>(7.5, 8, 0);
96  t[1] = fcl::Vector3<S>(4.2, 8, 0);
97  t[2] = fcl::Vector3<S>(0.8, 8, 0);
98  t[3] = fcl::Vector3<S>(-2.5, 8, 0);
99 
100  fcl::Vector3<S> r[4];
101  r[0] = fcl::Vector3<S>(0, 0, 3.141593);
102  r[1] = fcl::Vector3<S>(0, 0, 3.141593);
103  r[2] = fcl::Vector3<S>(0, 0, 3.141593);
104  r[3] = fcl::Vector3<S>(0, 0, 3.141593);
105 
106  auto motion_a = fcl::make_aligned_shared<fcl::SplineMotion<S>>(
107  t[0], t[1], t[2], t[3],
108  r[0], r[1], r[2], r[3]);
109 
110  t[0] = fcl::Vector3<S>(0.0, 8, 0);
111  t[1] = fcl::Vector3<S>(1.25, 8, 0);
112  t[2] = fcl::Vector3<S>(3.0, 8, 0);
113  t[3] = fcl::Vector3<S>(4.6, 8, 0);
114 
115  r[0] = fcl::Vector3<S>(0, 0, 0);
116  r[1] = fcl::Vector3<S>(0, 0, 0);
117  r[2] = fcl::Vector3<S>(0, 0, 0);
118  r[3] = fcl::Vector3<S>(0, 0, 0);
119 
120  auto motion_b = fcl::make_aligned_shared<fcl::SplineMotion<S>>(
121  t[0], t[1], t[2], t[3],
122  r[0], r[1], r[2], r[3]);
123 
124  // Test collision with unit spheres
125  auto shape_a = std::make_shared<fcl::Sphere<S>>(1.0);
126  const auto obj_a = fcl::ContinuousCollisionObject<S>(
127  shape_a,
128  motion_a);
129 
130  auto shape_b = std::make_shared<fcl::Sphere<S>>(1.0);
131  const auto obj_b = fcl::ContinuousCollisionObject<S>(
132  shape_b,
133  motion_b);
134 
135  fcl::ContinuousCollisionRequest<S> request;
136  request.ccd_solver_type = fcl::CCDC_CONSERVATIVE_ADVANCEMENT;
137  request.gjk_solver_type = fcl::GST_LIBCCD;
138 
139  fcl::ContinuousCollisionResult<S> result;
140  fcl::collide(&obj_a, &obj_b, request, result);
141 
142  EXPECT_TRUE(result.is_collide);
143 }
144 
145 GTEST_TEST(FCL_COLLISION, test_SplineMotion_rotated_spline_collide_test)
146 {
147  test_SplineMotion_rotated_spline_collide_test<double>();
148 }
149 
150 template <typename S>
151 void test_OBB_Box_test()
152 {
153  S r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
154  aligned_vector<Transform3<S>> rotate_transform;
155  test::generateRandomTransforms(r_extents, rotate_transform, 1);
156 
157  AABB<S> aabb1;
158  aabb1.min_ = Vector3<S>(-600, -600, -600);
159  aabb1.max_ = Vector3<S>(600, 600, 600);
160 
161  OBB<S> obb1;
162  convertBV(aabb1, rotate_transform[0], obb1);
163  Box<S> box1;
164  Transform3<S> box1_tf;
165  constructBox(aabb1, rotate_transform[0], box1, box1_tf);
166 
167  S extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
168  std::size_t n = 1000;
169 
170  aligned_vector<Transform3<S>> transforms;
171  test::generateRandomTransforms(extents, transforms, n);
172 
173  for(std::size_t i = 0; i < transforms.size(); ++i)
174  {
175  AABB<S> aabb;
176  aabb.min_ = aabb1.min_ * 0.5;
177  aabb.max_ = aabb1.max_ * 0.5;
178 
179  OBB<S> obb2;
180  convertBV(aabb, transforms[i], obb2);
181 
182  Box<S> box2;
183  Transform3<S> box2_tf;
184  constructBox(aabb, transforms[i], box2, box2_tf);
185 
186  detail::GJKSolver_libccd<S> solver;
187 
188  bool overlap_obb = obb1.overlap(obb2);
189  bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, nullptr);
190 
191  EXPECT_TRUE(overlap_obb == overlap_box);
192  }
193 }
194 
195 template <typename S>
196 void test_OBB_shape_test()
197 {
198  S r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
199  aligned_vector<Transform3<S>> rotate_transform;
200  test::generateRandomTransforms(r_extents, rotate_transform, 1);
201 
202  AABB<S> aabb1;
203  aabb1.min_ = Vector3<S>(-600, -600, -600);
204  aabb1.max_ = Vector3<S>(600, 600, 600);
205 
206  OBB<S> obb1;
207  convertBV(aabb1, rotate_transform[0], obb1);
208  Box<S> box1;
209  Transform3<S> box1_tf;
210  constructBox(aabb1, rotate_transform[0], box1, box1_tf);
211 
212  S extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
213  std::size_t n = 1000;
214 
215  aligned_vector<Transform3<S>> transforms;
216  test::generateRandomTransforms(extents, transforms, n);
217 
218  for(std::size_t i = 0; i < transforms.size(); ++i)
219  {
220  S len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5;
221  OBB<S> obb2;
222  detail::GJKSolver_libccd<S> solver;
223 
224  {
225  Sphere<S> sphere(len);
226  computeBV(sphere, transforms[i], obb2);
227 
228  bool overlap_obb = obb1.overlap(obb2);
229  bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], nullptr);
230  EXPECT_TRUE(overlap_obb >= overlap_sphere);
231  }
232 
233  {
234  Ellipsoid<S> ellipsoid(len, len, len);
235  computeBV(ellipsoid, transforms[i], obb2);
236 
237  bool overlap_obb = obb1.overlap(obb2);
238  bool overlap_ellipsoid = solver.shapeIntersect(box1, box1_tf, ellipsoid, transforms[i], nullptr);
239  EXPECT_TRUE(overlap_obb >= overlap_ellipsoid);
240  }
241 
242  {
243  Capsule<S> capsule(len, 2 * len);
244  computeBV(capsule, transforms[i], obb2);
245 
246  bool overlap_obb = obb1.overlap(obb2);
247  bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], nullptr);
248  EXPECT_TRUE(overlap_obb >= overlap_capsule);
249  }
250 
251  {
252  Cone<S> cone(len, 2 * len);
253  computeBV(cone, transforms[i], obb2);
254 
255  bool overlap_obb = obb1.overlap(obb2);
256  bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], nullptr);
257  EXPECT_TRUE(overlap_obb >= overlap_cone);
258  }
259 
260  {
261  Cylinder<S> cylinder(len, 2 * len);
262  computeBV(cylinder, transforms[i], obb2);
263 
264  bool overlap_obb = obb1.overlap(obb2);
265  bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], nullptr);
266  EXPECT_TRUE(overlap_obb >= overlap_cylinder);
267  }
268  }
269 }
270 
271 template <typename S>
272 void test_OBB_AABB_test()
273 {
274  S extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
275  std::size_t n = 1000;
276 
277  aligned_vector<Transform3<S>> transforms;
278  test::generateRandomTransforms(extents, transforms, n);
279 
280  AABB<S> aabb1;
281  aabb1.min_ = Vector3<S>(-600, -600, -600);
282  aabb1.max_ = Vector3<S>(600, 600, 600);
283 
284  OBB<S> obb1;
285  convertBV(aabb1, Transform3<S>::Identity(), obb1);
286 
287  for(std::size_t i = 0; i < transforms.size(); ++i)
288  {
289  AABB<S> aabb;
290  aabb.min_ = aabb1.min_ * 0.5;
291  aabb.max_ = aabb1.max_ * 0.5;
292 
293  AABB<S> aabb2 = translate(aabb, transforms[i].translation());
294 
295  OBB<S> obb2;
296  convertBV(aabb, Transform3<S>(Translation3<S>(transforms[i].translation())), obb2);
297 
298  bool overlap_aabb = aabb1.overlap(aabb2);
299  bool overlap_obb = obb1.overlap(obb2);
300  if(overlap_aabb != overlap_obb)
301  {
302  std::cout << aabb1.min_.transpose() << " " << aabb1.max_.transpose() << std::endl;
303  std::cout << aabb2.min_.transpose() << " " << aabb2.max_.transpose() << std::endl;
304  std::cout << obb1.To.transpose() << " " << obb1.extent.transpose() << " " << obb1.axis.col(0).transpose() << " " << obb1.axis.col(1).transpose() << " " << obb1.axis.col(2).transpose() << std::endl;
305  std::cout << obb2.To.transpose() << " " << obb2.extent.transpose() << " " << obb2.axis.col(0).transpose() << " " << obb2.axis.col(1).transpose() << " " << obb2.axis.col(2).transpose() << std::endl;
306  }
307 
308  EXPECT_TRUE(overlap_aabb == overlap_obb);
309  }
310  std::cout << std::endl;
311 }
312 
313 template <typename S>
314 void test_mesh_mesh()
315 {
316  std::vector<Vector3<S>> p1, p2;
317  std::vector<Triangle> t1, t2;
318 
319  test::loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1);
320  test::loadOBJFile(TEST_RESOURCES_DIR"/rob.obj", p2, t2);
321 
322  aligned_vector<Transform3<S>> transforms;
323  S extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
324 #ifdef NDEBUG
325  std::size_t n = 10;
326 #else
327  std::size_t n = 1;
328 #endif
329  bool verbose = false;
330 
331  test::generateRandomTransforms(extents, transforms, n);
332 
333  // collision
334  for(std::size_t i = 0; i < transforms.size(); ++i)
335  {
336  global_pairs<S>().clear();
337  global_pairs_now<S>().clear();
338 
339  collide_Test<OBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
340 
341  collide_Test<OBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
342  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
343  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
344  {
345  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
346  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
347  }
348 
349  collide_Test<OBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
350  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
351  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
352  {
353  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
354  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
355  }
356 
357  collide_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
358  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
359  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
360  {
361  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
362  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
363  }
364 
365  collide_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
366  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
367  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
368  {
369  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
370  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
371  }
372 
373  collide_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
374  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
375  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
376  {
377  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
378  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
379  }
380 
381  collide_Test<AABB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
382  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
383  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
384  {
385  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
386  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
387  }
388 
389  collide_Test<AABB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
390  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
391  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
392  {
393  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
394  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
395  }
396 
397  collide_Test<AABB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
398  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
399  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
400  {
401  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
402  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
403  }
404 
405  collide_Test<KDOP<S, 24> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
406  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
407  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
408  {
409  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
410  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
411  }
412 
413  collide_Test<KDOP<S, 24> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
414  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
415  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
416  {
417  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
418  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
419  }
420 
421  collide_Test<KDOP<S, 24> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
422  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
423  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
424  {
425  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
426  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
427  }
428 
429  collide_Test<KDOP<S, 18> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
430  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
431  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
432  {
433  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
434  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
435  }
436 
437  collide_Test<KDOP<S, 18> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
438  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
439  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
440  {
441  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
442  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
443  }
444 
445  collide_Test<KDOP<S, 18> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
446  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
447  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
448  {
449  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
450  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
451  }
452 
453  collide_Test<KDOP<S, 16> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
454  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
455  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
456  {
457  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
458  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
459  }
460 
461  collide_Test<KDOP<S, 16> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
462  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
463  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
464  {
465  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
466  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
467  }
468 
469  collide_Test<KDOP<S, 16> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
470  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
471  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
472  {
473  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
474  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
475  }
476 
477  collide_Test2<OBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
478  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
479  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
480  {
481  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
482  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
483  }
484 
485  collide_Test2<OBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
486  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
487  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
488  {
489  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
490  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
491  }
492 
493  collide_Test2<OBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
494  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
495  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
496  {
497  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
498  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
499  }
500 
501  collide_Test2<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
502  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
503  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
504  {
505  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
506  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
507  }
508 
509  collide_Test2<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
510  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
511  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
512  {
513  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
514  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
515  }
516 
517  collide_Test2<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
518  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
519  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
520  {
521  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
522  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
523  }
524 
525  collide_Test2<AABB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
526  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
527  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
528  {
529  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
530  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
531  }
532 
533  collide_Test2<AABB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
534  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
535  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
536  {
537  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
538  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
539  }
540 
541  collide_Test2<AABB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
542  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
543  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
544  {
545  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
546  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
547  }
548 
549  collide_Test2<KDOP<S, 24> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
550  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
551  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
552  {
553  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
554  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
555  }
556 
557  collide_Test2<KDOP<S, 24> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
558  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
559  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
560  {
561  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
562  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
563  }
564 
565  collide_Test2<KDOP<S, 24> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
566  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
567  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
568  {
569  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
570  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
571  }
572 
573  collide_Test2<KDOP<S, 18> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
574  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
575  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
576  {
577  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
578  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
579  }
580 
581  collide_Test2<KDOP<S, 18> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
582  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
583  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
584  {
585  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
586  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
587  }
588 
589  collide_Test2<KDOP<S, 18> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
590  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
591  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
592  {
593  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
594  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
595  }
596 
597  collide_Test2<KDOP<S, 16> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
598  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
599  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
600  {
601  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
602  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
603  }
604 
605  collide_Test2<KDOP<S, 16> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
606  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
607  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
608  {
609  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
610  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
611  }
612 
613  collide_Test2<KDOP<S, 16> >(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
614  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
615  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
616  {
617  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
618  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
619  }
620 
621  collide_Test_Oriented<OBB<S>, detail::MeshCollisionTraversalNodeOBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
622 
623  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
624  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
625  {
626  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
627  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
628  }
629 
630  collide_Test_Oriented<OBB<S>, detail::MeshCollisionTraversalNodeOBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
631 
632  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
633  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
634  {
635  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
636  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
637  }
638 
639  collide_Test_Oriented<OBB<S>, detail::MeshCollisionTraversalNodeOBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
640  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
641  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
642  {
643  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
644  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
645  }
646 
647  collide_Test_Oriented<RSS<S>, detail::MeshCollisionTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
648  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
649  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
650  {
651  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
652  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
653  }
654 
655  collide_Test_Oriented<RSS<S>, detail::MeshCollisionTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
656  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
657  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
658  {
659  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
660  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
661  }
662 
663  collide_Test_Oriented<RSS<S>, detail::MeshCollisionTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
664  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
665  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
666  {
667  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
668  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
669  }
670 
671  test_collide_func<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN);
672  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
673  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
674  {
675  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
676  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
677  }
678 
679  test_collide_func<OBB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN);
680  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
681  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
682  {
683  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
684  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
685  }
686 
687  test_collide_func<AABB<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN);
688  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
689  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
690  {
691  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
692  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
693  }
694 
695 
696  collide_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
697  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
698  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
699  {
700  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
701  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
702  }
703 
704  collide_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
705  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
706  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
707  {
708  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
709  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
710  }
711 
712  collide_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
713  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
714  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
715  {
716  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
717  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
718  }
719 
720  collide_Test2<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
721  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
722  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
723  {
724  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
725  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
726  }
727 
728  collide_Test2<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
729  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
730  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
731  {
732  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
733  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
734  }
735 
736  collide_Test2<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
737  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
738  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
739  {
740  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
741  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
742  }
743 
744  collide_Test_Oriented<kIOS<S>, detail::MeshCollisionTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
745  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
746  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
747  {
748  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
749  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
750  }
751 
752  collide_Test_Oriented<kIOS<S>, detail::MeshCollisionTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
753  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
754  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
755  {
756  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
757  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
758  }
759 
760  collide_Test_Oriented<kIOS<S>, detail::MeshCollisionTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
761  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
762  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
763  {
764  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
765  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
766  }
767 
768  test_collide_func<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN);
769  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
770  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
771  {
772  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
773  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
774  }
775 
776  test_collide_func<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN);
777  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
778  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
779  {
780  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
781  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
782  }
783 
784  test_collide_func<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER);
785  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
786  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
787  {
788  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
789  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
790  }
791 
792  collide_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
793  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
794  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
795  {
796  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
797  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
798  }
799 
800  collide_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
801  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
802  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
803  {
804  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
805  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
806  }
807 
808  collide_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
809  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
810  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
811  {
812  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
813  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
814  }
815 
816  collide_Test2<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
817  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
818  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
819  {
820  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
821  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
822  }
823 
824  collide_Test2<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
825  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
826  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
827  {
828  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
829  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
830  }
831 
832  collide_Test2<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
833  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
834  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
835  {
836  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
837  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
838  }
839 
840  collide_Test_Oriented<OBBRSS<S>, detail::MeshCollisionTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
841  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
842  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
843  {
844  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
845  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
846  }
847 
848  collide_Test_Oriented<OBBRSS<S>, detail::MeshCollisionTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, verbose);
849  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
850  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
851  {
852  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
853  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
854  }
855 
856  collide_Test_Oriented<OBBRSS<S>, detail::MeshCollisionTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, verbose);
857  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
858  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
859  {
860  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
861  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
862  }
863 
864  test_collide_func<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN);
865  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
866  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
867  {
868  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
869  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
870  }
871 
872  test_collide_func<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN);
873  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
874  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
875  {
876  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
877  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
878  }
879 
880  test_collide_func<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER);
881  EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
882  for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
883  {
884  EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
885  EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
886  }
887  }
888 }
889 
890 GTEST_TEST(FCL_COLLISION, OBB_Box_test)
891 {
892 // test_OBB_Box_test<float>();
893  // Disabled for particular configurations: macOS + release + double (see #202)
894 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
895  test_OBB_Box_test<double>();
896 #endif
897 }
898 
899 GTEST_TEST(FCL_COLLISION, OBB_shape_test)
900 {
901 // test_OBB_shape_test<float>();
902  test_OBB_shape_test<double>();
903 }
904 
905 GTEST_TEST(FCL_COLLISION, OBB_AABB_test)
906 {
907 // test_OBB_AABB_test<float>();
908  test_OBB_AABB_test<double>();
909 }
910 
911 GTEST_TEST(FCL_COLLISION, mesh_mesh)
912 {
913 // test_mesh_mesh<float>();
914  test_mesh_mesh<double>();
915 }
916 
917 template<typename BV>
918 bool collide_Test2(const Transform3<typename BV::S>& tf,
919  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
920  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method, bool verbose)
921 {
922  using S = typename BV::S;
923 
924  BVHModel<BV> m1;
925  BVHModel<BV> m2;
926  m1.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
927  m2.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
928 
929  std::vector<Vector3<S>> vertices1_new(vertices1.size());
930  for(unsigned int i = 0; i < vertices1_new.size(); ++i)
931  {
932  vertices1_new[i] = tf * vertices1[i];
933  }
934 
935 
936  m1.beginModel();
937  m1.addSubModel(vertices1_new, triangles1);
938  m1.endModel();
939 
940  m2.beginModel();
941  m2.addSubModel(vertices2, triangles2);
942  m2.endModel();
943 
944  Transform3<S> pose1 = Transform3<S>::Identity();
945  Transform3<S> pose2 = Transform3<S>::Identity();
946 
947  CollisionResult<S> local_result;
948  detail::MeshCollisionTraversalNode<BV> node;
949 
950  if(!detail::initialize<BV>(node, m1, pose1, m2, pose2,
951  CollisionRequest<S>(num_max_contacts, enable_contact), local_result))
952  std::cout << "initialize error" << std::endl;
953 
954  node.enable_statistics = verbose;
955 
956  collide(&node);
957 
958 
959  if(local_result.numContacts() > 0)
960  {
961  if(global_pairs<S>().size() == 0)
962  {
963  local_result.getContacts(global_pairs<S>());
964  std::sort(global_pairs<S>().begin(), global_pairs<S>().end());
965  }
966  else
967  {
968  local_result.getContacts(global_pairs_now<S>());
969  std::sort(global_pairs_now<S>().begin(), global_pairs_now<S>().end());
970  }
971 
972 
973  if(verbose)
974  std::cout << "in collision " << local_result.numContacts() << ": " << std::endl;
975  if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
976  return true;
977  }
978  else
979  {
980  if(verbose) std::cout << "collision free " << std::endl;
981  if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
982  return false;
983  }
984 }
985 
986 template<typename BV>
987 bool collide_Test(const Transform3<typename BV::S>& tf,
988  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
989  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method, bool verbose)
990 {
991  using S = typename BV::S;
992 
993  BVHModel<BV> m1;
994  BVHModel<BV> m2;
995  m1.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
996  m2.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
997 
998  m1.beginModel();
999  m1.addSubModel(vertices1, triangles1);
1000  m1.endModel();
1001 
1002  m2.beginModel();
1003  m2.addSubModel(vertices2, triangles2);
1004  m2.endModel();
1005 
1006  Transform3<S> pose1(tf);
1007  Transform3<S> pose2 = Transform3<S>::Identity();
1008 
1009  CollisionResult<S> local_result;
1010  detail::MeshCollisionTraversalNode<BV> node;
1011 
1012  if(!detail::initialize<BV>(node, m1, pose1, m2, pose2,
1013  CollisionRequest<S>(num_max_contacts, enable_contact), local_result))
1014  std::cout << "initialize error" << std::endl;
1015 
1016  node.enable_statistics = verbose;
1017 
1018  collide(&node);
1019 
1020 
1021  if(local_result.numContacts() > 0)
1022  {
1023  if(global_pairs<S>().size() == 0)
1024  {
1025  local_result.getContacts(global_pairs<S>());
1026  std::sort(global_pairs<S>().begin(), global_pairs<S>().end());
1027  }
1028  else
1029  {
1030  local_result.getContacts(global_pairs_now<S>());
1031  std::sort(global_pairs_now<S>().begin(), global_pairs_now<S>().end());
1032  }
1033 
1034  if(verbose)
1035  std::cout << "in collision " << local_result.numContacts() << ": " << std::endl;
1036  if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
1037  return true;
1038  }
1039  else
1040  {
1041  if(verbose) std::cout << "collision free " << std::endl;
1042  if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
1043  return false;
1044  }
1045 }
1046 
1047 template<typename BV, typename TraversalNode>
1048 bool collide_Test_Oriented(const Transform3<typename BV::S>& tf,
1049  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
1050  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method, bool verbose)
1051 {
1052  using S = typename BV::S;
1053 
1054  BVHModel<BV> m1;
1055  BVHModel<BV> m2;
1056  m1.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
1057  m2.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
1058 
1059  m1.beginModel();
1060  m1.addSubModel(vertices1, triangles1);
1061  m1.endModel();
1062 
1063  m2.beginModel();
1064  m2.addSubModel(vertices2, triangles2);
1065  m2.endModel();
1066 
1067  Transform3<S> pose1(tf);
1068  Transform3<S> pose2 = Transform3<S>::Identity();
1069 
1070  CollisionResult<S> local_result;
1071  TraversalNode node;
1072  if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2,
1073  CollisionRequest<S>(num_max_contacts, enable_contact), local_result))
1074  std::cout << "initialize error" << std::endl;
1075 
1076  node.enable_statistics = verbose;
1077 
1078  collide(&node);
1079 
1080  if(local_result.numContacts() > 0)
1081  {
1082  if(global_pairs<S>().size() == 0)
1083  {
1084  local_result.getContacts(global_pairs<S>());
1085  std::sort(global_pairs<S>().begin(), global_pairs<S>().end());
1086  }
1087  else
1088  {
1089  local_result.getContacts(global_pairs_now<S>());
1090  std::sort(global_pairs_now<S>().begin(), global_pairs_now<S>().end());
1091  }
1092 
1093  if(verbose)
1094  std::cout << "in collision " << local_result.numContacts() << ": " << std::endl;
1095  if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
1096  return true;
1097  }
1098  else
1099  {
1100  if(verbose) std::cout << "collision free " << std::endl;
1101  if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
1102  return false;
1103  }
1104 }
1105 
1106 
1107 template<typename BV>
1108 bool test_collide_func(const Transform3<typename BV::S>& tf,
1109  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
1110  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method)
1111 {
1112  using S = typename BV::S;
1113 
1114  BVHModel<BV> m1;
1115  BVHModel<BV> m2;
1116  m1.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
1117  m2.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
1118 
1119  m1.beginModel();
1120  m1.addSubModel(vertices1, triangles1);
1121  m1.endModel();
1122 
1123  m2.beginModel();
1124  m2.addSubModel(vertices2, triangles2);
1125  m2.endModel();
1126 
1127  Transform3<S> pose1(tf);
1128  Transform3<S> pose2 = Transform3<S>::Identity();
1129 
1130  std::vector<Contact<S>> contacts;
1131 
1132  CollisionRequest<S> request(num_max_contacts, enable_contact);
1133  CollisionResult<S> result;
1134  int num_contacts = collide(&m1, pose1, &m2, pose2, request, result);
1135 
1136  result.getContacts(contacts);
1137 
1138  global_pairs_now<S>().resize(num_contacts);
1139 
1140  for(int i = 0; i < num_contacts; ++i)
1141  {
1142  global_pairs_now<S>()[i].b1 = contacts[i].b1;
1143  global_pairs_now<S>()[i].b2 = contacts[i].b2;
1144  }
1145 
1146  std::sort(global_pairs_now<S>().begin(), global_pairs_now<S>().end());
1147 
1148  if(num_contacts > 0) return true;
1149  else return false;
1150 }
1151 
1152 //==============================================================================
1153 int main(int argc, char* argv[])
1154 {
1155  ::testing::InitGoogleTest(&argc, argv);
1156  return RUN_ALL_TESTS();
1157 }
fcl::BVHModel::bv_splitter
std::shared_ptr< detail::BVSplitterBase< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH_model.h:180
fcl::OBB::axis
Matrix3< S > axis
Orientation of OBB. The axes of the rotation matrix are the principle directions of the box....
Definition: OBB.h:62
GTEST_TEST
GTEST_TEST(FCL_COLLISION, test_SplineMotion_rotated_spline_collide_test)
Definition: test_fcl_collision.cpp:145
fcl::detail::SPLIT_METHOD_MEDIAN
@ SPLIT_METHOD_MEDIAN
Definition: BV_splitter.h:59
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:92
fcl::convertBV
FCL_EXPORT void convertBV(const BV1 &bv1, const Transform3< typename BV1::S > &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: math/bv/utility-inl.h:973
fcl::test::loadOBJFile
void loadOBJFile(const char *filename, std::vector< Vector3< S >> &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: test_fcl_utility.h:194
fcl::detail::BVHCollisionTraversalNode::num_leaf_tests
int num_leaf_tests
Definition: bvh_collision_traversal_node.h:93
fcl::detail::GJKSolver_libccd
collision and distance solver based on libccd library.
Definition: gjk_solver_libccd.h:54
fcl::detail::GJKSolver_libccd::shapeIntersect
FCL_DEPRECATED bool shapeIntersect(const Shape1 &s1, const Transform3< S > &tf1, const Shape2 &s2, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) const
intersection checking between two shapes
global_pairs
std::vector< Contact< S > > & global_pairs()
Definition: test_fcl_collision.cpp:78
fcl::ContinuousCollisionRequest
Definition: continuous_collision_request.h:52
fcl::detail::MeshCollisionTraversalNode
Traversal node for collision between two meshes.
Definition: mesh_collision_traversal_node.h:58
fcl::translate
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
fcl::computeBV
FCL_EXPORT void computeBV(const Shape &s, const Transform3< typename BV::S > &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: geometry/shape/utility-inl.h:1056
fcl::CCDC_CONSERVATIVE_ADVANCEMENT
@ CCDC_CONSERVATIVE_ADVANCEMENT
Definition: continuous_collision_request.h:49
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::ContinuousCollisionRequest::ccd_solver_type
CCDSolverType ccd_solver_type
ccd solver type
Definition: continuous_collision_request.h:67
fcl::detail::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: BV_splitter.h:56
fcl::ContinuousCollisionRequest::gjk_solver_type
GJKSolverType gjk_solver_type
gjk solver type
Definition: continuous_collision_request.h:64
fcl::ContinuousCollisionObject
the object for continuous collision or distance computation, contains the geometry and the motion inf...
Definition: continuous_collision_object.h:51
fcl::AABB< S >
fcl::BVHModel::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model-inl.h:450
test_fcl_utility.h
fcl::CollisionResult
collision result
Definition: collision_request.h:48
EXPECT_TRUE
#define EXPECT_TRUE(args)
test_mesh_mesh
void test_mesh_mesh()
Definition: test_fcl_collision.cpp:314
fcl::detail::CollisionTraversalNodeBase< BV::S >::enable_statistics
bool enable_statistics
Whether stores statistics.
Definition: collision_traversal_node_base.h:78
collide_Test
bool collide_Test(const Transform3< typename BV::S > &tf, const std::vector< Vector3< typename BV::S >> &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vector3< typename BV::S >> &vertices2, const std::vector< Triangle > &triangles2, detail::SplitMethodType split_method, bool verbose=true)
Definition: test_fcl_collision.cpp:987
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:71
collide_Test_Oriented
bool collide_Test_Oriented(const Transform3< typename BV::S > &tf, const std::vector< Vector3< typename BV::S >> &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vector3< typename BV::S >> &vertices2, const std::vector< Triangle > &triangles2, detail::SplitMethodType split_method, bool verbose=true)
Definition: test_fcl_collision.cpp:1048
gjk_solver_libccd.h
fcl::OBB< S >
fcl::AABB::max_
Vector3< S > max_
The max point in the AABB.
Definition: AABB.h:59
fcl::ContinuousCollisionResult::is_collide
bool is_collide
collision or not
Definition: continuous_collision_result.h:51
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
fcl::detail::MeshCollisionTraversalNodeOBBRSS
Definition: mesh_collision_traversal_node.h:213
fcl::detail::BVHCollisionTraversalNode::num_bv_tests
int num_bv_tests
statistical information
Definition: bvh_collision_traversal_node.h:92
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_result-inl.h:83
fcl::AABB::min_
Vector3< S > min_
The min point in the AABB.
Definition: AABB.h:56
test_OBB_Box_test
void test_OBB_Box_test()
Definition: test_fcl_collision.cpp:151
fcl::detail::initialize
template bool initialize(MeshCollisionTraversalNodeOBB< double > &node, const BVHModel< OBB< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBB< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:95
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
continuous_collision.h
test_OBB_AABB_test
void test_OBB_AABB_test()
Definition: test_fcl_collision.cpp:272
r
S r
Definition: test_sphere_box.cpp:171
fcl::detail::SPLIT_METHOD_MEAN
@ SPLIT_METHOD_MEAN
Definition: BV_splitter.h:58
fcl::detail::SPLIT_METHOD_BV_CENTER
@ SPLIT_METHOD_BV_CENTER
Definition: BV_splitter.h:60
fcl::BVHModel::beginModel
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model-inl.h:207
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:51
fcl::test::generateRandomTransforms
void generateRandomTransforms(S extents[6], aligned_vector< Transform3< S >> &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: test_fcl_utility.h:349
fcl::constructBox
template void constructBox(const OBB< double > &bv, Box< double > &box, Transform3< double > &tf)
test_OBB_shape_test
void test_OBB_shape_test()
Definition: test_fcl_collision.cpp:196
global_pairs_now
std::vector< Contact< S > > & global_pairs_now()
Definition: test_fcl_collision.cpp:85
fcl::CollisionResult::getContacts
void getContacts(std::vector< Contact< S >> &contacts_)
get all the contacts
Definition: collision_result-inl.h:107
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::ContinuousCollisionResult
continuous collision result
Definition: continuous_collision_result.h:48
fcl::detail::MeshCollisionTraversalNodekIOS
Definition: mesh_collision_traversal_node.h:181
utility.h
test_collide_func
bool test_collide_func(const Transform3< typename BV::S > &tf, const std::vector< Vector3< typename BV::S >> &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vector3< typename BV::S >> &vertices2, const std::vector< Triangle > &triangles2, detail::SplitMethodType split_method)
Definition: test_fcl_collision.cpp:1108
gjk_solver_indep.h
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
collide_Test2
bool collide_Test2(const Transform3< typename BV::S > &tf, const std::vector< Vector3< typename BV::S >> &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vector3< typename BV::S >> &vertices2, const std::vector< Triangle > &triangles2, detail::SplitMethodType split_method, bool verbose=true)
Definition: test_fcl_collision.cpp:918
fcl::AABB::overlap
bool overlap(const AABB< S > &other) const
Check whether two AABB are overlap.
Definition: AABB-inl.h:98
enable_contact
bool enable_contact
Definition: test_fcl_collision.cpp:75
fcl::detail::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BV_splitter.h:65
fcl::BVHModel::addSubModel
int addSubModel(const std::vector< Vector3< S >> &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Definition: BVH_model-inl.h:383
collision_node.h
fcl::OBB::overlap
bool overlap(const OBB< S > &other) const
Check collision between two OBB, return true if collision happens.
Definition: OBB-inl.h:98
fcl::Cone
Center at zero cone.
Definition: cone.h:51
aabb
SaPCollisionManager< S >::SaPAABB * aabb
back pointer to SAP interval
Definition: broadphase_SaP.h:184
main
int main(int argc, char *argv[])
Definition: test_fcl_collision.cpp:1153
extents
std::array< S, 6 > & extents()
Definition: test_fcl_geometric_shapes.cpp:63
num_max_contacts
int num_max_contacts
Definition: test_fcl_collision.cpp:74
test_SplineMotion_rotated_spline_collide_test
void test_SplineMotion_rotated_spline_collide_test()
Definition: test_fcl_collision.cpp:92
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::OBB::extent
Vector3< S > extent
Half dimensions of OBB.
Definition: OBB.h:68
fcl::detail::MeshCollisionTraversalNodeRSS
Definition: mesh_collision_traversal_node.h:139
fcl::detail::MeshCollisionTraversalNodeOBB
Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB,...
Definition: mesh_collision_traversal_node.h:99
fcl::OBB::To
Vector3< S > To
Center of OBB.
Definition: OBB.h:65
fcl::aligned_vector
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: types.h:123
fcl::Capsule< S >
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:51
collision.h
verbose
bool verbose
Definition: test_fcl_distance.cpp:50

fcl
Author(s):
autogenerated on Fri Mar 14 2025 02:38:18