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 
36 /** \author Jia Pan */
37 
38 #define BOOST_TEST_MODULE "FCL_COLLISION"
39 #include <boost/test/unit_test.hpp>
40 
41 #include "fcl/traversal/traversal_node_bvhs.h"
42 #include "fcl/traversal/traversal_node_setup.h"
43 #include "fcl/collision_node.h"
44 #include "fcl/collision.h"
45 #include "fcl/BV/BV.h"
46 #include "fcl/shape/geometric_shapes.h"
47 #include "fcl/narrowphase/narrowphase.h"
48 #include "test_fcl_utility.h"
49 #include "fcl_resources/config.h"
50 #include <boost/filesystem.hpp>
51 
52 using namespace fcl;
53 
54 template<typename BV>
55 bool collide_Test(const Transform3f& tf,
56                   const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
57                   const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
58 
59 template<typename BV>
60 bool collide_Test2(const Transform3f& tf,
61                    const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
62                    const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
63 
64 template<typename BV, typename TraversalNode>
65 bool collide_Test_Oriented(const Transform3f& tf,
66                            const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
67                            const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
68 
69 
70 template<typename BV>
71 bool test_collide_func(const Transform3f& tf,
72                        const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
73                        const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method);
74 
75 int num_max_contacts = std::numeric_limits<int>::max();
76 bool enable_contact = true;
77 
78 std::vector<Contact> global_pairs;
79 std::vector<Contact> global_pairs_now;
80 
BOOST_AUTO_TEST_CASE(OBB_Box_test)81 BOOST_AUTO_TEST_CASE(OBB_Box_test)
82 {
83   FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
84   std::vector<Transform3f> rotate_transform;
85   generateRandomTransforms(r_extents, rotate_transform, 1);
86 
87   AABB aabb1;
88   aabb1.min_ = Vec3f(-600, -600, -600);
89   aabb1.max_ = Vec3f(600, 600, 600);
90 
91   OBB obb1;
92   convertBV(aabb1, rotate_transform[0], obb1);
93   Box box1;
94   Transform3f box1_tf;
95   constructBox(aabb1, rotate_transform[0], box1, box1_tf);
96 
97   FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
98   std::size_t n = 1000;
99 
100   std::vector<Transform3f> transforms;
101   generateRandomTransforms(extents, transforms, n);
102 
103   for(std::size_t i = 0; i < transforms.size(); ++i)
104   {
105     AABB aabb;
106     aabb.min_ = aabb1.min_ * 0.5;
107     aabb.max_ = aabb1.max_ * 0.5;
108 
109     OBB obb2;
110     convertBV(aabb, transforms[i], obb2);
111 
112     Box box2;
113     Transform3f box2_tf;
114     constructBox(aabb, transforms[i], box2, box2_tf);
115 
116     GJKSolver_libccd solver;
117 
118     bool overlap_obb = obb1.overlap(obb2);
119     bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, NULL);
120 
121     BOOST_CHECK(overlap_obb == overlap_box);
122   }
123 }
124 
BOOST_AUTO_TEST_CASE(OBB_shape_test)125 BOOST_AUTO_TEST_CASE(OBB_shape_test)
126 {
127   FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
128   std::vector<Transform3f> rotate_transform;
129   generateRandomTransforms(r_extents, rotate_transform, 1);
130 
131   AABB aabb1;
132   aabb1.min_ = Vec3f(-600, -600, -600);
133   aabb1.max_ = Vec3f(600, 600, 600);
134 
135   OBB obb1;
136   convertBV(aabb1, rotate_transform[0], obb1);
137   Box box1;
138   Transform3f box1_tf;
139   constructBox(aabb1, rotate_transform[0], box1, box1_tf);
140 
141   FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
142   std::size_t n = 1000;
143 
144   std::vector<Transform3f> transforms;
145   generateRandomTransforms(extents, transforms, n);
146 
147   for(std::size_t i = 0; i < transforms.size(); ++i)
148   {
149     FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5;
150     OBB obb2;
151     GJKSolver_libccd solver;
152 
153     {
154       Sphere sphere(len);
155       computeBV(sphere, transforms[i], obb2);
156 
157       bool overlap_obb = obb1.overlap(obb2);
158       bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], NULL);
159       BOOST_CHECK(overlap_obb >= overlap_sphere);
160     }
161 
162     {
163       Ellipsoid ellipsoid(len, len, len);
164       computeBV(ellipsoid, transforms[i], obb2);
165 
166       bool overlap_obb = obb1.overlap(obb2);
167       bool overlap_ellipsoid = solver.shapeIntersect(box1, box1_tf, ellipsoid, transforms[i], NULL);
168       BOOST_CHECK(overlap_obb >= overlap_ellipsoid);
169     }
170 
171     {
172       Capsule capsule(len, 2 * len);
173       computeBV(capsule, transforms[i], obb2);
174 
175       bool overlap_obb = obb1.overlap(obb2);
176       bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], NULL);
177       BOOST_CHECK(overlap_obb >= overlap_capsule);
178     }
179 
180     {
181       Cone cone(len, 2 * len);
182       computeBV(cone, transforms[i], obb2);
183 
184       bool overlap_obb = obb1.overlap(obb2);
185       bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], NULL);
186       BOOST_CHECK(overlap_obb >= overlap_cone);
187     }
188 
189     {
190       Cylinder cylinder(len, 2 * len);
191       computeBV(cylinder, transforms[i], obb2);
192 
193       bool overlap_obb = obb1.overlap(obb2);
194       bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], NULL);
195       BOOST_CHECK(overlap_obb >= overlap_cylinder);
196     }
197   }
198 }
199 
BOOST_AUTO_TEST_CASE(OBB_AABB_test)200 BOOST_AUTO_TEST_CASE(OBB_AABB_test)
201 {
202   FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
203   std::size_t n = 1000;
204 
205   std::vector<Transform3f> transforms;
206   generateRandomTransforms(extents, transforms, n);
207 
208   AABB aabb1;
209   aabb1.min_ = Vec3f(-600, -600, -600);
210   aabb1.max_ = Vec3f(600, 600, 600);
211 
212   OBB obb1;
213   convertBV(aabb1, Transform3f(), obb1);
214 
215   for(std::size_t i = 0; i < transforms.size(); ++i)
216   {
217     AABB aabb;
218     aabb.min_ = aabb1.min_ * 0.5;
219     aabb.max_ = aabb1.max_ * 0.5;
220 
221     AABB aabb2 = translate(aabb, transforms[i].getTranslation());
222 
223     OBB obb2;
224     convertBV(aabb, Transform3f(transforms[i].getTranslation()), obb2);
225 
226     bool overlap_aabb = aabb1.overlap(aabb2);
227     bool overlap_obb = obb1.overlap(obb2);
228     if(overlap_aabb != overlap_obb)
229     {
230       std::cout << aabb1.min_ << " " << aabb1.max_ << std::endl;
231       std::cout << aabb2.min_ << " " << aabb2.max_ << std::endl;
232       std::cout << obb1.To << " " << obb1.extent << " " << obb1.axis[0] << " " << obb1.axis[1] << " " << obb1.axis[2] << std::endl;
233       std::cout << obb2.To << " " << obb2.extent << " " << obb2.axis[0] << " " << obb2.axis[1] << " " << obb2.axis[2] << std::endl;
234     }
235 
236     BOOST_CHECK(overlap_aabb == overlap_obb);
237   }
238   std::cout << std::endl;
239 }
240 
BOOST_AUTO_TEST_CASE(mesh_mesh)241 BOOST_AUTO_TEST_CASE(mesh_mesh)
242 {
243   std::vector<Vec3f> p1, p2;
244   std::vector<Triangle> t1, t2;
245   boost::filesystem::path path(TEST_RESOURCES_DIR);
246 
247   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
248   loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
249 
250   std::vector<Transform3f> transforms;
251   FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
252   std::size_t n = 10;
253   bool verbose = false;
254 
255   generateRandomTransforms(extents, transforms, n);
256 
257   // collision
258   for(std::size_t i = 0; i < transforms.size(); ++i)
259   {
260     global_pairs.clear();
261     global_pairs_now.clear();
262 
263     collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
264 
265     collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
266     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
267     for(std::size_t j = 0; j < global_pairs.size(); ++j)
268     {
269       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
270       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
271     }
272 
273     collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
274     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
275     for(std::size_t j = 0; j < global_pairs.size(); ++j)
276     {
277       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
278       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
279     }
280 
281     collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
282     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
283     for(std::size_t j = 0; j < global_pairs.size(); ++j)
284     {
285       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
286       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
287     }
288 
289     collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
290     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
291     for(std::size_t j = 0; j < global_pairs.size(); ++j)
292     {
293       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
294       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
295     }
296 
297     collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
298     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
299     for(std::size_t j = 0; j < global_pairs.size(); ++j)
300     {
301       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
302       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
303     }
304 
305     collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
306     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
307     for(std::size_t j = 0; j < global_pairs.size(); ++j)
308     {
309       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
310       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
311     }
312 
313     collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
314     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
315     for(std::size_t j = 0; j < global_pairs.size(); ++j)
316     {
317       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
318       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
319     }
320 
321     collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
322     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
323     for(std::size_t j = 0; j < global_pairs.size(); ++j)
324     {
325       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
326       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
327     }
328 
329     collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
330     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
331     for(std::size_t j = 0; j < global_pairs.size(); ++j)
332     {
333       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
334       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
335     }
336 
337     collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
338     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
339     for(std::size_t j = 0; j < global_pairs.size(); ++j)
340     {
341       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
342       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
343     }
344 
345     collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
346     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
347     for(std::size_t j = 0; j < global_pairs.size(); ++j)
348     {
349       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
350       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
351     }
352 
353     collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
354     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
355     for(std::size_t j = 0; j < global_pairs.size(); ++j)
356     {
357       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
358       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
359     }
360 
361     collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
362     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
363     for(std::size_t j = 0; j < global_pairs.size(); ++j)
364     {
365       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
366       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
367     }
368 
369     collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
370     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
371     for(std::size_t j = 0; j < global_pairs.size(); ++j)
372     {
373       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
374       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
375     }
376 
377     collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
378     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
379     for(std::size_t j = 0; j < global_pairs.size(); ++j)
380     {
381       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
382       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
383     }
384 
385     collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
386     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
387     for(std::size_t j = 0; j < global_pairs.size(); ++j)
388     {
389       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
390       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
391     }
392 
393     collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
394     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
395     for(std::size_t j = 0; j < global_pairs.size(); ++j)
396     {
397       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
398       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
399     }
400 
401     collide_Test2<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
402     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
403     for(std::size_t j = 0; j < global_pairs.size(); ++j)
404     {
405       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
406       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
407     }
408 
409     collide_Test2<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
410     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
411     for(std::size_t j = 0; j < global_pairs.size(); ++j)
412     {
413       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
414       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
415     }
416 
417     collide_Test2<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
418     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
419     for(std::size_t j = 0; j < global_pairs.size(); ++j)
420     {
421       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
422       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
423     }
424 
425     collide_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
426     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
427     for(std::size_t j = 0; j < global_pairs.size(); ++j)
428     {
429       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
430       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
431     }
432 
433     collide_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
434     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
435     for(std::size_t j = 0; j < global_pairs.size(); ++j)
436     {
437       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
438       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
439     }
440 
441     collide_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
442     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
443     for(std::size_t j = 0; j < global_pairs.size(); ++j)
444     {
445       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
446       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
447     }
448 
449     collide_Test2<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
450     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
451     for(std::size_t j = 0; j < global_pairs.size(); ++j)
452     {
453       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
454       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
455     }
456 
457     collide_Test2<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
458     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
459     for(std::size_t j = 0; j < global_pairs.size(); ++j)
460     {
461       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
462       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
463     }
464 
465     collide_Test2<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
466     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
467     for(std::size_t j = 0; j < global_pairs.size(); ++j)
468     {
469       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
470       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
471     }
472 
473     collide_Test2<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
474     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
475     for(std::size_t j = 0; j < global_pairs.size(); ++j)
476     {
477       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
478       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
479     }
480 
481     collide_Test2<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
482     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
483     for(std::size_t j = 0; j < global_pairs.size(); ++j)
484     {
485       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
486       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
487     }
488 
489     collide_Test2<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
490     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
491     for(std::size_t j = 0; j < global_pairs.size(); ++j)
492     {
493       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
494       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
495     }
496 
497     collide_Test2<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
498     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
499     for(std::size_t j = 0; j < global_pairs.size(); ++j)
500     {
501       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
502       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
503     }
504 
505     collide_Test2<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
506     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
507     for(std::size_t j = 0; j < global_pairs.size(); ++j)
508     {
509       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
510       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
511     }
512 
513     collide_Test2<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
514     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
515     for(std::size_t j = 0; j < global_pairs.size(); ++j)
516     {
517       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
518       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
519     }
520 
521     collide_Test2<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
522     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
523     for(std::size_t j = 0; j < global_pairs.size(); ++j)
524     {
525       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
526       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
527     }
528 
529     collide_Test2<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
530     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
531     for(std::size_t j = 0; j < global_pairs.size(); ++j)
532     {
533       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
534       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
535     }
536 
537     collide_Test2<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
538     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
539     for(std::size_t j = 0; j < global_pairs.size(); ++j)
540     {
541       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
542       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
543     }
544 
545     collide_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
546 
547     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
548     for(std::size_t j = 0; j < global_pairs.size(); ++j)
549     {
550       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
551       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
552     }
553 
554     collide_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
555 
556     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
557     for(std::size_t j = 0; j < global_pairs.size(); ++j)
558     {
559       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
560       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
561     }
562 
563     collide_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
564     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
565     for(std::size_t j = 0; j < global_pairs.size(); ++j)
566     {
567       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
568       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
569     }
570 
571     collide_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
572     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
573     for(std::size_t j = 0; j < global_pairs.size(); ++j)
574     {
575       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
576       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
577     }
578 
579     collide_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
580     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
581     for(std::size_t j = 0; j < global_pairs.size(); ++j)
582     {
583       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
584       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
585     }
586 
587     collide_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
588     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
589     for(std::size_t j = 0; j < global_pairs.size(); ++j)
590     {
591       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
592       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
593     }
594 
595     test_collide_func<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
596     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
597     for(std::size_t j = 0; j < global_pairs.size(); ++j)
598     {
599       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
600       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
601     }
602 
603     test_collide_func<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
604     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
605     for(std::size_t j = 0; j < global_pairs.size(); ++j)
606     {
607       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
608       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
609     }
610 
611     test_collide_func<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
612     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
613     for(std::size_t j = 0; j < global_pairs.size(); ++j)
614     {
615       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
616       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
617     }
618 
619 
620     collide_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
621     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
622     for(std::size_t j = 0; j < global_pairs.size(); ++j)
623     {
624       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
625       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
626     }
627 
628     collide_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
629     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
630     for(std::size_t j = 0; j < global_pairs.size(); ++j)
631     {
632       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
633       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
634     }
635 
636     collide_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
637     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
638     for(std::size_t j = 0; j < global_pairs.size(); ++j)
639     {
640       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
641       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
642     }
643 
644     collide_Test2<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
645     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
646     for(std::size_t j = 0; j < global_pairs.size(); ++j)
647     {
648       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
649       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
650     }
651 
652     collide_Test2<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
653     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
654     for(std::size_t j = 0; j < global_pairs.size(); ++j)
655     {
656       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
657       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
658     }
659 
660     collide_Test2<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
661     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
662     for(std::size_t j = 0; j < global_pairs.size(); ++j)
663     {
664       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
665       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
666     }
667 
668     collide_Test_Oriented<kIOS, MeshCollisionTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
669     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
670     for(std::size_t j = 0; j < global_pairs.size(); ++j)
671     {
672       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
673       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
674     }
675 
676     collide_Test_Oriented<kIOS, MeshCollisionTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
677     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
678     for(std::size_t j = 0; j < global_pairs.size(); ++j)
679     {
680       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
681       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
682     }
683 
684     collide_Test_Oriented<kIOS, MeshCollisionTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
685     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
686     for(std::size_t j = 0; j < global_pairs.size(); ++j)
687     {
688       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
689       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
690     }
691 
692     test_collide_func<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN);
693     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
694     for(std::size_t j = 0; j < global_pairs.size(); ++j)
695     {
696       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
697       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
698     }
699 
700     test_collide_func<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
701     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
702     for(std::size_t j = 0; j < global_pairs.size(); ++j)
703     {
704       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
705       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
706     }
707 
708     test_collide_func<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER);
709     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
710     for(std::size_t j = 0; j < global_pairs.size(); ++j)
711     {
712       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
713       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
714     }
715 
716     collide_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
717     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
718     for(std::size_t j = 0; j < global_pairs.size(); ++j)
719     {
720       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
721       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
722     }
723 
724     collide_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
725     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
726     for(std::size_t j = 0; j < global_pairs.size(); ++j)
727     {
728       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
729       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
730     }
731 
732     collide_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
733     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
734     for(std::size_t j = 0; j < global_pairs.size(); ++j)
735     {
736       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
737       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
738     }
739 
740     collide_Test2<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
741     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
742     for(std::size_t j = 0; j < global_pairs.size(); ++j)
743     {
744       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
745       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
746     }
747 
748     collide_Test2<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
749     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
750     for(std::size_t j = 0; j < global_pairs.size(); ++j)
751     {
752       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
753       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
754     }
755 
756     collide_Test2<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
757     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
758     for(std::size_t j = 0; j < global_pairs.size(); ++j)
759     {
760       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
761       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
762     }
763 
764     collide_Test_Oriented<OBBRSS, MeshCollisionTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
765     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
766     for(std::size_t j = 0; j < global_pairs.size(); ++j)
767     {
768       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
769       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
770     }
771 
772     collide_Test_Oriented<OBBRSS, MeshCollisionTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
773     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
774     for(std::size_t j = 0; j < global_pairs.size(); ++j)
775     {
776       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
777       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
778     }
779 
780     collide_Test_Oriented<OBBRSS, MeshCollisionTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
781     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
782     for(std::size_t j = 0; j < global_pairs.size(); ++j)
783     {
784       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
785       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
786     }
787 
788     test_collide_func<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN);
789     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
790     for(std::size_t j = 0; j < global_pairs.size(); ++j)
791     {
792       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
793       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
794     }
795 
796     test_collide_func<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
797     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
798     for(std::size_t j = 0; j < global_pairs.size(); ++j)
799     {
800       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
801       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
802     }
803 
804     test_collide_func<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER);
805     BOOST_CHECK(global_pairs.size() == global_pairs_now.size());
806     for(std::size_t j = 0; j < global_pairs.size(); ++j)
807     {
808       BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1);
809       BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2);
810     }
811   }
812 }
813 
814 
815 template<typename BV>
collide_Test2(const Transform3f & tf,const std::vector<Vec3f> & vertices1,const std::vector<Triangle> & triangles1,const std::vector<Vec3f> & vertices2,const std::vector<Triangle> & triangles2,SplitMethodType split_method,bool verbose)816 bool collide_Test2(const Transform3f& tf,
817                    const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
818                    const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
819 {
820   BVHModel<BV> m1;
821   BVHModel<BV> m2;
822   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
823   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
824 
825   std::vector<Vec3f> vertices1_new(vertices1.size());
826   for(unsigned int i = 0; i < vertices1_new.size(); ++i)
827   {
828     vertices1_new[i] = tf.transform(vertices1[i]);
829   }
830 
831 
832   m1.beginModel();
833   m1.addSubModel(vertices1_new, triangles1);
834   m1.endModel();
835 
836   m2.beginModel();
837   m2.addSubModel(vertices2, triangles2);
838   m2.endModel();
839 
840   Transform3f pose1, pose2;
841 
842   CollisionResult local_result;
843   MeshCollisionTraversalNode<BV> node;
844 
845   if(!initialize<BV>(node, m1, pose1, m2, pose2,
846                      CollisionRequest(num_max_contacts, enable_contact), local_result))
847     std::cout << "initialize error" << std::endl;
848 
849   node.enable_statistics = verbose;
850 
851   collide(&node);
852 
853 
854   if(local_result.numContacts() > 0)
855   {
856     if(global_pairs.size() == 0)
857     {
858       local_result.getContacts(global_pairs);
859       std::sort(global_pairs.begin(), global_pairs.end());
860     }
861     else
862     {
863       local_result.getContacts(global_pairs_now);
864       std::sort(global_pairs_now.begin(), global_pairs_now.end());
865     }
866 
867 
868     if(verbose)
869       std::cout << "in collision " << local_result.numContacts() << ": " << std::endl;
870     if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
871     return true;
872   }
873   else
874   {
875     if(verbose) std::cout << "collision free " << std::endl;
876     if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
877     return false;
878   }
879 }
880 
881 template<typename BV>
collide_Test(const Transform3f & tf,const std::vector<Vec3f> & vertices1,const std::vector<Triangle> & triangles1,const std::vector<Vec3f> & vertices2,const std::vector<Triangle> & triangles2,SplitMethodType split_method,bool verbose)882 bool collide_Test(const Transform3f& tf,
883                   const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
884                   const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
885 {
886   BVHModel<BV> m1;
887   BVHModel<BV> m2;
888   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
889   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
890 
891   m1.beginModel();
892   m1.addSubModel(vertices1, triangles1);
893   m1.endModel();
894 
895   m2.beginModel();
896   m2.addSubModel(vertices2, triangles2);
897   m2.endModel();
898 
899   Transform3f pose1(tf), pose2;
900 
901   CollisionResult local_result;
902   MeshCollisionTraversalNode<BV> node;
903 
904   if(!initialize<BV>(node, m1, pose1, m2, pose2,
905                      CollisionRequest(num_max_contacts, enable_contact), local_result))
906     std::cout << "initialize error" << std::endl;
907 
908   node.enable_statistics = verbose;
909 
910   collide(&node);
911 
912 
913   if(local_result.numContacts() > 0)
914   {
915     if(global_pairs.size() == 0)
916     {
917       local_result.getContacts(global_pairs);
918       std::sort(global_pairs.begin(), global_pairs.end());
919     }
920     else
921     {
922       local_result.getContacts(global_pairs_now);
923       std::sort(global_pairs_now.begin(), global_pairs_now.end());
924     }
925 
926     if(verbose)
927       std::cout << "in collision " << local_result.numContacts() << ": " << std::endl;
928     if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
929     return true;
930   }
931   else
932   {
933     if(verbose) std::cout << "collision free " << std::endl;
934     if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
935     return false;
936   }
937 }
938 
939 template<typename BV, typename TraversalNode>
collide_Test_Oriented(const Transform3f & tf,const std::vector<Vec3f> & vertices1,const std::vector<Triangle> & triangles1,const std::vector<Vec3f> & vertices2,const std::vector<Triangle> & triangles2,SplitMethodType split_method,bool verbose)940 bool collide_Test_Oriented(const Transform3f& tf,
941                            const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
942                            const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
943 {
944   BVHModel<BV> m1;
945   BVHModel<BV> m2;
946   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
947   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
948 
949   m1.beginModel();
950   m1.addSubModel(vertices1, triangles1);
951   m1.endModel();
952 
953   m2.beginModel();
954   m2.addSubModel(vertices2, triangles2);
955   m2.endModel();
956 
957   Transform3f pose1(tf), pose2;
958 
959   CollisionResult local_result;
960   TraversalNode node;
961   if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2,
962                  CollisionRequest(num_max_contacts, enable_contact), local_result))
963     std::cout << "initialize error" << std::endl;
964 
965   node.enable_statistics = verbose;
966 
967   collide(&node);
968 
969   if(local_result.numContacts() > 0)
970   {
971     if(global_pairs.size() == 0)
972     {
973       local_result.getContacts(global_pairs);
974       std::sort(global_pairs.begin(), global_pairs.end());
975     }
976     else
977     {
978       local_result.getContacts(global_pairs_now);
979       std::sort(global_pairs_now.begin(), global_pairs_now.end());
980     }
981 
982     if(verbose)
983       std::cout << "in collision " << local_result.numContacts() << ": " << std::endl;
984     if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
985     return true;
986   }
987   else
988   {
989     if(verbose) std::cout << "collision free " << std::endl;
990     if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
991     return false;
992   }
993 }
994 
995 
996 template<typename BV>
test_collide_func(const Transform3f & tf,const std::vector<Vec3f> & vertices1,const std::vector<Triangle> & triangles1,const std::vector<Vec3f> & vertices2,const std::vector<Triangle> & triangles2,SplitMethodType split_method)997 bool test_collide_func(const Transform3f& tf,
998                        const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
999                        const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method)
1000 {
1001   BVHModel<BV> m1;
1002   BVHModel<BV> m2;
1003   m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
1004   m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
1005 
1006   m1.beginModel();
1007   m1.addSubModel(vertices1, triangles1);
1008   m1.endModel();
1009 
1010   m2.beginModel();
1011   m2.addSubModel(vertices2, triangles2);
1012   m2.endModel();
1013 
1014   Transform3f pose1(tf), pose2;
1015 
1016   std::vector<Contact> contacts;
1017 
1018   CollisionRequest request(num_max_contacts, enable_contact);
1019   CollisionResult result;
1020   int num_contacts = collide(&m1, pose1, &m2, pose2,
1021                              request, result);
1022 
1023   result.getContacts(contacts);
1024 
1025   global_pairs_now.resize(num_contacts);
1026 
1027   for(int i = 0; i < num_contacts; ++i)
1028   {
1029     global_pairs_now[i].b1 = contacts[i].b1;
1030     global_pairs_now[i].b2 = contacts[i].b2;
1031   }
1032 
1033   std::sort(global_pairs_now.begin(), global_pairs_now.end());
1034 
1035   if(num_contacts > 0) return true;
1036   else return false;
1037 }
1038