test_fcl_geometric_shapes.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 #include <iostream>
40 #include <limits>
41 
42 #include <gtest/gtest.h>
43 
44 #include "fcl/common/unused.h"
45 
46 #include "fcl/math/motion/translation_motion.h"
47 
48 #include "fcl/geometry/shape/cone.h"
49 #include "fcl/geometry/shape/capsule.h"
50 #include "fcl/geometry/shape/ellipsoid.h"
51 #include "fcl/geometry/shape/halfspace.h"
52 #include "fcl/geometry/shape/plane.h"
53 
54 #include "fcl/narrowphase/detail/gjk_solver_indep.h"
55 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
56 #include "fcl/narrowphase/collision.h"
57 
58 #include "test_fcl_utility.h"
59 
60 using namespace fcl;
61 
62 template <typename S>
63 std::array<S, 6>& extents()
64 {
65  static std::array<S, 6> static_extents{ {0, 0, 0, 10, 10, 10} };
66  return static_extents;
67 }
68 
69 template <typename S>
70 detail::GJKSolver_libccd<S>& solver1()
71 {
72  static detail::GJKSolver_libccd<S> static_solver1;
73  return static_solver1;
74 }
75 
76 template <typename S>
77 detail::GJKSolver_indep<S>& solver2()
78 {
79  static detail::GJKSolver_indep<S> static_solver2;
80  return static_solver2;
81 }
82 
83 template <typename S>
84 S tolerance();
85 
86 template <>
87 float tolerance() { return 1e-4; }
88 
89 template <>
90 double tolerance() { return 1e-12; }
91 
92 template <typename S>
93 void test_sphere_shape()
94 {
95  const S radius = 5.0;
96  const S pi = constants<S>::pi();
97 
98  Sphere<S> s(radius);
99 
100  const auto volume = 4.0 / 3.0 * pi * radius * radius * radius;
101  EXPECT_NEAR(volume, s.computeVolume(), tolerance<S>());
102 }
103 
104 GTEST_TEST(FCL_GEOMETRIC_SHAPES, sphere_shape)
105 {
106 // test_sphere_shape<float>();
107  test_sphere_shape<double>();
108 }
109 
110 template <typename S>
111 void test_gjkcache()
112 {
113  Cylinder<S> s1(5, 10);
114  Cone<S> s2(5, 10);
115 
116  CollisionRequest<S> request;
117  request.enable_cached_gjk_guess = true;
118  request.gjk_solver_type = GST_INDEP;
119 
120  TranslationMotion<S> motion(Transform3<S>(Translation3<S>(Vector3<S>(-20.0, -20.0, -20.0))), Transform3<S>(Translation3<S>(Vector3<S>(20.0, 20.0, 20.0))));
121 
122  int N = 1000;
123  S dt = 1.0 / (N - 1);
124 
126  test::Timer timer1;
127  timer1.start();
128  std::vector<bool> result1(N);
129  for(int i = 0; i < N; ++i)
130  {
131  motion.integrate(dt * i);
132  Transform3<S> tf;
133  motion.getCurrentTransform(tf);
134 
135  CollisionResult<S> result;
136 
137  collide(&s1, Transform3<S>::Identity(), &s2, tf, request, result);
138  result1[i] = result.isCollision();
139  request.cached_gjk_guess = result.cached_gjk_guess; // use cached guess
140  }
141 
142  timer1.stop();
143 
145  test::Timer timer2;
146  timer2.start();
147  std::vector<bool> result2(N);
148  request.enable_cached_gjk_guess = false;
149  for(int i = 0; i < N; ++i)
150  {
151  motion.integrate(dt * i);
152  Transform3<S> tf;
153  motion.getCurrentTransform(tf);
154 
155  CollisionResult<S> result;
156 
157  collide(&s1, Transform3<S>::Identity(), &s2, tf, request, result);
158  result2[i] = result.isCollision();
159  }
160 
161  timer2.stop();
162 
163  std::cout << timer1.getElapsedTime() << " " << timer2.getElapsedTime() << std::endl;
164 
165  for(std::size_t i = 0; i < result1.size(); ++i)
166  {
167  EXPECT_TRUE(result1[i] == result2[i]);
168  }
169 }
170 
171 GTEST_TEST(FCL_GEOMETRIC_SHAPES, gjkcache)
172 {
173 // test_gjkcache<float>();
174  test_gjkcache<double>();
175 }
176 
177 template <typename Shape1, typename Shape2>
178 void printComparisonError(const std::string& comparison_type,
179  const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
180  const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
181  GJKSolverType solver_type,
182  const Vector3<typename Shape1::S>& expected_contact_or_normal,
183  const Vector3<typename Shape1::S>& actual_contact_or_normal,
184  bool check_opposite_normal,
185  typename Shape1::S tol)
186 {
187  std::cout << "Disagreement between " << comparison_type
188  << " and expected_" << comparison_type << " for "
189  << test::getNodeTypeName(s1.getNodeType()) << " and "
190  << test::getNodeTypeName(s2.getNodeType()) << " with '"
191  << test::getGJKSolverName(solver_type) << "' solver." << std::endl
192  << "tf1.linear: \n" << tf1.linear() << std::endl
193  << "tf1.translation: " << tf1.translation().transpose() << std::endl
194  << "tf2.linear: \n" << tf2.linear() << std::endl
195  << "tf2.translation: " << tf2.translation().transpose() << std::endl
196  << "expected_" << comparison_type << ": " << expected_contact_or_normal
197  << "actual_" << comparison_type << " : " << actual_contact_or_normal << std::endl;
198 
199  if (check_opposite_normal)
200  std::cout << " or " << -expected_contact_or_normal;
201 
202  std::cout << std::endl
203  << "difference: " << (actual_contact_or_normal - expected_contact_or_normal).norm() << std::endl
204  << "tolerance: " << tol << std::endl;
205 }
206 
207 template <typename Shape1, typename Shape2>
208 void printComparisonError(const std::string& comparison_type,
209  const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
210  const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
211  GJKSolverType solver_type,
212  typename Shape1::S expected_depth,
213  typename Shape1::S actual_depth,
214  typename Shape1::S tol)
215 {
216  std::cout << "Disagreement between " << comparison_type
217  << " and expected_" << comparison_type << " for "
218  << test::getNodeTypeName(s1.getNodeType()) << " and "
219  << test::getNodeTypeName(s2.getNodeType()) << " with '"
220  << test::getGJKSolverName(solver_type) << "' solver." << std::endl
221  << "tf1.linear: \n" << tf1.linear() << std::endl
222  << "tf1.translation: " << tf1.translation().transpose() << std::endl
223  << "tf2.linear: \n" << tf2.linear() << std::endl
224  << "tf2.translation: " << tf2.translation().transpose() << std::endl
225  << "expected_depth: " << expected_depth << std::endl
226  << "actual_depth : " << actual_depth << std::endl
227  << "difference: " << std::abs(actual_depth - expected_depth) << std::endl
228  << "tolerance: " << tol << std::endl;
229 }
230 
231 template <typename Shape1, typename Shape2>
232 bool checkContactPointds(const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
233  const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
234  GJKSolverType solver_type,
235  const ContactPoint<typename Shape1::S>& expected, const ContactPoint<typename Shape1::S>& actual,
236  bool check_position = false,
237  bool check_depth = false,
238  bool check_normal = false,
239  bool check_opposite_normal = false,
240  typename Shape1::S tol = 1e-9)
241 {
242  FCL_UNUSED(s1);
243  FCL_UNUSED(tf1);
244  FCL_UNUSED(s2);
245  FCL_UNUSED(tf2);
246  FCL_UNUSED(solver_type);
247 
248  if (check_position)
249  {
250  bool contact_equal = actual.pos.isApprox(expected.pos, tol);
251  if (!contact_equal)
252  return false;
253  }
254 
255  if (check_depth)
256  {
257  bool depth_equal = std::abs(actual.penetration_depth - expected.penetration_depth) < tol;
258  if (!depth_equal)
259  return false;
260  }
261 
262  if (check_normal)
263  {
264  bool normal_equal = actual.normal.isApprox(expected.normal, tol);
265 
266  if (!normal_equal && check_opposite_normal)
267  normal_equal = actual.normal.isApprox(-expected.normal, tol);
268 
269  if (!normal_equal)
270  return false;
271  }
272 
273  return true;
274 }
275 
276 template <typename Shape1, typename Shape2>
277 bool inspectContactPointds(const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
278  const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
279  GJKSolverType solver_type,
280  const std::vector<ContactPoint<typename Shape1::S>>& expected_contacts,
281  const std::vector<ContactPoint<typename Shape1::S>>& actual_contacts,
282  bool check_position = false,
283  bool check_depth = false,
284  bool check_normal = false,
285  bool check_opposite_normal = false,
286  typename Shape1::S tol = 1e-9)
287 {
288  using S = typename Shape1::S;
289 
290  // Check number of contact points
291  bool sameNumContacts = (actual_contacts.size() == expected_contacts.size());
292  EXPECT_TRUE(sameNumContacts);
293  if (!sameNumContacts)
294  {
295  std::cout << "\n"
296  << "===== [ geometric shape collision test failure report ] ======\n"
297  << "\n"
298  << "Solver type: " << test::getGJKSolverName(solver_type) << "\n"
299  << "\n"
300  << "[ Shape 1 ]\n"
301  << "Shape type : " << test::getNodeTypeName(s1.getNodeType()) << "\n"
302  << "tf1.linear : \n" << tf1.linear() << "\n"
303  << "tf1.translation: " << tf1.translation().transpose() << "\n"
304  << "\n"
305  << "[ Shape 2 ]\n"
306  << "Shape type : " << test::getNodeTypeName(s2.getNodeType()) << "\n"
307  << "tf2.linear : \n" << tf2.linear() << "\n"
308  << "tf2.translation: " << tf2.translation().transpose() << "\n"
309  << "\n"
310  << "The numbers of expected contacts '"
311  << expected_contacts.size()
312  << "' and the number of actual contacts '"
313  << actual_contacts.size()
314  << "' are not equal.\n"
315  << "\n";
316  return false;
317  }
318 
319  // Check if actual contacts and expected contacts are matched
320  const size_t numContacts = actual_contacts.size();
321 
322  std::vector<int> index_to_actual_contacts(numContacts, -1);
323  std::vector<int> index_to_expected_contacts(numContacts, -1);
324 
325  bool foundAll = true;
326  for (size_t i = 0; i < numContacts; ++i)
327  {
328  const ContactPoint<S>& expected = expected_contacts[i];
329 
330  // Check if expected contact is in the list of actual contacts
331  for (size_t j = 0; j < numContacts; ++j)
332  {
333  if (index_to_expected_contacts[j] != -1)
334  continue;
335 
336  const ContactPoint<S>& actual = actual_contacts[j];
337 
338  bool found = checkContactPointds(
339  s1, tf1, s2, tf2, solver_type,
340  expected, actual,
341  check_position,
342  check_depth,
343  check_normal, check_opposite_normal,
344  tol);
345 
346  if (found)
347  {
348  index_to_actual_contacts[i] = j;
349  index_to_expected_contacts[j] = i;
350  break;
351  }
352  }
353 
354  if (index_to_actual_contacts[i] == -1)
355  foundAll = false;
356  }
357 
358  if (!foundAll)
359  {
360  std::cout << "\n"
361  << "===== [ geometric shape collision test failure report ] ======\n"
362  << "\n"
363  << "Solver type: " << test::getGJKSolverName(solver_type) << "\n"
364  << "\n"
365  << "[ Shape 1 ]\n"
366  << "Shape type : " << test::getNodeTypeName(s1.getNodeType()) << "\n"
367  << "tf1.linear : \n" << tf1.linear() << "\n"
368  << "tf1.translation: " << tf1.translation().transpose() << "\n"
369  << "\n"
370  << "[ Shape 2 ]\n"
371  << "Shape type : " << test::getNodeTypeName(s2.getNodeType()) << "\n"
372  << "tf2.linear : \n" << tf2.linear() << "\n"
373  << "tf2.translation: " << tf2.translation().transpose() << "\n"
374  << "\n"
375  << "[ Expected Contacts: " << numContacts << " ]\n";
376  for (size_t i = 0; i < numContacts; ++i)
377  {
378  const ContactPoint<S>& expected = expected_contacts[i];
379 
380  std::cout << "(" << i << ") pos: " << expected.pos.transpose() << ", "
381  << "normal: " << expected.normal.transpose() << ", "
382  << "depth: " << expected.penetration_depth
383  << " ---- ";
384  if (index_to_actual_contacts[i] != -1)
385  std::cout << "found, actual (" << index_to_actual_contacts[i] << ")\n";
386  else
387  std::cout << "not found!\n";
388  }
389  std::cout << "\n"
390  << "[ Actual Contacts: " << numContacts << " ]\n";
391  for (size_t i = 0; i < numContacts; ++i)
392  {
393  const ContactPoint<S>& actual = actual_contacts[i];
394 
395  std::cout << "(" << i << ") pos: " << actual.pos.transpose() << ", "
396  << "normal: " << actual.normal.transpose() << ", "
397  << "depth: " << actual.penetration_depth
398  << " ---- ";
399  if (index_to_expected_contacts[i] != -1)
400  std::cout << "found, expected (" << index_to_expected_contacts[i] << ")\n";
401  else
402  std::cout << "not found!\n";
403  }
404 
405  std::cout << "\n";
406  }
407 
408  return foundAll;
409 }
410 
411 template <typename S>
412 void getContactPointdsFromResult(std::vector<ContactPoint<S>>& contacts, const CollisionResult<S>& result)
413 {
414  const size_t numContacts = result.numContacts();
415  contacts.resize(numContacts);
416 
417  for (size_t i = 0; i < numContacts; ++i)
418  {
419  const auto& cnt = result.getContact(i);
420 
421  contacts[i].pos = cnt.pos;
422  contacts[i].normal = cnt.normal;
423  contacts[i].penetration_depth = cnt.penetration_depth;
424  }
425 }
426 
427 template <typename Shape1, typename Shape2>
428 void testShapeIntersection(
429  const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
430  const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
431  GJKSolverType solver_type,
432  bool expected_res,
433  const std::vector<ContactPoint<typename Shape1::S>>& expected_contacts = std::vector<ContactPoint<typename Shape1::S>>(),
434  bool check_position = true,
435  bool check_depth = true,
436  bool check_normal = true,
437  bool check_opposite_normal = false,
438  typename Shape1::S tol = 1e-9)
439 {
440  using S = typename Shape1::S;
441 
442  CollisionRequest<S> request;
443  request.gjk_solver_type = solver_type;
444  request.num_max_contacts = std::numeric_limits<size_t>::max();
445  CollisionResult<S> result;
446 
447  std::vector<ContactPoint<S>> actual_contacts;
448 
449  bool res;
450 
451  // Part A: Check collisions using shapeIntersect()
452 
453  // Check only whether they are colliding or not.
454  if (solver_type == GST_LIBCCD)
455  {
456  res = solver1<S>().shapeIntersect(s1, tf1, s2, tf2, nullptr);
457  }
458  else if (solver_type == GST_INDEP)
459  {
460  res = solver2<S>().shapeIntersect(s1, tf1, s2, tf2, nullptr);
461  }
462  else
463  {
464  std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
465  return;
466  }
467  EXPECT_EQ(expected_res, res);
468 
469  // Check contact information as well
470  if (solver_type == GST_LIBCCD)
471  {
472  res = solver1<S>().shapeIntersect(s1, tf1, s2, tf2, &actual_contacts);
473  }
474  else if (solver_type == GST_INDEP)
475  {
476  res = solver2<S>().shapeIntersect(s1, tf1, s2, tf2, &actual_contacts);
477  }
478  else
479  {
480  std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
481  return;
482  }
483  EXPECT_EQ(expected_res, res);
484  if (expected_res)
485  {
486  EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, solver_type,
487  expected_contacts, actual_contacts,
488  check_position,
489  check_depth,
490  check_normal, check_opposite_normal,
491  tol));
492  }
493 
494  // Part B: Check collisions using collide()
495 
496  // Check only whether they are colliding or not.
497  request.enable_contact = false;
498  result.clear();
499  res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
500  EXPECT_EQ(expected_res, res);
501 
502  // Check contact information as well
503  request.enable_contact = true;
504  result.clear();
505  res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
506  EXPECT_EQ(expected_res, res);
507  if (expected_res)
508  {
509  getContactPointdsFromResult(actual_contacts, result);
510  EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, solver_type,
511  expected_contacts, actual_contacts,
512  check_position,
513  check_depth,
514  check_normal, check_opposite_normal,
515  tol));
516  }
517 }
518 
519 // Shape intersection test coverage (libccd)
520 //
521 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
522 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
523 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
524 // | box | O | O | | | | | O | O | |
525 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
526 // | sphere |/////| O | | | | | O | O | O |
527 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
528 // | ellipsoid |/////|////////| O | | | | O | O | |
529 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
530 // | capsule |/////|////////|///////////| | | | O | O | |
531 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
532 // | cone |/////|////////|///////////|/////////| O | O | O | O | |
533 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
534 // | cylinder |/////|////////|///////////|/////////|//////| O | O | O | |
535 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
536 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
537 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
538 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
539 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
540 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
541 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
542 
543 template <typename S>
544 void test_shapeIntersection_spheresphere()
545 {
546  Sphere<S> s1(20);
547  Sphere<S> s2(10);
548 
549  Transform3<S> tf1;
550  Transform3<S> tf2;
551 
552  Transform3<S> transform = Transform3<S>::Identity();
553  test::generateRandomTransform(extents<S>(), transform);
554 
555  std::vector<ContactPoint<S>> contacts;
556 
557  tf1 = Transform3<S>::Identity();
558  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
559  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
560 
561  tf1 = transform;
562  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
563  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
564 
565  tf1 = Transform3<S>::Identity();
566  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
567  contacts.resize(1);
568  contacts[0].normal << 1, 0, 0;
569  contacts[0].pos << 20, 0, 0;
570  contacts[0].penetration_depth = 0.0;
571  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);
572 
573  tf1 = Transform3<S>::Identity();
574  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
575  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
576 
577  tf1 = transform;
578  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
579  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
580 
581  tf1 = Transform3<S>::Identity();
582  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
583  contacts.resize(1);
584  contacts[0].normal << 1, 0, 0;
585  contacts[0].pos << 20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0;
586  contacts[0].penetration_depth = 0.1;
587  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);
588 
589  tf1 = transform;
590  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
591  contacts.resize(1);
592  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
593  contacts[0].pos = transform * Vector3<S>(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0);
594  contacts[0].penetration_depth = 0.1;
595  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);
596 
597  tf1 = Transform3<S>::Identity();
598  tf2 = Transform3<S>::Identity();
599  contacts.resize(1);
600  contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
601  contacts[0].pos.setZero();
602  contacts[0].penetration_depth = 20.0 + 10.0;
603  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);
604 
605  tf1 = transform;
606  tf2 = transform;
607  contacts.resize(1);
608  contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
609  contacts[0].pos = transform * Vector3<S>::Zero();
610  contacts[0].penetration_depth = 20.0 + 10.0;
611  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);
612 
613  tf1 = Transform3<S>::Identity();
614  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
615  contacts.resize(1);
616  contacts[0].normal << -1, 0, 0;
617  contacts[0].pos << -20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0;
618  contacts[0].penetration_depth = 0.1;
619  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);
620 
621  tf1 = transform;
622  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
623  contacts.resize(1);
624  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
625  contacts[0].pos = transform * Vector3<S>(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0);
626  contacts[0].penetration_depth = 0.1;
627  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);
628 
629  tf1 = Transform3<S>::Identity();
630  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.0, 0, 0)));
631  contacts.resize(1);
632  contacts[0].normal << -1, 0, 0;
633  contacts[0].pos << -20, 0, 0;
634  contacts[0].penetration_depth = 0.0;
635  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);
636 
637  tf1 = Transform3<S>::Identity();
638  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
639  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
640 
641  tf1 = transform;
642  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
643  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
644 }
645 
646 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheresphere)
647 {
648 // test_shapeIntersection_spheresphere<float>();
649  test_shapeIntersection_spheresphere<double>();
650 }
651 
652 template <typename S>
653 bool compareContactPointds1(const Vector3<S>& c1,const Vector3<S>& c2)
654 {
655  return c1[2] < c2[2];
656 } // Ascending order
657 
658 template <typename S>
659 bool compareContactPointds2(const ContactPoint<S>& cp1,const ContactPoint<S>& cp2)
660 {
661  return cp1.pos[2] < cp2.pos[2];
662 } // Ascending order
663 
664 // Simple aligned boxes intersecting each other
665 //
666 // s2 ┌───┐
667 // │ │
668 // ┏━━━━━━━━━┿━━━┿━━━━━━━━━┓┄┄┄┄┄ z = 0
669 // ┃ │┄┄┄│┄┄┄┄┄┄┄┄┄┃┄┄┄ Reported contact depth
670 // ┃ └───┘ ┃
671 // ┃ ┃
672 // ┃ ┃
673 // ┃ s1 ┃
674 // ┃ ┃
675 // ┃ ┃
676 // ┃ ┃
677 // ┗━━━━━━━━━━━━━━━━━━━━━━━┛
678 //
679 // s1 is a cube, 100 units to a side. Shifted downward 50 units so that it's
680 // top face is at z = 0.
681 // s2 is a box with dimensions 10, 20, 30 in the x-, y-, and z-directions,
682 // respectively. It is centered on the origin and oriented according to the
683 // provided rotation matrix `R`.
684 // The total penetration depth is the |z_max| where z_max is the z-position of
685 // the most deeply penetrating vertex on s2. The contact position will be
686 // located at -z_max / 2 (with the assumption that all penetration happens
687 // *below* the z = 0 axis.
688 template <typename Derived>
689 void testBoxBoxContactPointds(const Eigen::MatrixBase<Derived>& R)
690 {
691  using S = typename Derived::RealScalar;
692 
693  Box<S> s1(100, 100, 100);
694  Box<S> s2(10, 20, 30);
695 
696  // Vertices of s2
697  std::vector<Vector3<S>> vertices(8);
698  vertices[0] << 1, 1, 1;
699  vertices[1] << 1, 1, -1;
700  vertices[2] << 1, -1, 1;
701  vertices[3] << 1, -1, -1;
702  vertices[4] << -1, 1, 1;
703  vertices[5] << -1, 1, -1;
704  vertices[6] << -1, -1, 1;
705  vertices[7] << -1, -1, -1;
706 
707  for (int i = 0; i < 8; ++i)
708  {
709  vertices[i][0] *= 0.5 * s2.side[0];
710  vertices[i][1] *= 0.5 * s2.side[1];
711  vertices[i][2] *= 0.5 * s2.side[2];
712  }
713 
714  Transform3<S> tf1 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -50)));
715  Transform3<S> tf2 = Transform3<S>(R);
716 
717  std::vector<ContactPoint<S>> contacts;
718 
719  // Make sure the two boxes are colliding
720  bool res = solver1<S>().shapeIntersect(s1, tf1, s2, tf2, &contacts);
721  EXPECT_TRUE(res);
722 
723  // Compute global vertices
724  for (int i = 0; i < 8; ++i)
725  vertices[i] = tf2 * vertices[i];
726 
727  // Sort the vertices so that the lowest vertex along z-axis comes first
728  std::sort(vertices.begin(), vertices.end(), compareContactPointds1<S>);
729  std::sort(contacts.begin(), contacts.end(), compareContactPointds2<S>);
730 
731  // The lowest n vertex along z-axis should be the contact point
732  size_t numContacts = contacts.size();
733  numContacts = std::min(static_cast<size_t>(1), numContacts);
734  // TODO: BoxBox algorithm seems not able to find all the colliding vertices.
735  // We just check the deepest one as workaround.
736  for (size_t i = 0; i < numContacts; ++i)
737  {
738  // The reported contact position will lie mid-way between the deepest
739  // penetrating vertex on s2 and the z = 0 plane.
740  Vector3<S> contact_pos(vertices[i]);
741  contact_pos(2) /= 2;
742  EXPECT_TRUE(contact_pos.isApprox(contacts[i].pos))
743  << "\n\tExpected: " << contact_pos
744  << "\n\tFound: " << contacts[i].pos;
745  EXPECT_TRUE(Vector3<S>(0, 0, 1).isApprox(contacts[i].normal));
746  }
747 }
748 
749 template <typename S>
750 void test_shapeIntersection_boxbox()
751 {
752  Box<S> s1(20, 40, 50);
753  Box<S> s2(10, 10, 10);
754 
755  Transform3<S> tf1;
756  Transform3<S> tf2;
757 
758  Transform3<S> transform = Transform3<S>::Identity();
759  test::generateRandomTransform(extents<S>(), transform);
760 
761  std::vector<ContactPoint<S>> contacts;
762 
763  Quaternion<S> q(AngleAxis<S>((S)3.140 / 6, Vector3<S>(0, 0, 1)));
764 
765  tf1 = Transform3<S>::Identity();
766  tf2 = Transform3<S>::Identity();
767  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
768  contacts.resize(4);
769  contacts[0].normal << 1, 0, 0;
770  contacts[1].normal << 1, 0, 0;
771  contacts[2].normal << 1, 0, 0;
772  contacts[3].normal << 1, 0, 0;
773  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
774 
775  tf1 = transform;
776  tf2 = transform;
777  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
778  contacts.resize(4);
779  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
780  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
781  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
782  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
783  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
784 
785  tf1 = Transform3<S>::Identity();
786  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(15, 0, 0)));
787  contacts.resize(4);
788  contacts[0].normal = Vector3<S>(1, 0, 0);
789  contacts[1].normal = Vector3<S>(1, 0, 0);
790  contacts[2].normal = Vector3<S>(1, 0, 0);
791  contacts[3].normal = Vector3<S>(1, 0, 0);
792  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
793 
794  tf1 = Transform3<S>::Identity();
795  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(15.01, 0, 0)));
796  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
797 
798  tf1 = Transform3<S>::Identity();
799  tf2 = Transform3<S>(q);
800  contacts.resize(4);
801  contacts[0].normal = Vector3<S>(1, 0, 0);
802  contacts[1].normal = Vector3<S>(1, 0, 0);
803  contacts[2].normal = Vector3<S>(1, 0, 0);
804  contacts[3].normal = Vector3<S>(1, 0, 0);
805  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
806 
807  tf1 = transform;
808  tf2 = transform * Transform3<S>(q);
809  contacts.resize(4);
810  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
811  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
812  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
813  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
814  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
815 
816  // Disabled for particular configurations: macOS + release + double (see #202)
817 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
818  uint32 numTests = 1e+2;
819  for (uint32 i = 0; i < numTests; ++i)
820  {
821  Transform3<S> tf;
822  test::generateRandomTransform(extents<S>(), tf);
823  testBoxBoxContactPointds(tf.linear());
824  }
825 #endif
826 }
827 
828 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_boxbox)
829 {
830 // test_shapeIntersection_boxbox<float>();
831  test_shapeIntersection_boxbox<double>();
832 }
833 
834 template <typename S>
835 void test_shapeIntersection_spherebox()
836 {
837  // A collection of bool constants to make reading lists of bool easier.
838  const bool collides = true;
839  const bool check_position = true;
840  const bool check_depth = true;
841  const bool check_normal = true;
842  const bool check_opposite_normal = false;
843 
844  Sphere<S> s1(20);
845  Box<S> s2(5, 5, 5);
846 
847  Transform3<S> tf1;
848  Transform3<S> tf2;
849 
850  Transform3<S> transform = Transform3<S>::Identity();
851  test::generateRandomTransform(extents<S>(), transform);
852 
853  std::vector<ContactPoint<S>> contacts;
854 
855  tf1 = Transform3<S>::Identity();
856  tf2 = Transform3<S>::Identity();
857  // NOTE: The centers of the box and sphere are coincident. The documented
858  // convention is that the normal is aligned with the smallest dimension of
859  // the box, pointing in the negative direction of that axis. *This* test is
860  // the driving basis for that definition.
861  contacts.resize(1);
862  contacts[0].normal << -1, 0, 0;
863  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
864  !check_position, !check_depth, check_normal);
865 
866  tf1 = transform;
867  tf2 = transform;
868  contacts.resize(1);
869  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
870  !check_position, !check_depth, !check_normal);
871 
872  tf1 = Transform3<S>::Identity();
873  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.5, 0, 0)));
874  // As documented in sphere_box.h, touching is considered collision, so this
875  // should produce a collision.
876  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
877  !check_position, !check_depth, !check_normal);
878 
879  tf1 = transform;
880  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.501, 0, 0)));
881  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, !collides);
882 
883  tf1 = Transform3<S>::Identity();
884  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
885  contacts.resize(1);
886  contacts[0].normal << 1, 0, 0;
887  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
888  !check_position, !check_depth, check_normal);
889 
890  tf1 = transform;
891  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
892  contacts.resize(1);
893  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
894  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
895  !check_position, !check_depth, check_normal,
896  !check_opposite_normal, 1e-4);
897 }
898 
899 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherebox)
900 {
901 // test_shapeIntersection_spherebox<float>();
902  test_shapeIntersection_spherebox<double>();
903 }
904 
905 template <typename S>
906 void test_shapeIntersection_spherecapsule()
907 {
908  Sphere<S> s1(20);
909  Capsule<S> s2(5, 10);
910 
911  Transform3<S> tf1;
912  Transform3<S> tf2;
913 
914  Transform3<S> transform = Transform3<S>::Identity();
915  test::generateRandomTransform(extents<S>(), transform);
916 
917  std::vector<ContactPoint<S>> contacts;
918 
919  tf1 = Transform3<S>::Identity();
920  tf2 = Transform3<S>::Identity();
921  // TODO: Need convention for normal when the centers of two objects are at same position.
922  contacts.resize(1);
923  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
924 
925  tf1 = transform;
926  tf2 = transform;
927  // TODO: Need convention for normal when the centers of two objects are at same position.
928  contacts.resize(1);
929  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
930 
931  tf1 = Transform3<S>::Identity();
932  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
933  contacts.resize(1);
934  contacts[0].normal << 1, 0, 0;
935  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
936 
937  tf1 = transform;
938  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
939  contacts.resize(1);
940  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
941  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
942 
943  tf1 = Transform3<S>::Identity();
944  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(25, 0, 0)));
945  contacts.resize(1);
946  contacts[0].normal << 1, 0, 0;
947  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
948 
949  tf1 = transform;
950  //tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25, 0, 0)));
951  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25 - 1e-6, 0, 0)));
952  contacts.resize(1);
953  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
954  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
955 
956  tf1 = Transform3<S>::Identity();
957  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(25.1, 0, 0)));
958  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
959 
960  tf1 = transform;
961  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25.1, 0, 0)));
962  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
963 }
964 
965 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherecapsule)
966 {
967 // test_shapeIntersection_spherecapsule<float>();
968  test_shapeIntersection_spherecapsule<double>();
969 }
970 
971 template <typename S>
972 void test_shapeIntersection_cylindercylinder()
973 {
974  Cylinder<S> s1(5, 10);
975  Cylinder<S> s2(5, 10);
976 
977  Transform3<S> tf1;
978  Transform3<S> tf2;
979 
980  Transform3<S> transform = Transform3<S>::Identity();
981  test::generateRandomTransform(extents<S>(), transform);
982 
983  std::vector<ContactPoint<S>> contacts;
984 
985  tf1 = Transform3<S>::Identity();
986  tf2 = Transform3<S>::Identity();
987  // TODO: Need convention for normal when the centers of two objects are at same position.
988  contacts.resize(1);
989  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
990 
991  tf1 = transform;
992  tf2 = transform;
993  // TODO: Need convention for normal when the centers of two objects are at same position.
994  contacts.resize(1);
995  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
996 
997  tf1 = Transform3<S>::Identity();
998  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
999  contacts.resize(1);
1000  contacts[0].normal << 1, 0, 0;
1001  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
1002 
1003  // Disabled for particular configurations: macOS + release + double (see #202)
1004 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
1005  tf1 = transform;
1006  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1007  contacts.resize(1);
1008  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1009  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5);
1010 #endif
1011 
1012  tf1 = Transform3<S>::Identity();
1013  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
1014  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1015 
1016  tf1 = transform;
1017  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
1018  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1019 }
1020 
1021 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercylinder)
1022 {
1023 // test_shapeIntersection_cylindercylinder<float>();
1024  test_shapeIntersection_cylindercylinder<double>();
1025 }
1026 
1027 template <typename S>
1028 void test_shapeIntersection_conecone()
1029 {
1030  Cone<S> s1(5, 10);
1031  Cone<S> s2(5, 10);
1032 
1033  Transform3<S> tf1;
1034  Transform3<S> tf2;
1035 
1036  Transform3<S> transform = Transform3<S>::Identity();
1037  test::generateRandomTransform(extents<S>(), transform);
1038 
1039  std::vector<ContactPoint<S>> contacts;
1040 
1041  tf1 = Transform3<S>::Identity();
1042  tf2 = Transform3<S>::Identity();
1043  // TODO: Need convention for normal when the centers of two objects are at same position.
1044  contacts.resize(1);
1045  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1046 
1047  tf1 = transform;
1048  tf2 = transform;
1049  // TODO: Need convention for normal when the centers of two objects are at same position.
1050  contacts.resize(1);
1051  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1052 
1053  tf1 = Transform3<S>::Identity();
1054  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1055  contacts.resize(1);
1056  contacts[0].normal << 1, 0, 0;
1057  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
1058 
1059  tf1 = transform;
1060  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1061  contacts.resize(1);
1062  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1063  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 5e-5);
1064 
1065  tf1 = Transform3<S>::Identity();
1066  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.001, 0, 0)));
1067  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1068 
1069  tf1 = transform;
1070  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.001, 0, 0)));
1071  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1072 
1073  tf1 = Transform3<S>::Identity();
1074  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
1075  contacts.resize(1);
1076  contacts[0].normal << 0, 0, 1;
1077  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
1078 
1079  tf1 = transform;
1080  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
1081  contacts.resize(1);
1082  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
1083  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5);
1084 }
1085 
1086 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_conecone)
1087 {
1088 // test_shapeIntersection_conecone<float>();
1089  test_shapeIntersection_conecone<double>();
1090 }
1091 
1092 template <typename S>
1093 void test_shapeIntersection_cylindercone()
1094 {
1095  Cylinder<S> s1(5, 10);
1096  Cone<S> s2(5, 10);
1097 
1098  Transform3<S> tf1;
1099  Transform3<S> tf2;
1100 
1101  Transform3<S> transform = Transform3<S>::Identity();
1102  test::generateRandomTransform(extents<S>(), transform);
1103 
1104  std::vector<ContactPoint<S>> contacts;
1105 
1106  tf1 = Transform3<S>::Identity();
1107  tf2 = Transform3<S>::Identity();
1108  // TODO: Need convention for normal when the centers of two objects are at same position.
1109  contacts.resize(1);
1110  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1111 
1112  tf1 = transform;
1113  tf2 = transform;
1114  // TODO: Need convention for normal when the centers of two objects are at same position.
1115  contacts.resize(1);
1116  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1117 
1118  tf1 = Transform3<S>::Identity();
1119  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1120  contacts.resize(1);
1121  contacts[0].normal << 1, 0, 0;
1122  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.061);
1123 
1124  tf1 = transform;
1125  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1126  contacts.resize(1);
1127  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1128  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.46);
1129 
1130  tf1 = Transform3<S>::Identity();
1131  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
1132  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1133 
1134  tf1 = transform;
1135  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
1136  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1137 
1138  tf1 = Transform3<S>::Identity();
1139  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
1140  contacts.resize(1);
1141  contacts[0].normal << 0, 0, 1;
1142  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
1143 
1144  tf1 = transform;
1145  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
1146  contacts.resize(1);
1147  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
1148  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-4);
1149 
1150  tf1 = Transform3<S>::Identity();
1151  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.01)));
1152  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1153 
1154  tf1 = transform;
1155  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.01)));
1156  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1157 }
1158 
1159 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercone)
1160 {
1161 // test_shapeIntersection_cylindercone<float>();
1162  test_shapeIntersection_cylindercone<double>();
1163 }
1164 
1165 template <typename S>
1166 void test_shapeIntersection_ellipsoidellipsoid()
1167 {
1168  Ellipsoid<S> s1(20, 40, 50);
1169  Ellipsoid<S> s2(10, 10, 10);
1170 
1171  Transform3<S> tf1;
1172  Transform3<S> tf2;
1173 
1174  Transform3<S> transform = Transform3<S>::Identity();
1175  test::generateRandomTransform(extents<S>(), transform);
1176  Transform3<S> identity;
1177 
1178  std::vector<ContactPoint<S>> contacts;
1179 
1180  tf1 = Transform3<S>::Identity();
1181  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
1182  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1183 
1184  tf1 = transform;
1185  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
1186  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1187 
1188  tf1 = Transform3<S>::Identity();
1189  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
1190  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1191 
1192  tf1 = transform;
1193  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
1194  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1195 
1196  tf1 = Transform3<S>::Identity();
1197  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.99, 0, 0)));
1198  contacts.resize(1);
1199  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1200 
1201  tf1 = transform;
1202  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
1203  contacts.resize(1);
1204  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1205 
1206  tf1 = Transform3<S>::Identity();
1207  tf2 = Transform3<S>::Identity();
1208  contacts.resize(1);
1209  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1210 
1211  tf1 = transform;
1212  tf2 = transform;
1213  contacts.resize(1);
1214  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1215 
1216  tf1 = Transform3<S>::Identity();
1217  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
1218  contacts.resize(1);
1219  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1220 
1221  tf1 = transform;
1222  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
1223  contacts.resize(1);
1224  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1225 
1226  tf1 = Transform3<S>::Identity();
1227  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30, 0, 0)));
1228  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1229 
1230  tf1 = transform;
1231  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
1232  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1233 }
1234 
1235 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_ellipsoidellipsoid)
1236 {
1237 // test_shapeIntersection_ellipsoidellipsoid<float>();
1238  test_shapeIntersection_ellipsoidellipsoid<double>();
1239 }
1240 
1241 template <typename S>
1242 void test_shapeIntersection_spheretriangle()
1243 {
1244  Sphere<S> s(10);
1245  Vector3<S> t[3];
1246  t[0] << 20, 0, 0;
1247  t[1] << -20, 0, 0;
1248  t[2] << 0, 20, 0;
1249 
1250  Transform3<S> transform = Transform3<S>::Identity();
1251  test::generateRandomTransform(extents<S>(), transform);
1252 
1253  Vector3<S> normal;
1254  bool res;
1255 
1256  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
1257  EXPECT_TRUE(res);
1258 
1259  res = solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1260  EXPECT_TRUE(res);
1261 
1262 
1263  t[0] << 30, 0, 0;
1264  t[1] << 9.9, -20, 0;
1265  t[2] << 9.9, 20, 0;
1266  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
1267  EXPECT_TRUE(res);
1268 
1269  res = solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1270  EXPECT_TRUE(res);
1271 
1272  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, &normal);
1273  EXPECT_TRUE(res);
1274  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
1275 
1276  res = solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
1277  EXPECT_TRUE(res);
1278  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
1279 }
1280 
1281 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheretriangle)
1282 {
1283 // test_shapeIntersection_spheretriangle<float>();
1284  test_shapeIntersection_spheretriangle<double>();
1285 }
1286 
1287 template <typename S>
1288 void test_shapeIntersection_halfspacetriangle()
1289 {
1290  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
1291  Vector3<S> t[3];
1292  t[0] << 20, 0, 0;
1293  t[1] << -20, 0, 0;
1294  t[2] << 0, 20, 0;
1295 
1296  Transform3<S> transform = Transform3<S>::Identity();
1297  test::generateRandomTransform(extents<S>(), transform);
1298 
1299  // Vector3<S> point;
1300  // S depth;
1301  Vector3<S> normal;
1302  bool res;
1303 
1304  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
1305  EXPECT_TRUE(res);
1306 
1307  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1308  EXPECT_TRUE(res);
1309 
1310 
1311  t[0] << 20, 0, 0;
1312  t[1] << 0, -20, 0;
1313  t[2] << 0, 20, 0;
1314  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
1315  EXPECT_TRUE(res);
1316 
1317  // Disabled for particular configurations: macOS + release + double (see #202)
1318 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
1319  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1320  EXPECT_TRUE(res);
1321 #endif
1322 
1323  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
1324  EXPECT_TRUE(res);
1325  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
1326 
1327  // Disabled for particular configurations: macOS + release + double (see #202)
1328 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
1329  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
1330  EXPECT_TRUE(res);
1331  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
1332 #endif
1333 }
1334 
1335 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacetriangle)
1336 {
1337 // test_shapeIntersection_halfspacetriangle<float>();
1338  test_shapeIntersection_halfspacetriangle<double>();
1339 }
1340 
1341 template <typename S>
1342 void test_shapeIntersection_planetriangle()
1343 {
1344  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
1345  Vector3<S> t[3];
1346  t[0] << 20, 0, 0;
1347  t[1] << -20, 0, 0;
1348  t[2] << 0, 20, 0;
1349 
1350  Transform3<S> transform = Transform3<S>::Identity();
1351  test::generateRandomTransform(extents<S>(), transform);
1352 
1353  // Vector3<S> point;
1354  // S depth;
1355  Vector3<S> normal;
1356  bool res;
1357 
1358  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
1359  EXPECT_TRUE(res);
1360 
1361  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1362  EXPECT_TRUE(res);
1363 
1364 
1365  t[0] << 20, 0, 0;
1366  t[1] << -0.1, -20, 0;
1367  t[2] << -0.1, 20, 0;
1368  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
1369  EXPECT_TRUE(res);
1370 
1371  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1372  EXPECT_TRUE(res);
1373 
1374  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
1375  EXPECT_TRUE(res);
1376  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
1377 
1378  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
1379  EXPECT_TRUE(res);
1380  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
1381 }
1382 
1383 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planetriangle)
1384 {
1385 // test_shapeIntersection_planetriangle<float>();
1386  test_shapeIntersection_planetriangle<double>();
1387 }
1388 
1389 template <typename S>
1390 void test_shapeIntersection_halfspacesphere()
1391 {
1392  Sphere<S> s(10);
1393  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
1394 
1395  Transform3<S> tf1;
1396  Transform3<S> tf2;
1397 
1398  Transform3<S> transform = Transform3<S>::Identity();
1399  test::generateRandomTransform(extents<S>(), transform);
1400 
1401  std::vector<ContactPoint<S>> contacts;
1402 
1403  tf1 = Transform3<S>::Identity();
1404  tf2 = Transform3<S>::Identity();
1405  contacts.resize(1);
1406  contacts[0].pos << -5, 0, 0;
1407  contacts[0].penetration_depth = 10;
1408  contacts[0].normal << -1, 0, 0;
1409  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1410 
1411  tf1 = transform;
1412  tf2 = transform;
1413  contacts.resize(1);
1414  contacts[0].pos = transform * Vector3<S>(-5, 0, 0);
1415  contacts[0].penetration_depth = 10;
1416  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1417  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1418 
1419  tf1 = Transform3<S>::Identity();
1420  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
1421  contacts.resize(1);
1422  contacts[0].pos << -2.5, 0, 0;
1423  contacts[0].penetration_depth = 15;
1424  contacts[0].normal << -1, 0, 0;
1425  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1426 
1427  tf1 = transform;
1428  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
1429  contacts.resize(1);
1430  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
1431  contacts[0].penetration_depth = 15;
1432  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1433  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1434 
1435  tf1 = Transform3<S>::Identity();
1436  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
1437  contacts.resize(1);
1438  contacts[0].pos << -7.5, 0, 0;
1439  contacts[0].penetration_depth = 5;
1440  contacts[0].normal << -1, 0, 0;
1441  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1442 
1443  tf1 = transform;
1444  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
1445  contacts.resize(1);
1446  contacts[0].pos = transform * Vector3<S>(-7.5, 0, 0);
1447  contacts[0].penetration_depth = 5;
1448  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1449  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1450 
1451  tf1 = Transform3<S>::Identity();
1452  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
1453  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1454 
1455  tf1 = transform;
1456  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
1457  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1458 
1459  tf1 = Transform3<S>::Identity();
1460  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
1461  contacts.resize(1);
1462  contacts[0].pos << 0.05, 0, 0;
1463  contacts[0].penetration_depth = 20.1;
1464  contacts[0].normal << -1, 0, 0;
1465  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1466 
1467  tf1 = transform;
1468  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
1469  contacts.resize(1);
1470  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
1471  contacts[0].penetration_depth = 20.1;
1472  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1473  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1474 }
1475 
1476 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere)
1477 {
1478 // test_shapeIntersection_halfspacesphere<float>();
1479  test_shapeIntersection_halfspacesphere<double>();
1480 }
1481 
1482 template <typename S>
1483 void test_shapeIntersection_planesphere()
1484 {
1485  Sphere<S> s(10);
1486  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
1487 
1488  Transform3<S> tf1;
1489  Transform3<S> tf2;
1490 
1491  Transform3<S> transform = Transform3<S>::Identity();
1492  test::generateRandomTransform(extents<S>(), transform);
1493 
1494  std::vector<ContactPoint<S>> contacts;
1495 
1496  tf1 = Transform3<S>::Identity();
1497  tf2 = Transform3<S>::Identity();
1498  contacts.resize(1);
1499  contacts[0].pos.setZero();
1500  contacts[0].penetration_depth = 10;
1501  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
1502  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
1503 
1504  tf1 = transform;
1505  tf2 = transform;
1506  contacts.resize(1);
1507  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
1508  contacts[0].penetration_depth = 10;
1509  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
1510  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
1511 
1512  tf1 = Transform3<S>::Identity();
1513  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
1514  contacts.resize(1);
1515  contacts[0].pos << 5, 0, 0;
1516  contacts[0].penetration_depth = 5;
1517  contacts[0].normal << 1, 0, 0;
1518  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1519 
1520  tf1 = transform;
1521  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
1522  contacts.resize(1);
1523  contacts[0].pos = transform * Vector3<S>(5, 0, 0);
1524  contacts[0].penetration_depth = 5;
1525  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1526  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1527 
1528  tf1 = Transform3<S>::Identity();
1529  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
1530  contacts.resize(1);
1531  contacts[0].pos << -5, 0, 0;
1532  contacts[0].penetration_depth = 5;
1533  contacts[0].normal << -1, 0, 0;
1534  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1535 
1536  tf1 = transform;
1537  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
1538  contacts.resize(1);
1539  contacts[0].pos = transform * Vector3<S>(-5, 0, 0);
1540  contacts[0].penetration_depth = 5;
1541  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1542  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1543 
1544  tf1 = Transform3<S>::Identity();
1545  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
1546  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1547 
1548  tf1 = transform;
1549  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
1550  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1551 
1552  tf1 = Transform3<S>::Identity();
1553  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
1554  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1555 
1556  tf1 = transform;
1557  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
1558  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1559 }
1560 
1561 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planesphere)
1562 {
1563 // test_shapeIntersection_planesphere<float>();
1564  test_shapeIntersection_planesphere<double>();
1565 }
1566 
1567 template <typename S>
1568 void test_shapeIntersection_halfspacebox()
1569 {
1570  Box<S> s(5, 10, 20);
1571  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
1572 
1573  Transform3<S> tf1;
1574  Transform3<S> tf2;
1575 
1576  Transform3<S> transform = Transform3<S>::Identity();
1577  test::generateRandomTransform(extents<S>(), transform);
1578 
1579  std::vector<ContactPoint<S>> contacts;
1580 
1581  tf1 = Transform3<S>::Identity();
1582  tf2 = Transform3<S>::Identity();
1583  contacts.resize(1);
1584  contacts[0].pos << -1.25, 0, 0;
1585  contacts[0].penetration_depth = 2.5;
1586  contacts[0].normal << -1, 0, 0;
1587  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1588 
1589  tf1 = transform;
1590  tf2 = transform;
1591  contacts.resize(1);
1592  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
1593  contacts[0].penetration_depth = 2.5;
1594  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1595  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1596 
1597  tf1 = Transform3<S>::Identity();
1598  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1599  contacts.resize(1);
1600  contacts[0].pos << -0.625, 0, 0;
1601  contacts[0].penetration_depth = 3.75;
1602  contacts[0].normal << -1, 0, 0;
1603  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1604 
1605  tf1 = transform;
1606  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1607  contacts.resize(1);
1608  contacts[0].pos = transform * Vector3<S>(-0.625, 0, 0);
1609  contacts[0].penetration_depth = 3.75;
1610  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1611  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1612 
1613  tf1 = Transform3<S>::Identity();
1614  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1615  contacts.resize(1);
1616  contacts[0].pos << -1.875, 0, 0;
1617  contacts[0].penetration_depth = 1.25;
1618  contacts[0].normal << -1, 0, 0;
1619  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1620 
1621  tf1 = transform;
1622  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1623  contacts.resize(1);
1624  contacts[0].pos = transform * Vector3<S>(-1.875, 0, 0);
1625  contacts[0].penetration_depth = 1.25;
1626  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1627  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1628 
1629  tf1 = Transform3<S>::Identity();
1630  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
1631  contacts.resize(1);
1632  contacts[0].pos << 0.005, 0, 0;
1633  contacts[0].penetration_depth = 5.01;
1634  contacts[0].normal << -1, 0, 0;
1635  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1636 
1637  tf1 = transform;
1638  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
1639  contacts.resize(1);
1640  contacts[0].pos = transform * Vector3<S>(0.005, 0, 0);
1641  contacts[0].penetration_depth = 5.01;
1642  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1643  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1644 
1645  tf1 = Transform3<S>::Identity();
1646  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
1647  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1648 
1649  tf1 = transform;
1650  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
1651  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1652 
1653  tf1 = Transform3<S>(transform.linear());
1654  tf2 = Transform3<S>::Identity();
1655  contacts.resize(1);
1656  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
1657 }
1658 
1659 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox)
1660 {
1661 // test_shapeIntersection_halfspacebox<float>();
1662  test_shapeIntersection_halfspacebox<double>();
1663 }
1664 
1665 template <typename S>
1666 void test_shapeIntersection_planebox()
1667 {
1668  Box<S> s(5, 10, 20);
1669  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
1670 
1671  Transform3<S> tf1;
1672  Transform3<S> tf2;
1673 
1674  Transform3<S> transform = Transform3<S>::Identity();
1675  test::generateRandomTransform(extents<S>(), transform);
1676 
1677  std::vector<ContactPoint<S>> contacts;
1678 
1679  tf1 = Transform3<S>::Identity();
1680  tf2 = Transform3<S>::Identity();
1681  contacts.resize(1);
1682  contacts[0].pos << 0, 0, 0;
1683  contacts[0].penetration_depth = 2.5;
1684  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
1685  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
1686 
1687  tf1 = transform;
1688  tf2 = transform;
1689  contacts.resize(1);
1690  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
1691  contacts[0].penetration_depth = 2.5;
1692  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
1693  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
1694 
1695  tf1 = Transform3<S>::Identity();
1696  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1697  contacts.resize(1);
1698  contacts[0].pos << 1.25, 0, 0;
1699  contacts[0].penetration_depth = 1.25;
1700  contacts[0].normal << 1, 0, 0;
1701  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1702 
1703  tf1 = transform;
1704  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1705  contacts.resize(1);
1706  contacts[0].pos = transform * Vector3<S>(1.25, 0, 0);
1707  contacts[0].penetration_depth = 1.25;
1708  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1709  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1710 
1711  tf1 = Transform3<S>::Identity();
1712  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1713  contacts.resize(1);
1714  contacts[0].pos << -1.25, 0, 0;
1715  contacts[0].penetration_depth = 1.25;
1716  contacts[0].normal << -1, 0, 0;
1717  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1718 
1719  tf1 = transform;
1720  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1721  contacts.resize(1);
1722  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
1723  contacts[0].penetration_depth = 1.25;
1724  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1725  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1726 
1727  tf1 = Transform3<S>::Identity();
1728  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
1729  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1730 
1731  tf1 = transform;
1732  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
1733  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1734 
1735  tf1 = Transform3<S>::Identity();
1736  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
1737  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1738 
1739  tf1 = transform;
1740  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
1741  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1742 
1743  tf1 = Transform3<S>(transform.linear());
1744  tf2 = Transform3<S>::Identity();
1745  contacts.resize(1);
1746  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
1747 }
1748 
1749 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planebox)
1750 {
1751 // test_shapeIntersection_planebox<float>();
1752  test_shapeIntersection_planebox<double>();
1753 }
1754 
1755 template <typename S>
1756 void test_shapeIntersection_halfspaceellipsoid()
1757 {
1758  Ellipsoid<S> s(5, 10, 20);
1759  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
1760 
1761  Transform3<S> tf1;
1762  Transform3<S> tf2;
1763 
1764  Transform3<S> transform = Transform3<S>::Identity();
1765  test::generateRandomTransform(extents<S>(), transform);
1766 
1767  std::vector<ContactPoint<S>> contacts;
1768 
1769  tf1 = Transform3<S>::Identity();
1770  tf2 = Transform3<S>::Identity();
1771  contacts.resize(1);
1772  contacts[0].pos << -2.5, 0, 0;
1773  contacts[0].penetration_depth = 5.0;
1774  contacts[0].normal << -1, 0, 0;
1775  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1776 
1777  tf1 = transform;
1778  tf2 = transform;
1779  contacts.resize(1);
1780  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
1781  contacts[0].penetration_depth = 5.0;
1782  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1783  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1784 
1785  tf1 = Transform3<S>::Identity();
1786  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1787  contacts.resize(1);
1788  contacts[0].pos << -1.875, 0, 0;
1789  contacts[0].penetration_depth = 6.25;
1790  contacts[0].normal << -1, 0, 0;
1791  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1792 
1793  tf1 = transform;
1794  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1795  contacts.resize(1);
1796  contacts[0].pos = transform * Vector3<S>(-1.875, 0, 0);
1797  contacts[0].penetration_depth = 6.25;
1798  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1799  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1800 
1801  tf1 = Transform3<S>::Identity();
1802  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1803  contacts.resize(1);
1804  contacts[0].pos << -3.125, 0, 0;
1805  contacts[0].penetration_depth = 3.75;
1806  contacts[0].normal << -1, 0, 0;
1807  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1808 
1809  tf1 = transform;
1810  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1811  contacts.resize(1);
1812  contacts[0].pos = transform * Vector3<S>(-3.125, 0, 0);
1813  contacts[0].penetration_depth = 3.75;
1814  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1815  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1816 
1817  tf1 = Transform3<S>::Identity();
1818  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
1819  contacts.resize(1);
1820  contacts[0].pos << 0.005, 0, 0;
1821  contacts[0].penetration_depth = 10.01;
1822  contacts[0].normal << -1, 0, 0;
1823  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1824 
1825  tf1 = transform;
1826  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
1827  contacts.resize(1);
1828  contacts[0].pos = transform * Vector3<S>(0.005, 0, 0);
1829  contacts[0].penetration_depth = 10.01;
1830  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1831  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1832 
1833  tf1 = Transform3<S>::Identity();
1834  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
1835  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1836 
1837  tf1 = transform;
1838  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
1839  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1840 
1841 
1842 
1843 
1844  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);
1845 
1846  tf1 = Transform3<S>::Identity();
1847  tf2 = Transform3<S>::Identity();
1848  contacts.resize(1);
1849  contacts[0].pos << 0, -5.0, 0;
1850  contacts[0].penetration_depth = 10.0;
1851  contacts[0].normal << 0, -1, 0;
1852  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1853 
1854  tf1 = transform;
1855  tf2 = transform;
1856  contacts.resize(1);
1857  contacts[0].pos = transform * Vector3<S>(0, -5.0, 0);
1858  contacts[0].penetration_depth = 10.0;
1859  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
1860  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1861 
1862  tf1 = Transform3<S>::Identity();
1863  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
1864  contacts.resize(1);
1865  contacts[0].pos << 0, -4.375, 0;
1866  contacts[0].penetration_depth = 11.25;
1867  contacts[0].normal << 0, -1, 0;
1868  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1869 
1870  tf1 = transform;
1871  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
1872  contacts.resize(1);
1873  contacts[0].pos = transform * Vector3<S>(0, -4.375, 0);
1874  contacts[0].penetration_depth = 11.25;
1875  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
1876  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1877 
1878  tf1 = Transform3<S>::Identity();
1879  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
1880  contacts.resize(1);
1881  contacts[0].pos << 0, -5.625, 0;
1882  contacts[0].penetration_depth = 8.75;
1883  contacts[0].normal << 0, -1, 0;
1884  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1885 
1886  tf1 = transform;
1887  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
1888  contacts.resize(1);
1889  contacts[0].pos = transform * Vector3<S>(0, -5.625, 0);
1890  contacts[0].penetration_depth = 8.75;
1891  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
1892  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1893 
1894  tf1 = Transform3<S>::Identity();
1895  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
1896  contacts.resize(1);
1897  contacts[0].pos << 0, 0.005, 0;
1898  contacts[0].penetration_depth = 20.01;
1899  contacts[0].normal << 0, -1, 0;
1900  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1901 
1902  tf1 = transform;
1903  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
1904  contacts.resize(1);
1905  contacts[0].pos = transform * Vector3<S>(0, 0.005, 0);
1906  contacts[0].penetration_depth = 20.01;
1907  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
1908  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1909 
1910  tf1 = Transform3<S>::Identity();
1911  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
1912  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1913 
1914  tf1 = transform;
1915  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
1916  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1917 
1918 
1919 
1920 
1921  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);
1922 
1923  tf1 = Transform3<S>::Identity();
1924  tf2 = Transform3<S>::Identity();
1925  contacts.resize(1);
1926  contacts[0].pos << 0, 0, -10.0;
1927  contacts[0].penetration_depth = 20.0;
1928  contacts[0].normal << 0, 0, -1;
1929  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1930 
1931  tf1 = transform;
1932  tf2 = transform;
1933  contacts.resize(1);
1934  contacts[0].pos = transform * Vector3<S>(0, 0, -10.0);
1935  contacts[0].penetration_depth = 20.0;
1936  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
1937  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1938 
1939  tf1 = Transform3<S>::Identity();
1940  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
1941  contacts.resize(1);
1942  contacts[0].pos << 0, 0, -9.375;
1943  contacts[0].penetration_depth = 21.25;
1944  contacts[0].normal << 0, 0, -1;
1945  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1946 
1947  tf1 = transform;
1948  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
1949  contacts.resize(1);
1950  contacts[0].pos = transform * Vector3<S>(0, 0, -9.375);
1951  contacts[0].penetration_depth = 21.25;
1952  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
1953  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1954 
1955  tf1 = Transform3<S>::Identity();
1956  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
1957  contacts.resize(1);
1958  contacts[0].pos << 0, 0, -10.625;
1959  contacts[0].penetration_depth = 18.75;
1960  contacts[0].normal << 0, 0, -1;
1961  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1962 
1963  tf1 = transform;
1964  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
1965  contacts.resize(1);
1966  contacts[0].pos = transform * Vector3<S>(0, 0, -10.625);
1967  contacts[0].penetration_depth = 18.75;
1968  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
1969  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1970 
1971  tf1 = Transform3<S>::Identity();
1972  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
1973  contacts.resize(1);
1974  contacts[0].pos << 0, 0, 0.005;
1975  contacts[0].penetration_depth = 40.01;
1976  contacts[0].normal << 0, 0, -1;
1977  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1978 
1979  tf1 = transform;
1980  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
1981  contacts.resize(1);
1982  contacts[0].pos = transform * Vector3<S>(0, 0, 0.005);
1983  contacts[0].penetration_depth = 40.01;
1984  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
1985  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1986 
1987  tf1 = Transform3<S>::Identity();
1988  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
1989  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1990 
1991  tf1 = transform;
1992  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
1993  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1994 }
1995 
1996 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid)
1997 {
1998 // test_shapeIntersection_halfspaceellipsoid<float>();
1999  test_shapeIntersection_halfspaceellipsoid<double>();
2000 }
2001 
2002 template <typename S>
2003 void test_shapeIntersection_planeellipsoid()
2004 {
2005  Ellipsoid<S> s(5, 10, 20);
2006  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
2007 
2008  Transform3<S> tf1;
2009  Transform3<S> tf2;
2010 
2011  Transform3<S> transform = Transform3<S>::Identity();
2012  test::generateRandomTransform(extents<S>(), transform);
2013 
2014  std::vector<ContactPoint<S>> contacts;
2015 
2016  tf1 = Transform3<S>::Identity();
2017  tf2 = Transform3<S>::Identity();
2018  contacts.resize(1);
2019  contacts[0].pos << 0, 0, 0;
2020  contacts[0].penetration_depth = 5.0;
2021  contacts[0].normal << -1, 0, 0;
2022  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2023 
2024  tf1 = transform;
2025  tf2 = transform;
2026  contacts.resize(1);
2027  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2028  contacts[0].penetration_depth = 5.0;
2029  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2030  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2031 
2032  tf1 = Transform3<S>::Identity();
2033  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
2034  contacts.resize(1);
2035  contacts[0].pos << 1.25, 0, 0;
2036  contacts[0].penetration_depth = 3.75;
2037  contacts[0].normal << 1, 0, 0;
2038  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2039 
2040  tf1 = transform;
2041  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
2042  contacts.resize(1);
2043  contacts[0].pos = transform * Vector3<S>(1.25, 0, 0);
2044  contacts[0].penetration_depth = 3.75;
2045  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
2046  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2047 
2048  tf1 = Transform3<S>::Identity();
2049  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
2050  contacts.resize(1);
2051  contacts[0].pos << -1.25, 0, 0;
2052  contacts[0].penetration_depth = 3.75;
2053  contacts[0].normal << -1, 0, 0;
2054  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2055 
2056  tf1 = transform;
2057  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
2058  contacts.resize(1);
2059  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
2060  contacts[0].penetration_depth = 3.75;
2061  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2062  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2063 
2064  tf1 = Transform3<S>::Identity();
2065  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
2066  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2067 
2068  tf1 = transform;
2069  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
2070  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2071 
2072  tf1 = Transform3<S>::Identity();
2073  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
2074  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2075 
2076  tf1 = transform;
2077  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
2078  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2079 
2080 
2081 
2082 
2083  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);
2084 
2085  tf1 = Transform3<S>::Identity();
2086  tf2 = Transform3<S>::Identity();
2087  contacts.resize(1);
2088  contacts[0].pos << 0, 0.0, 0;
2089  contacts[0].penetration_depth = 10.0;
2090  contacts[0].normal << 0, -1, 0;
2091  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2092 
2093  tf1 = transform;
2094  tf2 = transform;
2095  contacts.resize(1);
2096  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2097  contacts[0].penetration_depth = 10.0;
2098  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2099  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2100 
2101  tf1 = Transform3<S>::Identity();
2102  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
2103  contacts.resize(1);
2104  contacts[0].pos << 0, 1.25, 0;
2105  contacts[0].penetration_depth = 8.75;
2106  contacts[0].normal << 0, 1, 0;
2107  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2108 
2109  tf1 = transform;
2110  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
2111  contacts.resize(1);
2112  contacts[0].pos = transform * Vector3<S>(0, 1.25, 0);
2113  contacts[0].penetration_depth = 8.75;
2114  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
2115  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2116 
2117  tf1 = Transform3<S>::Identity();
2118  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
2119  contacts.resize(1);
2120  contacts[0].pos << 0, -1.25, 0;
2121  contacts[0].penetration_depth = 8.75;
2122  contacts[0].normal << 0, -1, 0;
2123  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2124 
2125  tf1 = transform;
2126  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
2127  contacts.resize(1);
2128  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
2129  contacts[0].penetration_depth = 8.75;
2130  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2131  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2132 
2133  tf1 = Transform3<S>::Identity();
2134  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
2135  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2136 
2137  tf1 = transform;
2138  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
2139  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2140 
2141  tf1 = Transform3<S>::Identity();
2142  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
2143  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2144 
2145  tf1 = transform;
2146  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
2147  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2148 
2149 
2150 
2151 
2152  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);
2153 
2154  tf1 = Transform3<S>::Identity();
2155  tf2 = Transform3<S>::Identity();
2156  contacts.resize(1);
2157  contacts[0].pos << 0, 0, 0;
2158  contacts[0].penetration_depth = 20.0;
2159  contacts[0].normal << 0, 0, -1;
2160  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2161 
2162  tf1 = transform;
2163  tf2 = transform;
2164  contacts.resize(1);
2165  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2166  contacts[0].penetration_depth = 20.0;
2167  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2168  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2169 
2170  tf1 = Transform3<S>::Identity();
2171  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
2172  contacts.resize(1);
2173  contacts[0].pos << 0, 0, 1.25;
2174  contacts[0].penetration_depth = 18.75;
2175  contacts[0].normal << 0, 0, 1;
2176  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2177 
2178  tf1 = transform;
2179  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
2180  contacts.resize(1);
2181  contacts[0].pos = transform * Vector3<S>(0, 0, 1.25);
2182  contacts[0].penetration_depth = 18.75;
2183  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
2184  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2185 
2186  tf1 = Transform3<S>::Identity();
2187  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
2188  contacts.resize(1);
2189  contacts[0].pos << 0, 0, -1.25;
2190  contacts[0].penetration_depth = 18.75;
2191  contacts[0].normal << 0, 0, -1;
2192  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2193 
2194  tf1 = transform;
2195  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
2196  contacts.resize(1);
2197  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
2198  contacts[0].penetration_depth = 18.75;
2199  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2200  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2201 
2202  tf1 = Transform3<S>::Identity();
2203  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
2204  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2205 
2206  tf1 = transform;
2207  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
2208  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2209 
2210  tf1 = Transform3<S>::Identity();
2211  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
2212  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2213 
2214  tf1 = transform;
2215  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
2216  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2217 }
2218 
2219 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid)
2220 {
2221 // test_shapeIntersection_planeellipsoid<float>();
2222  test_shapeIntersection_planeellipsoid<double>();
2223 }
2224 
2225 template <typename S>
2226 void test_shapeIntersection_halfspacecapsule()
2227 {
2228  Capsule<S> s(5, 10);
2229  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
2230 
2231  Transform3<S> tf1;
2232  Transform3<S> tf2;
2233 
2234  Transform3<S> transform = Transform3<S>::Identity();
2235  test::generateRandomTransform(extents<S>(), transform);
2236 
2237  std::vector<ContactPoint<S>> contacts;
2238 
2239  tf1 = Transform3<S>::Identity();
2240  tf2 = Transform3<S>::Identity();
2241  contacts.resize(1);
2242  contacts[0].pos << -2.5, 0, 0;
2243  contacts[0].penetration_depth = 5;
2244  contacts[0].normal << -1, 0, 0;
2245  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2246 
2247  tf1 = transform;
2248  tf2 = transform;
2249  contacts.resize(1);
2250  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
2251  contacts[0].penetration_depth = 5;
2252  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2253  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2254 
2255  tf1 = Transform3<S>::Identity();
2256  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2257  contacts.resize(1);
2258  contacts[0].pos << -1.25, 0, 0;
2259  contacts[0].penetration_depth = 7.5;
2260  contacts[0].normal << -1, 0, 0;
2261  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2262 
2263  tf1 = transform;
2264  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2265  contacts.resize(1);
2266  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
2267  contacts[0].penetration_depth = 7.5;
2268  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2269  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2270 
2271  tf1 = Transform3<S>::Identity();
2272  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2273  contacts.resize(1);
2274  contacts[0].pos << -3.75, 0, 0;
2275  contacts[0].penetration_depth = 2.5;
2276  contacts[0].normal << -1, 0, 0;
2277  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2278 
2279  tf1 = transform;
2280  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2281  contacts.resize(1);
2282  contacts[0].pos = transform * Vector3<S>(-3.75, 0, 0);
2283  contacts[0].penetration_depth = 2.5;
2284  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2285  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2286 
2287  tf1 = Transform3<S>::Identity();
2288  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2289  contacts.resize(1);
2290  contacts[0].pos << 0.05, 0, 0;
2291  contacts[0].penetration_depth = 10.1;
2292  contacts[0].normal << -1, 0, 0;
2293  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2294 
2295  tf1 = transform;
2296  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2297  contacts.resize(1);
2298  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
2299  contacts[0].penetration_depth = 10.1;
2300  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2301  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2302 
2303  tf1 = Transform3<S>::Identity();
2304  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2305  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2306 
2307  tf1 = transform;
2308  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2309  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2310 
2311 
2312 
2313 
2314  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);
2315 
2316  tf1 = Transform3<S>::Identity();
2317  tf2 = Transform3<S>::Identity();
2318  contacts.resize(1);
2319  contacts[0].pos << 0, -2.5, 0;
2320  contacts[0].penetration_depth = 5;
2321  contacts[0].normal << 0, -1, 0;
2322  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2323 
2324  tf1 = transform;
2325  tf2 = transform;
2326  contacts.resize(1);
2327  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
2328  contacts[0].penetration_depth = 5;
2329  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2330  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2331 
2332  tf1 = Transform3<S>::Identity();
2333  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2334  contacts.resize(1);
2335  contacts[0].pos << 0, -1.25, 0;
2336  contacts[0].penetration_depth = 7.5;
2337  contacts[0].normal << 0, -1, 0;
2338  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2339 
2340  tf1 = transform;
2341  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2342  contacts.resize(1);
2343  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
2344  contacts[0].penetration_depth = 7.5;
2345  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2346  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2347 
2348  tf1 = Transform3<S>::Identity();
2349  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2350  contacts.resize(1);
2351  contacts[0].pos << 0, -3.75, 0;
2352  contacts[0].penetration_depth = 2.5;
2353  contacts[0].normal << 0, -1, 0;
2354  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2355 
2356  tf1 = transform;
2357  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2358  contacts.resize(1);
2359  contacts[0].pos = transform * Vector3<S>(0, -3.75, 0);
2360  contacts[0].penetration_depth = 2.5;
2361  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2362  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2363 
2364  tf1 = Transform3<S>::Identity();
2365  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2366  contacts.resize(1);
2367  contacts[0].pos << 0, 0.05, 0;
2368  contacts[0].penetration_depth = 10.1;
2369  contacts[0].normal << 0, -1, 0;
2370  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2371 
2372  tf1 = transform;
2373  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2374  contacts.resize(1);
2375  contacts[0].pos = transform * Vector3<S>(0, 0.05, 0);
2376  contacts[0].penetration_depth = 10.1;
2377  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2378  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2379 
2380  tf1 = Transform3<S>::Identity();
2381  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2382  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2383 
2384  tf1 = transform;
2385  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2386  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2387 
2388 
2389 
2390 
2391  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);
2392 
2393  tf1 = Transform3<S>::Identity();
2394  tf2 = Transform3<S>::Identity();
2395  contacts.resize(1);
2396  contacts[0].pos << 0, 0, -5;
2397  contacts[0].penetration_depth = 10;
2398  contacts[0].normal << 0, 0, -1;
2399  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2400 
2401  tf1 = transform;
2402  tf2 = transform;
2403  contacts.resize(1);
2404  contacts[0].pos = transform * Vector3<S>(0, 0, -5);
2405  contacts[0].penetration_depth = 10;
2406  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2407  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2408 
2409  tf1 = Transform3<S>::Identity();
2410  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2411  contacts.resize(1);
2412  contacts[0].pos << 0, 0, -3.75;
2413  contacts[0].penetration_depth = 12.5;
2414  contacts[0].normal << 0, 0, -1;
2415  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2416 
2417  tf1 = transform;
2418  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2419  contacts.resize(1);
2420  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
2421  contacts[0].penetration_depth = 12.5;
2422  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2423  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2424 
2425  tf1 = Transform3<S>::Identity();
2426  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2427  contacts.resize(1);
2428  contacts[0].pos << 0, 0, -6.25;
2429  contacts[0].penetration_depth = 7.5;
2430  contacts[0].normal << 0, 0, -1;
2431  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2432 
2433  tf1 = transform;
2434  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2435  contacts.resize(1);
2436  contacts[0].pos = transform * Vector3<S>(0, 0, -6.25);
2437  contacts[0].penetration_depth = 7.5;
2438  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2439  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2440 
2441  tf1 = Transform3<S>::Identity();
2442  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
2443  contacts.resize(1);
2444  contacts[0].pos << 0, 0, 0.05;
2445  contacts[0].penetration_depth = 20.1;
2446  contacts[0].normal << 0, 0, -1;
2447  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2448 
2449  tf1 = transform;
2450  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
2451  contacts.resize(1);
2452  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
2453  contacts[0].penetration_depth = 20.1;
2454  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2455  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2456 
2457  tf1 = Transform3<S>::Identity();
2458  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
2459  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2460 
2461  tf1 = transform;
2462  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
2463  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2464 }
2465 
2466 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule)
2467 {
2468 // test_shapeIntersection_halfspacecapsule<float>();
2469  test_shapeIntersection_halfspacecapsule<double>();
2470 }
2471 
2472 template <typename S>
2473 void test_shapeIntersection_planecapsule()
2474 {
2475  Capsule<S> s(5, 10);
2476  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
2477 
2478  Transform3<S> tf1;
2479  Transform3<S> tf2;
2480 
2481  Transform3<S> transform = Transform3<S>::Identity();
2482  test::generateRandomTransform(extents<S>(), transform);
2483 
2484  std::vector<ContactPoint<S>> contacts;
2485 
2486  tf1 = Transform3<S>::Identity();
2487  tf2 = Transform3<S>::Identity();
2488  contacts.resize(1);
2489  contacts[0].pos << 0, 0, 0;
2490  contacts[0].penetration_depth = 5;
2491  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
2492  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2493 
2494  tf1 = transform;
2495  tf2 = transform;
2496  contacts.resize(1);
2497  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2498  contacts[0].penetration_depth = 5;
2499  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
2500  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2501 
2502  tf1 = Transform3<S>::Identity();
2503  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2504  contacts.resize(1);
2505  contacts[0].pos << 2.5, 0, 0;
2506  contacts[0].penetration_depth = 2.5;
2507  contacts[0].normal << 1, 0, 0;
2508  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2509 
2510  tf1 = transform;
2511  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2512  contacts.resize(1);
2513  contacts[0].pos = transform * Vector3<S>(2.5, 0, 0);
2514  contacts[0].penetration_depth = 2.5;
2515  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
2516  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2517 
2518  tf1 = Transform3<S>::Identity();
2519  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2520  contacts.resize(1);
2521  contacts[0].pos << -2.5, 0, 0;
2522  contacts[0].penetration_depth = 2.5;
2523  contacts[0].normal << -1, 0, 0;
2524  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2525 
2526  tf1 = transform;
2527  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2528  contacts.resize(1);
2529  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
2530  contacts[0].penetration_depth = 2.5;
2531  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2532  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2533 
2534  tf1 = Transform3<S>::Identity();
2535  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2536  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2537 
2538  tf1 = transform;
2539  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2540  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2541 
2542  tf1 = Transform3<S>::Identity();
2543  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2544  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2545 
2546  tf1 = transform;
2547  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2548  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2549 
2550 
2551 
2552 
2553  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);
2554 
2555  tf1 = Transform3<S>::Identity();
2556  tf2 = Transform3<S>::Identity();
2557  contacts.resize(1);
2558  contacts[0].pos << 0, 0, 0;
2559  contacts[0].penetration_depth = 5;
2560  contacts[0].normal << 0, 1, 0; // (0, 1, 0) or (0, -1, 0)
2561  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2562 
2563  tf1 = transform;
2564  tf2 = transform;
2565  contacts.resize(1);
2566  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2567  contacts[0].penetration_depth = 5;
2568  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0); // (0, 1, 0) or (0, -1, 0)
2569  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2570 
2571  tf1 = Transform3<S>::Identity();
2572  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2573  contacts.resize(1);
2574  contacts[0].pos << 0, 2.5, 0;
2575  contacts[0].penetration_depth = 2.5;
2576  contacts[0].normal << 0, 1, 0;
2577  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2578 
2579  tf1 = transform;
2580  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2581  contacts.resize(1);
2582  contacts[0].pos = transform * Vector3<S>(0, 2.5, 0);
2583  contacts[0].penetration_depth = 2.5;
2584  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
2585  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2586 
2587  tf1 = Transform3<S>::Identity();
2588  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2589  contacts.resize(1);
2590  contacts[0].pos << 0, -2.5, 0;
2591  contacts[0].penetration_depth = 2.5;
2592  contacts[0].normal << 0, -1, 0;
2593  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2594 
2595  tf1 = transform;
2596  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2597  contacts.resize(1);
2598  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
2599  contacts[0].penetration_depth = 2.5;
2600  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2601  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2602 
2603  tf1 = Transform3<S>::Identity();
2604  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2605  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2606 
2607  tf1 = transform;
2608  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2609  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2610 
2611  tf1 = Transform3<S>::Identity();
2612  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2613  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2614 
2615  tf1 = transform;
2616  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2617  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2618 
2619 
2620 
2621 
2622  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);
2623 
2624  tf1 = Transform3<S>::Identity();
2625  tf2 = Transform3<S>::Identity();
2626  contacts.resize(1);
2627  contacts[0].pos << 0, 0, 0;
2628  contacts[0].penetration_depth = 10;
2629  contacts[0].normal << 0, 0, 1; // (0, 0, 1) or (0, 0, -1)
2630  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2631 
2632  tf1 = transform;
2633  tf2 = transform;
2634  contacts.resize(1);
2635  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2636  contacts[0].penetration_depth = 10;
2637  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1); // (0, 0, 1) or (0, 0, -1)
2638  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2639 
2640  tf1 = Transform3<S>::Identity();
2641  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2642  contacts.resize(1);
2643  contacts[0].pos << 0, 0, 2.5;
2644  contacts[0].penetration_depth = 7.5;
2645  contacts[0].normal << 0, 0, 1;
2646  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2647 
2648  tf1 = transform;
2649  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2650  contacts.resize(1);
2651  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
2652  contacts[0].penetration_depth = 7.5;
2653  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
2654  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2655 
2656  tf1 = Transform3<S>::Identity();
2657  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2658  contacts.resize(1);
2659  contacts[0].pos << 0, 0, -2.5;
2660  contacts[0].penetration_depth = 7.5;
2661  contacts[0].normal << 0, 0, -1;
2662  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2663 
2664  tf1 = transform;
2665  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2666  contacts.resize(1);
2667  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
2668  contacts[0].penetration_depth = 7.5;
2669  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2670  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2671 
2672  tf1 = Transform3<S>::Identity();
2673  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
2674  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2675 
2676  tf1 = transform;
2677  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
2678  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2679 
2680  tf1 = Transform3<S>::Identity();
2681  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
2682  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2683 
2684  tf1 = transform;
2685  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
2686  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2687 }
2688 
2689 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule)
2690 {
2691 // test_shapeIntersection_planecapsule<float>();
2692  test_shapeIntersection_planecapsule<double>();
2693 }
2694 
2695 template <typename S>
2696 void test_shapeIntersection_halfspacecylinder()
2697 {
2698  Cylinder<S> s(5, 10);
2699  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
2700 
2701  Transform3<S> tf1;
2702  Transform3<S> tf2;
2703 
2704  Transform3<S> transform = Transform3<S>::Identity();
2705  test::generateRandomTransform(extents<S>(), transform);
2706 
2707  std::vector<ContactPoint<S>> contacts;
2708 
2709  tf1 = Transform3<S>::Identity();
2710  tf2 = Transform3<S>::Identity();
2711  contacts.resize(1);
2712  contacts[0].pos << -2.5, 0, 0;
2713  contacts[0].penetration_depth = 5;
2714  contacts[0].normal << -1, 0, 0;
2715  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2716 
2717  tf1 = transform;
2718  tf2 = transform;
2719  contacts.resize(1);
2720  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
2721  contacts[0].penetration_depth = 5;
2722  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2723  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2724 
2725  tf1 = Transform3<S>::Identity();
2726  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2727  contacts.resize(1);
2728  contacts[0].pos << -1.25, 0, 0;
2729  contacts[0].penetration_depth = 7.5;
2730  contacts[0].normal << -1, 0, 0;
2731  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2732 
2733  tf1 = transform;
2734  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2735  contacts.resize(1);
2736  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
2737  contacts[0].penetration_depth = 7.5;
2738  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2739  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2740 
2741  tf1 = Transform3<S>::Identity();
2742  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2743  contacts.resize(1);
2744  contacts[0].pos << -3.75, 0, 0;
2745  contacts[0].penetration_depth = 2.5;
2746  contacts[0].normal << -1, 0, 0;
2747  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2748 
2749  tf1 = transform;
2750  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2751  contacts.resize(1);
2752  contacts[0].pos = transform * Vector3<S>(-3.75, 0, 0);
2753  contacts[0].penetration_depth = 2.5;
2754  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2755  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2756 
2757  tf1 = Transform3<S>::Identity();
2758  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2759  contacts.resize(1);
2760  contacts[0].pos << 0.05, 0, 0;
2761  contacts[0].penetration_depth = 10.1;
2762  contacts[0].normal << -1, 0, 0;
2763  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2764 
2765  tf1 = transform;
2766  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2767  contacts.resize(1);
2768  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
2769  contacts[0].penetration_depth = 10.1;
2770  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2771  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2772 
2773  tf1 = Transform3<S>::Identity();
2774  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2775  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2776 
2777  tf1 = transform;
2778  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2779  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2780 
2781 
2782 
2783 
2784  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);
2785 
2786  tf1 = Transform3<S>::Identity();
2787  tf2 = Transform3<S>::Identity();
2788  contacts.resize(1);
2789  contacts[0].pos << 0, -2.5, 0;
2790  contacts[0].penetration_depth = 5;
2791  contacts[0].normal << 0, -1, 0;
2792  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2793 
2794  tf1 = transform;
2795  tf2 = transform;
2796  contacts.resize(1);
2797  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
2798  contacts[0].penetration_depth = 5;
2799  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2800  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2801 
2802  tf1 = Transform3<S>::Identity();
2803  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2804  contacts.resize(1);
2805  contacts[0].pos << 0, -1.25, 0;
2806  contacts[0].penetration_depth = 7.5;
2807  contacts[0].normal << 0, -1, 0;
2808  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2809 
2810  tf1 = transform;
2811  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2812  contacts.resize(1);
2813  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
2814  contacts[0].penetration_depth = 7.5;
2815  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2816  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2817 
2818  tf1 = Transform3<S>::Identity();
2819  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2820  contacts.resize(1);
2821  contacts[0].pos << 0, -3.75, 0;
2822  contacts[0].penetration_depth = 2.5;
2823  contacts[0].normal << 0, -1, 0;
2824  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2825 
2826  tf1 = transform;
2827  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2828  contacts.resize(1);
2829  contacts[0].pos = transform * Vector3<S>(0, -3.75, 0);
2830  contacts[0].penetration_depth = 2.5;
2831  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2832  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2833 
2834  tf1 = Transform3<S>::Identity();
2835  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2836  contacts.resize(1);
2837  contacts[0].pos << 0, 0.05, 0;
2838  contacts[0].penetration_depth = 10.1;
2839  contacts[0].normal << 0, -1, 0;
2840  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2841 
2842  tf1 = transform;
2843  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2844  contacts.resize(1);
2845  contacts[0].pos = transform * Vector3<S>(0, 0.05, 0);
2846  contacts[0].penetration_depth = 10.1;
2847  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2848  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2849 
2850  tf1 = Transform3<S>::Identity();
2851  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2852  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2853 
2854  tf1 = transform;
2855  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2856  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2857 
2858 
2859 
2860 
2861  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);
2862 
2863  tf1 = Transform3<S>::Identity();
2864  tf2 = Transform3<S>::Identity();
2865  contacts.resize(1);
2866  contacts[0].pos << 0, 0, -2.5;
2867  contacts[0].penetration_depth = 5;
2868  contacts[0].normal << 0, 0, -1;
2869  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2870 
2871  tf1 = transform;
2872  tf2 = transform;
2873  contacts.resize(1);
2874  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
2875  contacts[0].penetration_depth = 5;
2876  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2877  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2878 
2879  tf1 = Transform3<S>::Identity();
2880  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2881  contacts.resize(1);
2882  contacts[0].pos << 0, 0, -1.25;
2883  contacts[0].penetration_depth = 7.5;
2884  contacts[0].normal << 0, 0, -1;
2885  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2886 
2887  tf1 = transform;
2888  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2889  contacts.resize(1);
2890  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
2891  contacts[0].penetration_depth = 7.5;
2892  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2893  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2894 
2895  tf1 = Transform3<S>::Identity();
2896  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2897  contacts.resize(1);
2898  contacts[0].pos << 0, 0, -3.75;
2899  contacts[0].penetration_depth = 2.5;
2900  contacts[0].normal << 0, 0, -1;
2901  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2902 
2903  tf1 = transform;
2904  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2905  contacts.resize(1);
2906  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
2907  contacts[0].penetration_depth = 2.5;
2908  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2909  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2910 
2911  tf1 = Transform3<S>::Identity();
2912  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
2913  contacts.resize(1);
2914  contacts[0].pos << 0, 0, 0.05;
2915  contacts[0].penetration_depth = 10.1;
2916  contacts[0].normal << 0, 0, -1;
2917  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2918 
2919  tf1 = transform;
2920  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
2921  contacts.resize(1);
2922  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
2923  contacts[0].penetration_depth = 10.1;
2924  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2925  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2926 
2927  tf1 = Transform3<S>::Identity();
2928  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
2929  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2930 
2931  tf1 = transform;
2932  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
2933  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2934 }
2935 
2936 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder)
2937 {
2938 // test_shapeIntersection_halfspacecylinder<float>();
2939  test_shapeIntersection_halfspacecylinder<double>();
2940 }
2941 
2942 template <typename S>
2943 void test_shapeIntersection_planecylinder()
2944 {
2945  Cylinder<S> s(5, 10);
2946  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
2947 
2948  Transform3<S> tf1;
2949  Transform3<S> tf2;
2950 
2951  Transform3<S> transform = Transform3<S>::Identity();
2952  test::generateRandomTransform(extents<S>(), transform);
2953 
2954  std::vector<ContactPoint<S>> contacts;
2955 
2956  tf1 = Transform3<S>::Identity();
2957  tf2 = Transform3<S>::Identity();
2958  contacts.resize(1);
2959  contacts[0].pos << 0, 0, 0;
2960  contacts[0].penetration_depth = 5;
2961  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
2962  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2963 
2964  tf1 = transform;
2965  tf2 = transform;
2966  contacts.resize(1);
2967  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2968  contacts[0].penetration_depth = 5;
2969  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
2970  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2971 
2972  tf1 = Transform3<S>::Identity();
2973  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2974  contacts.resize(1);
2975  contacts[0].pos << 2.5, 0, 0;
2976  contacts[0].penetration_depth = 2.5;
2977  contacts[0].normal << 1, 0, 0;
2978  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2979 
2980  tf1 = transform;
2981  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2982  contacts.resize(1);
2983  contacts[0].pos = transform * Vector3<S>(2.5, 0, 0);
2984  contacts[0].penetration_depth = 2.5;
2985  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
2986  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2987 
2988  tf1 = Transform3<S>::Identity();
2989  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2990  contacts.resize(1);
2991  contacts[0].pos << -2.5, 0, 0;
2992  contacts[0].penetration_depth = 2.5;
2993  contacts[0].normal << -1, 0, 0;
2994  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2995 
2996  tf1 = transform;
2997  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2998  contacts.resize(1);
2999  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
3000  contacts[0].penetration_depth = 2.5;
3001  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3002  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3003 
3004  tf1 = Transform3<S>::Identity();
3005  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3006  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3007 
3008  tf1 = transform;
3009  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3010  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3011 
3012  tf1 = Transform3<S>::Identity();
3013  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3014  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3015 
3016  tf1 = transform;
3017  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3018  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3019 
3020 
3021 
3022 
3023  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);
3024 
3025  tf1 = Transform3<S>::Identity();
3026  tf2 = Transform3<S>::Identity();
3027  contacts.resize(1);
3028  contacts[0].pos << 0, 0, 0;
3029  contacts[0].penetration_depth = 5;
3030  contacts[0].normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0)
3031  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3032 
3033  tf1 = transform;
3034  tf2 = transform;
3035  contacts.resize(1);
3036  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3037  contacts[0].penetration_depth = 5;
3038  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
3039  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3040 
3041  tf1 = Transform3<S>::Identity();
3042  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3043  contacts.resize(1);
3044  contacts[0].pos << 0, 2.5, 0;
3045  contacts[0].penetration_depth = 2.5;
3046  contacts[0].normal << 0, 1, 0;
3047  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3048 
3049  tf1 = transform;
3050  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3051  contacts.resize(1);
3052  contacts[0].pos = transform * Vector3<S>(0, 2.5, 0);
3053  contacts[0].penetration_depth = 2.5;
3054  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
3055  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3056 
3057  tf1 = Transform3<S>::Identity();
3058  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3059  contacts.resize(1);
3060  contacts[0].pos << 0, -2.5, 0;
3061  contacts[0].penetration_depth = 2.5;
3062  contacts[0].normal << 0, -1, 0;
3063  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3064 
3065  tf1 = transform;
3066  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3067  contacts.resize(1);
3068  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
3069  contacts[0].penetration_depth = 2.5;
3070  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3071  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3072 
3073  tf1 = Transform3<S>::Identity();
3074  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3075  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3076 
3077  tf1 = transform;
3078  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3079  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3080 
3081  tf1 = Transform3<S>::Identity();
3082  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3083  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3084 
3085  tf1 = transform;
3086  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3087  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3088 
3089 
3090 
3091 
3092  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);
3093 
3094  tf1 = Transform3<S>::Identity();
3095  tf2 = Transform3<S>::Identity();
3096  contacts.resize(1);
3097  contacts[0].pos << 0, 0, 0;
3098  contacts[0].penetration_depth = 5;
3099  contacts[0].normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0)
3100  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3101 
3102  tf1 = transform;
3103  tf2 = transform;
3104  contacts.resize(1);
3105  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3106  contacts[0].penetration_depth = 5;
3107  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
3108  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3109 
3110  tf1 = Transform3<S>::Identity();
3111  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3112  contacts.resize(1);
3113  contacts[0].pos << 0, 0, 2.5;
3114  contacts[0].penetration_depth = 2.5;
3115  contacts[0].normal << 0, 0, 1;
3116  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3117 
3118  tf1 = transform;
3119  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3120  contacts.resize(1);
3121  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
3122  contacts[0].penetration_depth = 2.5;
3123  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
3124  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3125 
3126  tf1 = Transform3<S>::Identity();
3127  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3128  contacts.resize(1);
3129  contacts[0].pos << 0, 0, -2.5;
3130  contacts[0].penetration_depth = 2.5;
3131  contacts[0].normal << 0, 0, -1;
3132  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3133 
3134  tf1 = transform;
3135  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3136  contacts.resize(1);
3137  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
3138  contacts[0].penetration_depth = 2.5;
3139  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3140  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3141 
3142  tf1 = Transform3<S>::Identity();
3143  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
3144  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3145 
3146  tf1 = transform;
3147  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
3148  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3149 
3150  tf1 = Transform3<S>::Identity();
3151  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
3152  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3153 
3154  tf1 = transform;
3155  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
3156  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3157 }
3158 
3159 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder)
3160 {
3161 // test_shapeIntersection_planecylinder<float>();
3162  test_shapeIntersection_planecylinder<double>();
3163 }
3164 
3165 template <typename S>
3166 void test_shapeIntersection_halfspacecone()
3167 {
3168  Cone<S> s(5, 10);
3169  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
3170 
3171  Transform3<S> tf1;
3172  Transform3<S> tf2;
3173 
3174  Transform3<S> transform = Transform3<S>::Identity();
3175  test::generateRandomTransform(extents<S>(), transform);
3176 
3177  std::vector<ContactPoint<S>> contacts;
3178 
3179  tf1 = Transform3<S>::Identity();
3180  tf2 = Transform3<S>::Identity();
3181  contacts.resize(1);
3182  contacts[0].pos << -2.5, 0, -5;
3183  contacts[0].penetration_depth = 5;
3184  contacts[0].normal << -1, 0, 0;
3185  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3186 
3187  tf1 = transform;
3188  tf2 = transform;
3189  contacts.resize(1);
3190  contacts[0].pos = transform * Vector3<S>(-2.5, 0, -5);
3191  contacts[0].penetration_depth = 5;
3192  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3193  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3194 
3195  tf1 = Transform3<S>::Identity();
3196  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
3197  contacts.resize(1);
3198  contacts[0].pos << -1.25, 0, -5;
3199  contacts[0].penetration_depth = 7.5;
3200  contacts[0].normal << -1, 0, 0;
3201  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3202 
3203  tf1 = transform;
3204  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
3205  contacts.resize(1);
3206  contacts[0].pos = transform * Vector3<S>(-1.25, 0, -5);
3207  contacts[0].penetration_depth = 7.5;
3208  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3209  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3210 
3211  tf1 = Transform3<S>::Identity();
3212  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3213  contacts.resize(1);
3214  contacts[0].pos << -3.75, 0, -5;
3215  contacts[0].penetration_depth = 2.5;
3216  contacts[0].normal << -1, 0, 0;
3217  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3218 
3219  tf1 = transform;
3220  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3221  contacts.resize(1);
3222  contacts[0].pos = transform * Vector3<S>(-3.75, 0, -5);
3223  contacts[0].penetration_depth = 2.5;
3224  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3225  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3226 
3227  tf1 = Transform3<S>::Identity();
3228  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3229  contacts.resize(1);
3230  contacts[0].pos << 0.05, 0, -5;
3231  contacts[0].penetration_depth = 10.1;
3232  contacts[0].normal << -1, 0, 0;
3233  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3234 
3235  tf1 = transform;
3236  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3237  contacts.resize(1);
3238  contacts[0].pos = transform * Vector3<S>(0.05, 0, -5);
3239  contacts[0].penetration_depth = 10.1;
3240  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3241  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3242 
3243  tf1 = Transform3<S>::Identity();
3244  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3245  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3246 
3247  tf1 = transform;
3248  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3249  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3250 
3251 
3252 
3253 
3254  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);
3255 
3256  tf1 = Transform3<S>::Identity();
3257  tf2 = Transform3<S>::Identity();
3258  contacts.resize(1);
3259  contacts[0].pos << 0, -2.5, -5;
3260  contacts[0].penetration_depth = 5;
3261  contacts[0].normal << 0, -1, 0;
3262  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3263 
3264  tf1 = transform;
3265  tf2 = transform;
3266  contacts.resize(1);
3267  contacts[0].pos = transform * Vector3<S>(0, -2.5, -5);
3268  contacts[0].penetration_depth = 5;
3269  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3270  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3271 
3272  tf1 = Transform3<S>::Identity();
3273  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3274  contacts.resize(1);
3275  contacts[0].pos << 0, -1.25, -5;
3276  contacts[0].penetration_depth = 7.5;
3277  contacts[0].normal << 0, -1, 0;
3278  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3279 
3280  tf1 = transform;
3281  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3282  contacts.resize(1);
3283  contacts[0].pos = transform * Vector3<S>(0, -1.25, -5);
3284  contacts[0].penetration_depth = 7.5;
3285  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3286  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3287 
3288  tf1 = Transform3<S>::Identity();
3289  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3290  contacts.resize(1);
3291  contacts[0].pos << 0, -3.75, -5;
3292  contacts[0].penetration_depth = 2.5;
3293  contacts[0].normal << 0, -1, 0;
3294  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3295 
3296  tf1 = transform;
3297  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3298  contacts.resize(1);
3299  contacts[0].pos = transform * Vector3<S>(0, -3.75, -5);
3300  contacts[0].penetration_depth = 2.5;
3301  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3302  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3303 
3304  tf1 = Transform3<S>::Identity();
3305  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3306  contacts.resize(1);
3307  contacts[0].pos << 0, 0.05, -5;
3308  contacts[0].penetration_depth = 10.1;
3309  contacts[0].normal << 0, -1, 0;
3310  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3311 
3312  tf1 = transform;
3313  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3314  contacts.resize(1);
3315  contacts[0].pos = transform * Vector3<S>(0, 0.05, -5);
3316  contacts[0].penetration_depth = 10.1;
3317  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3318  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3319 
3320  tf1 = Transform3<S>::Identity();
3321  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3322  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3323 
3324  tf1 = transform;
3325  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3326  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3327 
3328 
3329 
3330 
3331  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);
3332 
3333  tf1 = Transform3<S>::Identity();
3334  tf2 = Transform3<S>::Identity();
3335  contacts.resize(1);
3336  contacts[0].pos << 0, 0, -2.5;
3337  contacts[0].penetration_depth = 5;
3338  contacts[0].normal << 0, 0, -1;
3339  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3340 
3341  tf1 = transform;
3342  tf2 = transform;
3343  contacts.resize(1);
3344  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
3345  contacts[0].penetration_depth = 5;
3346  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3347  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3348 
3349  tf1 = Transform3<S>::Identity();
3350  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3351  contacts.resize(1);
3352  contacts[0].pos << 0, 0, -1.25;
3353  contacts[0].penetration_depth = 7.5;
3354  contacts[0].normal << 0, 0, -1;
3355  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3356 
3357  tf1 = transform;
3358  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3359  contacts.resize(1);
3360  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
3361  contacts[0].penetration_depth = 7.5;
3362  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3363  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3364 
3365  tf1 = Transform3<S>::Identity();
3366  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3367  contacts.resize(1);
3368  contacts[0].pos << 0, 0, -3.75;
3369  contacts[0].penetration_depth= 2.5;
3370  contacts[0].normal << 0, 0, -1;
3371  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3372 
3373  tf1 = transform;
3374  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3375  contacts.resize(1);
3376  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
3377  contacts[0].penetration_depth = 2.5;
3378  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3379  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3380 
3381  tf1 = Transform3<S>::Identity();
3382  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
3383  contacts.resize(1);
3384  contacts[0].pos << 0, 0, 0.05;
3385  contacts[0].penetration_depth = 10.1;
3386  contacts[0].normal << 0, 0, -1;
3387  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3388 
3389  tf1 = transform;
3390  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
3391  contacts.resize(1);
3392  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
3393  contacts[0].penetration_depth = 10.1;
3394  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3395  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3396 
3397  tf1 = Transform3<S>::Identity();
3398  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
3399  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3400 
3401  tf1 = transform;
3402  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
3403  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3404 }
3405 
3406 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone)
3407 {
3408 // test_shapeIntersection_halfspacecone<float>();
3409  test_shapeIntersection_halfspacecone<double>();
3410 }
3411 
3412 template <typename S>
3413 void test_shapeIntersection_planecone()
3414 {
3415  Cone<S> s(5, 10);
3416  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
3417 
3418  Transform3<S> tf1;
3419  Transform3<S> tf2;
3420 
3421  Transform3<S> transform = Transform3<S>::Identity();
3422  test::generateRandomTransform(extents<S>(), transform);
3423 
3424  std::vector<ContactPoint<S>> contacts;
3425 
3426  tf1 = Transform3<S>::Identity();
3427  tf2 = Transform3<S>::Identity();
3428  contacts.resize(1);
3429  contacts[0].pos << 0, 0, 0;
3430  contacts[0].penetration_depth = 5;
3431  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
3432  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3433 
3434  tf1 = transform;
3435  tf2 = transform;
3436  contacts.resize(1);
3437  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3438  contacts[0].penetration_depth = 5;
3439  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
3440  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3441 
3442  tf1 = Transform3<S>::Identity();
3443  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
3444  contacts.resize(1);
3445  contacts[0].pos << 2.5, 0, -2.5;
3446  contacts[0].penetration_depth = 2.5;
3447  contacts[0].normal << 1, 0, 0;
3448  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3449 
3450  tf1 = transform;
3451  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
3452  contacts.resize(1);
3453  contacts[0].pos = transform * Vector3<S>(2.5, 0, -2.5);
3454  contacts[0].penetration_depth = 2.5;
3455  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
3456  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3457 
3458  tf1 = Transform3<S>::Identity();
3459  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3460  contacts.resize(1);
3461  contacts[0].pos << -2.5, 0, -2.5;
3462  contacts[0].penetration_depth = 2.5;
3463  contacts[0].normal << -1, 0, 0;
3464  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3465 
3466  tf1 = transform;
3467  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3468  contacts.resize(1);
3469  contacts[0].pos = transform * Vector3<S>(-2.5, 0, -2.5);
3470  contacts[0].penetration_depth = 2.5;
3471  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3472  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3473 
3474  tf1 = Transform3<S>::Identity();
3475  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3476  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3477 
3478  tf1 = transform;
3479  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3480  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3481 
3482  tf1 = Transform3<S>::Identity();
3483  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3484  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3485 
3486  tf1 = transform;
3487  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3488  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3489 
3490 
3491 
3492 
3493  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);
3494 
3495  tf1 = Transform3<S>::Identity();
3496  tf2 = Transform3<S>::Identity();
3497  contacts.resize(1);
3498  contacts[0].pos << 0, 0, 0;
3499  contacts[0].penetration_depth = 5;
3500  contacts[0].normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0)
3501  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3502 
3503  tf1 = transform;
3504  tf2 = transform;
3505  contacts.resize(1);
3506  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3507  contacts[0].penetration_depth = 5;
3508  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
3509  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3510 
3511  tf1 = Transform3<S>::Identity();
3512  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3513  contacts.resize(1);
3514  contacts[0].pos << 0, 2.5, -2.5;
3515  contacts[0].penetration_depth = 2.5;
3516  contacts[0].normal << 0, 1, 0;
3517  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3518 
3519  tf1 = transform;
3520  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3521  contacts.resize(1);
3522  contacts[0].pos = transform * Vector3<S>(0, 2.5, -2.5);
3523  contacts[0].penetration_depth = 2.5;
3524  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
3525  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3526 
3527  tf1 = Transform3<S>::Identity();
3528  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3529  contacts.resize(1);
3530  contacts[0].pos << 0, -2.5, -2.5;
3531  contacts[0].penetration_depth = 2.5;
3532  contacts[0].normal << 0, -1, 0;
3533  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3534 
3535  tf1 = transform;
3536  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3537  contacts.resize(1);
3538  contacts[0].pos = transform * Vector3<S>(0, -2.5, -2.5);
3539  contacts[0].penetration_depth = 2.5;
3540  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3541  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3542 
3543  tf1 = Transform3<S>::Identity();
3544  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3545  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3546 
3547  tf1 = transform;
3548  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3549  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3550 
3551  tf1 = Transform3<S>::Identity();
3552  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3553  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3554 
3555  tf1 = transform;
3556  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3557  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3558 
3559 
3560 
3561 
3562  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);
3563 
3564  tf1 = Transform3<S>::Identity();
3565  tf2 = Transform3<S>::Identity();
3566  contacts.resize(1);
3567  contacts[0].pos << 0, 0, 0;
3568  contacts[0].penetration_depth = 5;
3569  contacts[0].normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0)
3570  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3571 
3572  tf1 = transform;
3573  tf2 = transform;
3574  contacts.resize(1);
3575  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3576  contacts[0].penetration_depth = 5;
3577  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
3578  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3579 
3580  tf1 = Transform3<S>::Identity();
3581  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3582  contacts.resize(1);
3583  contacts[0].pos << 0, 0, 2.5;
3584  contacts[0].penetration_depth = 2.5;
3585  contacts[0].normal << 0, 0, 1;
3586  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3587 
3588  tf1 = transform;
3589  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3590  contacts.resize(1);
3591  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
3592  contacts[0].penetration_depth = 2.5;
3593  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
3594  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3595 
3596  tf1 = Transform3<S>::Identity();
3597  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3598  contacts.resize(1);
3599  contacts[0].pos << 0, 0, -2.5;
3600  contacts[0].penetration_depth = 2.5;
3601  contacts[0].normal << 0, 0, -1;
3602  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3603 
3604  tf1 = transform;
3605  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3606  contacts.resize(1);
3607  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
3608  contacts[0].penetration_depth = 2.5;
3609  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3610  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3611 
3612  tf1 = Transform3<S>::Identity();
3613  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
3614  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3615 
3616  tf1 = transform;
3617  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
3618  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3619 
3620  tf1 = Transform3<S>::Identity();
3621  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
3622  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3623 
3624  tf1 = transform;
3625  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
3626  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3627 }
3628 
3629 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone)
3630 {
3631 // test_shapeIntersection_planecone<float>();
3632  test_shapeIntersection_planecone<double>();
3633 }
3634 
3635 // Shape distance test coverage (libccd)
3636 //
3637 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3638 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
3639 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3640 // | box | O | O | | | | | | | |
3641 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3642 // | sphere |/////| O | | | | | | | |
3643 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3644 // | ellipsoid |/////|////////| O | | | | | | |
3645 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3646 // | capsule |/////|////////|///////////| | | | | | |
3647 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3648 // | cone |/////|////////|///////////|/////////| O | O | | | |
3649 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3650 // | cylinder |/////|////////|///////////|/////////|//////| O | | | |
3651 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3652 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
3653 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3654 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
3655 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3656 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
3657 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3658 
3659 template <typename S>
3660 void test_shapeDistance_spheresphere()
3661 {
3662  Sphere<S> s1(20);
3663  Sphere<S> s2(10);
3664 
3665  Transform3<S> transform = Transform3<S>::Identity();
3666  //test::generateRandomTransform(extents<S>(), transform);
3667 
3668  bool res;
3669  S dist = -1;
3670  Vector3<S> closest_p1, closest_p2;
3671  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 40, 0))), &dist, &closest_p1, &closest_p2);
3672  EXPECT_TRUE(fabs(dist - 10) < 0.001);
3673  EXPECT_TRUE(res);
3674 
3675  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
3676  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3677  EXPECT_TRUE(res);
3678 
3679  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
3680  EXPECT_TRUE(dist < 0);
3681  EXPECT_FALSE(res);
3682 
3683  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
3684  EXPECT_TRUE(fabs(dist - 10) < 0.001);
3685  EXPECT_TRUE(res);
3686 
3687  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
3688  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3689  EXPECT_TRUE(res);
3690 
3691  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
3692  EXPECT_TRUE(dist < 0);
3693  EXPECT_FALSE(res);
3694 
3695 
3696  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3697  // this is one problem: the precise is low sometimes
3698  EXPECT_TRUE(fabs(dist - 10) < 0.1);
3699  EXPECT_TRUE(res);
3700 
3701  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
3702  EXPECT_TRUE(fabs(dist - 0.1) < 0.06);
3703  EXPECT_TRUE(res);
3704 
3705  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
3706  EXPECT_TRUE(dist < 0);
3707  EXPECT_FALSE(res);
3708 
3709  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
3710  EXPECT_TRUE(fabs(dist - 10) < 0.1);
3711  EXPECT_TRUE(res);
3712 
3713  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
3714  EXPECT_TRUE(fabs(dist - 0.1) < 0.1);
3715  EXPECT_TRUE(res);
3716 
3717  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
3718  EXPECT_TRUE(dist < 0);
3719  EXPECT_FALSE(res);
3720 }
3721 
3722 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_spheresphere)
3723 {
3724 // test_shapeDistance_spheresphere<float>();
3725  test_shapeDistance_spheresphere<double>();
3726 }
3727 
3728 template <typename S>
3729 void test_shapeDistance_boxbox()
3730 {
3731  Box<S> s1(20, 40, 50);
3732  Box<S> s2(10, 10, 10);
3733  Vector3<S> closest_p1, closest_p2;
3734 
3735  Transform3<S> transform = Transform3<S>::Identity();
3736  //test::generateRandomTransform(extents<S>(), transform);
3737 
3738  bool res;
3739  S dist;
3740 
3741  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3742  EXPECT_TRUE(dist < 0);
3743  EXPECT_FALSE(res);
3744 
3745  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3746  EXPECT_TRUE(dist < 0);
3747  EXPECT_FALSE(res);
3748 
3749  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist, &closest_p1, &closest_p2);
3750  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3751  EXPECT_TRUE(res);
3752 
3753  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20.1, 0, 0))), &dist, &closest_p1, &closest_p2);
3754  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
3755  EXPECT_TRUE(res);
3756 
3757  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 20.2, 0))), &dist, &closest_p1, &closest_p2);
3758  EXPECT_TRUE(fabs(dist - 10.2) < 0.001);
3759  EXPECT_TRUE(res);
3760 
3761  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 10.1, 0))), &dist, &closest_p1, &closest_p2);
3762  EXPECT_TRUE(fabs(dist - 0.1 * 1.414) < 0.001);
3763  EXPECT_TRUE(res);
3764 
3765  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist, &closest_p1, &closest_p2);
3766  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3767  EXPECT_TRUE(res);
3768 
3769  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20.1, 0, 0))), &dist, &closest_p1, &closest_p2);
3770  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
3771  EXPECT_TRUE(res);
3772 
3773  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 20.1, 0))), &dist, &closest_p1, &closest_p2);
3774  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
3775  EXPECT_TRUE(res);
3776 
3777  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 10.1, 0))), &dist, &closest_p1, &closest_p2);
3778  EXPECT_TRUE(fabs(dist - 0.1 * 1.414) < 0.001);
3779  EXPECT_TRUE(res);
3780 
3781 
3782  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
3783  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3784  EXPECT_TRUE(res);
3785 
3786  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
3787  EXPECT_TRUE(fabs(dist - 5) < 0.001);
3788  EXPECT_TRUE(res);
3789 
3790  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
3791  EXPECT_TRUE(fabs(dist - 5) < 0.001);
3792  EXPECT_TRUE(res);
3793 }
3794 
3795 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxbox)
3796 {
3797 // test_shapeDistance_boxbox<float>();
3798  test_shapeDistance_boxbox<double>();
3799 }
3800 
3801 template <typename S>
3802 void test_shapeDistance_boxsphere()
3803 {
3804  Sphere<S> s1(20);
3805  Box<S> s2(5, 5, 5);
3806 
3807  Transform3<S> transform = Transform3<S>::Identity();
3808  test::generateRandomTransform(extents<S>(), transform);
3809 
3810  bool res;
3811  S dist;
3812 
3813  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3814  EXPECT_TRUE(dist < 0);
3815  EXPECT_FALSE(res);
3816 
3817  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3818  EXPECT_TRUE(dist < 0);
3819  EXPECT_FALSE(res);
3820 
3821  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
3822  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3823  EXPECT_TRUE(res);
3824 
3825  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
3826  EXPECT_TRUE(fabs(dist - 0.1) < 0.05);
3827  EXPECT_TRUE(res);
3828 
3829  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3830  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
3831  EXPECT_TRUE(res);
3832 
3833  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3834  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
3835  EXPECT_TRUE(res);
3836 }
3837 
3838 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxsphere)
3839 {
3840 // test_shapeDistance_boxsphere<float>();
3841  test_shapeDistance_boxsphere<double>();
3842 }
3843 
3844 template <typename S>
3845 void test_shapeDistance_cylindercylinder()
3846 {
3847  Cylinder<S> s1(5, 10);
3848  Cylinder<S> s2(5, 10);
3849 
3850  Transform3<S> transform = Transform3<S>::Identity();
3851  test::generateRandomTransform(extents<S>(), transform);
3852 
3853  bool res;
3854  S dist;
3855 
3856  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3857  EXPECT_TRUE(dist < 0);
3858  EXPECT_FALSE(res);
3859 
3860  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3861  EXPECT_TRUE(dist < 0);
3862  EXPECT_FALSE(res);
3863 
3864  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3865  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3866  EXPECT_TRUE(res);
3867 
3868  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3869  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3870  EXPECT_TRUE(res);
3871 
3872  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3873  EXPECT_TRUE(fabs(dist - 30) < 0.001);
3874  EXPECT_TRUE(res);
3875 
3876  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3877  EXPECT_TRUE(fabs(dist - 30) < 0.001);
3878  EXPECT_TRUE(res);
3879 }
3880 
3881 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_cylindercylinder)
3882 {
3883 // test_shapeDistance_cylindercylinder<float>();
3884  test_shapeDistance_cylindercylinder<double>();
3885 }
3886 
3887 template <typename S>
3888 void test_shapeDistance_conecone()
3889 {
3890  Cone<S> s1(5, 10);
3891  Cone<S> s2(5, 10);
3892 
3893  Transform3<S> transform = Transform3<S>::Identity();
3894  test::generateRandomTransform(extents<S>(), transform);
3895 
3896  bool res;
3897  S dist;
3898 
3899  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3900  EXPECT_TRUE(dist < 0);
3901  EXPECT_FALSE(res);
3902 
3903  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3904  EXPECT_TRUE(dist < 0);
3905  EXPECT_FALSE(res);
3906 
3907  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3908  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3909  EXPECT_TRUE(res);
3910 
3911  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3912  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3913  EXPECT_TRUE(res);
3914 
3915  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
3916  EXPECT_TRUE(fabs(dist - 30) < 1);
3917  EXPECT_TRUE(res);
3918 
3919  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
3920  EXPECT_TRUE(fabs(dist - 30) < 1);
3921  EXPECT_TRUE(res);
3922 }
3923 
3924 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecone)
3925 {
3926 // test_shapeDistance_conecone<float>();
3927  test_shapeDistance_conecone<double>();
3928 }
3929 
3930 template <typename S>
3931 void test_shapeDistance_conecylinder()
3932 {
3933  Cylinder<S> s1(5, 10);
3934  Cone<S> s2(5, 10);
3935 
3936  Transform3<S> transform = Transform3<S>::Identity();
3937  test::generateRandomTransform(extents<S>(), transform);
3938 
3939  bool res;
3940  S dist;
3941 
3942  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3943  EXPECT_TRUE(dist < 0);
3944  EXPECT_FALSE(res);
3945 
3946  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3947  EXPECT_TRUE(dist < 0);
3948  EXPECT_FALSE(res);
3949 
3950  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3951  EXPECT_TRUE(fabs(dist - 0.1) < 0.01);
3952  EXPECT_TRUE(res);
3953 
3954  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3955  EXPECT_TRUE(fabs(dist - 0.1) < 0.02);
3956  EXPECT_TRUE(res);
3957 
3958  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3959  EXPECT_TRUE(fabs(dist - 30) < 0.01);
3960  EXPECT_TRUE(res);
3961 
3962  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3963  EXPECT_TRUE(fabs(dist - 30) < 0.1);
3964  EXPECT_TRUE(res);
3965 }
3966 
3967 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecylinder)
3968 {
3969 // test_shapeDistance_conecylinder<float>();
3970  test_shapeDistance_conecylinder<double>();
3971 }
3972 
3973 template <typename S>
3974 void test_shapeDistance_ellipsoidellipsoid()
3975 {
3976  Ellipsoid<S> s1(20, 40, 50);
3977  Ellipsoid<S> s2(10, 10, 10);
3978 
3979  Transform3<S> transform = Transform3<S>::Identity();
3980  test::generateRandomTransform(extents<S>(), transform);
3981 
3982  bool res;
3983  S dist = -1;
3984  Vector3<S> closest_p1, closest_p2;
3985 
3986  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist, &closest_p1, &closest_p2);
3987  EXPECT_TRUE(fabs(dist - 10) < 0.001);
3988  EXPECT_TRUE(res);
3989 
3990  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
3991  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3992  EXPECT_TRUE(res);
3993 
3994  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
3995  EXPECT_TRUE(dist < 0);
3996  EXPECT_FALSE(res);
3997 
3998  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
3999  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4000  EXPECT_TRUE(res);
4001 
4002  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4003  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4004  EXPECT_TRUE(res);
4005 
4006  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4007  EXPECT_TRUE(dist < 0);
4008  EXPECT_FALSE(res);
4009 
4010 
4011  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
4012  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4013  EXPECT_TRUE(res);
4014 
4015  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
4016  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4017  EXPECT_TRUE(res);
4018 
4019  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
4020  EXPECT_TRUE(dist < 0);
4021  EXPECT_FALSE(res);
4022 
4023  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
4024  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4025  EXPECT_TRUE(res);
4026 
4027  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
4028  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4029  EXPECT_TRUE(res);
4030 
4031  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
4032  EXPECT_TRUE(dist < 0);
4033  EXPECT_FALSE(res);
4034 }
4035 
4036 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_ellipsoidellipsoid)
4037 {
4038 // test_shapeDistance_ellipsoidellipsoid<float>();
4039  test_shapeDistance_ellipsoidellipsoid<double>();
4040 }
4041 
4042 // Shape intersection test coverage (built-in GJK)
4043 //
4044 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4045 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
4046 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4047 // | box | O | O | | | | | | | |
4048 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4049 // | sphere |/////| O | | | | | | | O |
4050 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4051 // | ellipsoid |/////|////////| O | | | | | | |
4052 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4053 // | capsule |/////|////////|///////////| | | | | | |
4054 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4055 // | cone |/////|////////|///////////|/////////| O | O | | | |
4056 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4057 // | cylinder |/////|////////|///////////|/////////|//////| O | | | |
4058 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4059 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
4060 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4061 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
4062 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4063 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
4064 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4065 
4066 template <typename S>
4067 void test_shapeIntersectionGJK_spheresphere()
4068 {
4069  Sphere<S> s1(20);
4070  Sphere<S> s2(10);
4071 
4072  Transform3<S> tf1;
4073  Transform3<S> tf2;
4074 
4075  Transform3<S> transform = Transform3<S>::Identity();
4076  test::generateRandomTransform(extents<S>(), transform);
4077 
4078  std::vector<ContactPoint<S>> contacts;
4079 
4080  tf1 = Transform3<S>::Identity();
4081  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
4082  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4083 
4084  tf1 = transform;
4085  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
4086  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4087 
4088  tf1 = Transform3<S>::Identity();
4089  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
4090  contacts.resize(1);
4091  contacts[0].normal << 1, 0, 0;
4092  contacts[0].pos << 20, 0, 0;
4093  contacts[0].penetration_depth = 0.0;
4094  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4095 
4096  tf1 = Transform3<S>::Identity();
4097  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
4098  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4099 
4100  tf1 = transform;
4101  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
4102  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4103 
4104  tf1 = Transform3<S>::Identity();
4105  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
4106  contacts.resize(1);
4107  contacts[0].normal << 1, 0, 0;
4108  contacts[0].pos << 20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0;
4109  contacts[0].penetration_depth = 0.1;
4110  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4111 
4112  tf1 = transform;
4113  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
4114  contacts.resize(1);
4115  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4116  contacts[0].pos = transform * Vector3<S>(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0);
4117  contacts[0].penetration_depth = 0.1;
4118  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4119 
4120  tf1 = Transform3<S>::Identity();
4121  tf2 = Transform3<S>::Identity();
4122  contacts.resize(1);
4123  contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
4124  contacts[0].pos.setZero();
4125  contacts[0].penetration_depth = 20.0 + 10.0;
4126  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4127 
4128  tf1 = transform;
4129  tf2 = transform;
4130  contacts.resize(1);
4131  contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
4132  contacts[0].pos = transform * Vector3<S>::Zero();
4133  contacts[0].penetration_depth = 20.0 + 10.0;
4134  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4135 
4136  tf1 = Transform3<S>::Identity();
4137  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
4138  contacts.resize(1);
4139  contacts[0].normal << -1, 0, 0;
4140  contacts[0].pos << -20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0;
4141  contacts[0].penetration_depth = 0.1;
4142  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4143 
4144  tf1 = transform;
4145  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
4146  contacts.resize(1);
4147  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
4148  contacts[0].pos = transform * Vector3<S>(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0);
4149  contacts[0].penetration_depth = 0.1;
4150  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4151 
4152  tf1 = Transform3<S>::Identity();
4153  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.0, 0, 0)));
4154  contacts.resize(1);
4155  contacts[0].normal << -1, 0, 0;
4156  contacts[0].pos << -20, 0, 0;
4157  contacts[0].penetration_depth = 0.0;
4158  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4159 
4160  tf1 = Transform3<S>::Identity();
4161  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
4162  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4163 
4164  tf1 = transform;
4165  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
4166  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4167 }
4168 
4169 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheresphere)
4170 {
4171 // test_shapeIntersectionGJK_spheresphere<float>();
4172  test_shapeIntersectionGJK_spheresphere<double>();
4173 }
4174 
4175 template <typename S>
4176 void test_shapeIntersectionGJK_boxbox()
4177 {
4178  Box<S> s1(20, 40, 50);
4179  Box<S> s2(10, 10, 10);
4180 
4181  Transform3<S> tf1;
4182  Transform3<S> tf2;
4183 
4184  Transform3<S> transform = Transform3<S>::Identity();
4185  test::generateRandomTransform(extents<S>(), transform);
4186 
4187  std::vector<ContactPoint<S>> contacts;
4188 
4189  Quaternion<S> q(AngleAxis<S>((S)3.140 / 6, Vector3<S>(0, 0, 1)));
4190 
4191  tf1 = Transform3<S>::Identity();
4192  tf2 = Transform3<S>::Identity();
4193  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
4194  contacts.resize(4);
4195  contacts[0].normal << 1, 0, 0;
4196  contacts[1].normal << 1, 0, 0;
4197  contacts[2].normal << 1, 0, 0;
4198  contacts[3].normal << 1, 0, 0;
4199  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4200 
4201  tf1 = transform;
4202  tf2 = transform;
4203  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
4204  contacts.resize(4);
4205  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4206  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
4207  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
4208  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
4209  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4210 
4211  tf1 = Transform3<S>::Identity();
4212  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(15, 0, 0)));
4213  contacts.resize(4);
4214  contacts[0].normal = Vector3<S>(1, 0, 0);
4215  contacts[1].normal = Vector3<S>(1, 0, 0);
4216  contacts[2].normal = Vector3<S>(1, 0, 0);
4217  contacts[3].normal = Vector3<S>(1, 0, 0);
4218  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4219 
4220  tf1 = transform;
4221  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(15.01, 0, 0)));
4222  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4223 
4224  tf1 = Transform3<S>::Identity();
4225  tf2 = Transform3<S>(q);
4226  contacts.resize(4);
4227  contacts[0].normal = Vector3<S>(1, 0, 0);
4228  contacts[1].normal = Vector3<S>(1, 0, 0);
4229  contacts[2].normal = Vector3<S>(1, 0, 0);
4230  contacts[3].normal = Vector3<S>(1, 0, 0);
4231  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4232 
4233  tf1 = transform;
4234  tf2 = transform * Transform3<S>(q);
4235  contacts.resize(4);
4236  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4237  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
4238  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
4239  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
4240  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4241 }
4242 
4243 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_boxbox)
4244 {
4245 // test_shapeIntersectionGJK_boxbox<float>();
4246  test_shapeIntersectionGJK_boxbox<double>();
4247 }
4248 
4249 template <typename S>
4250 void test_shapeIntersectionGJK_spherebox()
4251 {
4252  Sphere<S> s1(20);
4253  Box<S> s2(5, 5, 5);
4254 
4255  Transform3<S> tf1;
4256  Transform3<S> tf2;
4257 
4258  Transform3<S> transform = Transform3<S>::Identity();
4259  test::generateRandomTransform(extents<S>(), transform);
4260 
4261  std::vector<ContactPoint<S>> contacts;
4262 
4263  tf1 = Transform3<S>::Identity();
4264  tf2 = Transform3<S>::Identity();
4265  // TODO: Need convention for normal when the centers of two objects are at same position.
4266  contacts.resize(1);
4267  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4268 
4269  tf1 = transform;
4270  tf2 = transform;
4271  // TODO: Need convention for normal when the centers of two objects are at same position.
4272  contacts.resize(1);
4273  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4274 
4275  tf1 = Transform3<S>::Identity();
4276  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.5, 0, 0)));
4277  contacts.resize(1);
4278  contacts[0].normal << 1, 0, 0;
4279  // TODO(SeanCurtis-TRI): This osculating contact is considered a collision
4280  // only for a relatively "loose" gjk tolerance. So, for this test to pass, we
4281  // set and restore the default gjk tolerances. We need to determine if this is
4282  // the correct/desired behavior and, if so, fix the defect that smaller
4283  // tolerances prevent this from reporting as a contact. NOTE: this is *not*
4284  // the same tolerance that is passed into the `testShapeIntersection` function
4285  // -- which isn't actually *used*.
4286  {
4287  detail::GJKSolver_indep<S>& solver = solver2<S>();
4288  const S old_gjk_tolerance = solver.gjk_tolerance;
4289  const S old_epa_tolerance = solver.epa_tolerance;
4290 
4291  // The historical tolerances for which this test passes.
4292  solver.gjk_tolerance = 1e-6;
4293  solver.epa_tolerance = 1e-6;
4294 
4295  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false,
4296  false, true, false,
4297  1e-7 // built-in GJK solver requires larger tolerance than libccd
4298  );
4299  // TODO(SeanCurtis-TRI): If testShapeIntersection fails an *assert* this
4300  // code will not get fired off and the static solvers will not get reset.
4301  solver.gjk_tolerance = old_gjk_tolerance;
4302  solver.epa_tolerance = old_epa_tolerance;
4303  }
4304 
4305  tf1 = transform;
4306  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.51, 0, 0)));
4307  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4308 
4309  tf1 = Transform3<S>::Identity();
4310  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
4311  contacts.resize(1);
4312  contacts[0].normal << 1, 0, 0;
4313  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd
4314 
4315  tf1 = transform;
4316  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
4317  contacts.resize(1);
4318  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4319  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4320  // built-in GJK solver returns incorrect normal.
4321 }
4322 
4323 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherebox)
4324 {
4325 // test_shapeIntersectionGJK_spherebox<float>();
4326  test_shapeIntersectionGJK_spherebox<double>();
4327 }
4328 
4329 template <typename S>
4330 void test_shapeIntersectionGJK_spherecapsule()
4331 {
4332  Sphere<S> s1(20);
4333  Capsule<S> s2(5, 10);
4334 
4335  Transform3<S> tf1;
4336  Transform3<S> tf2;
4337 
4338  Transform3<S> transform = Transform3<S>::Identity();
4339  test::generateRandomTransform(extents<S>(), transform);
4340 
4341  std::vector<ContactPoint<S>> contacts;
4342 
4343  tf1 = Transform3<S>::Identity();
4344  tf2 = Transform3<S>::Identity();
4345  // TODO: Need convention for normal when the centers of two objects are at same position.
4346  contacts.resize(1);
4347  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4348 
4349  tf1 = transform;
4350  tf2 = transform;
4351  // TODO: Need convention for normal when the centers of two objects are at same position.
4352  contacts.resize(1);
4353  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4354 
4355  tf1 = Transform3<S>::Identity();
4356  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
4357  contacts.resize(1);
4358  contacts[0].normal << 1, 0, 0;
4359  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4360 
4361  tf1 = transform;
4362  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
4363  contacts.resize(1);
4364  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4365  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4366 
4367  tf1 = Transform3<S>::Identity();
4368  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(25, 0, 0)));
4369  contacts.resize(1);
4370  contacts[0].normal << 1, 0, 0;
4371  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4372 
4373  tf1 = transform;
4374  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25.1, 0, 0)));
4375  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4376 }
4377 
4378 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherecapsule)
4379 {
4380 // test_shapeIntersectionGJK_spherecapsule<float>();
4381  test_shapeIntersectionGJK_spherecapsule<double>();
4382 }
4383 
4384 template <typename S>
4385 void test_shapeIntersectionGJK_cylindercylinder()
4386 {
4387  Cylinder<S> s1(5, 10);
4388  Cylinder<S> s2(5, 10);
4389 
4390  Transform3<S> tf1;
4391  Transform3<S> tf2;
4392 
4393  Transform3<S> transform = Transform3<S>::Identity();
4394  test::generateRandomTransform(extents<S>(), transform);
4395 
4396  std::vector<ContactPoint<S>> contacts;
4397 
4398  tf1 = Transform3<S>::Identity();
4399  tf2 = Transform3<S>::Identity();
4400  // TODO: Need convention for normal when the centers of two objects are at same position.
4401  contacts.resize(1);
4402  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4403 
4404  tf1 = transform;
4405  tf2 = transform;
4406  // TODO: Need convention for normal when the centers of two objects are at same position.
4407  contacts.resize(1);
4408  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4409 
4410  tf1 = Transform3<S>::Identity();
4411  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4412  contacts.resize(1);
4413  contacts[0].normal << 1, 0, 0;
4414  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd
4415 
4416  tf1 = transform;
4417  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4418  contacts.resize(1);
4419  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4420  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4421  // built-in GJK solver returns incorrect normal.
4422 
4423  tf1 = Transform3<S>::Identity();
4424  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
4425  contacts.resize(1);
4426  contacts[0].normal << 1, 0, 0;
4427  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4428 
4429  tf1 = transform;
4430  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
4431  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4432 }
4433 
4434 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercylinder)
4435 {
4436 // test_shapeIntersectionGJK_cylindercylinder<float>();
4437  test_shapeIntersectionGJK_cylindercylinder<double>();
4438 }
4439 
4440 template <typename S>
4441 void test_shapeIntersectionGJK_conecone()
4442 {
4443  Cone<S> s1(5, 10);
4444  Cone<S> s2(5, 10);
4445 
4446  Transform3<S> tf1;
4447  Transform3<S> tf2;
4448 
4449  Transform3<S> transform = Transform3<S>::Identity();
4450  test::generateRandomTransform(extents<S>(), transform);
4451 
4452  std::vector<ContactPoint<S>> contacts;
4453 
4454  tf1 = Transform3<S>::Identity();
4455  tf2 = Transform3<S>::Identity();
4456  // TODO: Need convention for normal when the centers of two objects are at same position.
4457  contacts.resize(1);
4458  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4459 
4460  tf1 = transform;
4461  tf2 = transform;
4462  // TODO: Need convention for normal when the centers of two objects are at same position.
4463  contacts.resize(1);
4464  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4465 
4466  tf1 = Transform3<S>::Identity();
4467  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4468  contacts.resize(1);
4469  contacts[0].normal << 1, 0, 0;
4470  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 5.7e-1); // built-in GJK solver requires larger tolerance than libccd
4471 
4472  tf1 = transform;
4473  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4474  contacts.resize(1);
4475  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4476  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4477  // built-in GJK solver returns incorrect normal.
4478 
4479  tf1 = Transform3<S>::Identity();
4480  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
4481  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4482 
4483  tf1 = transform;
4484  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
4485  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4486 
4487  tf1 = Transform3<S>::Identity();
4488  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
4489  contacts.resize(1);
4490  contacts[0].normal << 0, 0, 1;
4491  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4492 
4493  tf1 = transform;
4494  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
4495  contacts.resize(1);
4496  // contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
4497  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4498  // built-in GJK solver returns incorrect normal.
4499 }
4500 
4501 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_conecone)
4502 {
4503 // test_shapeIntersectionGJK_conecone<float>();
4504  test_shapeIntersectionGJK_conecone<double>();
4505 }
4506 
4507 template <typename S>
4508 void test_shapeIntersectionGJK_cylindercone()
4509 {
4510  Cylinder<S> s1(5, 10);
4511  Cone<S> s2(5, 10);
4512 
4513  Transform3<S> tf1;
4514  Transform3<S> tf2;
4515 
4516  Transform3<S> transform = Transform3<S>::Identity();
4517  test::generateRandomTransform(extents<S>(), transform);
4518 
4519  std::vector<ContactPoint<S>> contacts;
4520  tf1 = Transform3<S>::Identity();
4521  tf2 = Transform3<S>::Identity();
4522  // TODO: Need convention for normal when the centers of two objects are at same position.
4523  contacts.resize(1);
4524  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4525 
4526  tf1 = transform;
4527  tf2 = transform;
4528  // TODO: Need convention for normal when the centers of two objects are at same position.
4529  contacts.resize(1);
4530  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4531 
4532  tf1 = Transform3<S>::Identity();
4533  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4534  contacts.resize(1);
4535  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4536 
4537  tf1 = transform;
4538  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4539  contacts.resize(1);
4540  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4541 
4542  tf1 = Transform3<S>::Identity();
4543  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
4544  contacts.resize(1);
4545  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4546 
4547  tf1 = transform;
4548  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
4549  contacts.resize(1);
4550  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4551 
4552  tf1 = Transform3<S>::Identity();
4553  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
4554  contacts.resize(1);
4555  contacts[0].normal << 0, 0, 1;
4556  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4557 
4558  tf1 = transform;
4559  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
4560  contacts.resize(1);
4561  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4562  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4563  // built-in GJK solver returns incorrect normal.
4564 
4565  tf1 = Transform3<S>::Identity();
4566  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10)));
4567  contacts.resize(1);
4568  contacts[0].normal << 0, 0, 1;
4569  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4570 
4571  tf1 = transform;
4572  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
4573  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4574 }
4575 
4576 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercone)
4577 {
4578 // test_shapeIntersectionGJK_cylindercone<float>();
4579  test_shapeIntersectionGJK_cylindercone<double>();
4580 }
4581 
4582 template <typename S>
4583 void test_shapeIntersectionGJK_ellipsoidellipsoid()
4584 {
4585  Ellipsoid<S> s1(20, 40, 50);
4586  Ellipsoid<S> s2(10, 10, 10);
4587 
4588  Transform3<S> tf1;
4589  Transform3<S> tf2;
4590 
4591  Transform3<S> transform = Transform3<S>::Identity();
4592  test::generateRandomTransform(extents<S>(), transform);
4593  Transform3<S> identity;
4594 
4595  std::vector<ContactPoint<S>> contacts;
4596 
4597  tf1 = Transform3<S>::Identity();
4598  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
4599  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4600 
4601  tf1 = transform;
4602  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
4603  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4604 
4605  tf1 = Transform3<S>::Identity();
4606  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
4607  contacts.resize(1);
4608  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4609 
4610  tf1 = transform;
4611  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
4612  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4613 
4614  tf1 = Transform3<S>::Identity();
4615  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.99, 0, 0)));
4616  contacts.resize(1);
4617  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4618 
4619  tf1 = transform;
4620  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
4621  contacts.resize(1);
4622  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4623 
4624  tf1 = Transform3<S>::Identity();
4625  tf2 = Transform3<S>::Identity();
4626  contacts.resize(1);
4627  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4628 
4629  tf1 = transform;
4630  tf2 = transform;
4631  contacts.resize(1);
4632  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4633 
4634  tf1 = Transform3<S>::Identity();
4635  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
4636  contacts.resize(1);
4637  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4638 
4639  tf1 = transform;
4640  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
4641  contacts.resize(1);
4642  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4643 
4644  tf1 = Transform3<S>::Identity();
4645  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30, 0, 0)));
4646  contacts.resize(1);
4647  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4648 
4649  tf1 = transform;
4650  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
4651  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4652 }
4653 
4654 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_ellipsoidellipsoid)
4655 {
4656 // test_shapeIntersectionGJK_ellipsoidellipsoid<float>();
4657  test_shapeIntersectionGJK_ellipsoidellipsoid<double>();
4658 }
4659 
4660 template <typename S>
4661 void test_shapeIntersectionGJK_spheretriangle()
4662 {
4663  Sphere<S> s(10);
4664  Vector3<S> t[3];
4665  t[0] << 20, 0, 0;
4666  t[1] << -20, 0, 0;
4667  t[2] << 0, 20, 0;
4668 
4669  Transform3<S> transform = Transform3<S>::Identity();
4670  test::generateRandomTransform(extents<S>(), transform);
4671 
4672  // Vector3<S> point;
4673  // S depth;
4674  Vector3<S> normal;
4675  bool res;
4676 
4677  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
4678  EXPECT_TRUE(res);
4679 
4680  res = solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4681  EXPECT_TRUE(res);
4682 
4683  t[0] << 30, 0, 0;
4684  t[1] << 9.9, -20, 0;
4685  t[2] << 9.9, 20, 0;
4686  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
4687  EXPECT_TRUE(res);
4688 
4689  res = solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4690  EXPECT_TRUE(res);
4691 
4692  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, &normal);
4693  EXPECT_TRUE(res);
4694  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
4695 
4696  res = solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
4697  EXPECT_TRUE(res);
4698  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
4699 }
4700 
4701 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheretriangle)
4702 {
4703 // test_shapeIntersectionGJK_spheretriangle<float>();
4704  test_shapeIntersectionGJK_spheretriangle<double>();
4705 }
4706 
4707 template <typename S>
4708 void test_shapeIntersectionGJK_halfspacetriangle()
4709 {
4710  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
4711  Vector3<S> t[3];
4712  t[0] << 20, 0, 0;
4713  t[1] << -20, 0, 0;
4714  t[2] << 0, 20, 0;
4715 
4716  Transform3<S> transform = Transform3<S>::Identity();
4717  test::generateRandomTransform(extents<S>(), transform);
4718 
4719  // Vector3<S> point;
4720  // S depth;
4721  Vector3<S> normal;
4722  bool res;
4723 
4724  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
4725  EXPECT_TRUE(res);
4726 
4727  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4728  EXPECT_TRUE(res);
4729 
4730 
4731  t[0] << 20, 0, 0;
4732  t[1] << -0.1, -20, 0;
4733  t[2] << -0.1, 20, 0;
4734  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
4735  EXPECT_TRUE(res);
4736 
4737  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4738  EXPECT_TRUE(res);
4739 
4740  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
4741  EXPECT_TRUE(res);
4742  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
4743 
4744  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
4745  EXPECT_TRUE(res);
4746  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
4747 }
4748 
4749 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_halfspacetriangle)
4750 {
4751 // test_shapeIntersectionGJK_halfspacetriangle<float>();
4752  test_shapeIntersectionGJK_halfspacetriangle<double>();
4753 }
4754 
4755 template <typename S>
4756 void test_shapeIntersectionGJK_planetriangle()
4757 {
4758  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
4759  Vector3<S> t[3];
4760  t[0] << 20, 0, 0;
4761  t[1] << -20, 0, 0;
4762  t[2] << 0, 20, 0;
4763 
4764  Transform3<S> transform = Transform3<S>::Identity();
4765  test::generateRandomTransform(extents<S>(), transform);
4766 
4767  // Vector3<S> point;
4768  // S depth;
4769  Vector3<S> normal;
4770  bool res;
4771 
4772  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
4773  EXPECT_TRUE(res);
4774 
4775  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4776  EXPECT_TRUE(res);
4777 
4778 
4779  t[0] << 20, 0, 0;
4780  t[1] << -0.1, -20, 0;
4781  t[2] << -0.1, 20, 0;
4782  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
4783  EXPECT_TRUE(res);
4784 
4785  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4786  EXPECT_TRUE(res);
4787 
4788  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
4789  EXPECT_TRUE(res);
4790  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
4791 
4792  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
4793  EXPECT_TRUE(res);
4794  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
4795 }
4796 
4797 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_planetriangle)
4798 {
4799 // test_shapeIntersectionGJK_planetriangle<float>();
4800  test_shapeIntersectionGJK_planetriangle<double>();
4801 }
4802 
4803 // Shape distance test coverage (built-in GJK)
4804 //
4805 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4806 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
4807 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4808 // | box | O | O | | | | | | | |
4809 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4810 // | sphere |/////| O | | | | | | | |
4811 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4812 // | ellipsoid |/////|////////| O | | | | | | |
4813 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4814 // | capsule |/////|////////|///////////| | | | | | |
4815 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4816 // | cone |/////|////////|///////////|/////////| O | | | | |
4817 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4818 // | cylinder |/////|////////|///////////|/////////|//////| O | | | |
4819 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4820 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
4821 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4822 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
4823 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4824 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
4825 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4826 
4827 template <typename S>
4828 void test_shapeDistanceGJK_spheresphere()
4829 {
4830  Sphere<S> s1(20);
4831  Sphere<S> s2(10);
4832 
4833  Transform3<S> transform = Transform3<S>::Identity();
4834  test::generateRandomTransform(extents<S>(), transform);
4835 
4836  bool res;
4837  S dist = -1;
4838 
4839  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
4840  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4841  EXPECT_TRUE(res);
4842 
4843  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
4844  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4845  EXPECT_TRUE(res);
4846 
4847  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
4848  EXPECT_TRUE(dist < 0);
4849  EXPECT_FALSE(res);
4850 
4851  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4852  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4853  EXPECT_TRUE(res);
4854 
4855  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4856  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4857  EXPECT_TRUE(res);
4858 
4859  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4860  EXPECT_TRUE(dist < 0);
4861  EXPECT_FALSE(res);
4862 
4863 
4864  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
4865  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4866  EXPECT_TRUE(res);
4867 
4868  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
4869  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4870  EXPECT_TRUE(res);
4871 
4872  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
4873  EXPECT_TRUE(dist < 0);
4874  EXPECT_FALSE(res);
4875 
4876  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
4877  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4878  EXPECT_TRUE(res);
4879 
4880  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
4881  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4882  EXPECT_TRUE(res);
4883 
4884  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
4885  EXPECT_TRUE(dist < 0);
4886  EXPECT_FALSE(res);
4887 }
4888 
4889 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_spheresphere)
4890 {
4891 // test_shapeDistanceGJK_spheresphere<float>();
4892  test_shapeDistanceGJK_spheresphere<double>();
4893 }
4894 
4895 template <typename S>
4896 void test_shapeDistanceGJK_boxbox()
4897 {
4898  Box<S> s1(20, 40, 50);
4899  Box<S> s2(10, 10, 10);
4900 
4901  Transform3<S> transform = Transform3<S>::Identity();
4902  test::generateRandomTransform(extents<S>(), transform);
4903 
4904  bool res;
4905  S dist;
4906 
4907  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
4908  EXPECT_TRUE(dist < 0);
4909  EXPECT_FALSE(res);
4910 
4911  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
4912  EXPECT_TRUE(dist < 0);
4913  EXPECT_FALSE(res);
4914 
4915  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
4916  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4917  EXPECT_TRUE(res);
4918 
4919  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
4920  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4921  EXPECT_TRUE(res);
4922 
4923  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
4924  EXPECT_TRUE(fabs(dist - 5) < 0.001);
4925  EXPECT_TRUE(res);
4926 
4927  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
4928  EXPECT_TRUE(fabs(dist - 5) < 0.001);
4929  EXPECT_TRUE(res);
4930 }
4931 
4932 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxbox)
4933 {
4934 // test_shapeDistanceGJK_boxbox<float>();
4935  test_shapeDistanceGJK_boxbox<double>();
4936 }
4937 
4938 template <typename S>
4939 void test_shapeDistanceGJK_boxsphere()
4940 {
4941  Sphere<S> s1(20);
4942  Box<S> s2(5, 5, 5);
4943 
4944  Transform3<S> transform = Transform3<S>::Identity();
4945  test::generateRandomTransform(extents<S>(), transform);
4946 
4947  bool res;
4948  S dist;
4949 
4950  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
4951