test_fcl_shape_mesh_consistency.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 <array>
39 
40 #include <gtest/gtest.h>
41 
42 #include "fcl/narrowphase/detail/gjk_solver_indep.h"
43 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
44 #include "fcl/geometry/geometric_shape_to_BVH_model.h"
45 #include "fcl/narrowphase/distance.h"
46 #include "fcl/narrowphase/collision.h"
47 #include "test_fcl_utility.h"
48 
49 using namespace fcl;
50 
51 template <typename S>
52 std::array<S, 6>& extents()
53 {
54  static std::array<S, 6> static_extents{ {0, 0, 0, 10, 10, 10} };
55  return static_extents;
56 }
57 
58 template <typename S>
59 void test_consistency_distance_spheresphere_libccd()
60 {
61  Sphere<S> s1(20);
62  Sphere<S> s2(20);
63  BVHModel<RSS<S>> s1_rss;
64  BVHModel<RSS<S>> s2_rss;
65 
66  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
67  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
68 
69  DistanceRequest<S> request;
70  DistanceResult<S> res, res1;
71 
72  Transform3<S> pose = Transform3<S>::Identity();
73 
74  pose.translation() = Vector3<S>(50, 0, 0);
75 
76  res.clear(); res1.clear();
77  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
78  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
79  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
80 
81  res1.clear();
82  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
83  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
84 
85  res1.clear();
86  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
87  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
88 
89  for(std::size_t i = 0; i < 10; ++i)
90  {
91  Transform3<S> t;
92  test::generateRandomTransform(extents<S>(), t);
93 
94  Transform3<S> pose1(t);
95  Transform3<S> pose2 = t * pose;
96 
97  res.clear(); res1.clear();
98  distance(&s1, pose1, &s2, pose2, request, res);
99  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
100  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
101 
102  res1.clear();
103  distance(&s1, pose1, &s2_rss, pose2, request, res1);
104  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
105 
106  res1.clear();
107  distance(&s1_rss, pose1, &s2, pose2, request, res1);
108  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
109  }
110 
111  pose.translation() = Vector3<S>(40.1, 0, 0);
112 
113  res.clear(), res1.clear();
114  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
115  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
116  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
117 
118  res1.clear();
119  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
120  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
121 
122  res1.clear();
123  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
124  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
125 
126 
127  for(std::size_t i = 0; i < 10; ++i)
128  {
129  Transform3<S> t;
130  test::generateRandomTransform(extents<S>(), t);
131 
132  Transform3<S> pose1(t);
133  Transform3<S> pose2 = t * pose;
134 
135  res.clear(); res1.clear();
136  distance(&s1, pose1, &s2, pose2, request, res);
137  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
138  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
139 
140  res1.clear();
141  distance(&s1, pose1, &s2_rss, pose2, request, res1);
142  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
143 
144  res1.clear();
145  distance(&s1_rss, pose1, &s2, pose2, request, res1);
146  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
147  }
148 }
149 
150 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_libccd)
151 {
152 // test_consistency_distance_spheresphere_libccd<float>();
153  test_consistency_distance_spheresphere_libccd<double>();
154 }
155 
156 template <typename S>
157 void test_consistency_distance_ellipsoidellipsoid_libccd()
158 {
159  Ellipsoid<S> s1(20, 40, 50);
160  Ellipsoid<S> s2(10, 10, 10);
161  BVHModel<RSS<S>> s1_rss;
162  BVHModel<RSS<S>> s2_rss;
163 
164  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
165  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
166 
167  DistanceRequest<S> request;
168  DistanceResult<S> res, res1;
169 
170  Transform3<S> pose = Transform3<S>::Identity();
171 
172  pose.translation() = Vector3<S>(40, 0, 0);
173 
174  res.clear(); res1.clear();
175  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
176  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
177  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
178 
179  res1.clear();
180  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
181  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
182 
183  res1.clear();
184  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
185  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
186 
187  for(std::size_t i = 0; i < 10; ++i)
188  {
189  Transform3<S> t;
190  test::generateRandomTransform(extents<S>(), t);
191 
192  Transform3<S> pose1(t);
193  Transform3<S> pose2 = t * pose;
194 
195  res.clear(); res1.clear();
196  distance(&s1, pose1, &s2, pose2, request, res);
197  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
198  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
199 
200  res1.clear();
201  distance(&s1, pose1, &s2_rss, pose2, request, res1);
202  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
203 
204  res1.clear();
205  distance(&s1_rss, pose1, &s2, pose2, request, res1);
206  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
207  }
208 
209  pose.translation() = Vector3<S>(30.1, 0, 0);
210 
211  res.clear(), res1.clear();
212  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
213  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
214  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
215 
216  res1.clear();
217  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
218  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
219 
220  res1.clear();
221  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
222  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
223 
224  for(std::size_t i = 0; i < 10; ++i)
225  {
226  Transform3<S> t;
227  test::generateRandomTransform(extents<S>(), t);
228 
229  Transform3<S> pose1(t);
230  Transform3<S> pose2 = t * pose;
231 
232  res.clear(); res1.clear();
233  distance(&s1, pose1, &s2, pose2, request, res);
234  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
235  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
236 
237  res1.clear();
238  distance(&s1, pose1, &s2_rss, pose2, request, res1);
239  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
240 
241  res1.clear();
242  distance(&s1_rss, pose1, &s2, pose2, request, res1);
243  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
244  }
245 }
246 
247 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_libccd)
248 {
249 // test_consistency_distance_ellipsoidellipsoid_libccd<float>();
250  test_consistency_distance_ellipsoidellipsoid_libccd<double>();
251 }
252 
253 template <typename S>
254 void test_consistency_distance_boxbox_libccd()
255 {
256  Box<S> s1(20, 40, 50);
257  Box<S> s2(10, 10, 10);
258 
259  BVHModel<RSS<S>> s1_rss;
260  BVHModel<RSS<S>> s2_rss;
261 
262  generateBVHModel(s1_rss, s1, Transform3<S>::Identity());
263  generateBVHModel(s2_rss, s2, Transform3<S>::Identity());
264 
265  DistanceRequest<S> request;
266  DistanceResult<S> res, res1;
267 
268  Transform3<S> pose = Transform3<S>::Identity();
269 
270  pose.translation() = Vector3<S>(50, 0, 0);
271 
272  res.clear(); res1.clear();
273  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
274  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
275  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
276 
277  res1.clear();
278  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
279  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
280 
281  res1.clear();
282  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
283  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
284 
285  for(std::size_t i = 0; i < 10; ++i)
286  {
287  Transform3<S> t;
288  test::generateRandomTransform(extents<S>(), t);
289 
290  Transform3<S> pose1(t);
291  Transform3<S> pose2 = t * pose;
292 
293  res.clear(); res1.clear();
294  distance(&s1, pose1, &s2, pose2, request, res);
295  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
296  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
297 
298  res1.clear();
299  distance(&s1, pose1, &s2_rss, pose2, request, res1);
300  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
301 
302  res1.clear();
303  distance(&s1_rss, pose1, &s2, pose2, request, res1);
304  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
305  }
306 
307  pose.translation() = Vector3<S>(15.1, 0, 0);
308 
309  res.clear(); res1.clear();
310  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
311  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
312  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
313 
314  res1.clear();
315  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
316  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
317 
318  res1.clear();
319  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
320  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
321 
322  for(std::size_t i = 0; i < 10; ++i)
323  {
324  Transform3<S> t;
325  test::generateRandomTransform(extents<S>(), t);
326 
327  Transform3<S> pose1(t);
328  Transform3<S> pose2 = t * pose;
329 
330  res.clear(); res1.clear();
331  distance(&s1, pose1, &s2, pose2, request, res);
332  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
333  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
334 
335  res1.clear();
336  distance(&s1, pose1, &s2_rss, pose2, request, res1);
337  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
338 
339  res1.clear();
340  distance(&s1_rss, pose1, &s2, pose2, request, res1);
341  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
342  }
343 }
344 
345 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_libccd)
346 {
347 // test_consistency_distance_boxbox_libccd<float>();
348  test_consistency_distance_boxbox_libccd<double>();
349 }
350 
351 template <typename S>
352 void test_consistency_distance_cylindercylinder_libccd()
353 {
354  Cylinder<S> s1(5, 10);
355  Cylinder<S> s2(5, 10);
356 
357  BVHModel<RSS<S>> s1_rss;
358  BVHModel<RSS<S>> s2_rss;
359 
360  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
361  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
362 
363  DistanceRequest<S> request;
364  DistanceResult<S> res, res1;
365 
366  Transform3<S> pose = Transform3<S>::Identity();
367 
368  pose.translation() = Vector3<S>(20, 0, 0);
369 
370  res.clear(); res1.clear();
371  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
372  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
373  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
374 
375  res1.clear();
376  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
377  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
378 
379  res1.clear();
380  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
381  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
382 
383  for(std::size_t i = 0; i < 10; ++i)
384  {
385  Transform3<S> t;
386  test::generateRandomTransform(extents<S>(), t);
387 
388  Transform3<S> pose1(t);
389  Transform3<S> pose2 = t * pose;
390 
391  res.clear(); res1.clear();
392  distance(&s1, pose1, &s2, pose2, request, res);
393  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
394  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
395 
396  res1.clear();
397  distance(&s1, pose1, &s2_rss, pose2, request, res1);
398  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
399 
400  res1.clear();
401  distance(&s1_rss, pose1, &s2, pose2, request, res1);
402  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
403  }
404 
405  pose.translation() = Vector3<S>(15, 0, 0); // libccd cannot use small value here :(
406 
407  res.clear(); res1.clear();
408  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
409  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
410  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
411 
412  res1.clear();
413  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
414  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
415 
416  res1.clear();
417  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
418  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
419 
420  for(std::size_t i = 0; i < 10; ++i)
421  {
422  Transform3<S> t;
423  test::generateRandomTransform(extents<S>(), t);
424 
425  Transform3<S> pose1(t);
426  Transform3<S> pose2 = t * pose;
427 
428  res.clear(); res1.clear();
429  distance(&s1, pose1, &s2, pose2, request, res);
430  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
431  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
432 
433  res1.clear();
434  distance(&s1, pose1, &s2_rss, pose2, request, res1);
435  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
436 
437  res1.clear();
438  distance(&s1_rss, pose1, &s2, pose2, request, res1);
439  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
440  }
441 }
442 
443 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_libccd)
444 {
445 // test_consistency_distance_cylindercylinder_libccd<float>();
446  test_consistency_distance_cylindercylinder_libccd<double>();
447 }
448 
449 template <typename S>
450 void test_consistency_distance_conecone_libccd()
451 {
452  Cone<S> s1(5, 10);
453  Cone<S> s2(5, 10);
454 
455  BVHModel<RSS<S>> s1_rss;
456  BVHModel<RSS<S>> s2_rss;
457 
458  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
459  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
460 
461  DistanceRequest<S> request;
462  DistanceResult<S> res, res1;
463 
464  Transform3<S> pose = Transform3<S>::Identity();
465 
466  pose.translation() = Vector3<S>(20, 0, 0);
467 
468  res.clear(); res1.clear();
469  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
470  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
471  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
472 
473  res1.clear();
474  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
475  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
476 
477  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
478  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
479 
480  for(std::size_t i = 0; i < 10; ++i)
481  {
482  Transform3<S> t;
483  test::generateRandomTransform(extents<S>(), t);
484 
485  Transform3<S> pose1(t);
486  Transform3<S> pose2 = t * pose;
487 
488  res.clear(); res1.clear();
489  distance(&s1, pose1, &s2, pose2, request, res);
490  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
491  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
492 
493  res1.clear();
494  distance(&s1, pose1, &s2_rss, pose2, request, res1);
495  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
496 
497  res1.clear();
498  distance(&s1_rss, pose1, &s2, pose2, request, res1);
499  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
500  }
501 
502  pose.translation() = Vector3<S>(15, 0, 0); // libccd cannot use small value here :(
503 
504  res.clear(); res1.clear();
505  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
506  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
507  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
508 
509  res1.clear();
510  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
511  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
512 
513  res1.clear();
514  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
515  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
516 
517  for(std::size_t i = 0; i < 10; ++i)
518  {
519  Transform3<S> t;
520  test::generateRandomTransform(extents<S>(), t);
521 
522  Transform3<S> pose1(t);
523  Transform3<S> pose2 = t * pose;
524 
525  res.clear(); res1.clear();
526  distance(&s1, pose1, &s2, pose2, request, res);
527  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
528  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
529 
530  res1.clear();
531  distance(&s1, pose1, &s2_rss, pose2, request, res1);
532  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
533 
534  res1.clear();
535  distance(&s1_rss, pose1, &s2, pose2, request, res1);
536  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
537  }
538 }
539 
540 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_libccd)
541 {
542 // test_consistency_distance_conecone_libccd<float>();
543  test_consistency_distance_conecone_libccd<double>();
544 }
545 
546 template <typename S>
547 void test_consistency_distance_spheresphere_GJK()
548 {
549  Sphere<S> s1(20);
550  Sphere<S> s2(20);
551  BVHModel<RSS<S>> s1_rss;
552  BVHModel<RSS<S>> s2_rss;
553 
554  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
555  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
556 
557  DistanceRequest<S> request;
558  request.gjk_solver_type = GST_INDEP;
559  DistanceResult<S> res, res1;
560 
561  Transform3<S> pose = Transform3<S>::Identity();
562 
563  pose.translation() = Vector3<S>(50, 0, 0);
564 
565  res.clear(); res1.clear();
566  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
567  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
568  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
569 
570  res1.clear();
571  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
572  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
573 
574  res1.clear();
575  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
576  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
577 
578 
579  for(std::size_t i = 0; i < 10; ++i)
580  {
581  Transform3<S> t;
582  test::generateRandomTransform(extents<S>(), t);
583 
584  Transform3<S> pose1(t);
585  Transform3<S> pose2 = t * pose;
586 
587  res.clear(); res1.clear();
588  distance(&s1, pose1, &s2, pose2, request, res);
589  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
590  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
591 
592  res1.clear();
593  distance(&s1, pose1, &s2_rss, pose2, request, res1);
594  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
595 
596  res1.clear();
597  distance(&s1_rss, pose1, &s2, pose2, request, res1);
598  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
599  }
600 
601  pose.translation() = Vector3<S>(40.1, 0, 0);
602 
603  res.clear(); res1.clear();
604  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
605  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
606  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
607 
608  res1.clear();
609  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
610  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
611 
612  res1.clear();
613  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
614  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
615 
616 
617  for(std::size_t i = 0; i < 10; ++i)
618  {
619  Transform3<S> t;
620  test::generateRandomTransform(extents<S>(), t);
621 
622  Transform3<S> pose1(t);
623  Transform3<S> pose2 = t * pose;
624 
625  res.clear(); res1.clear();
626  distance(&s1, pose1, &s2, pose2, request, res);
627  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
628  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
629 
630  res1.clear();
631  distance(&s1, pose1, &s2_rss, pose2, request, res1);
632  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
633 
634  res1.clear();
635  distance(&s1_rss, pose1, &s2, pose2, request, res1);
636  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
637  }
638 }
639 
640 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_GJK)
641 {
642 // test_consistency_distance_spheresphere_GJK<float>();
643  test_consistency_distance_spheresphere_GJK<double>();
644 }
645 
646 template <typename S>
647 void test_consistency_distance_ellipsoidellipsoid_GJK()
648 {
649  Ellipsoid<S> s1(20, 40, 50);
650  Ellipsoid<S> s2(10, 10, 10);
651  BVHModel<RSS<S>> s1_rss;
652  BVHModel<RSS<S>> s2_rss;
653 
654  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
655  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
656 
657  DistanceRequest<S> request;
658  request.gjk_solver_type = GST_INDEP;
659  DistanceResult<S> res, res1;
660 
661  Transform3<S> pose = Transform3<S>::Identity();
662 
663  pose.translation() = Vector3<S>(40, 0, 0);
664 
665  res.clear(); res1.clear();
666  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
667  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
668  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
669 
670  res1.clear();
671  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
672  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
673 
674  res1.clear();
675  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
676  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
677 
678 
679  for(std::size_t i = 0; i < 10; ++i)
680  {
681  Transform3<S> t;
682  test::generateRandomTransform(extents<S>(), t);
683 
684  Transform3<S> pose1(t);
685  Transform3<S> pose2 = t * pose;
686 
687  res.clear(); res1.clear();
688  distance(&s1, pose1, &s2, pose2, request, res);
689  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
690  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
691 
692  res1.clear();
693  distance(&s1, pose1, &s2_rss, pose2, request, res1);
694  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
695 
696  res1.clear();
697  distance(&s1_rss, pose1, &s2, pose2, request, res1);
698  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
699  }
700 
701  pose.translation() = Vector3<S>(30.1, 0, 0);
702 
703  res.clear(); res1.clear();
704  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
705  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
706  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
707 
708  res1.clear();
709  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
710  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
711 
712  res1.clear();
713  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
714  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
715 
716 
717  for(std::size_t i = 0; i < 10; ++i)
718  {
719  Transform3<S> t;
720  test::generateRandomTransform(extents<S>(), t);
721 
722  Transform3<S> pose1(t);
723  Transform3<S> pose2 = t * pose;
724 
725  res.clear(); res1.clear();
726  distance(&s1, pose1, &s2, pose2, request, res);
727  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
728  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
729 
730  res1.clear();
731  distance(&s1, pose1, &s2_rss, pose2, request, res1);
732  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
733 
734  res1.clear();
735  distance(&s1_rss, pose1, &s2, pose2, request, res1);
736  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
737  }
738 }
739 
740 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_GJK)
741 {
742 // test_consistency_distance_ellipsoidellipsoid_GJK<float>();
743  test_consistency_distance_ellipsoidellipsoid_GJK<double>();
744 }
745 
746 template <typename S>
747 void test_consistency_distance_boxbox_GJK()
748 {
749  Box<S> s1(20, 40, 50);
750  Box<S> s2(10, 10, 10);
751 
752  BVHModel<RSS<S>> s1_rss;
753  BVHModel<RSS<S>> s2_rss;
754 
755  generateBVHModel(s1_rss, s1, Transform3<S>::Identity());
756  generateBVHModel(s2_rss, s2, Transform3<S>::Identity());
757 
758  DistanceRequest<S> request;
759  request.gjk_solver_type = GST_INDEP;
760  DistanceResult<S> res, res1;
761 
762  Transform3<S> pose = Transform3<S>::Identity();
763 
764  pose.translation() = Vector3<S>(50, 0, 0);
765 
766  res.clear(); res1.clear();
767  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
768  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
769  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
770 
771  res1.clear();
772  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
773  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
774 
775  res1.clear();
776  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
777  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
778 
779  for(std::size_t i = 0; i < 10; ++i)
780  {
781  Transform3<S> t;
782  test::generateRandomTransform(extents<S>(), t);
783 
784  Transform3<S> pose1(t);
785  Transform3<S> pose2 = t * pose;
786 
787  res.clear(); res1.clear();
788  distance(&s1, pose1, &s2, pose2, request, res);
789  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
790  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
791 
792  res1.clear();
793  distance(&s1, pose1, &s2_rss, pose2, request, res1);
794  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
795 
796  res1.clear();
797  distance(&s1_rss, pose1, &s2, pose2, request, res1);
798  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
799  }
800 
801  pose.translation() = Vector3<S>(15.1, 0, 0);
802 
803  res.clear(); res1.clear();
804  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
805  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
806  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
807 
808  res1.clear();
809  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
810  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
811 
812  res1.clear();
813  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
814  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
815 
816  for(std::size_t i = 0; i < 10; ++i)
817  {
818  Transform3<S> t;
819  test::generateRandomTransform(extents<S>(), t);
820 
821  Transform3<S> pose1(t);
822  Transform3<S> pose2 = t * pose;
823 
824  res.clear(); res1.clear();
825  distance(&s1, pose1, &s2, pose2, request, res);
826  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
827  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
828 
829  res1.clear();
830  distance(&s1, pose1, &s2_rss, pose2, request, res1);
831  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
832 
833  res1.clear();
834  distance(&s1_rss, pose1, &s2, pose2, request, res1);
835  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
836  }
837 }
838 
839 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_GJK)
840 {
841 // test_consistency_distance_boxbox_GJK<float>();
842  test_consistency_distance_boxbox_GJK<double>();
843 }
844 
845 template <typename S>
846 void test_consistency_distance_cylindercylinder_GJK()
847 {
848  Cylinder<S> s1(5, 10);
849  Cylinder<S> s2(5, 10);
850 
851  BVHModel<RSS<S>> s1_rss;
852  BVHModel<RSS<S>> s2_rss;
853 
854  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
855  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
856 
857  DistanceRequest<S> request;
858  request.gjk_solver_type = GST_INDEP;
859  DistanceResult<S> res, res1;
860 
861  Transform3<S> pose = Transform3<S>::Identity();
862 
863  pose.translation() = Vector3<S>(20, 0, 0);
864 
865  res.clear(); res1.clear();
866  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
867  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
868  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
869 
870  res1.clear();
871  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
872  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
873 
874  res1.clear();
875  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
876  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
877 
878  for(std::size_t i = 0; i < 10; ++i)
879  {
880  Transform3<S> t;
881  test::generateRandomTransform(extents<S>(), t);
882 
883  Transform3<S> pose1(t);
884  Transform3<S> pose2 = t * pose;
885 
886  res.clear(); res1.clear();
887  distance(&s1, pose1, &s2, pose2, request, res);
888  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
889  if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
890  std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
891  else
892  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
893 
894  res1.clear();
895  distance(&s1, pose1, &s2_rss, pose2, request, res1);
896  if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
897  std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
898  else
899  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
900 
901  res1.clear();
902  distance(&s1_rss, pose1, &s2, pose2, request, res1);
903  if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
904  std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
905  else
906  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
907  }
908 
909  pose.translation() = Vector3<S>(10.1, 0, 0);
910 
911  res.clear(); res1.clear();
912  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
913  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
914  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
915 
916  res1.clear();
917  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
918  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
919 
920  res1.clear();
921  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
922  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
923 
924  for(std::size_t i = 0; i < 10; ++i)
925  {
926  Transform3<S> t;
927  test::generateRandomTransform(extents<S>(), t);
928 
929  Transform3<S> pose1(t);
930  Transform3<S> pose2 = t * pose;
931 
932  res.clear(); res1.clear();
933  distance(&s1, pose1, &s2, pose2, request, res);
934  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
935  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
936 
937  res1.clear();
938  distance(&s1, pose1, &s2_rss, pose2, request, res1);
939  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
940 
941  res1.clear();
942  distance(&s1_rss, pose1, &s2, pose2, request, res1);
943  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
944  }
945 }
946 
947 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_GJK)
948 {
949 // test_consistency_distance_cylindercylinder_GJK<float>();
950  test_consistency_distance_cylindercylinder_GJK<double>();
951 }
952 
953 template <typename S>
954 void test_consistency_distance_conecone_GJK()
955 {
956  Cone<S> s1(5, 10);
957  Cone<S> s2(5, 10);
958 
959  BVHModel<RSS<S>> s1_rss;
960  BVHModel<RSS<S>> s2_rss;
961 
962  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
963  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
964 
965  DistanceRequest<S> request;
966  request.gjk_solver_type = GST_INDEP;
967  DistanceResult<S> res, res1;
968 
969  Transform3<S> pose = Transform3<S>::Identity();
970 
971  pose.translation() = Vector3<S>(20, 0, 0);
972 
973  res.clear(); res1.clear();
974  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
975  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
976  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
977 
978  res1.clear();
979  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
980  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
981 
982  res1.clear();
983  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
984  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
985 
986  for(std::size_t i = 0; i < 10; ++i)
987  {
988  Transform3<S> t;
989  test::generateRandomTransform(extents<S>(), t);
990 
991  Transform3<S> pose1(t);
992  Transform3<S> pose2 = t * pose;
993 
994  res.clear(); res1.clear();
995  distance(&s1, pose1, &s2, pose2, request, res);
996  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
997  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
998 
999  res1.clear();
1000  distance(&s1, pose1, &s2_rss, pose2, request, res1);
1001  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
1002 
1003  res1.clear();
1004  distance(&s1_rss, pose1, &s2, pose2, request, res1);
1005  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
1006  }
1007 
1008  pose.translation() = Vector3<S>(10.1, 0, 0);
1009 
1010  res.clear(); res1.clear();
1011  distance(&s1, Transform3<S>::Identity(), &s2, pose, request, res);
1012  distance(&s1_rss, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
1013  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
1014 
1015  res1.clear();
1016  distance(&s1, Transform3<S>::Identity(), &s2_rss, pose, request, res1);
1017  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
1018 
1019  res1.clear();
1020  distance(&s1_rss, Transform3<S>::Identity(), &s2, pose, request, res1);
1021  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
1022 
1023  for(std::size_t i = 0; i < 10; ++i)
1024  {
1025  Transform3<S> t;
1026  test::generateRandomTransform(extents<S>(), t);
1027 
1028  Transform3<S> pose1(t);
1029  Transform3<S> pose2 = t * pose;
1030 
1031  res.clear(); res1.clear();
1032  distance(&s1, pose1, &s2, pose2, request, res);
1033  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
1034  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
1035 
1036  res1.clear();
1037  distance(&s1, pose1, &s2_rss, pose2, request, res1);
1038  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
1039 
1040  res1.clear();
1041  distance(&s1_rss, pose1, &s2, pose2, request, res1);
1042  EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
1043  }
1044 }
1045 
1046 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_GJK)
1047 {
1048 // test_consistency_distance_conecone_GJK<float>();
1049  test_consistency_distance_conecone_GJK<double>();
1050 }
1051 
1052 template <typename S>
1053 void test_consistency_collision_spheresphere_libccd()
1054 {
1055  Sphere<S> s1(20);
1056  Sphere<S> s2(10);
1057  BVHModel<AABB<S>> s1_aabb;
1058  BVHModel<AABB<S>> s2_aabb;
1059  BVHModel<OBB<S>> s1_obb;
1060  BVHModel<OBB<S>> s2_obb;
1061 
1062  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
1063  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
1064  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
1065  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
1066 
1067  CollisionRequest<S> request;
1068  CollisionResult<S> result;
1069 
1070  bool res;
1071 
1072  Transform3<S> pose = Transform3<S>::Identity();
1073  Transform3<S> pose_aabb = Transform3<S>::Identity();
1074  Transform3<S> pose_obb = Transform3<S>::Identity();
1075 
1076 
1077  // s2 is within s1
1078  // both are shapes --> collision
1079  // s1 is shape, s2 is mesh --> in collision
1080  // s1 is mesh, s2 is shape --> collision free
1081  // all are reasonable
1082  result.clear();
1083  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1084  EXPECT_TRUE(res);
1085  result.clear();
1086  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1087  EXPECT_TRUE(res);
1088  result.clear();
1089  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1090  EXPECT_TRUE(res);
1091  result.clear();
1092  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1093  EXPECT_TRUE(res);
1094  result.clear();
1095  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1096  EXPECT_TRUE(res);
1097  result.clear();
1098  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1099  EXPECT_FALSE(res);
1100  result.clear();
1101  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1102  EXPECT_FALSE(res);
1103  result.clear();
1104  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1105  EXPECT_FALSE(res);
1106  result.clear();
1107  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1108  EXPECT_FALSE(res);
1109 
1110  pose.translation() = Vector3<S>(40, 0, 0);
1111  pose_aabb.translation() = Vector3<S>(40, 0, 0);
1112  pose_obb.translation() = Vector3<S>(40, 0, 0);
1113 
1114  result.clear();
1115  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1116  EXPECT_FALSE(res);
1117  result.clear();
1118  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1119  EXPECT_FALSE(res);
1120  result.clear();
1121  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1122  EXPECT_FALSE(res);
1123  result.clear();
1124  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1125  EXPECT_FALSE(res);
1126  result.clear();
1127  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1128  EXPECT_FALSE(res);
1129  result.clear();
1130  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1131  EXPECT_FALSE(res);
1132  result.clear();
1133  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1134  EXPECT_FALSE(res);
1135  result.clear();
1136  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1137  EXPECT_FALSE(res);
1138  result.clear();
1139  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1140  EXPECT_FALSE(res);
1141 
1142  pose.translation() = Vector3<S>(30, 0, 0);
1143  pose_aabb.translation() = Vector3<S>(30, 0, 0);
1144  pose_obb.translation() = Vector3<S>(30, 0, 0);
1145 
1146  result.clear();
1147  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1148  EXPECT_TRUE(res);
1149  result.clear();
1150  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1151  EXPECT_FALSE(res);
1152  result.clear();
1153  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1154  EXPECT_FALSE(res);
1155  result.clear();
1156  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1157  EXPECT_FALSE(res);
1158  result.clear();
1159  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1160  EXPECT_FALSE(res);
1161  result.clear();
1162  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1163  EXPECT_FALSE(res);
1164  result.clear();
1165  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1166  EXPECT_FALSE(res);
1167  result.clear();
1168  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1169  EXPECT_FALSE(res);
1170  result.clear();
1171  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1172  EXPECT_FALSE(res);
1173 
1174  pose.translation() = Vector3<S>(29.9, 0, 0);
1175  pose_aabb.translation() = Vector3<S>(29.8, 0, 0); // 29.9 fails, result depends on mesh precision
1176  pose_obb.translation() = Vector3<S>(29.8, 0, 0); // 29.9 fails, result depends on mesh precision
1177 
1178  result.clear();
1179  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1180  EXPECT_TRUE(res);
1181  result.clear();
1182  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1183  EXPECT_TRUE(res);
1184  result.clear();
1185  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1186  EXPECT_TRUE(res);
1187  result.clear();
1188  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1189  EXPECT_TRUE(res);
1190  result.clear();
1191  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1192  EXPECT_TRUE(res);
1193  result.clear();
1194  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1195  EXPECT_TRUE(res);
1196  result.clear();
1197  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1198  EXPECT_TRUE(res);
1199  result.clear();
1200  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1201  EXPECT_TRUE(res);
1202  result.clear();
1203  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1204  EXPECT_TRUE(res);
1205 
1206 
1207  pose.translation() = Vector3<S>(-29.9, 0, 0);
1208  pose_aabb.translation() = Vector3<S>(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision
1209  pose_obb.translation() = Vector3<S>(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision
1210 
1211  result.clear();
1212  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1213  EXPECT_TRUE(res);
1214  result.clear();
1215  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1216  EXPECT_TRUE(res);
1217  result.clear();
1218  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1219  EXPECT_TRUE(res);
1220  result.clear();
1221  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1222  EXPECT_TRUE(res);
1223  result.clear();
1224  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1225  EXPECT_TRUE(res);
1226  result.clear();
1227  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1228  EXPECT_TRUE(res);
1229  result.clear();
1230  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1231  EXPECT_TRUE(res);
1232  result.clear();
1233  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1234  EXPECT_TRUE(res);
1235  result.clear();
1236  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1237  EXPECT_TRUE(res);
1238 
1239  pose.translation() = Vector3<S>(-30, 0, 0);
1240  pose_aabb.translation() = Vector3<S>(-30, 0, 0);
1241  pose_obb.translation() = Vector3<S>(-30, 0, 0);
1242 
1243  result.clear();
1244  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1245  EXPECT_TRUE(res);
1246  result.clear();
1247  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1248  EXPECT_FALSE(res);
1249  result.clear();
1250  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1251  EXPECT_FALSE(res);
1252  result.clear();
1253  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1254  EXPECT_FALSE(res);
1255  result.clear();
1256  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1257  EXPECT_FALSE(res);
1258  result.clear();
1259  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1260  EXPECT_FALSE(res);
1261  result.clear();
1262  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1263  EXPECT_FALSE(res);
1264  result.clear();
1265  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1266  EXPECT_FALSE(res);
1267  result.clear();
1268  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1269  EXPECT_FALSE(res);
1270 }
1271 
1272 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spheresphere_libccd)
1273 {
1274 // test_consistency_collision_spheresphere_libccd<float>();
1275  test_consistency_collision_spheresphere_libccd<double>();
1276 }
1277 
1278 template <typename S>
1279 void test_consistency_collision_ellipsoidellipsoid_libccd()
1280 {
1281  Ellipsoid<S> s1(20, 40, 50);
1282  Ellipsoid<S> s2(10, 10, 10);
1283  BVHModel<AABB<S>> s1_aabb;
1284  BVHModel<AABB<S>> s2_aabb;
1285  BVHModel<OBB<S>> s1_obb;
1286  BVHModel<OBB<S>> s2_obb;
1287 
1288  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
1289  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
1290  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
1291  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
1292 
1293  CollisionRequest<S> request;
1294  CollisionResult<S> result;
1295 
1296  bool res;
1297 
1298  Transform3<S> pose = Transform3<S>::Identity();
1299  Transform3<S> pose_aabb = Transform3<S>::Identity();
1300  Transform3<S> pose_obb = Transform3<S>::Identity();
1301 
1302 
1303  // s2 is within s1
1304  // both are shapes --> collision
1305  // s1 is shape, s2 is mesh --> in collision
1306  // s1 is mesh, s2 is shape --> collision free
1307  // all are reasonable
1308  result.clear();
1309  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1310  EXPECT_TRUE(res);
1311  result.clear();
1312  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1313  EXPECT_TRUE(res);
1314  result.clear();
1315  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1316  EXPECT_TRUE(res);
1317  result.clear();
1318  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1319  EXPECT_TRUE(res);
1320  result.clear();
1321  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1322  EXPECT_TRUE(res);
1323  result.clear();
1324  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1325  EXPECT_FALSE(res);
1326  result.clear();
1327  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1328  EXPECT_FALSE(res);
1329  result.clear();
1330  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1331  EXPECT_FALSE(res);
1332  result.clear();
1333  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1334  EXPECT_FALSE(res);
1335 
1336  pose.translation() = Vector3<S>(40, 0, 0);
1337  pose_aabb.translation() = Vector3<S>(40, 0, 0);
1338  pose_obb.translation() = Vector3<S>(40, 0, 0);
1339 
1340  result.clear();
1341  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1342  EXPECT_FALSE(res);
1343  result.clear();
1344  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1345  EXPECT_FALSE(res);
1346  result.clear();
1347  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1348  EXPECT_FALSE(res);
1349  result.clear();
1350  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1351  EXPECT_FALSE(res);
1352  result.clear();
1353  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1354  EXPECT_FALSE(res);
1355  result.clear();
1356  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1357  EXPECT_FALSE(res);
1358  result.clear();
1359  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1360  EXPECT_FALSE(res);
1361  result.clear();
1362  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1363  EXPECT_FALSE(res);
1364  result.clear();
1365  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1366  EXPECT_FALSE(res);
1367 
1368  pose.translation() = Vector3<S>(30, 0, 0);
1369  pose_aabb.translation() = Vector3<S>(30, 0, 0);
1370  pose_obb.translation() = Vector3<S>(30, 0, 0);
1371 
1372  result.clear();
1373  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1374  EXPECT_FALSE(res); // libccd cannot detect collision when two ellipsoid is exactly touching each other
1375  result.clear();
1376  res = (collide(&s1, Transform3<S>::Identity(), &s2, Transform3<S>(Translation3<S>(Vector3<S>(29.999, 0, 0))), request, result) > 0);
1377  EXPECT_TRUE(res);
1378  result.clear();
1379  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1380  EXPECT_FALSE(res);
1381  result.clear();
1382  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1383  EXPECT_FALSE(res);
1384  result.clear();
1385  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1386  EXPECT_FALSE(res);
1387  result.clear();
1388  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1389  EXPECT_FALSE(res);
1390  result.clear();
1391  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1392  EXPECT_FALSE(res);
1393  result.clear();
1394  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1395  EXPECT_FALSE(res);
1396  result.clear();
1397  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1398  EXPECT_FALSE(res);
1399  result.clear();
1400  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1401  EXPECT_FALSE(res);
1402 
1403  pose.translation() = Vector3<S>(29.9, 0, 0);
1404  pose_aabb.translation() = Vector3<S>(29.9, 0, 0); // 29.9 fails, result depends on mesh precision
1405  pose_obb.translation() = Vector3<S>(29.9, 0, 0); // 29.9 fails, result depends on mesh precision
1406 
1407  result.clear();
1408  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1409  EXPECT_TRUE(res);
1410  result.clear();
1411  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1412  EXPECT_TRUE(res);
1413  result.clear();
1414  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1415  EXPECT_TRUE(res);
1416  result.clear();
1417  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1418  EXPECT_TRUE(res);
1419  result.clear();
1420  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1421  EXPECT_TRUE(res);
1422  result.clear();
1423  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1424  EXPECT_TRUE(res);
1425  result.clear();
1426  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1427  EXPECT_TRUE(res);
1428  result.clear();
1429  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1430  EXPECT_TRUE(res);
1431  result.clear();
1432  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1433  EXPECT_TRUE(res);
1434 
1435  pose.translation() = Vector3<S>(-29.9, 0, 0);
1436  pose_aabb.translation() = Vector3<S>(-29.9, 0, 0);
1437  pose_obb.translation() = Vector3<S>(-29.9, 0, 0);
1438 
1439  result.clear();
1440  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1441  EXPECT_TRUE(res);
1442  result.clear();
1443  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1444  EXPECT_TRUE(res);
1445  result.clear();
1446  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1447  EXPECT_TRUE(res);
1448  result.clear();
1449  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1450  EXPECT_TRUE(res);
1451  result.clear();
1452  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1453  EXPECT_TRUE(res);
1454  result.clear();
1455  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1456  EXPECT_TRUE(res);
1457  result.clear();
1458  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1459  EXPECT_TRUE(res);
1460  result.clear();
1461  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1462  EXPECT_TRUE(res);
1463  result.clear();
1464  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1465  EXPECT_TRUE(res);
1466 
1467  pose.translation() = Vector3<S>(-30, 0, 0);
1468  pose_aabb.translation() = Vector3<S>(-30, 0, 0);
1469  pose_obb.translation() = Vector3<S>(-30, 0, 0);
1470 
1471  result.clear();
1472  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1473  EXPECT_FALSE(res);
1474  result.clear();
1475  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1476  EXPECT_FALSE(res);
1477  result.clear();
1478  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1479  EXPECT_FALSE(res);
1480  result.clear();
1481  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1482  EXPECT_FALSE(res);
1483  result.clear();
1484  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1485  EXPECT_FALSE(res);
1486  result.clear();
1487  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1488  EXPECT_FALSE(res);
1489  result.clear();
1490  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1491  EXPECT_FALSE(res);
1492  result.clear();
1493  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1494  EXPECT_FALSE(res);
1495  result.clear();
1496  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1497  EXPECT_FALSE(res);
1498 }
1499 
1500 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_ellipsoidellipsoid_libccd)
1501 {
1502 // test_consistency_collision_ellipsoidellipsoid_libccd<float>();
1503  test_consistency_collision_ellipsoidellipsoid_libccd<double>();
1504 }
1505 
1506 template <typename S>
1507 void test_consistency_collision_boxbox_libccd()
1508 {
1509  Box<S> s1(20, 40, 50);
1510  Box<S> s2(10, 10, 10);
1511 
1512  BVHModel<AABB<S>> s1_aabb;
1513  BVHModel<AABB<S>> s2_aabb;
1514  BVHModel<OBB<S>> s1_obb;
1515  BVHModel<OBB<S>> s2_obb;
1516 
1517  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity());
1518  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity());
1519  generateBVHModel(s1_obb, s1, Transform3<S>::Identity());
1520  generateBVHModel(s2_obb, s2, Transform3<S>::Identity());
1521 
1522  CollisionRequest<S> request;
1523  CollisionResult<S> result;
1524 
1525  bool res;
1526 
1527  Transform3<S> pose = Transform3<S>::Identity();
1528  Transform3<S> pose_aabb = Transform3<S>::Identity();
1529  Transform3<S> pose_obb = Transform3<S>::Identity();
1530 
1531  // s2 is within s1
1532  // both are shapes --> collision
1533  // s1 is shape, s2 is mesh --> in collision
1534  // s1 is mesh, s2 is shape --> collision free
1535  // all are reasonable
1536  result.clear();
1537  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1538  EXPECT_TRUE(res);
1539  result.clear();
1540  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1541  EXPECT_TRUE(res);
1542  result.clear();
1543  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1544  EXPECT_TRUE(res);
1545  result.clear();
1546  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1547  EXPECT_TRUE(res);
1548  result.clear();
1549  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1550  EXPECT_TRUE(res);
1551  result.clear();
1552  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1553  EXPECT_FALSE(res);
1554  result.clear();
1555  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1556  EXPECT_FALSE(res);
1557  result.clear();
1558  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1559  EXPECT_FALSE(res);
1560  result.clear();
1561  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1562  EXPECT_FALSE(res);
1563 
1564  pose.translation() = Vector3<S>(15.01, 0, 0);
1565  pose_aabb.translation() = Vector3<S>(15.01, 0, 0);
1566  pose_obb.translation() = Vector3<S>(15.01, 0, 0);
1567 
1568  result.clear();
1569  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1570  EXPECT_FALSE(res);
1571  result.clear();
1572  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1573  EXPECT_FALSE(res);
1574  result.clear();
1575  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1576  EXPECT_FALSE(res);
1577  result.clear();
1578  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1579  EXPECT_FALSE(res);
1580  result.clear();
1581  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1582  EXPECT_FALSE(res);
1583  result.clear();
1584  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1585  EXPECT_FALSE(res);
1586  result.clear();
1587  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1588  EXPECT_FALSE(res);
1589  result.clear();
1590  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1591  EXPECT_FALSE(res);
1592  result.clear();
1593  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1594  EXPECT_FALSE(res);
1595 
1596  pose.translation() = Vector3<S>(14.99, 0, 0);
1597  pose_aabb.translation() = Vector3<S>(14.99, 0, 0);
1598  pose_obb.translation() = Vector3<S>(14.99, 0, 0);
1599 
1600  result.clear();
1601  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1602  EXPECT_TRUE(res);
1603  result.clear();
1604  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1605  EXPECT_TRUE(res);
1606  result.clear();
1607  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1608  EXPECT_TRUE(res);
1609  result.clear();
1610  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1611  EXPECT_TRUE(res);
1612  result.clear();
1613  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1614  EXPECT_TRUE(res);
1615  result.clear();
1616  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1617  EXPECT_TRUE(res);
1618  result.clear();
1619  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1620  EXPECT_TRUE(res);
1621  result.clear();
1622  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1623  EXPECT_TRUE(res);
1624  result.clear();
1625  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1626  EXPECT_TRUE(res);
1627 }
1628 
1629 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_boxbox_libccd)
1630 {
1631 // test_consistency_collision_boxbox_libccd<float>();
1632  test_consistency_collision_boxbox_libccd<double>();
1633 }
1634 
1635 template <typename S>
1636 void test_consistency_collision_spherebox_libccd()
1637 {
1638  Sphere<S> s1(20);
1639  Box<S> s2(5, 5, 5);
1640 
1641  BVHModel<AABB<S>> s1_aabb;
1642  BVHModel<AABB<S>> s2_aabb;
1643  BVHModel<OBB<S>> s1_obb;
1644  BVHModel<OBB<S>> s2_obb;
1645 
1646  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
1647  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity());
1648  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
1649  generateBVHModel(s2_obb, s2, Transform3<S>::Identity());
1650 
1651  CollisionRequest<S> request;
1652  CollisionResult<S> result;
1653 
1654  bool res;
1655 
1656  Transform3<S> pose = Transform3<S>::Identity();
1657  Transform3<S> pose_aabb = Transform3<S>::Identity();
1658  Transform3<S> pose_obb = Transform3<S>::Identity();
1659 
1660  // s2 is within s1
1661  // both are shapes --> collision
1662  // s1 is shape, s2 is mesh --> in collision
1663  // s1 is mesh, s2 is shape --> collision free
1664  // all are reasonable
1665  result.clear();
1666  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1667  EXPECT_TRUE(res);
1668  result.clear();
1669  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1670  EXPECT_TRUE(res);
1671  result.clear();
1672  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1673  EXPECT_TRUE(res);
1674  result.clear();
1675  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1676  EXPECT_TRUE(res);
1677  result.clear();
1678  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1679  EXPECT_TRUE(res);
1680  result.clear();
1681  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1682  EXPECT_FALSE(res);
1683  result.clear();
1684  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1685  EXPECT_FALSE(res);
1686  result.clear();
1687  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1688  EXPECT_FALSE(res);
1689  result.clear();
1690  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1691  EXPECT_FALSE(res);
1692 
1693  pose.translation() = Vector3<S>(22.4, 0, 0);
1694  pose_aabb.translation() = Vector3<S>(22.4, 0, 0);
1695  pose_obb.translation() = Vector3<S>(22.4, 0, 0);
1696 
1697  result.clear();
1698  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1699  EXPECT_TRUE(res);
1700  result.clear();
1701  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1702  EXPECT_TRUE(res);
1703  result.clear();
1704  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1705  EXPECT_TRUE(res);
1706  result.clear();
1707  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1708  EXPECT_TRUE(res);
1709  result.clear();
1710  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1711  EXPECT_TRUE(res);
1712  result.clear();
1713  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1714  EXPECT_TRUE(res);
1715  result.clear();
1716  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1717  EXPECT_TRUE(res);
1718  result.clear();
1719  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1720  EXPECT_TRUE(res);
1721  result.clear();
1722  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1723  EXPECT_TRUE(res);
1724 
1725  pose.translation() = Vector3<S>(22.51, 0, 0);
1726  pose_aabb.translation() = Vector3<S>(22.51, 0, 0);
1727  pose_obb.translation() = Vector3<S>(22.51, 0, 0);
1728 
1729  result.clear();
1730  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1731  EXPECT_FALSE(res);
1732  result.clear();
1733  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1734  EXPECT_FALSE(res);
1735  result.clear();
1736  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1737  EXPECT_FALSE(res);
1738  result.clear();
1739  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1740  EXPECT_FALSE(res);
1741  result.clear();
1742  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1743  EXPECT_FALSE(res);
1744  result.clear();
1745  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1746  EXPECT_FALSE(res);
1747  result.clear();
1748  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1749  EXPECT_FALSE(res);
1750  result.clear();
1751  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1752  EXPECT_FALSE(res);
1753  result.clear();
1754  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1755  EXPECT_FALSE(res);
1756 }
1757 
1758 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spherebox_libccd)
1759 {
1760 // test_consistency_collision_spherebox_libccd<float>();
1761  test_consistency_collision_spherebox_libccd<double>();
1762 }
1763 
1764 template <typename S>
1765 void test_consistency_collision_cylindercylinder_libccd()
1766 {
1767  Cylinder<S> s1(5, 10);
1768  Cylinder<S> s2(5, 10);
1769 
1770  BVHModel<AABB<S>> s1_aabb;
1771  BVHModel<AABB<S>> s2_aabb;
1772  BVHModel<OBB<S>> s1_obb;
1773  BVHModel<OBB<S>> s2_obb;
1774 
1775  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
1776  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
1777  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
1778  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
1779 
1780  CollisionRequest<S> request;
1781  CollisionResult<S> result;
1782 
1783  bool res;
1784 
1785  Transform3<S> pose = Transform3<S>::Identity();
1786  Transform3<S> pose_aabb = Transform3<S>::Identity();
1787  Transform3<S> pose_obb = Transform3<S>::Identity();
1788 
1789  pose.translation() = Vector3<S>(9.99, 0, 0);
1790  pose_aabb.translation() = Vector3<S>(9.99, 0, 0);
1791  pose_obb.translation() = Vector3<S>(9.99, 0, 0);
1792 
1793  result.clear();
1794  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1795  EXPECT_TRUE(res);
1796  result.clear();
1797  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1798  EXPECT_TRUE(res);
1799  result.clear();
1800  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1801  EXPECT_TRUE(res);
1802  result.clear();
1803  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1804  EXPECT_TRUE(res);
1805  result.clear();
1806  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1807  EXPECT_TRUE(res);
1808  result.clear();
1809  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1810  EXPECT_TRUE(res);
1811  result.clear();
1812  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1813  EXPECT_TRUE(res);
1814  result.clear();
1815  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1816  EXPECT_TRUE(res);
1817  result.clear();
1818  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1819  EXPECT_TRUE(res);
1820 
1821  pose.translation() = Vector3<S>(10.01, 0, 0);
1822  pose_aabb.translation() = Vector3<S>(10.01, 0, 0);
1823  pose_obb.translation() = Vector3<S>(10.01, 0, 0);
1824 
1825  result.clear();
1826  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1827  EXPECT_FALSE(res);
1828  result.clear();
1829  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1830  EXPECT_FALSE(res);
1831  result.clear();
1832  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1833  EXPECT_FALSE(res);
1834  result.clear();
1835  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1836  EXPECT_FALSE(res);
1837  result.clear();
1838  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1839  EXPECT_FALSE(res);
1840  result.clear();
1841  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1842  EXPECT_FALSE(res);
1843  result.clear();
1844  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1845  EXPECT_FALSE(res);
1846  result.clear();
1847  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1848  EXPECT_FALSE(res);
1849  result.clear();
1850  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1851  EXPECT_FALSE(res);
1852 }
1853 
1854 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_cylindercylinder_libccd)
1855 {
1856 // test_consistency_collision_cylindercylinder_libccd<float>();
1857  test_consistency_collision_cylindercylinder_libccd<double>();
1858 }
1859 
1860 template <typename S>
1861 void test_consistency_collision_conecone_libccd()
1862 {
1863  Cone<S> s1(5, 10);
1864  Cone<S> s2(5, 10);
1865 
1866  BVHModel<AABB<S>> s1_aabb;
1867  BVHModel<AABB<S>> s2_aabb;
1868  BVHModel<OBB<S>> s1_obb;
1869  BVHModel<OBB<S>> s2_obb;
1870 
1871  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
1872  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
1873  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
1874  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
1875 
1876  CollisionRequest<S> request;
1877  CollisionResult<S> result;
1878 
1879  bool res;
1880 
1881  Transform3<S> pose = Transform3<S>::Identity();
1882  Transform3<S> pose_aabb = Transform3<S>::Identity();
1883  Transform3<S> pose_obb = Transform3<S>::Identity();
1884 
1885  pose.translation() = Vector3<S>(9.9, 0, 0);
1886  pose_aabb.translation() = Vector3<S>(9.9, 0, 0);
1887  pose_obb.translation() = Vector3<S>(9.9, 0, 0);
1888 
1889  result.clear();
1890  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1891  EXPECT_TRUE(res);
1892  result.clear();
1893  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1894  EXPECT_TRUE(res);
1895  result.clear();
1896  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1897  EXPECT_TRUE(res);
1898  result.clear();
1899  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1900  EXPECT_TRUE(res);
1901  result.clear();
1902  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1903  EXPECT_TRUE(res);
1904  result.clear();
1905  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1906  EXPECT_TRUE(res);
1907  result.clear();
1908  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1909  EXPECT_TRUE(res);
1910  result.clear();
1911  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1912  EXPECT_TRUE(res);
1913  result.clear();
1914  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1915  EXPECT_TRUE(res);
1916 
1917  pose.translation() = Vector3<S>(10.1, 0, 0);
1918  pose_aabb.translation() = Vector3<S>(10.1, 0, 0);
1919  pose_obb.translation() = Vector3<S>(10.1, 0, 0);
1920 
1921  result.clear();
1922  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1923  EXPECT_FALSE(res);
1924  result.clear();
1925  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1926  EXPECT_FALSE(res);
1927  result.clear();
1928  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1929  EXPECT_FALSE(res);
1930  result.clear();
1931  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1932  EXPECT_FALSE(res);
1933  result.clear();
1934  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1935  EXPECT_FALSE(res);
1936  result.clear();
1937  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1938  EXPECT_FALSE(res);
1939  result.clear();
1940  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1941  EXPECT_FALSE(res);
1942  result.clear();
1943  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1944  EXPECT_FALSE(res);
1945  result.clear();
1946  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1947  EXPECT_FALSE(res);
1948 
1949  pose.translation() = Vector3<S>(0, 0, 9.9);
1950  pose_aabb.translation() = Vector3<S>(0, 0, 9.9);
1951  pose_obb.translation() = Vector3<S>(0, 0, 9.9);
1952 
1953  result.clear();
1954  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1955  EXPECT_TRUE(res);
1956  result.clear();
1957  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1958  EXPECT_TRUE(res);
1959  result.clear();
1960  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1961  EXPECT_TRUE(res);
1962  result.clear();
1963  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1964  EXPECT_TRUE(res);
1965  result.clear();
1966  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1967  EXPECT_TRUE(res);
1968  result.clear();
1969  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
1970  EXPECT_TRUE(res);
1971  result.clear();
1972  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
1973  EXPECT_TRUE(res);
1974  result.clear();
1975  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1976  EXPECT_TRUE(res);
1977  result.clear();
1978  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1979  EXPECT_TRUE(res);
1980 
1981  pose.translation() = Vector3<S>(0, 0, 10.1);
1982  pose_aabb.translation() = Vector3<S>(0, 0, 10.1);
1983  pose_obb.translation() = Vector3<S>(0, 0, 10.1);
1984 
1985  result.clear();
1986  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
1987  EXPECT_FALSE(res);
1988  result.clear();
1989  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
1990  EXPECT_FALSE(res);
1991  result.clear();
1992  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
1993  EXPECT_FALSE(res);
1994  result.clear();
1995  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
1996  EXPECT_FALSE(res);
1997  result.clear();
1998  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
1999  EXPECT_FALSE(res);
2000  result.clear();
2001  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2002  EXPECT_FALSE(res);
2003  result.clear();
2004  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2005  EXPECT_FALSE(res);
2006  result.clear();
2007  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2008  EXPECT_FALSE(res);
2009  result.clear();
2010  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2011  EXPECT_FALSE(res);
2012 }
2013 
2014 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_conecone_libccd)
2015 {
2016 // test_consistency_collision_conecone_libccd<float>();
2017  test_consistency_collision_conecone_libccd<double>();
2018 }
2019 
2020 template <typename S>
2021 void test_consistency_collision_spheresphere_GJK()
2022 {
2023  Sphere<S> s1(20);
2024  Sphere<S> s2(10);
2025  BVHModel<AABB<S>> s1_aabb;
2026  BVHModel<AABB<S>> s2_aabb;
2027  BVHModel<OBB<S>> s1_obb;
2028  BVHModel<OBB<S>> s2_obb;
2029 
2030  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
2031  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
2032  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
2033  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
2034 
2035  CollisionRequest<S> request;
2036  request.gjk_solver_type = GST_INDEP;
2037 
2038  CollisionResult<S> result;
2039 
2040  bool res;
2041 
2042  Transform3<S> pose = Transform3<S>::Identity();
2043  Transform3<S> pose_aabb = Transform3<S>::Identity();
2044  Transform3<S> pose_obb = Transform3<S>::Identity();
2045 
2046 
2047  // s2 is within s1
2048  // both are shapes --> collision
2049  // s1 is shape, s2 is mesh --> in collision
2050  // s1 is mesh, s2 is shape --> collision free
2051  // all are reasonable
2052  result.clear();
2053  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2054  EXPECT_TRUE(res);
2055  result.clear();
2056  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2057  EXPECT_TRUE(res);
2058  result.clear();
2059  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2060  EXPECT_TRUE(res);
2061  result.clear();
2062  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2063  EXPECT_TRUE(res);
2064  result.clear();
2065  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2066  EXPECT_TRUE(res);
2067  result.clear();
2068  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2069  EXPECT_FALSE(res);
2070  result.clear();
2071  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2072  EXPECT_FALSE(res);
2073  result.clear();
2074  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2075  EXPECT_FALSE(res);
2076  result.clear();
2077  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2078  EXPECT_FALSE(res);
2079 
2080  pose.translation() = Vector3<S>(40, 0, 0);
2081  pose_aabb.translation() = Vector3<S>(40, 0, 0);
2082  pose_obb.translation() = Vector3<S>(40, 0, 0);
2083 
2084  result.clear();
2085  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2086  EXPECT_FALSE(res);
2087  result.clear();
2088  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2089  EXPECT_FALSE(res);
2090  result.clear();
2091  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2092  EXPECT_FALSE(res);
2093  result.clear();
2094  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2095  EXPECT_FALSE(res);
2096  result.clear();
2097  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2098  EXPECT_FALSE(res);
2099  result.clear();
2100  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2101  EXPECT_FALSE(res);
2102  result.clear();
2103  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2104  EXPECT_FALSE(res);
2105  result.clear();
2106  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2107  EXPECT_FALSE(res);
2108  result.clear();
2109  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2110  EXPECT_FALSE(res);
2111 
2112  pose.translation() = Vector3<S>(30, 0, 0);
2113  pose_aabb.translation() = Vector3<S>(30, 0, 0);
2114  pose_obb.translation() = Vector3<S>(30, 0, 0);
2115 
2116  result.clear();
2117  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2118  EXPECT_TRUE(res);
2119  result.clear();
2120  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2121  EXPECT_FALSE(res);
2122  result.clear();
2123  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2124  EXPECT_FALSE(res);
2125  result.clear();
2126  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2127  EXPECT_FALSE(res);
2128  result.clear();
2129  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2130  EXPECT_FALSE(res);
2131  result.clear();
2132  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2133  EXPECT_FALSE(res);
2134  result.clear();
2135  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2136  EXPECT_FALSE(res);
2137  result.clear();
2138  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2139  EXPECT_FALSE(res);
2140  result.clear();
2141  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2142  EXPECT_FALSE(res);
2143 
2144  pose.translation() = Vector3<S>(29.9, 0, 0);
2145  pose_aabb.translation() = Vector3<S>(29.8, 0, 0); // 29.9 fails, result depends on mesh precision
2146  pose_obb.translation() = Vector3<S>(29.8, 0, 0); // 29.9 fails, result depends on mesh precision
2147 
2148  result.clear();
2149  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2150  EXPECT_TRUE(res);
2151  result.clear();
2152  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2153  EXPECT_TRUE(res);
2154  result.clear();
2155  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2156  EXPECT_TRUE(res);
2157  result.clear();
2158  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2159  EXPECT_TRUE(res);
2160  result.clear();
2161  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2162  EXPECT_TRUE(res);
2163  result.clear();
2164  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2165  EXPECT_TRUE(res);
2166  result.clear();
2167  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2168  EXPECT_TRUE(res);
2169  result.clear();
2170  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2171  EXPECT_TRUE(res);
2172  result.clear();
2173  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2174  EXPECT_TRUE(res);
2175 
2176 
2177  pose.translation() = Vector3<S>(-29.9, 0, 0);
2178  pose_aabb.translation() = Vector3<S>(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision
2179  pose_obb.translation() = Vector3<S>(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision
2180 
2181  result.clear();
2182  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2183  EXPECT_TRUE(res);
2184  result.clear();
2185  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2186  EXPECT_TRUE(res);
2187  result.clear();
2188  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2189  EXPECT_TRUE(res);
2190  result.clear();
2191  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2192  EXPECT_TRUE(res);
2193  result.clear();
2194  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2195  EXPECT_TRUE(res);
2196  result.clear();
2197  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2198  EXPECT_TRUE(res);
2199  result.clear();
2200  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2201  EXPECT_TRUE(res);
2202  result.clear();
2203  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2204  EXPECT_TRUE(res);
2205  result.clear();
2206  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2207  EXPECT_TRUE(res);
2208 
2209  pose.translation() = Vector3<S>(-30, 0, 0);
2210  pose_aabb.translation() = Vector3<S>(-30, 0, 0);
2211  pose_obb.translation() = Vector3<S>(-30, 0, 0);
2212 
2213  result.clear();
2214  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2215  EXPECT_TRUE(res);
2216  result.clear();
2217  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2218  EXPECT_FALSE(res);
2219  result.clear();
2220  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2221  EXPECT_FALSE(res);
2222  result.clear();
2223  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2224  EXPECT_FALSE(res);
2225  result.clear();
2226  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2227  EXPECT_FALSE(res);
2228  result.clear();
2229  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2230  EXPECT_FALSE(res);
2231  result.clear();
2232  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2233  EXPECT_FALSE(res);
2234  result.clear();
2235  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2236  EXPECT_FALSE(res);
2237  result.clear();
2238  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2239  EXPECT_FALSE(res);
2240 }
2241 
2242 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spheresphere_GJK)
2243 {
2244 // test_consistency_collision_spheresphere_GJK<float>();
2245  test_consistency_collision_spheresphere_GJK<double>();
2246 }
2247 
2248 template <typename S>
2249 void test_consistency_collision_ellipsoidellipsoid_GJK()
2250 {
2251  Ellipsoid<S> s1(20, 40, 50);
2252  Ellipsoid<S> s2(10, 10, 10);
2253  BVHModel<AABB<S>> s1_aabb;
2254  BVHModel<AABB<S>> s2_aabb;
2255  BVHModel<OBB<S>> s1_obb;
2256  BVHModel<OBB<S>> s2_obb;
2257 
2258  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
2259  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
2260  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
2261  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
2262 
2263  CollisionRequest<S> request;
2264  request.gjk_solver_type = GST_INDEP;
2265 
2266  CollisionResult<S> result;
2267 
2268  bool res;
2269 
2270  Transform3<S> pose = Transform3<S>::Identity();
2271  Transform3<S> pose_aabb = Transform3<S>::Identity();
2272  Transform3<S> pose_obb = Transform3<S>::Identity();
2273 
2274 
2275  // s2 is within s1
2276  // both are shapes --> collision
2277  // s1 is shape, s2 is mesh --> in collision
2278  // s1 is mesh, s2 is shape --> collision free
2279  // all are reasonable
2280  result.clear();
2281  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2282  EXPECT_TRUE(res);
2283  result.clear();
2284  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2285  EXPECT_TRUE(res);
2286  result.clear();
2287  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2288  EXPECT_TRUE(res);
2289  result.clear();
2290  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2291  EXPECT_TRUE(res);
2292  result.clear();
2293  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2294  EXPECT_TRUE(res);
2295  result.clear();
2296  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2297  EXPECT_FALSE(res);
2298  result.clear();
2299  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2300  EXPECT_FALSE(res);
2301  result.clear();
2302  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2303  EXPECT_FALSE(res);
2304  result.clear();
2305  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2306  EXPECT_FALSE(res);
2307 
2308  pose.translation() = Vector3<S>(40, 0, 0);
2309  pose_aabb.translation() = Vector3<S>(40, 0, 0);
2310  pose_obb.translation() = Vector3<S>(40, 0, 0);
2311 
2312  result.clear();
2313  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2314  EXPECT_FALSE(res);
2315  result.clear();
2316  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2317  EXPECT_FALSE(res);
2318  result.clear();
2319  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2320  EXPECT_FALSE(res);
2321  result.clear();
2322  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2323  EXPECT_FALSE(res);
2324  result.clear();
2325  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2326  EXPECT_FALSE(res);
2327  result.clear();
2328  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2329  EXPECT_FALSE(res);
2330  result.clear();
2331  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2332  EXPECT_FALSE(res);
2333  result.clear();
2334  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2335  EXPECT_FALSE(res);
2336  result.clear();
2337  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2338  EXPECT_FALSE(res);
2339 
2340  pose.translation() = Vector3<S>(30, 0, 0);
2341  pose_aabb.translation() = Vector3<S>(30, 0, 0);
2342  pose_obb.translation() = Vector3<S>(30, 0, 0);
2343 
2344  result.clear();
2345  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2346  EXPECT_TRUE(res);
2347  result.clear();
2348  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2349  EXPECT_FALSE(res);
2350  result.clear();
2351  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2352  EXPECT_FALSE(res);
2353  result.clear();
2354  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2355  EXPECT_FALSE(res);
2356  result.clear();
2357  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2358  EXPECT_FALSE(res);
2359  result.clear();
2360  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2361  EXPECT_FALSE(res);
2362  result.clear();
2363  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2364  EXPECT_FALSE(res);
2365  result.clear();
2366  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2367  EXPECT_FALSE(res);
2368  result.clear();
2369  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2370  EXPECT_FALSE(res);
2371 
2372  pose.translation() = Vector3<S>(29.9, 0, 0);
2373  pose_aabb.translation() = Vector3<S>(29.9, 0, 0);
2374  pose_obb.translation() = Vector3<S>(29.9, 0, 0);
2375 
2376  result.clear();
2377  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2378  EXPECT_TRUE(res);
2379  result.clear();
2380  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2381  EXPECT_TRUE(res);
2382  result.clear();
2383  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2384  EXPECT_TRUE(res);
2385  result.clear();
2386  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2387  EXPECT_TRUE(res);
2388  result.clear();
2389  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2390  EXPECT_TRUE(res);
2391  result.clear();
2392  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2393  EXPECT_TRUE(res);
2394  result.clear();
2395  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2396  EXPECT_TRUE(res);
2397  result.clear();
2398  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2399  EXPECT_TRUE(res);
2400  result.clear();
2401  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2402  EXPECT_TRUE(res);
2403 
2404 
2405  pose.translation() = Vector3<S>(-29.9, 0, 0);
2406  pose_aabb.translation() = Vector3<S>(-29.9, 0, 0);
2407  pose_obb.translation() = Vector3<S>(-29.9, 0, 0);
2408 
2409  result.clear();
2410  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2411  EXPECT_TRUE(res);
2412  result.clear();
2413  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2414  EXPECT_TRUE(res);
2415  result.clear();
2416  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2417  EXPECT_TRUE(res);
2418  result.clear();
2419  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2420  EXPECT_TRUE(res);
2421  result.clear();
2422  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2423  EXPECT_TRUE(res);
2424  result.clear();
2425  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2426  EXPECT_TRUE(res);
2427  result.clear();
2428  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2429  EXPECT_TRUE(res);
2430  result.clear();
2431  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2432  EXPECT_TRUE(res);
2433  result.clear();
2434  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2435  EXPECT_TRUE(res);
2436 
2437  pose.translation() = Vector3<S>(-30, 0, 0);
2438  pose_aabb.translation() = Vector3<S>(-30, 0, 0);
2439  pose_obb.translation() = Vector3<S>(-30, 0, 0);
2440 
2441  result.clear();
2442  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2443  EXPECT_TRUE(res);
2444  result.clear();
2445  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2446  EXPECT_FALSE(res);
2447  result.clear();
2448  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2449  EXPECT_FALSE(res);
2450  result.clear();
2451  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2452  EXPECT_FALSE(res);
2453  result.clear();
2454  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2455  EXPECT_FALSE(res);
2456  result.clear();
2457  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2458  EXPECT_FALSE(res);
2459  result.clear();
2460  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2461  EXPECT_FALSE(res);
2462  result.clear();
2463  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2464  EXPECT_FALSE(res);
2465  result.clear();
2466  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2467  EXPECT_FALSE(res);
2468 }
2469 
2470 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_ellipsoidellipsoid_GJK)
2471 {
2472 // test_consistency_collision_ellipsoidellipsoid_GJK<float>();
2473  test_consistency_collision_ellipsoidellipsoid_GJK<double>();
2474 }
2475 
2476 template <typename S>
2477 void test_consistency_collision_boxbox_GJK()
2478 {
2479  Box<S> s1(20, 40, 50);
2480  Box<S> s2(10, 10, 10);
2481 
2482  BVHModel<AABB<S>> s1_aabb;
2483  BVHModel<AABB<S>> s2_aabb;
2484  BVHModel<OBB<S>> s1_obb;
2485  BVHModel<OBB<S>> s2_obb;
2486 
2487  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity());
2488  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity());
2489  generateBVHModel(s1_obb, s1, Transform3<S>::Identity());
2490  generateBVHModel(s2_obb, s2, Transform3<S>::Identity());
2491 
2492  CollisionRequest<S> request;
2493  request.gjk_solver_type = GST_INDEP;
2494 
2495  CollisionResult<S> result;
2496 
2497  bool res;
2498 
2499  Transform3<S> pose = Transform3<S>::Identity();
2500  Transform3<S> pose_aabb = Transform3<S>::Identity();
2501  Transform3<S> pose_obb = Transform3<S>::Identity();
2502 
2503  // s2 is within s1
2504  // both are shapes --> collision
2505  // s1 is shape, s2 is mesh --> in collision
2506  // s1 is mesh, s2 is shape --> collision free
2507  // all are reasonable
2508  result.clear();
2509  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2510  EXPECT_TRUE(res);
2511  result.clear();
2512  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2513  EXPECT_TRUE(res);
2514  result.clear();
2515  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2516  EXPECT_TRUE(res);
2517  result.clear();
2518  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2519  EXPECT_TRUE(res);
2520  result.clear();
2521  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2522  EXPECT_TRUE(res);
2523  result.clear();
2524  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2525  EXPECT_FALSE(res);
2526  result.clear();
2527  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2528  EXPECT_FALSE(res);
2529  result.clear();
2530  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2531  EXPECT_FALSE(res);
2532  result.clear();
2533  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2534  EXPECT_FALSE(res);
2535 
2536  pose.translation() = Vector3<S>(15.01, 0, 0);
2537  pose_aabb.translation() = Vector3<S>(15.01, 0, 0);
2538  pose_obb.translation() = Vector3<S>(15.01, 0, 0);
2539 
2540  result.clear();
2541  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2542  EXPECT_FALSE(res);
2543  result.clear();
2544  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2545  EXPECT_FALSE(res);
2546  result.clear();
2547  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2548  EXPECT_FALSE(res);
2549  result.clear();
2550  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2551  EXPECT_FALSE(res);
2552  result.clear();
2553  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2554  EXPECT_FALSE(res);
2555  result.clear();
2556  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2557  EXPECT_FALSE(res);
2558  result.clear();
2559  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2560  EXPECT_FALSE(res);
2561  result.clear();
2562  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2563  EXPECT_FALSE(res);
2564  result.clear();
2565  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2566  EXPECT_FALSE(res);
2567 
2568  pose.translation() = Vector3<S>(14.99, 0, 0);
2569  pose_aabb.translation() = Vector3<S>(14.99, 0, 0);
2570  pose_obb.translation() = Vector3<S>(14.99, 0, 0);
2571 
2572  result.clear();
2573  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2574  EXPECT_TRUE(res);
2575  result.clear();
2576  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2577  EXPECT_TRUE(res);
2578  result.clear();
2579  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2580  EXPECT_TRUE(res);
2581  result.clear();
2582  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2583  EXPECT_TRUE(res);
2584  result.clear();
2585  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2586  EXPECT_TRUE(res);
2587  result.clear();
2588  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2589  EXPECT_TRUE(res);
2590  result.clear();
2591  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2592  EXPECT_TRUE(res);
2593  result.clear();
2594  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2595  EXPECT_TRUE(res);
2596  result.clear();
2597  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2598  EXPECT_TRUE(res);
2599 }
2600 
2601 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_boxbox_GJK)
2602 {
2603 // test_consistency_collision_boxbox_GJK<float>();
2604  test_consistency_collision_boxbox_GJK<double>();
2605 }
2606 
2607 template <typename S>
2608 void test_consistency_collision_spherebox_GJK()
2609 {
2610  Sphere<S> s1(20);
2611  Box<S> s2(5, 5, 5);
2612 
2613  BVHModel<AABB<S>> s1_aabb;
2614  BVHModel<AABB<S>> s2_aabb;
2615  BVHModel<OBB<S>> s1_obb;
2616  BVHModel<OBB<S>> s2_obb;
2617 
2618  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
2619  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity());
2620  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
2621  generateBVHModel(s2_obb, s2, Transform3<S>::Identity());
2622 
2623  CollisionRequest<S> request;
2624  request.gjk_solver_type = GST_INDEP;
2625 
2626  CollisionResult<S> result;
2627 
2628  bool res;
2629 
2630  Transform3<S> pose = Transform3<S>::Identity();
2631  Transform3<S> pose_aabb = Transform3<S>::Identity();
2632  Transform3<S> pose_obb = Transform3<S>::Identity();
2633 
2634  // s2 is within s1
2635  // both are shapes --> collision
2636  // s1 is shape, s2 is mesh --> in collision
2637  // s1 is mesh, s2 is shape --> collision free
2638  // all are reasonable
2639  result.clear();
2640  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2641  EXPECT_TRUE(res);
2642  result.clear();
2643  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2644  EXPECT_TRUE(res);
2645  result.clear();
2646  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2647  EXPECT_TRUE(res);
2648  result.clear();
2649  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2650  EXPECT_TRUE(res);
2651  result.clear();
2652  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2653  EXPECT_TRUE(res);
2654  result.clear();
2655  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2656  EXPECT_FALSE(res);
2657  result.clear();
2658  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2659  EXPECT_FALSE(res);
2660  result.clear();
2661  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2662  EXPECT_FALSE(res);
2663  result.clear();
2664  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2665  EXPECT_FALSE(res);
2666 
2667  pose.translation() = Vector3<S>(22.4, 0, 0);
2668  pose_aabb.translation() = Vector3<S>(22.4, 0, 0);
2669  pose_obb.translation() = Vector3<S>(22.4, 0, 0);
2670 
2671  result.clear();
2672  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2673  EXPECT_TRUE(res);
2674  result.clear();
2675  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2676  EXPECT_TRUE(res);
2677  result.clear();
2678  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2679  EXPECT_TRUE(res);
2680  result.clear();
2681  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2682  EXPECT_TRUE(res);
2683  result.clear();
2684  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2685  EXPECT_TRUE(res);
2686  result.clear();
2687  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2688  EXPECT_TRUE(res);
2689  result.clear();
2690  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2691  EXPECT_TRUE(res);
2692  result.clear();
2693  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2694  EXPECT_TRUE(res);
2695  result.clear();
2696  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2697  EXPECT_TRUE(res);
2698 
2699  pose.translation() = Vector3<S>(22.51, 0, 0);
2700  pose_aabb.translation() = Vector3<S>(22.51, 0, 0);
2701  pose_obb.translation() = Vector3<S>(22.51, 0, 0);
2702 
2703  result.clear();
2704  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2705  EXPECT_FALSE(res);
2706  result.clear();
2707  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2708  EXPECT_FALSE(res);
2709  result.clear();
2710  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2711  EXPECT_FALSE(res);
2712  result.clear();
2713  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2714  EXPECT_FALSE(res);
2715  result.clear();
2716  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2717  EXPECT_FALSE(res);
2718  result.clear();
2719  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2720  EXPECT_FALSE(res);
2721  result.clear();
2722  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2723  EXPECT_FALSE(res);
2724  result.clear();
2725  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2726  EXPECT_FALSE(res);
2727  result.clear();
2728  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2729  EXPECT_FALSE(res);
2730 }
2731 
2732 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spherebox_GJK)
2733 {
2734 // test_consistency_collision_spherebox_GJK<float>();
2735  test_consistency_collision_spherebox_GJK<double>();
2736 }
2737 
2738 template <typename S>
2739 void test_consistency_collision_cylindercylinder_GJK()
2740 {
2741  Cylinder<S> s1(5, 10);
2742  Cylinder<S> s2(5, 10);
2743 
2744  BVHModel<AABB<S>> s1_aabb;
2745  BVHModel<AABB<S>> s2_aabb;
2746  BVHModel<OBB<S>> s1_obb;
2747  BVHModel<OBB<S>> s2_obb;
2748 
2749  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
2750  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
2751  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
2752  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
2753 
2754  CollisionRequest<S> request;
2755  request.gjk_solver_type = GST_INDEP;
2756 
2757  CollisionResult<S> result;
2758 
2759  bool res;
2760 
2761  Transform3<S> pose = Transform3<S>::Identity();
2762  Transform3<S> pose_aabb = Transform3<S>::Identity();
2763  Transform3<S> pose_obb = Transform3<S>::Identity();
2764 
2765  pose.translation() = Vector3<S>(9.99, 0, 0);
2766  pose_aabb.translation() = Vector3<S>(9.99, 0, 0);
2767  pose_obb.translation() = Vector3<S>(9.99, 0, 0);
2768 
2769  result.clear();
2770  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2771  EXPECT_TRUE(res);
2772  result.clear();
2773  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2774  EXPECT_TRUE(res);
2775  result.clear();
2776  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2777  EXPECT_TRUE(res);
2778  result.clear();
2779  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2780  EXPECT_TRUE(res);
2781  result.clear();
2782  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2783  EXPECT_TRUE(res);
2784  result.clear();
2785  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2786  EXPECT_TRUE(res);
2787  result.clear();
2788  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2789  EXPECT_TRUE(res);
2790  result.clear();
2791  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2792  EXPECT_TRUE(res);
2793  result.clear();
2794  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2795  EXPECT_TRUE(res);
2796 
2797  pose.translation() = Vector3<S>(10.01, 0, 0);
2798  pose_aabb.translation() = Vector3<S>(10.01, 0, 0);
2799  pose_obb.translation() = Vector3<S>(10.01, 0, 0);
2800 
2801  result.clear();
2802  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2803  EXPECT_FALSE(res);
2804  result.clear();
2805  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2806  EXPECT_FALSE(res);
2807  result.clear();
2808  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2809  EXPECT_FALSE(res);
2810  result.clear();
2811  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2812  EXPECT_FALSE(res);
2813  result.clear();
2814  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2815  EXPECT_FALSE(res);
2816  result.clear();
2817  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2818  EXPECT_FALSE(res);
2819  result.clear();
2820  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2821  EXPECT_FALSE(res);
2822  result.clear();
2823  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2824  EXPECT_FALSE(res);
2825  result.clear();
2826  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2827  EXPECT_FALSE(res);
2828 }
2829 
2830 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_cylindercylinder_GJK)
2831 {
2832 // test_consistency_collision_cylindercylinder_GJK<float>();
2833  test_consistency_collision_cylindercylinder_GJK<double>();
2834 }
2835 
2836 template <typename S>
2837 void test_consistency_collision_conecone_GJK()
2838 {
2839  Cone<S> s1(5, 10);
2840  Cone<S> s2(5, 10);
2841 
2842  BVHModel<AABB<S>> s1_aabb;
2843  BVHModel<AABB<S>> s2_aabb;
2844  BVHModel<OBB<S>> s1_obb;
2845  BVHModel<OBB<S>> s2_obb;
2846 
2847  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
2848  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
2849  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
2850  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
2851 
2852  CollisionRequest<S> request;
2853  request.gjk_solver_type = GST_INDEP;
2854 
2855  CollisionResult<S> result;
2856 
2857  bool res;
2858 
2859  Transform3<S> pose = Transform3<S>::Identity();
2860  Transform3<S> pose_aabb = Transform3<S>::Identity();
2861  Transform3<S> pose_obb = Transform3<S>::Identity();
2862 
2863  pose.translation() = Vector3<S>(9.9, 0, 0);
2864  pose_aabb.translation() = Vector3<S>(9.9, 0, 0);
2865  pose_obb.translation() = Vector3<S>(9.9, 0, 0);
2866 
2867  result.clear();
2868  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2869  EXPECT_TRUE(res);
2870  result.clear();
2871  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2872  EXPECT_TRUE(res);
2873  result.clear();
2874  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2875  EXPECT_TRUE(res);
2876  result.clear();
2877  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2878  EXPECT_TRUE(res);
2879  result.clear();
2880  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2881  EXPECT_TRUE(res);
2882  result.clear();
2883  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2884  EXPECT_TRUE(res);
2885  result.clear();
2886  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2887  EXPECT_TRUE(res);
2888  result.clear();
2889  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2890  EXPECT_TRUE(res);
2891  result.clear();
2892  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2893  EXPECT_TRUE(res);
2894 
2895  pose.translation() = Vector3<S>(10.1, 0, 0);
2896  pose_aabb.translation() = Vector3<S>(10.1, 0, 0);
2897  pose_obb.translation() = Vector3<S>(10.1, 0, 0);
2898 
2899  result.clear();
2900  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2901  EXPECT_FALSE(res);
2902  result.clear();
2903  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2904  EXPECT_FALSE(res);
2905  result.clear();
2906  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2907  EXPECT_FALSE(res);
2908  result.clear();
2909  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2910  EXPECT_FALSE(res);
2911  result.clear();
2912  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2913  EXPECT_FALSE(res);
2914  result.clear();
2915  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2916  EXPECT_FALSE(res);
2917  result.clear();
2918  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2919  EXPECT_FALSE(res);
2920  result.clear();
2921  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2922  EXPECT_FALSE(res);
2923  result.clear();
2924  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2925  EXPECT_FALSE(res);
2926 
2927  pose.translation() = Vector3<S>(0, 0, 9.9);
2928  pose_aabb.translation() = Vector3<S>(0, 0, 9.9);
2929  pose_obb.translation() = Vector3<S>(0, 0, 9.9);
2930 
2931  result.clear();
2932  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2933  EXPECT_TRUE(res);
2934  result.clear();
2935  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2936  EXPECT_TRUE(res);
2937  result.clear();
2938  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2939  EXPECT_TRUE(res);
2940  result.clear();
2941  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2942  EXPECT_TRUE(res);
2943  result.clear();
2944  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2945  EXPECT_TRUE(res);
2946  result.clear();
2947  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2948  EXPECT_TRUE(res);
2949  result.clear();
2950  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2951  EXPECT_TRUE(res);
2952  result.clear();
2953  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2954  EXPECT_TRUE(res);
2955  result.clear();
2956  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2957  EXPECT_TRUE(res);
2958 
2959  pose.translation() = Vector3<S>(0, 0, 10.1);
2960  pose_aabb.translation() = Vector3<S>(0, 0, 10.1);
2961  pose_obb.translation() = Vector3<S>(0, 0, 10.1);
2962 
2963  result.clear();
2964  res = (collide(&s1, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2965  EXPECT_FALSE(res);
2966  result.clear();
2967  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3<S>::Identity(), request, result) > 0);
2968  EXPECT_FALSE(res);
2969  result.clear();
2970  res = (collide(&s2_obb, pose_obb, &s1, Transform3<S>::Identity(), request, result) > 0);
2971  EXPECT_FALSE(res);
2972  result.clear();
2973  res = (collide(&s1, Transform3<S>::Identity(), &s2_aabb, pose_aabb, request, result) > 0);
2974  EXPECT_FALSE(res);
2975  result.clear();
2976  res = (collide(&s1, Transform3<S>::Identity(), &s2_obb, pose_obb, request, result) > 0);
2977  EXPECT_FALSE(res);
2978  result.clear();
2979  res = (collide(&s2, pose, &s1_aabb, Transform3<S>::Identity(), request, result) > 0);
2980  EXPECT_FALSE(res);
2981  result.clear();
2982  res = (collide(&s2, pose, &s1_obb, Transform3<S>::Identity(), request, result) > 0);
2983  EXPECT_FALSE(res);
2984  result.clear();
2985  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2986  EXPECT_FALSE(res);
2987  result.clear();
2988  res = (collide(&s1_aabb, Transform3<S>::Identity(), &s2, pose, request, result) > 0);
2989  EXPECT_FALSE(res);
2990 }
2991 
2992 GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_conecone_GJK)
2993 {
2994 // test_consistency_collision_conecone_GJK<float>();
2995  test_consistency_collision_conecone_GJK<double>();
2996 }
2997 
2998 //==============================================================================
2999 int main(int argc, char* argv[])
3000 {
3001  ::testing::InitGoogleTest(&argc, argv);
3002  return RUN_ALL_TESTS();
3003 }
fcl::DistanceRequest
request to the distance computation
Definition: distance_request.h:52
test_consistency_collision_spheresphere_GJK
void test_consistency_collision_spheresphere_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:2021
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::CollisionRequest::gjk_solver_type
GJKSolverType gjk_solver_type
Enumeration indicating the GJK solver implementation to use.
Definition: collision_request.h:78
test_consistency_distance_cylindercylinder_GJK
void test_consistency_distance_cylindercylinder_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:846
test_consistency_collision_ellipsoidellipsoid_GJK
void test_consistency_collision_ellipsoidellipsoid_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:2249
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
test_consistency_collision_boxbox_GJK
void test_consistency_collision_boxbox_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:2477
main
int main(int argc, char *argv[])
Definition: test_fcl_shape_mesh_consistency.cpp:2999
fcl::CollisionResult::clear
void clear()
clear the results obtained
Definition: collision_result-inl.h:125
test_fcl_utility.h
test_consistency_collision_boxbox_libccd
void test_consistency_collision_boxbox_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:1507
fcl::CollisionResult
collision result
Definition: collision_request.h:48
EXPECT_TRUE
#define EXPECT_TRUE(args)
test_consistency_distance_ellipsoidellipsoid_GJK
void test_consistency_distance_ellipsoidellipsoid_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:647
test_consistency_distance_spheresphere_libccd
void test_consistency_distance_spheresphere_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:59
test_consistency_collision_cylindercylinder_libccd
void test_consistency_collision_cylindercylinder_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:1765
fcl::DistanceResult
distance result
Definition: distance_request.h:48
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
distance.h
gjk_solver_libccd.h
test_consistency_collision_conecone_libccd
void test_consistency_collision_conecone_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:1861
fcl::DistanceRequest::gjk_solver_type
GJKSolverType gjk_solver_type
narrow phase solver type
Definition: distance_request.h:102
GTEST_TEST
GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_libccd)
Definition: test_fcl_shape_mesh_consistency.cpp:150
test_consistency_distance_spheresphere_GJK
void test_consistency_distance_spheresphere_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:547
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
test_consistency_distance_cylindercylinder_libccd
void test_consistency_distance_cylindercylinder_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:352
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:94
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::DistanceResult::min_distance
S min_distance
Minimum distance between two objects.
Definition: distance_result.h:64
test_consistency_distance_conecone_GJK
void test_consistency_distance_conecone_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:954
test_consistency_distance_boxbox_GJK
void test_consistency_distance_boxbox_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:747
test_consistency_distance_boxbox_libccd
void test_consistency_distance_boxbox_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:254
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
test_consistency_collision_spherebox_libccd
void test_consistency_collision_spherebox_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:1636
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:51
test_consistency_collision_spherebox_GJK
void test_consistency_collision_spherebox_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:2608
extents
std::array< S, 6 > & extents()
Definition: test_fcl_shape_mesh_consistency.cpp:52
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
gjk_solver_indep.h
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
test_consistency_collision_cylindercylinder_GJK
void test_consistency_collision_cylindercylinder_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:2739
geometric_shape_to_BVH_model.h
fcl::test::generateRandomTransform
void generateRandomTransform(S extents[6], Transform3< S > &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
fcl::generateBVHModel
int generateBVHModel(BVHModel< BV > &model, const Box< typename BV::S > &shape, const Transform3< typename BV::S > &pose, FinalizeModel finalize_model)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model-inl.h:69
test_consistency_distance_ellipsoidellipsoid_libccd
void test_consistency_distance_ellipsoidellipsoid_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:157
test_consistency_collision_conecone_GJK
void test_consistency_collision_conecone_GJK()
Definition: test_fcl_shape_mesh_consistency.cpp:2837
fcl::Cone
Center at zero cone.
Definition: cone.h:51
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
test_consistency_collision_spheresphere_libccd
void test_consistency_collision_spheresphere_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:1053
test_consistency_collision_ellipsoidellipsoid_libccd
void test_consistency_collision_ellipsoidellipsoid_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:1279
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
EXPECT_FALSE
#define EXPECT_FALSE(args)
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:51
test_consistency_distance_conecone_libccd
void test_consistency_distance_conecone_libccd()
Definition: test_fcl_shape_mesh_consistency.cpp:450
fcl::DistanceResult::clear
void clear()
clear the result
Definition: distance_result-inl.h:127
collision.h

fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49