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