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  // Create an arbitrary transform. It is worth noting that for other rotations
981  // the tolerance on comparisons below is *not* necessarily sufficient.
982  Transform3<S> transform = Transform3<S>::Identity();
983  Matrix3<S> R;
984  test::eulerToMatrix(0.25, -0.7, 0.4, R);
985  transform.linear() = R;
986  transform.translation() << 8.40188, 3.94383, 7.83099;
987 
988  std::vector<ContactPoint<S>> contacts;
989 
990  tf1 = Transform3<S>::Identity();
991  tf2 = Transform3<S>::Identity();
992  // TODO: Need convention for normal when the centers of two objects are at same position.
993  contacts.resize(1);
994  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
995 
996  tf1 = transform;
997  tf2 = transform;
998  // TODO: Need convention for normal when the centers of two objects are at same position.
999  contacts.resize(1);
1000  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1001 
1002  tf1 = Transform3<S>::Identity();
1003  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1004  contacts.resize(1);
1005  contacts[0].normal << 1, 0, 0;
1006  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
1007 
1008  // Disabled for particular configurations: macOS + release + double (see #202)
1009 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
1010  // This test seems to be quite sensitive to the frame basis. For some
1011  // orientations, the tolerance of 1e-5 is simply too small.
1012  tf1 = transform;
1013  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1014  contacts.resize(1);
1015  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1016  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5);
1017 #endif
1018 
1019  tf1 = Transform3<S>::Identity();
1020  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
1021  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1022 
1023  tf1 = transform;
1024  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
1025  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1026 }
1027 
1028 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercylinder)
1029 {
1030 // test_shapeIntersection_cylindercylinder<float>();
1031  test_shapeIntersection_cylindercylinder<double>();
1032 }
1033 
1034 template <typename S>
1035 void test_shapeIntersection_conecone()
1036 {
1037  Cone<S> s1(5, 10);
1038  Cone<S> s2(5, 10);
1039 
1040  Transform3<S> tf1;
1041  Transform3<S> tf2;
1042 
1043  Transform3<S> transform = Transform3<S>::Identity();
1044  test::generateRandomTransform(extents<S>(), transform);
1045 
1046  std::vector<ContactPoint<S>> contacts;
1047 
1048  tf1 = Transform3<S>::Identity();
1049  tf2 = Transform3<S>::Identity();
1050  // TODO: Need convention for normal when the centers of two objects are at same position.
1051  contacts.resize(1);
1052  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1053 
1054  tf1 = transform;
1055  tf2 = transform;
1056  // TODO: Need convention for normal when the centers of two objects are at same position.
1057  contacts.resize(1);
1058  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1059 
1060  tf1 = Transform3<S>::Identity();
1061  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1062  contacts.resize(1);
1063  contacts[0].normal << 1, 0, 0;
1064  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
1065 
1066  tf1 = transform;
1067  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1068  contacts.resize(1);
1069  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1070  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 5e-5);
1071 
1072  tf1 = Transform3<S>::Identity();
1073  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.001, 0, 0)));
1074  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1075 
1076  tf1 = transform;
1077  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.001, 0, 0)));
1078  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1079 
1080  tf1 = Transform3<S>::Identity();
1081  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
1082  contacts.resize(1);
1083  contacts[0].normal << 0, 0, 1;
1084  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
1085 
1086  tf1 = transform;
1087  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
1088  contacts.resize(1);
1089  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
1090  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5);
1091 }
1092 
1093 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_conecone)
1094 {
1095 // test_shapeIntersection_conecone<float>();
1096  test_shapeIntersection_conecone<double>();
1097 }
1098 
1099 template <typename S>
1100 void test_shapeIntersection_cylindercone()
1101 {
1102  Cylinder<S> s1(5, 10);
1103  Cone<S> s2(5, 10);
1104 
1105  Transform3<S> tf1;
1106  Transform3<S> tf2;
1107 
1108  Transform3<S> transform = Transform3<S>::Identity();
1109  test::generateRandomTransform(extents<S>(), transform);
1110 
1111  std::vector<ContactPoint<S>> contacts;
1112 
1113  tf1 = Transform3<S>::Identity();
1114  tf2 = Transform3<S>::Identity();
1115  // TODO: Need convention for normal when the centers of two objects are at same position.
1116  contacts.resize(1);
1117  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1118 
1119  tf1 = transform;
1120  tf2 = transform;
1121  // TODO: Need convention for normal when the centers of two objects are at same position.
1122  contacts.resize(1);
1123  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1124 
1125  tf1 = Transform3<S>::Identity();
1126  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1127  contacts.resize(1);
1128  contacts[0].normal << 1, 0, 0;
1129  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.061);
1130 
1131  tf1 = transform;
1132  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
1133  contacts.resize(1);
1134  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1135  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.46);
1136 
1137  tf1 = Transform3<S>::Identity();
1138  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
1139  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1140 
1141  tf1 = transform;
1142  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
1143  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1144 
1145  tf1 = Transform3<S>::Identity();
1146  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
1147  contacts.resize(1);
1148  contacts[0].normal << 0, 0, 1;
1149  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);
1150 
1151  tf1 = transform;
1152  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
1153  contacts.resize(1);
1154  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
1155  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-4);
1156 
1157  tf1 = Transform3<S>::Identity();
1158  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.01)));
1159  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1160 
1161  tf1 = transform;
1162  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.01)));
1163  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1164 }
1165 
1166 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercone)
1167 {
1168 // test_shapeIntersection_cylindercone<float>();
1169  test_shapeIntersection_cylindercone<double>();
1170 }
1171 
1172 template <typename S>
1173 void test_shapeIntersection_ellipsoidellipsoid()
1174 {
1175  Ellipsoid<S> s1(20, 40, 50);
1176  Ellipsoid<S> s2(10, 10, 10);
1177 
1178  Transform3<S> tf1;
1179  Transform3<S> tf2;
1180 
1181  Transform3<S> transform = Transform3<S>::Identity();
1182  test::generateRandomTransform(extents<S>(), transform);
1183  Transform3<S> identity;
1184 
1185  std::vector<ContactPoint<S>> contacts;
1186 
1187  tf1 = Transform3<S>::Identity();
1188  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
1189  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1190 
1191  tf1 = transform;
1192  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
1193  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1194 
1195  tf1 = Transform3<S>::Identity();
1196  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
1197  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1198 
1199  tf1 = transform;
1200  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
1201  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1202 
1203  tf1 = Transform3<S>::Identity();
1204  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.99, 0, 0)));
1205  contacts.resize(1);
1206  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1207 
1208  tf1 = transform;
1209  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
1210  contacts.resize(1);
1211  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1212 
1213  tf1 = Transform3<S>::Identity();
1214  tf2 = Transform3<S>::Identity();
1215  contacts.resize(1);
1216  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1217 
1218  tf1 = transform;
1219  tf2 = transform;
1220  contacts.resize(1);
1221  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1222 
1223  tf1 = Transform3<S>::Identity();
1224  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
1225  contacts.resize(1);
1226  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1227 
1228  tf1 = transform;
1229  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
1230  contacts.resize(1);
1231  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
1232 
1233  tf1 = Transform3<S>::Identity();
1234  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30, 0, 0)));
1235  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1236 
1237  tf1 = transform;
1238  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
1239  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
1240 }
1241 
1242 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_ellipsoidellipsoid)
1243 {
1244 // test_shapeIntersection_ellipsoidellipsoid<float>();
1245  test_shapeIntersection_ellipsoidellipsoid<double>();
1246 }
1247 
1248 template <typename S>
1249 void test_shapeIntersection_spheretriangle()
1250 {
1251  Sphere<S> s(10);
1252  Vector3<S> t[3];
1253  t[0] << 20, 0, 0;
1254  t[1] << -20, 0, 0;
1255  t[2] << 0, 20, 0;
1256 
1257  Transform3<S> transform = Transform3<S>::Identity();
1258  test::generateRandomTransform(extents<S>(), transform);
1259 
1260  Vector3<S> normal;
1261  bool res;
1262 
1263  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
1264  EXPECT_TRUE(res);
1265 
1266  res = solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1267  EXPECT_TRUE(res);
1268 
1269 
1270  t[0] << 30, 0, 0;
1271  t[1] << 9.9, -20, 0;
1272  t[2] << 9.9, 20, 0;
1273  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
1274  EXPECT_TRUE(res);
1275 
1276  res = solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1277  EXPECT_TRUE(res);
1278 
1279  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, &normal);
1280  EXPECT_TRUE(res);
1281  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
1282 
1283  res = solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
1284  EXPECT_TRUE(res);
1285  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
1286 }
1287 
1288 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheretriangle)
1289 {
1290 // test_shapeIntersection_spheretriangle<float>();
1291  test_shapeIntersection_spheretriangle<double>();
1292 }
1293 
1294 template <typename S>
1295 void test_shapeIntersection_halfspacetriangle()
1296 {
1297  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
1298  Vector3<S> t[3];
1299  t[0] << 20, 0, 0;
1300  t[1] << -20, 0, 0;
1301  t[2] << 0, 20, 0;
1302 
1303  Transform3<S> transform = Transform3<S>::Identity();
1304  test::generateRandomTransform(extents<S>(), transform);
1305 
1306  // Vector3<S> point;
1307  // S depth;
1308  Vector3<S> normal;
1309  bool res;
1310 
1311  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
1312  EXPECT_TRUE(res);
1313 
1314  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1315  EXPECT_TRUE(res);
1316 
1317 
1318  t[0] << 20, 0, 0;
1319  t[1] << 0, -20, 0;
1320  t[2] << 0, 20, 0;
1321  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
1322  EXPECT_TRUE(res);
1323 
1324  // Disabled for particular configurations: macOS + release + double (see #202)
1325 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
1326  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1327  EXPECT_TRUE(res);
1328 #endif
1329 
1330  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
1331  EXPECT_TRUE(res);
1332  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
1333 
1334  // Disabled for particular configurations: macOS + release + double (see #202)
1335 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
1336  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
1337  EXPECT_TRUE(res);
1338  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
1339 #endif
1340 }
1341 
1342 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacetriangle)
1343 {
1344 // test_shapeIntersection_halfspacetriangle<float>();
1345  test_shapeIntersection_halfspacetriangle<double>();
1346 }
1347 
1348 template <typename S>
1349 void test_shapeIntersection_planetriangle()
1350 {
1351  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
1352  Vector3<S> t[3];
1353  t[0] << 20, 0, 0;
1354  t[1] << -20, 0, 0;
1355  t[2] << 0, 20, 0;
1356 
1357  Transform3<S> transform = Transform3<S>::Identity();
1358  test::generateRandomTransform(extents<S>(), transform);
1359 
1360  // Vector3<S> point;
1361  // S depth;
1362  Vector3<S> normal;
1363  bool res;
1364 
1365  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
1366  EXPECT_TRUE(res);
1367 
1368  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1369  EXPECT_TRUE(res);
1370 
1371 
1372  t[0] << 20, 0, 0;
1373  t[1] << -0.1, -20, 0;
1374  t[2] << -0.1, 20, 0;
1375  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
1376  EXPECT_TRUE(res);
1377 
1378  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
1379  EXPECT_TRUE(res);
1380 
1381  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
1382  EXPECT_TRUE(res);
1383  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
1384 
1385  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
1386  EXPECT_TRUE(res);
1387  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
1388 }
1389 
1390 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planetriangle)
1391 {
1392 // test_shapeIntersection_planetriangle<float>();
1393  test_shapeIntersection_planetriangle<double>();
1394 }
1395 
1396 template <typename S>
1397 void test_shapeIntersection_halfspacesphere()
1398 {
1399  Sphere<S> s(10);
1400  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
1401 
1402  Transform3<S> tf1;
1403  Transform3<S> tf2;
1404 
1405  Transform3<S> transform = Transform3<S>::Identity();
1406  test::generateRandomTransform(extents<S>(), transform);
1407 
1408  std::vector<ContactPoint<S>> contacts;
1409 
1410  tf1 = Transform3<S>::Identity();
1411  tf2 = Transform3<S>::Identity();
1412  contacts.resize(1);
1413  contacts[0].pos << -5, 0, 0;
1414  contacts[0].penetration_depth = 10;
1415  contacts[0].normal << -1, 0, 0;
1416  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1417 
1418  tf1 = transform;
1419  tf2 = transform;
1420  contacts.resize(1);
1421  contacts[0].pos = transform * Vector3<S>(-5, 0, 0);
1422  contacts[0].penetration_depth = 10;
1423  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1424  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1425 
1426  tf1 = Transform3<S>::Identity();
1427  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
1428  contacts.resize(1);
1429  contacts[0].pos << -2.5, 0, 0;
1430  contacts[0].penetration_depth = 15;
1431  contacts[0].normal << -1, 0, 0;
1432  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1433 
1434  tf1 = transform;
1435  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
1436  contacts.resize(1);
1437  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
1438  contacts[0].penetration_depth = 15;
1439  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1440  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1441 
1442  tf1 = Transform3<S>::Identity();
1443  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
1444  contacts.resize(1);
1445  contacts[0].pos << -7.5, 0, 0;
1446  contacts[0].penetration_depth = 5;
1447  contacts[0].normal << -1, 0, 0;
1448  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1449 
1450  tf1 = transform;
1451  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
1452  contacts.resize(1);
1453  contacts[0].pos = transform * Vector3<S>(-7.5, 0, 0);
1454  contacts[0].penetration_depth = 5;
1455  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1456  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1457 
1458  tf1 = Transform3<S>::Identity();
1459  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
1460  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1461 
1462  tf1 = transform;
1463  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
1464  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1465 
1466  tf1 = Transform3<S>::Identity();
1467  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
1468  contacts.resize(1);
1469  contacts[0].pos << 0.05, 0, 0;
1470  contacts[0].penetration_depth = 20.1;
1471  contacts[0].normal << -1, 0, 0;
1472  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1473 
1474  tf1 = transform;
1475  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
1476  contacts.resize(1);
1477  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
1478  contacts[0].penetration_depth = 20.1;
1479  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1480  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1481 }
1482 
1483 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere)
1484 {
1485 // test_shapeIntersection_halfspacesphere<float>();
1486  test_shapeIntersection_halfspacesphere<double>();
1487 }
1488 
1489 template <typename S>
1490 void test_shapeIntersection_planesphere()
1491 {
1492  Sphere<S> s(10);
1493  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
1494 
1495  Transform3<S> tf1;
1496  Transform3<S> tf2;
1497 
1498  Transform3<S> transform = Transform3<S>::Identity();
1499  test::generateRandomTransform(extents<S>(), transform);
1500 
1501  std::vector<ContactPoint<S>> contacts;
1502 
1503  tf1 = Transform3<S>::Identity();
1504  tf2 = Transform3<S>::Identity();
1505  contacts.resize(1);
1506  contacts[0].pos.setZero();
1507  contacts[0].penetration_depth = 10;
1508  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
1509  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
1510 
1511  tf1 = transform;
1512  tf2 = transform;
1513  contacts.resize(1);
1514  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
1515  contacts[0].penetration_depth = 10;
1516  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
1517  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
1518 
1519  tf1 = Transform3<S>::Identity();
1520  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
1521  contacts.resize(1);
1522  contacts[0].pos << 5, 0, 0;
1523  contacts[0].penetration_depth = 5;
1524  contacts[0].normal << 1, 0, 0;
1525  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1526 
1527  tf1 = transform;
1528  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
1529  contacts.resize(1);
1530  contacts[0].pos = transform * Vector3<S>(5, 0, 0);
1531  contacts[0].penetration_depth = 5;
1532  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1533  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1534 
1535  tf1 = Transform3<S>::Identity();
1536  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
1537  contacts.resize(1);
1538  contacts[0].pos << -5, 0, 0;
1539  contacts[0].penetration_depth = 5;
1540  contacts[0].normal << -1, 0, 0;
1541  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1542 
1543  tf1 = transform;
1544  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
1545  contacts.resize(1);
1546  contacts[0].pos = transform * Vector3<S>(-5, 0, 0);
1547  contacts[0].penetration_depth = 5;
1548  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1549  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1550 
1551  tf1 = Transform3<S>::Identity();
1552  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
1553  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1554 
1555  tf1 = transform;
1556  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
1557  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1558 
1559  tf1 = Transform3<S>::Identity();
1560  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
1561  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1562 
1563  tf1 = transform;
1564  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
1565  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1566 }
1567 
1568 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planesphere)
1569 {
1570 // test_shapeIntersection_planesphere<float>();
1571  test_shapeIntersection_planesphere<double>();
1572 }
1573 
1574 template <typename S>
1575 void test_shapeIntersection_halfspacebox()
1576 {
1577  Box<S> s(5, 10, 20);
1578  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
1579 
1580  Transform3<S> tf1;
1581  Transform3<S> tf2;
1582 
1583  Transform3<S> transform = Transform3<S>::Identity();
1584  test::generateRandomTransform(extents<S>(), transform);
1585 
1586  std::vector<ContactPoint<S>> contacts;
1587 
1588  tf1 = Transform3<S>::Identity();
1589  tf2 = Transform3<S>::Identity();
1590  contacts.resize(1);
1591  contacts[0].pos << -1.25, 0, 0;
1592  contacts[0].penetration_depth = 2.5;
1593  contacts[0].normal << -1, 0, 0;
1594  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1595 
1596  tf1 = transform;
1597  tf2 = transform;
1598  contacts.resize(1);
1599  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
1600  contacts[0].penetration_depth = 2.5;
1601  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1602  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1603 
1604  tf1 = Transform3<S>::Identity();
1605  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1606  contacts.resize(1);
1607  contacts[0].pos << -0.625, 0, 0;
1608  contacts[0].penetration_depth = 3.75;
1609  contacts[0].normal << -1, 0, 0;
1610  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1611 
1612  tf1 = transform;
1613  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1614  contacts.resize(1);
1615  contacts[0].pos = transform * Vector3<S>(-0.625, 0, 0);
1616  contacts[0].penetration_depth = 3.75;
1617  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1618  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1619 
1620  tf1 = Transform3<S>::Identity();
1621  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1622  contacts.resize(1);
1623  contacts[0].pos << -1.875, 0, 0;
1624  contacts[0].penetration_depth = 1.25;
1625  contacts[0].normal << -1, 0, 0;
1626  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1627 
1628  tf1 = transform;
1629  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1630  contacts.resize(1);
1631  contacts[0].pos = transform * Vector3<S>(-1.875, 0, 0);
1632  contacts[0].penetration_depth = 1.25;
1633  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1634  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1635 
1636  tf1 = Transform3<S>::Identity();
1637  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
1638  contacts.resize(1);
1639  contacts[0].pos << 0.005, 0, 0;
1640  contacts[0].penetration_depth = 5.01;
1641  contacts[0].normal << -1, 0, 0;
1642  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1643 
1644  tf1 = transform;
1645  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
1646  contacts.resize(1);
1647  contacts[0].pos = transform * Vector3<S>(0.005, 0, 0);
1648  contacts[0].penetration_depth = 5.01;
1649  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1650  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1651 
1652  tf1 = Transform3<S>::Identity();
1653  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
1654  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1655 
1656  tf1 = transform;
1657  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
1658  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1659 
1660  tf1 = Transform3<S>(transform.linear());
1661  tf2 = Transform3<S>::Identity();
1662  contacts.resize(1);
1663  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
1664 }
1665 
1666 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox)
1667 {
1668 // test_shapeIntersection_halfspacebox<float>();
1669  test_shapeIntersection_halfspacebox<double>();
1670 }
1671 
1672 template <typename S>
1673 void test_shapeIntersection_planebox()
1674 {
1675  Box<S> s(5, 10, 20);
1676  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
1677 
1678  Transform3<S> tf1;
1679  Transform3<S> tf2;
1680 
1681  Transform3<S> transform = Transform3<S>::Identity();
1682  test::generateRandomTransform(extents<S>(), transform);
1683 
1684  std::vector<ContactPoint<S>> contacts;
1685 
1686  tf1 = Transform3<S>::Identity();
1687  tf2 = Transform3<S>::Identity();
1688  contacts.resize(1);
1689  contacts[0].pos << 0, 0, 0;
1690  contacts[0].penetration_depth = 2.5;
1691  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
1692  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
1693 
1694  tf1 = transform;
1695  tf2 = transform;
1696  contacts.resize(1);
1697  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
1698  contacts[0].penetration_depth = 2.5;
1699  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
1700  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
1701 
1702  tf1 = Transform3<S>::Identity();
1703  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1704  contacts.resize(1);
1705  contacts[0].pos << 1.25, 0, 0;
1706  contacts[0].penetration_depth = 1.25;
1707  contacts[0].normal << 1, 0, 0;
1708  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1709 
1710  tf1 = transform;
1711  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1712  contacts.resize(1);
1713  contacts[0].pos = transform * Vector3<S>(1.25, 0, 0);
1714  contacts[0].penetration_depth = 1.25;
1715  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
1716  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1717 
1718  tf1 = Transform3<S>::Identity();
1719  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1720  contacts.resize(1);
1721  contacts[0].pos << -1.25, 0, 0;
1722  contacts[0].penetration_depth = 1.25;
1723  contacts[0].normal << -1, 0, 0;
1724  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1725 
1726  tf1 = transform;
1727  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1728  contacts.resize(1);
1729  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
1730  contacts[0].penetration_depth = 1.25;
1731  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1732  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1733 
1734  tf1 = Transform3<S>::Identity();
1735  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
1736  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1737 
1738  tf1 = transform;
1739  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
1740  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1741 
1742  tf1 = Transform3<S>::Identity();
1743  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
1744  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1745 
1746  tf1 = transform;
1747  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
1748  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1749 
1750  tf1 = Transform3<S>(transform.linear());
1751  tf2 = Transform3<S>::Identity();
1752  contacts.resize(1);
1753  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
1754 }
1755 
1756 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planebox)
1757 {
1758 // test_shapeIntersection_planebox<float>();
1759  test_shapeIntersection_planebox<double>();
1760 }
1761 
1762 template <typename S>
1763 void test_shapeIntersection_halfspaceellipsoid()
1764 {
1765  Ellipsoid<S> s(5, 10, 20);
1766  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
1767 
1768  Transform3<S> tf1;
1769  Transform3<S> tf2;
1770 
1771  Transform3<S> transform = Transform3<S>::Identity();
1772  test::generateRandomTransform(extents<S>(), transform);
1773 
1774  std::vector<ContactPoint<S>> contacts;
1775 
1776  tf1 = Transform3<S>::Identity();
1777  tf2 = Transform3<S>::Identity();
1778  contacts.resize(1);
1779  contacts[0].pos << -2.5, 0, 0;
1780  contacts[0].penetration_depth = 5.0;
1781  contacts[0].normal << -1, 0, 0;
1782  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1783 
1784  tf1 = transform;
1785  tf2 = transform;
1786  contacts.resize(1);
1787  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
1788  contacts[0].penetration_depth = 5.0;
1789  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1790  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1791 
1792  tf1 = Transform3<S>::Identity();
1793  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1794  contacts.resize(1);
1795  contacts[0].pos << -1.875, 0, 0;
1796  contacts[0].penetration_depth = 6.25;
1797  contacts[0].normal << -1, 0, 0;
1798  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1799 
1800  tf1 = transform;
1801  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
1802  contacts.resize(1);
1803  contacts[0].pos = transform * Vector3<S>(-1.875, 0, 0);
1804  contacts[0].penetration_depth = 6.25;
1805  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1806  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1807 
1808  tf1 = Transform3<S>::Identity();
1809  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1810  contacts.resize(1);
1811  contacts[0].pos << -3.125, 0, 0;
1812  contacts[0].penetration_depth = 3.75;
1813  contacts[0].normal << -1, 0, 0;
1814  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1815 
1816  tf1 = transform;
1817  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
1818  contacts.resize(1);
1819  contacts[0].pos = transform * Vector3<S>(-3.125, 0, 0);
1820  contacts[0].penetration_depth = 3.75;
1821  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1822  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1823 
1824  tf1 = Transform3<S>::Identity();
1825  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
1826  contacts.resize(1);
1827  contacts[0].pos << 0.005, 0, 0;
1828  contacts[0].penetration_depth = 10.01;
1829  contacts[0].normal << -1, 0, 0;
1830  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1831 
1832  tf1 = transform;
1833  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
1834  contacts.resize(1);
1835  contacts[0].pos = transform * Vector3<S>(0.005, 0, 0);
1836  contacts[0].penetration_depth = 10.01;
1837  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
1838  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1839 
1840  tf1 = Transform3<S>::Identity();
1841  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
1842  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1843 
1844  tf1 = transform;
1845  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
1846  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1847 
1848 
1849 
1850 
1851  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);
1852 
1853  tf1 = Transform3<S>::Identity();
1854  tf2 = Transform3<S>::Identity();
1855  contacts.resize(1);
1856  contacts[0].pos << 0, -5.0, 0;
1857  contacts[0].penetration_depth = 10.0;
1858  contacts[0].normal << 0, -1, 0;
1859  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1860 
1861  tf1 = transform;
1862  tf2 = transform;
1863  contacts.resize(1);
1864  contacts[0].pos = transform * Vector3<S>(0, -5.0, 0);
1865  contacts[0].penetration_depth = 10.0;
1866  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
1867  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1868 
1869  tf1 = Transform3<S>::Identity();
1870  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
1871  contacts.resize(1);
1872  contacts[0].pos << 0, -4.375, 0;
1873  contacts[0].penetration_depth = 11.25;
1874  contacts[0].normal << 0, -1, 0;
1875  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1876 
1877  tf1 = transform;
1878  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
1879  contacts.resize(1);
1880  contacts[0].pos = transform * Vector3<S>(0, -4.375, 0);
1881  contacts[0].penetration_depth = 11.25;
1882  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
1883  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1884 
1885  tf1 = Transform3<S>::Identity();
1886  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
1887  contacts.resize(1);
1888  contacts[0].pos << 0, -5.625, 0;
1889  contacts[0].penetration_depth = 8.75;
1890  contacts[0].normal << 0, -1, 0;
1891  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1892 
1893  tf1 = transform;
1894  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
1895  contacts.resize(1);
1896  contacts[0].pos = transform * Vector3<S>(0, -5.625, 0);
1897  contacts[0].penetration_depth = 8.75;
1898  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
1899  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1900 
1901  tf1 = Transform3<S>::Identity();
1902  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
1903  contacts.resize(1);
1904  contacts[0].pos << 0, 0.005, 0;
1905  contacts[0].penetration_depth = 20.01;
1906  contacts[0].normal << 0, -1, 0;
1907  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1908 
1909  tf1 = transform;
1910  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
1911  contacts.resize(1);
1912  contacts[0].pos = transform * Vector3<S>(0, 0.005, 0);
1913  contacts[0].penetration_depth = 20.01;
1914  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
1915  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1916 
1917  tf1 = Transform3<S>::Identity();
1918  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
1919  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1920 
1921  tf1 = transform;
1922  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
1923  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1924 
1925 
1926 
1927 
1928  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);
1929 
1930  tf1 = Transform3<S>::Identity();
1931  tf2 = Transform3<S>::Identity();
1932  contacts.resize(1);
1933  contacts[0].pos << 0, 0, -10.0;
1934  contacts[0].penetration_depth = 20.0;
1935  contacts[0].normal << 0, 0, -1;
1936  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1937 
1938  tf1 = transform;
1939  tf2 = transform;
1940  contacts.resize(1);
1941  contacts[0].pos = transform * Vector3<S>(0, 0, -10.0);
1942  contacts[0].penetration_depth = 20.0;
1943  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
1944  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1945 
1946  tf1 = Transform3<S>::Identity();
1947  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
1948  contacts.resize(1);
1949  contacts[0].pos << 0, 0, -9.375;
1950  contacts[0].penetration_depth = 21.25;
1951  contacts[0].normal << 0, 0, -1;
1952  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1953 
1954  tf1 = transform;
1955  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
1956  contacts.resize(1);
1957  contacts[0].pos = transform * Vector3<S>(0, 0, -9.375);
1958  contacts[0].penetration_depth = 21.25;
1959  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
1960  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1961 
1962  tf1 = Transform3<S>::Identity();
1963  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
1964  contacts.resize(1);
1965  contacts[0].pos << 0, 0, -10.625;
1966  contacts[0].penetration_depth = 18.75;
1967  contacts[0].normal << 0, 0, -1;
1968  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1969 
1970  tf1 = transform;
1971  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
1972  contacts.resize(1);
1973  contacts[0].pos = transform * Vector3<S>(0, 0, -10.625);
1974  contacts[0].penetration_depth = 18.75;
1975  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
1976  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1977 
1978  tf1 = Transform3<S>::Identity();
1979  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
1980  contacts.resize(1);
1981  contacts[0].pos << 0, 0, 0.005;
1982  contacts[0].penetration_depth = 40.01;
1983  contacts[0].normal << 0, 0, -1;
1984  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1985 
1986  tf1 = transform;
1987  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
1988  contacts.resize(1);
1989  contacts[0].pos = transform * Vector3<S>(0, 0, 0.005);
1990  contacts[0].penetration_depth = 40.01;
1991  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
1992  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
1993 
1994  tf1 = Transform3<S>::Identity();
1995  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
1996  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
1997 
1998  tf1 = transform;
1999  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
2000  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2001 }
2002 
2003 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid)
2004 {
2005 // test_shapeIntersection_halfspaceellipsoid<float>();
2006  test_shapeIntersection_halfspaceellipsoid<double>();
2007 }
2008 
2009 template <typename S>
2010 void test_shapeIntersection_planeellipsoid()
2011 {
2012  Ellipsoid<S> s(5, 10, 20);
2013  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
2014 
2015  Transform3<S> tf1;
2016  Transform3<S> tf2;
2017 
2018  Transform3<S> transform = Transform3<S>::Identity();
2019  test::generateRandomTransform(extents<S>(), transform);
2020 
2021  std::vector<ContactPoint<S>> contacts;
2022 
2023  tf1 = Transform3<S>::Identity();
2024  tf2 = Transform3<S>::Identity();
2025  contacts.resize(1);
2026  contacts[0].pos << 0, 0, 0;
2027  contacts[0].penetration_depth = 5.0;
2028  contacts[0].normal << -1, 0, 0;
2029  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2030 
2031  tf1 = transform;
2032  tf2 = transform;
2033  contacts.resize(1);
2034  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2035  contacts[0].penetration_depth = 5.0;
2036  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2037  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2038 
2039  tf1 = Transform3<S>::Identity();
2040  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
2041  contacts.resize(1);
2042  contacts[0].pos << 1.25, 0, 0;
2043  contacts[0].penetration_depth = 3.75;
2044  contacts[0].normal << 1, 0, 0;
2045  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2046 
2047  tf1 = transform;
2048  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
2049  contacts.resize(1);
2050  contacts[0].pos = transform * Vector3<S>(1.25, 0, 0);
2051  contacts[0].penetration_depth = 3.75;
2052  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
2053  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2054 
2055  tf1 = Transform3<S>::Identity();
2056  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
2057  contacts.resize(1);
2058  contacts[0].pos << -1.25, 0, 0;
2059  contacts[0].penetration_depth = 3.75;
2060  contacts[0].normal << -1, 0, 0;
2061  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2062 
2063  tf1 = transform;
2064  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
2065  contacts.resize(1);
2066  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
2067  contacts[0].penetration_depth = 3.75;
2068  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2069  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2070 
2071  tf1 = Transform3<S>::Identity();
2072  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
2073  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2074 
2075  tf1 = transform;
2076  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
2077  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2078 
2079  tf1 = Transform3<S>::Identity();
2080  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
2081  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2082 
2083  tf1 = transform;
2084  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
2085  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2086 
2087 
2088 
2089 
2090  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);
2091 
2092  tf1 = Transform3<S>::Identity();
2093  tf2 = Transform3<S>::Identity();
2094  contacts.resize(1);
2095  contacts[0].pos << 0, 0.0, 0;
2096  contacts[0].penetration_depth = 10.0;
2097  contacts[0].normal << 0, -1, 0;
2098  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2099 
2100  tf1 = transform;
2101  tf2 = transform;
2102  contacts.resize(1);
2103  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2104  contacts[0].penetration_depth = 10.0;
2105  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2106  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2107 
2108  tf1 = Transform3<S>::Identity();
2109  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
2110  contacts.resize(1);
2111  contacts[0].pos << 0, 1.25, 0;
2112  contacts[0].penetration_depth = 8.75;
2113  contacts[0].normal << 0, 1, 0;
2114  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2115 
2116  tf1 = transform;
2117  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
2118  contacts.resize(1);
2119  contacts[0].pos = transform * Vector3<S>(0, 1.25, 0);
2120  contacts[0].penetration_depth = 8.75;
2121  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
2122  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2123 
2124  tf1 = Transform3<S>::Identity();
2125  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
2126  contacts.resize(1);
2127  contacts[0].pos << 0, -1.25, 0;
2128  contacts[0].penetration_depth = 8.75;
2129  contacts[0].normal << 0, -1, 0;
2130  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2131 
2132  tf1 = transform;
2133  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
2134  contacts.resize(1);
2135  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
2136  contacts[0].penetration_depth = 8.75;
2137  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2138  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2139 
2140  tf1 = Transform3<S>::Identity();
2141  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
2142  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2143 
2144  tf1 = transform;
2145  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
2146  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2147 
2148  tf1 = Transform3<S>::Identity();
2149  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
2150  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2151 
2152  tf1 = transform;
2153  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
2154  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2155 
2156 
2157 
2158 
2159  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);
2160 
2161  tf1 = Transform3<S>::Identity();
2162  tf2 = Transform3<S>::Identity();
2163  contacts.resize(1);
2164  contacts[0].pos << 0, 0, 0;
2165  contacts[0].penetration_depth = 20.0;
2166  contacts[0].normal << 0, 0, -1;
2167  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2168 
2169  tf1 = transform;
2170  tf2 = transform;
2171  contacts.resize(1);
2172  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2173  contacts[0].penetration_depth = 20.0;
2174  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2175  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2176 
2177  tf1 = Transform3<S>::Identity();
2178  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
2179  contacts.resize(1);
2180  contacts[0].pos << 0, 0, 1.25;
2181  contacts[0].penetration_depth = 18.75;
2182  contacts[0].normal << 0, 0, 1;
2183  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2184 
2185  tf1 = transform;
2186  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
2187  contacts.resize(1);
2188  contacts[0].pos = transform * Vector3<S>(0, 0, 1.25);
2189  contacts[0].penetration_depth = 18.75;
2190  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
2191  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2192 
2193  tf1 = Transform3<S>::Identity();
2194  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
2195  contacts.resize(1);
2196  contacts[0].pos << 0, 0, -1.25;
2197  contacts[0].penetration_depth = 18.75;
2198  contacts[0].normal << 0, 0, -1;
2199  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2200 
2201  tf1 = transform;
2202  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
2203  contacts.resize(1);
2204  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
2205  contacts[0].penetration_depth = 18.75;
2206  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2207  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2208 
2209  tf1 = Transform3<S>::Identity();
2210  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
2211  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2212 
2213  tf1 = transform;
2214  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
2215  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2216 
2217  tf1 = Transform3<S>::Identity();
2218  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
2219  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2220 
2221  tf1 = transform;
2222  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
2223  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2224 }
2225 
2226 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid)
2227 {
2228 // test_shapeIntersection_planeellipsoid<float>();
2229  test_shapeIntersection_planeellipsoid<double>();
2230 }
2231 
2232 template <typename S>
2233 void test_shapeIntersection_halfspacecapsule()
2234 {
2235  Capsule<S> s(5, 10);
2236  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
2237 
2238  Transform3<S> tf1;
2239  Transform3<S> tf2;
2240 
2241  Transform3<S> transform = Transform3<S>::Identity();
2242  test::generateRandomTransform(extents<S>(), transform);
2243 
2244  std::vector<ContactPoint<S>> contacts;
2245 
2246  tf1 = Transform3<S>::Identity();
2247  tf2 = Transform3<S>::Identity();
2248  contacts.resize(1);
2249  contacts[0].pos << -2.5, 0, 0;
2250  contacts[0].penetration_depth = 5;
2251  contacts[0].normal << -1, 0, 0;
2252  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2253 
2254  tf1 = transform;
2255  tf2 = transform;
2256  contacts.resize(1);
2257  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
2258  contacts[0].penetration_depth = 5;
2259  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2260  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2261 
2262  tf1 = Transform3<S>::Identity();
2263  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2264  contacts.resize(1);
2265  contacts[0].pos << -1.25, 0, 0;
2266  contacts[0].penetration_depth = 7.5;
2267  contacts[0].normal << -1, 0, 0;
2268  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2269 
2270  tf1 = transform;
2271  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2272  contacts.resize(1);
2273  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
2274  contacts[0].penetration_depth = 7.5;
2275  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2276  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2277 
2278  tf1 = Transform3<S>::Identity();
2279  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2280  contacts.resize(1);
2281  contacts[0].pos << -3.75, 0, 0;
2282  contacts[0].penetration_depth = 2.5;
2283  contacts[0].normal << -1, 0, 0;
2284  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2285 
2286  tf1 = transform;
2287  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2288  contacts.resize(1);
2289  contacts[0].pos = transform * Vector3<S>(-3.75, 0, 0);
2290  contacts[0].penetration_depth = 2.5;
2291  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2292  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2293 
2294  tf1 = Transform3<S>::Identity();
2295  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2296  contacts.resize(1);
2297  contacts[0].pos << 0.05, 0, 0;
2298  contacts[0].penetration_depth = 10.1;
2299  contacts[0].normal << -1, 0, 0;
2300  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2301 
2302  tf1 = transform;
2303  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2304  contacts.resize(1);
2305  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
2306  contacts[0].penetration_depth = 10.1;
2307  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2308  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2309 
2310  tf1 = Transform3<S>::Identity();
2311  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2312  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2313 
2314  tf1 = transform;
2315  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2316  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2317 
2318 
2319 
2320 
2321  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);
2322 
2323  tf1 = Transform3<S>::Identity();
2324  tf2 = Transform3<S>::Identity();
2325  contacts.resize(1);
2326  contacts[0].pos << 0, -2.5, 0;
2327  contacts[0].penetration_depth = 5;
2328  contacts[0].normal << 0, -1, 0;
2329  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2330 
2331  tf1 = transform;
2332  tf2 = transform;
2333  contacts.resize(1);
2334  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
2335  contacts[0].penetration_depth = 5;
2336  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2337  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2338 
2339  tf1 = Transform3<S>::Identity();
2340  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2341  contacts.resize(1);
2342  contacts[0].pos << 0, -1.25, 0;
2343  contacts[0].penetration_depth = 7.5;
2344  contacts[0].normal << 0, -1, 0;
2345  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2346 
2347  tf1 = transform;
2348  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2349  contacts.resize(1);
2350  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
2351  contacts[0].penetration_depth = 7.5;
2352  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2353  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2354 
2355  tf1 = Transform3<S>::Identity();
2356  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2357  contacts.resize(1);
2358  contacts[0].pos << 0, -3.75, 0;
2359  contacts[0].penetration_depth = 2.5;
2360  contacts[0].normal << 0, -1, 0;
2361  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2362 
2363  tf1 = transform;
2364  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2365  contacts.resize(1);
2366  contacts[0].pos = transform * Vector3<S>(0, -3.75, 0);
2367  contacts[0].penetration_depth = 2.5;
2368  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2369  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2370 
2371  tf1 = Transform3<S>::Identity();
2372  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2373  contacts.resize(1);
2374  contacts[0].pos << 0, 0.05, 0;
2375  contacts[0].penetration_depth = 10.1;
2376  contacts[0].normal << 0, -1, 0;
2377  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2378 
2379  tf1 = transform;
2380  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2381  contacts.resize(1);
2382  contacts[0].pos = transform * Vector3<S>(0, 0.05, 0);
2383  contacts[0].penetration_depth = 10.1;
2384  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2385  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2386 
2387  tf1 = Transform3<S>::Identity();
2388  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2389  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2390 
2391  tf1 = transform;
2392  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2393  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2394 
2395 
2396 
2397 
2398  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);
2399 
2400  tf1 = Transform3<S>::Identity();
2401  tf2 = Transform3<S>::Identity();
2402  contacts.resize(1);
2403  contacts[0].pos << 0, 0, -5;
2404  contacts[0].penetration_depth = 10;
2405  contacts[0].normal << 0, 0, -1;
2406  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2407 
2408  tf1 = transform;
2409  tf2 = transform;
2410  contacts.resize(1);
2411  contacts[0].pos = transform * Vector3<S>(0, 0, -5);
2412  contacts[0].penetration_depth = 10;
2413  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2414  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2415 
2416  tf1 = Transform3<S>::Identity();
2417  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2418  contacts.resize(1);
2419  contacts[0].pos << 0, 0, -3.75;
2420  contacts[0].penetration_depth = 12.5;
2421  contacts[0].normal << 0, 0, -1;
2422  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2423 
2424  tf1 = transform;
2425  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2426  contacts.resize(1);
2427  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
2428  contacts[0].penetration_depth = 12.5;
2429  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2430  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2431 
2432  tf1 = Transform3<S>::Identity();
2433  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2434  contacts.resize(1);
2435  contacts[0].pos << 0, 0, -6.25;
2436  contacts[0].penetration_depth = 7.5;
2437  contacts[0].normal << 0, 0, -1;
2438  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2439 
2440  tf1 = transform;
2441  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2442  contacts.resize(1);
2443  contacts[0].pos = transform * Vector3<S>(0, 0, -6.25);
2444  contacts[0].penetration_depth = 7.5;
2445  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2446  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2447 
2448  tf1 = Transform3<S>::Identity();
2449  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
2450  contacts.resize(1);
2451  contacts[0].pos << 0, 0, 0.05;
2452  contacts[0].penetration_depth = 20.1;
2453  contacts[0].normal << 0, 0, -1;
2454  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2455 
2456  tf1 = transform;
2457  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
2458  contacts.resize(1);
2459  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
2460  contacts[0].penetration_depth = 20.1;
2461  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2462  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2463 
2464  tf1 = Transform3<S>::Identity();
2465  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
2466  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2467 
2468  tf1 = transform;
2469  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
2470  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2471 }
2472 
2473 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule)
2474 {
2475 // test_shapeIntersection_halfspacecapsule<float>();
2476  test_shapeIntersection_halfspacecapsule<double>();
2477 }
2478 
2479 template <typename S>
2480 void test_shapeIntersection_planecapsule()
2481 {
2482  Capsule<S> s(5, 10);
2483  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
2484 
2485  Transform3<S> tf1;
2486  Transform3<S> tf2;
2487 
2488  Transform3<S> transform = Transform3<S>::Identity();
2489  test::generateRandomTransform(extents<S>(), transform);
2490 
2491  std::vector<ContactPoint<S>> contacts;
2492 
2493  tf1 = Transform3<S>::Identity();
2494  tf2 = Transform3<S>::Identity();
2495  contacts.resize(1);
2496  contacts[0].pos << 0, 0, 0;
2497  contacts[0].penetration_depth = 5;
2498  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
2499  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2500 
2501  tf1 = transform;
2502  tf2 = transform;
2503  contacts.resize(1);
2504  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2505  contacts[0].penetration_depth = 5;
2506  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
2507  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2508 
2509  tf1 = Transform3<S>::Identity();
2510  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2511  contacts.resize(1);
2512  contacts[0].pos << 2.5, 0, 0;
2513  contacts[0].penetration_depth = 2.5;
2514  contacts[0].normal << 1, 0, 0;
2515  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2516 
2517  tf1 = transform;
2518  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2519  contacts.resize(1);
2520  contacts[0].pos = transform * Vector3<S>(2.5, 0, 0);
2521  contacts[0].penetration_depth = 2.5;
2522  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
2523  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2524 
2525  tf1 = Transform3<S>::Identity();
2526  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2527  contacts.resize(1);
2528  contacts[0].pos << -2.5, 0, 0;
2529  contacts[0].penetration_depth = 2.5;
2530  contacts[0].normal << -1, 0, 0;
2531  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2532 
2533  tf1 = transform;
2534  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2535  contacts.resize(1);
2536  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
2537  contacts[0].penetration_depth = 2.5;
2538  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2539  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2540 
2541  tf1 = Transform3<S>::Identity();
2542  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2543  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2544 
2545  tf1 = transform;
2546  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2547  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2548 
2549  tf1 = Transform3<S>::Identity();
2550  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2551  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2552 
2553  tf1 = transform;
2554  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2555  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2556 
2557 
2558 
2559 
2560  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);
2561 
2562  tf1 = Transform3<S>::Identity();
2563  tf2 = Transform3<S>::Identity();
2564  contacts.resize(1);
2565  contacts[0].pos << 0, 0, 0;
2566  contacts[0].penetration_depth = 5;
2567  contacts[0].normal << 0, 1, 0; // (0, 1, 0) or (0, -1, 0)
2568  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2569 
2570  tf1 = transform;
2571  tf2 = transform;
2572  contacts.resize(1);
2573  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2574  contacts[0].penetration_depth = 5;
2575  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0); // (0, 1, 0) or (0, -1, 0)
2576  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2577 
2578  tf1 = Transform3<S>::Identity();
2579  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2580  contacts.resize(1);
2581  contacts[0].pos << 0, 2.5, 0;
2582  contacts[0].penetration_depth = 2.5;
2583  contacts[0].normal << 0, 1, 0;
2584  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2585 
2586  tf1 = transform;
2587  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2588  contacts.resize(1);
2589  contacts[0].pos = transform * Vector3<S>(0, 2.5, 0);
2590  contacts[0].penetration_depth = 2.5;
2591  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
2592  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2593 
2594  tf1 = Transform3<S>::Identity();
2595  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2596  contacts.resize(1);
2597  contacts[0].pos << 0, -2.5, 0;
2598  contacts[0].penetration_depth = 2.5;
2599  contacts[0].normal << 0, -1, 0;
2600  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2601 
2602  tf1 = transform;
2603  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2604  contacts.resize(1);
2605  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
2606  contacts[0].penetration_depth = 2.5;
2607  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2608  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2609 
2610  tf1 = Transform3<S>::Identity();
2611  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2612  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2613 
2614  tf1 = transform;
2615  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2616  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2617 
2618  tf1 = Transform3<S>::Identity();
2619  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2620  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2621 
2622  tf1 = transform;
2623  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2624  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2625 
2626 
2627 
2628 
2629  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);
2630 
2631  tf1 = Transform3<S>::Identity();
2632  tf2 = Transform3<S>::Identity();
2633  contacts.resize(1);
2634  contacts[0].pos << 0, 0, 0;
2635  contacts[0].penetration_depth = 10;
2636  contacts[0].normal << 0, 0, 1; // (0, 0, 1) or (0, 0, -1)
2637  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2638 
2639  tf1 = transform;
2640  tf2 = transform;
2641  contacts.resize(1);
2642  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2643  contacts[0].penetration_depth = 10;
2644  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1); // (0, 0, 1) or (0, 0, -1)
2645  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2646 
2647  tf1 = Transform3<S>::Identity();
2648  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2649  contacts.resize(1);
2650  contacts[0].pos << 0, 0, 2.5;
2651  contacts[0].penetration_depth = 7.5;
2652  contacts[0].normal << 0, 0, 1;
2653  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2654 
2655  tf1 = transform;
2656  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2657  contacts.resize(1);
2658  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
2659  contacts[0].penetration_depth = 7.5;
2660  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
2661  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2662 
2663  tf1 = Transform3<S>::Identity();
2664  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2665  contacts.resize(1);
2666  contacts[0].pos << 0, 0, -2.5;
2667  contacts[0].penetration_depth = 7.5;
2668  contacts[0].normal << 0, 0, -1;
2669  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2670 
2671  tf1 = transform;
2672  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2673  contacts.resize(1);
2674  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
2675  contacts[0].penetration_depth = 7.5;
2676  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2677  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2678 
2679  tf1 = Transform3<S>::Identity();
2680  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
2681  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2682 
2683  tf1 = transform;
2684  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
2685  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2686 
2687  tf1 = Transform3<S>::Identity();
2688  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
2689  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2690 
2691  tf1 = transform;
2692  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
2693  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2694 }
2695 
2696 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule)
2697 {
2698 // test_shapeIntersection_planecapsule<float>();
2699  test_shapeIntersection_planecapsule<double>();
2700 }
2701 
2702 template <typename S>
2703 void test_shapeIntersection_halfspacecylinder()
2704 {
2705  Cylinder<S> s(5, 10);
2706  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
2707 
2708  Transform3<S> tf1;
2709  Transform3<S> tf2;
2710 
2711  Transform3<S> transform = Transform3<S>::Identity();
2712  test::generateRandomTransform(extents<S>(), transform);
2713 
2714  std::vector<ContactPoint<S>> contacts;
2715 
2716  tf1 = Transform3<S>::Identity();
2717  tf2 = Transform3<S>::Identity();
2718  contacts.resize(1);
2719  contacts[0].pos << -2.5, 0, 0;
2720  contacts[0].penetration_depth = 5;
2721  contacts[0].normal << -1, 0, 0;
2722  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2723 
2724  tf1 = transform;
2725  tf2 = transform;
2726  contacts.resize(1);
2727  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
2728  contacts[0].penetration_depth = 5;
2729  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2730  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2731 
2732  tf1 = Transform3<S>::Identity();
2733  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2734  contacts.resize(1);
2735  contacts[0].pos << -1.25, 0, 0;
2736  contacts[0].penetration_depth = 7.5;
2737  contacts[0].normal << -1, 0, 0;
2738  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2739 
2740  tf1 = transform;
2741  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2742  contacts.resize(1);
2743  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
2744  contacts[0].penetration_depth = 7.5;
2745  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2746  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2747 
2748  tf1 = Transform3<S>::Identity();
2749  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2750  contacts.resize(1);
2751  contacts[0].pos << -3.75, 0, 0;
2752  contacts[0].penetration_depth = 2.5;
2753  contacts[0].normal << -1, 0, 0;
2754  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2755 
2756  tf1 = transform;
2757  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2758  contacts.resize(1);
2759  contacts[0].pos = transform * Vector3<S>(-3.75, 0, 0);
2760  contacts[0].penetration_depth = 2.5;
2761  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2762  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2763 
2764  tf1 = Transform3<S>::Identity();
2765  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2766  contacts.resize(1);
2767  contacts[0].pos << 0.05, 0, 0;
2768  contacts[0].penetration_depth = 10.1;
2769  contacts[0].normal << -1, 0, 0;
2770  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2771 
2772  tf1 = transform;
2773  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
2774  contacts.resize(1);
2775  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
2776  contacts[0].penetration_depth = 10.1;
2777  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
2778  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2779 
2780  tf1 = Transform3<S>::Identity();
2781  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2782  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2783 
2784  tf1 = transform;
2785  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
2786  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2787 
2788 
2789 
2790 
2791  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);
2792 
2793  tf1 = Transform3<S>::Identity();
2794  tf2 = Transform3<S>::Identity();
2795  contacts.resize(1);
2796  contacts[0].pos << 0, -2.5, 0;
2797  contacts[0].penetration_depth = 5;
2798  contacts[0].normal << 0, -1, 0;
2799  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2800 
2801  tf1 = transform;
2802  tf2 = transform;
2803  contacts.resize(1);
2804  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
2805  contacts[0].penetration_depth = 5;
2806  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2807  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2808 
2809  tf1 = Transform3<S>::Identity();
2810  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2811  contacts.resize(1);
2812  contacts[0].pos << 0, -1.25, 0;
2813  contacts[0].penetration_depth = 7.5;
2814  contacts[0].normal << 0, -1, 0;
2815  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2816 
2817  tf1 = transform;
2818  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
2819  contacts.resize(1);
2820  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
2821  contacts[0].penetration_depth = 7.5;
2822  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2823  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2824 
2825  tf1 = Transform3<S>::Identity();
2826  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2827  contacts.resize(1);
2828  contacts[0].pos << 0, -3.75, 0;
2829  contacts[0].penetration_depth = 2.5;
2830  contacts[0].normal << 0, -1, 0;
2831  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2832 
2833  tf1 = transform;
2834  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
2835  contacts.resize(1);
2836  contacts[0].pos = transform * Vector3<S>(0, -3.75, 0);
2837  contacts[0].penetration_depth = 2.5;
2838  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2839  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2840 
2841  tf1 = Transform3<S>::Identity();
2842  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2843  contacts.resize(1);
2844  contacts[0].pos << 0, 0.05, 0;
2845  contacts[0].penetration_depth = 10.1;
2846  contacts[0].normal << 0, -1, 0;
2847  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2848 
2849  tf1 = transform;
2850  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
2851  contacts.resize(1);
2852  contacts[0].pos = transform * Vector3<S>(0, 0.05, 0);
2853  contacts[0].penetration_depth = 10.1;
2854  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
2855  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2856 
2857  tf1 = Transform3<S>::Identity();
2858  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2859  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2860 
2861  tf1 = transform;
2862  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
2863  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2864 
2865 
2866 
2867 
2868  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);
2869 
2870  tf1 = Transform3<S>::Identity();
2871  tf2 = Transform3<S>::Identity();
2872  contacts.resize(1);
2873  contacts[0].pos << 0, 0, -2.5;
2874  contacts[0].penetration_depth = 5;
2875  contacts[0].normal << 0, 0, -1;
2876  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2877 
2878  tf1 = transform;
2879  tf2 = transform;
2880  contacts.resize(1);
2881  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
2882  contacts[0].penetration_depth = 5;
2883  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2884  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2885 
2886  tf1 = Transform3<S>::Identity();
2887  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2888  contacts.resize(1);
2889  contacts[0].pos << 0, 0, -1.25;
2890  contacts[0].penetration_depth = 7.5;
2891  contacts[0].normal << 0, 0, -1;
2892  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2893 
2894  tf1 = transform;
2895  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
2896  contacts.resize(1);
2897  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
2898  contacts[0].penetration_depth = 7.5;
2899  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2900  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2901 
2902  tf1 = Transform3<S>::Identity();
2903  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2904  contacts.resize(1);
2905  contacts[0].pos << 0, 0, -3.75;
2906  contacts[0].penetration_depth = 2.5;
2907  contacts[0].normal << 0, 0, -1;
2908  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2909 
2910  tf1 = transform;
2911  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
2912  contacts.resize(1);
2913  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
2914  contacts[0].penetration_depth = 2.5;
2915  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2916  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2917 
2918  tf1 = Transform3<S>::Identity();
2919  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
2920  contacts.resize(1);
2921  contacts[0].pos << 0, 0, 0.05;
2922  contacts[0].penetration_depth = 10.1;
2923  contacts[0].normal << 0, 0, -1;
2924  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2925 
2926  tf1 = transform;
2927  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
2928  contacts.resize(1);
2929  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
2930  contacts[0].penetration_depth = 10.1;
2931  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
2932  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2933 
2934  tf1 = Transform3<S>::Identity();
2935  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
2936  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2937 
2938  tf1 = transform;
2939  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
2940  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
2941 }
2942 
2943 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder)
2944 {
2945 // test_shapeIntersection_halfspacecylinder<float>();
2946  test_shapeIntersection_halfspacecylinder<double>();
2947 }
2948 
2949 template <typename S>
2950 void test_shapeIntersection_planecylinder()
2951 {
2952  Cylinder<S> s(5, 10);
2953  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
2954 
2955  Transform3<S> tf1;
2956  Transform3<S> tf2;
2957 
2958  Transform3<S> transform = Transform3<S>::Identity();
2959  test::generateRandomTransform(extents<S>(), transform);
2960 
2961  std::vector<ContactPoint<S>> contacts;
2962 
2963  tf1 = Transform3<S>::Identity();
2964  tf2 = Transform3<S>::Identity();
2965  contacts.resize(1);
2966  contacts[0].pos << 0, 0, 0;
2967  contacts[0].penetration_depth = 5;
2968  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
2969  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2970 
2971  tf1 = transform;
2972  tf2 = transform;
2973  contacts.resize(1);
2974  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
2975  contacts[0].penetration_depth = 5;
2976  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
2977  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
2978 
2979  tf1 = Transform3<S>::Identity();
2980  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2981  contacts.resize(1);
2982  contacts[0].pos << 2.5, 0, 0;
2983  contacts[0].penetration_depth = 2.5;
2984  contacts[0].normal << 1, 0, 0;
2985  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2986 
2987  tf1 = transform;
2988  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
2989  contacts.resize(1);
2990  contacts[0].pos = transform * Vector3<S>(2.5, 0, 0);
2991  contacts[0].penetration_depth = 2.5;
2992  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
2993  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
2994 
2995  tf1 = Transform3<S>::Identity();
2996  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
2997  contacts.resize(1);
2998  contacts[0].pos << -2.5, 0, 0;
2999  contacts[0].penetration_depth = 2.5;
3000  contacts[0].normal << -1, 0, 0;
3001  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3002 
3003  tf1 = transform;
3004  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3005  contacts.resize(1);
3006  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
3007  contacts[0].penetration_depth = 2.5;
3008  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3009  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3010 
3011  tf1 = Transform3<S>::Identity();
3012  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3013  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3014 
3015  tf1 = transform;
3016  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3017  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3018 
3019  tf1 = Transform3<S>::Identity();
3020  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3021  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3022 
3023  tf1 = transform;
3024  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3025  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3026 
3027 
3028 
3029 
3030  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);
3031 
3032  tf1 = Transform3<S>::Identity();
3033  tf2 = Transform3<S>::Identity();
3034  contacts.resize(1);
3035  contacts[0].pos << 0, 0, 0;
3036  contacts[0].penetration_depth = 5;
3037  contacts[0].normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0)
3038  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3039 
3040  tf1 = transform;
3041  tf2 = transform;
3042  contacts.resize(1);
3043  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3044  contacts[0].penetration_depth = 5;
3045  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
3046  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3047 
3048  tf1 = Transform3<S>::Identity();
3049  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3050  contacts.resize(1);
3051  contacts[0].pos << 0, 2.5, 0;
3052  contacts[0].penetration_depth = 2.5;
3053  contacts[0].normal << 0, 1, 0;
3054  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3055 
3056  tf1 = transform;
3057  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3058  contacts.resize(1);
3059  contacts[0].pos = transform * Vector3<S>(0, 2.5, 0);
3060  contacts[0].penetration_depth = 2.5;
3061  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
3062  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3063 
3064  tf1 = Transform3<S>::Identity();
3065  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3066  contacts.resize(1);
3067  contacts[0].pos << 0, -2.5, 0;
3068  contacts[0].penetration_depth = 2.5;
3069  contacts[0].normal << 0, -1, 0;
3070  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3071 
3072  tf1 = transform;
3073  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3074  contacts.resize(1);
3075  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
3076  contacts[0].penetration_depth = 2.5;
3077  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3078  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3079 
3080  tf1 = Transform3<S>::Identity();
3081  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3082  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3083 
3084  tf1 = transform;
3085  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3086  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3087 
3088  tf1 = Transform3<S>::Identity();
3089  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3090  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3091 
3092  tf1 = transform;
3093  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3094  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3095 
3096 
3097 
3098 
3099  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);
3100 
3101  tf1 = Transform3<S>::Identity();
3102  tf2 = Transform3<S>::Identity();
3103  contacts.resize(1);
3104  contacts[0].pos << 0, 0, 0;
3105  contacts[0].penetration_depth = 5;
3106  contacts[0].normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0)
3107  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3108 
3109  tf1 = transform;
3110  tf2 = transform;
3111  contacts.resize(1);
3112  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3113  contacts[0].penetration_depth = 5;
3114  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
3115  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3116 
3117  tf1 = Transform3<S>::Identity();
3118  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3119  contacts.resize(1);
3120  contacts[0].pos << 0, 0, 2.5;
3121  contacts[0].penetration_depth = 2.5;
3122  contacts[0].normal << 0, 0, 1;
3123  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3124 
3125  tf1 = transform;
3126  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3127  contacts.resize(1);
3128  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
3129  contacts[0].penetration_depth = 2.5;
3130  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
3131  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3132 
3133  tf1 = Transform3<S>::Identity();
3134  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3135  contacts.resize(1);
3136  contacts[0].pos << 0, 0, -2.5;
3137  contacts[0].penetration_depth = 2.5;
3138  contacts[0].normal << 0, 0, -1;
3139  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3140 
3141  tf1 = transform;
3142  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3143  contacts.resize(1);
3144  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
3145  contacts[0].penetration_depth = 2.5;
3146  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3147  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3148 
3149  tf1 = Transform3<S>::Identity();
3150  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
3151  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3152 
3153  tf1 = transform;
3154  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
3155  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3156 
3157  tf1 = Transform3<S>::Identity();
3158  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
3159  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3160 
3161  tf1 = transform;
3162  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
3163  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3164 }
3165 
3166 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder)
3167 {
3168 // test_shapeIntersection_planecylinder<float>();
3169  test_shapeIntersection_planecylinder<double>();
3170 }
3171 
3172 template <typename S>
3173 void test_shapeIntersection_halfspacecone()
3174 {
3175  Cone<S> s(5, 10);
3176  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
3177 
3178  Transform3<S> tf1;
3179  Transform3<S> tf2;
3180 
3181  Transform3<S> transform = Transform3<S>::Identity();
3182  test::generateRandomTransform(extents<S>(), transform);
3183 
3184  std::vector<ContactPoint<S>> contacts;
3185 
3186  tf1 = Transform3<S>::Identity();
3187  tf2 = Transform3<S>::Identity();
3188  contacts.resize(1);
3189  contacts[0].pos << -2.5, 0, -5;
3190  contacts[0].penetration_depth = 5;
3191  contacts[0].normal << -1, 0, 0;
3192  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3193 
3194  tf1 = transform;
3195  tf2 = transform;
3196  contacts.resize(1);
3197  contacts[0].pos = transform * Vector3<S>(-2.5, 0, -5);
3198  contacts[0].penetration_depth = 5;
3199  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3200  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3201 
3202  tf1 = Transform3<S>::Identity();
3203  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
3204  contacts.resize(1);
3205  contacts[0].pos << -1.25, 0, -5;
3206  contacts[0].penetration_depth = 7.5;
3207  contacts[0].normal << -1, 0, 0;
3208  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3209 
3210  tf1 = transform;
3211  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
3212  contacts.resize(1);
3213  contacts[0].pos = transform * Vector3<S>(-1.25, 0, -5);
3214  contacts[0].penetration_depth = 7.5;
3215  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3216  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3217 
3218  tf1 = Transform3<S>::Identity();
3219  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3220  contacts.resize(1);
3221  contacts[0].pos << -3.75, 0, -5;
3222  contacts[0].penetration_depth = 2.5;
3223  contacts[0].normal << -1, 0, 0;
3224  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3225 
3226  tf1 = transform;
3227  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3228  contacts.resize(1);
3229  contacts[0].pos = transform * Vector3<S>(-3.75, 0, -5);
3230  contacts[0].penetration_depth = 2.5;
3231  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3232  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3233 
3234  tf1 = Transform3<S>::Identity();
3235  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3236  contacts.resize(1);
3237  contacts[0].pos << 0.05, 0, -5;
3238  contacts[0].penetration_depth = 10.1;
3239  contacts[0].normal << -1, 0, 0;
3240  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3241 
3242  tf1 = transform;
3243  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3244  contacts.resize(1);
3245  contacts[0].pos = transform * Vector3<S>(0.05, 0, -5);
3246  contacts[0].penetration_depth = 10.1;
3247  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3248  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3249 
3250  tf1 = Transform3<S>::Identity();
3251  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3252  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3253 
3254  tf1 = transform;
3255  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3256  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3257 
3258 
3259 
3260 
3261  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);
3262 
3263  tf1 = Transform3<S>::Identity();
3264  tf2 = Transform3<S>::Identity();
3265  contacts.resize(1);
3266  contacts[0].pos << 0, -2.5, -5;
3267  contacts[0].penetration_depth = 5;
3268  contacts[0].normal << 0, -1, 0;
3269  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3270 
3271  tf1 = transform;
3272  tf2 = transform;
3273  contacts.resize(1);
3274  contacts[0].pos = transform * Vector3<S>(0, -2.5, -5);
3275  contacts[0].penetration_depth = 5;
3276  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3277  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3278 
3279  tf1 = Transform3<S>::Identity();
3280  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3281  contacts.resize(1);
3282  contacts[0].pos << 0, -1.25, -5;
3283  contacts[0].penetration_depth = 7.5;
3284  contacts[0].normal << 0, -1, 0;
3285  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3286 
3287  tf1 = transform;
3288  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3289  contacts.resize(1);
3290  contacts[0].pos = transform * Vector3<S>(0, -1.25, -5);
3291  contacts[0].penetration_depth = 7.5;
3292  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3293  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3294 
3295  tf1 = Transform3<S>::Identity();
3296  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3297  contacts.resize(1);
3298  contacts[0].pos << 0, -3.75, -5;
3299  contacts[0].penetration_depth = 2.5;
3300  contacts[0].normal << 0, -1, 0;
3301  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3302 
3303  tf1 = transform;
3304  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3305  contacts.resize(1);
3306  contacts[0].pos = transform * Vector3<S>(0, -3.75, -5);
3307  contacts[0].penetration_depth = 2.5;
3308  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3309  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3310 
3311  tf1 = Transform3<S>::Identity();
3312  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3313  contacts.resize(1);
3314  contacts[0].pos << 0, 0.05, -5;
3315  contacts[0].penetration_depth = 10.1;
3316  contacts[0].normal << 0, -1, 0;
3317  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3318 
3319  tf1 = transform;
3320  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3321  contacts.resize(1);
3322  contacts[0].pos = transform * Vector3<S>(0, 0.05, -5);
3323  contacts[0].penetration_depth = 10.1;
3324  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3325  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3326 
3327  tf1 = Transform3<S>::Identity();
3328  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3329  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3330 
3331  tf1 = transform;
3332  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3333  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3334 
3335 
3336 
3337 
3338  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);
3339 
3340  tf1 = Transform3<S>::Identity();
3341  tf2 = Transform3<S>::Identity();
3342  contacts.resize(1);
3343  contacts[0].pos << 0, 0, -2.5;
3344  contacts[0].penetration_depth = 5;
3345  contacts[0].normal << 0, 0, -1;
3346  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3347 
3348  tf1 = transform;
3349  tf2 = transform;
3350  contacts.resize(1);
3351  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
3352  contacts[0].penetration_depth = 5;
3353  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3354  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3355 
3356  tf1 = Transform3<S>::Identity();
3357  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3358  contacts.resize(1);
3359  contacts[0].pos << 0, 0, -1.25;
3360  contacts[0].penetration_depth = 7.5;
3361  contacts[0].normal << 0, 0, -1;
3362  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3363 
3364  tf1 = transform;
3365  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3366  contacts.resize(1);
3367  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
3368  contacts[0].penetration_depth = 7.5;
3369  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3370  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3371 
3372  tf1 = Transform3<S>::Identity();
3373  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3374  contacts.resize(1);
3375  contacts[0].pos << 0, 0, -3.75;
3376  contacts[0].penetration_depth= 2.5;
3377  contacts[0].normal << 0, 0, -1;
3378  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3379 
3380  tf1 = transform;
3381  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3382  contacts.resize(1);
3383  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
3384  contacts[0].penetration_depth = 2.5;
3385  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3386  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3387 
3388  tf1 = Transform3<S>::Identity();
3389  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
3390  contacts.resize(1);
3391  contacts[0].pos << 0, 0, 0.05;
3392  contacts[0].penetration_depth = 10.1;
3393  contacts[0].normal << 0, 0, -1;
3394  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3395 
3396  tf1 = transform;
3397  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
3398  contacts.resize(1);
3399  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
3400  contacts[0].penetration_depth = 10.1;
3401  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3402  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3403 
3404  tf1 = Transform3<S>::Identity();
3405  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
3406  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3407 
3408  tf1 = transform;
3409  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
3410  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3411 }
3412 
3413 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone)
3414 {
3415 // test_shapeIntersection_halfspacecone<float>();
3416  test_shapeIntersection_halfspacecone<double>();
3417 }
3418 
3419 template <typename S>
3420 void test_shapeIntersection_planecone()
3421 {
3422  Cone<S> s(5, 10);
3423  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
3424 
3425  Transform3<S> tf1;
3426  Transform3<S> tf2;
3427 
3428  Transform3<S> transform = Transform3<S>::Identity();
3429  test::generateRandomTransform(extents<S>(), transform);
3430 
3431  std::vector<ContactPoint<S>> contacts;
3432 
3433  tf1 = Transform3<S>::Identity();
3434  tf2 = Transform3<S>::Identity();
3435  contacts.resize(1);
3436  contacts[0].pos << 0, 0, 0;
3437  contacts[0].penetration_depth = 5;
3438  contacts[0].normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
3439  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3440 
3441  tf1 = transform;
3442  tf2 = transform;
3443  contacts.resize(1);
3444  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3445  contacts[0].penetration_depth = 5;
3446  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
3447  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3448 
3449  tf1 = Transform3<S>::Identity();
3450  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
3451  contacts.resize(1);
3452  contacts[0].pos << 2.5, 0, -2.5;
3453  contacts[0].penetration_depth = 2.5;
3454  contacts[0].normal << 1, 0, 0;
3455  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3456 
3457  tf1 = transform;
3458  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
3459  contacts.resize(1);
3460  contacts[0].pos = transform * Vector3<S>(2.5, 0, -2.5);
3461  contacts[0].penetration_depth = 2.5;
3462  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
3463  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3464 
3465  tf1 = Transform3<S>::Identity();
3466  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3467  contacts.resize(1);
3468  contacts[0].pos << -2.5, 0, -2.5;
3469  contacts[0].penetration_depth = 2.5;
3470  contacts[0].normal << -1, 0, 0;
3471  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3472 
3473  tf1 = transform;
3474  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
3475  contacts.resize(1);
3476  contacts[0].pos = transform * Vector3<S>(-2.5, 0, -2.5);
3477  contacts[0].penetration_depth = 2.5;
3478  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
3479  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3480 
3481  tf1 = Transform3<S>::Identity();
3482  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3483  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3484 
3485  tf1 = transform;
3486  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
3487  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3488 
3489  tf1 = Transform3<S>::Identity();
3490  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3491  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3492 
3493  tf1 = transform;
3494  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
3495  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3496 
3497 
3498 
3499 
3500  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);
3501 
3502  tf1 = Transform3<S>::Identity();
3503  tf2 = Transform3<S>::Identity();
3504  contacts.resize(1);
3505  contacts[0].pos << 0, 0, 0;
3506  contacts[0].penetration_depth = 5;
3507  contacts[0].normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0)
3508  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3509 
3510  tf1 = transform;
3511  tf2 = transform;
3512  contacts.resize(1);
3513  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3514  contacts[0].penetration_depth = 5;
3515  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
3516  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3517 
3518  tf1 = Transform3<S>::Identity();
3519  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3520  contacts.resize(1);
3521  contacts[0].pos << 0, 2.5, -2.5;
3522  contacts[0].penetration_depth = 2.5;
3523  contacts[0].normal << 0, 1, 0;
3524  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3525 
3526  tf1 = transform;
3527  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
3528  contacts.resize(1);
3529  contacts[0].pos = transform * Vector3<S>(0, 2.5, -2.5);
3530  contacts[0].penetration_depth = 2.5;
3531  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
3532  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3533 
3534  tf1 = Transform3<S>::Identity();
3535  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3536  contacts.resize(1);
3537  contacts[0].pos << 0, -2.5, -2.5;
3538  contacts[0].penetration_depth = 2.5;
3539  contacts[0].normal << 0, -1, 0;
3540  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3541 
3542  tf1 = transform;
3543  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
3544  contacts.resize(1);
3545  contacts[0].pos = transform * Vector3<S>(0, -2.5, -2.5);
3546  contacts[0].penetration_depth = 2.5;
3547  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
3548  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3549 
3550  tf1 = Transform3<S>::Identity();
3551  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3552  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3553 
3554  tf1 = transform;
3555  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
3556  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3557 
3558  tf1 = Transform3<S>::Identity();
3559  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3560  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3561 
3562  tf1 = transform;
3563  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
3564  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3565 
3566 
3567 
3568 
3569  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);
3570 
3571  tf1 = Transform3<S>::Identity();
3572  tf2 = Transform3<S>::Identity();
3573  contacts.resize(1);
3574  contacts[0].pos << 0, 0, 0;
3575  contacts[0].penetration_depth = 5;
3576  contacts[0].normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0)
3577  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3578 
3579  tf1 = transform;
3580  tf2 = transform;
3581  contacts.resize(1);
3582  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
3583  contacts[0].penetration_depth = 5;
3584  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
3585  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
3586 
3587  tf1 = Transform3<S>::Identity();
3588  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3589  contacts.resize(1);
3590  contacts[0].pos << 0, 0, 2.5;
3591  contacts[0].penetration_depth = 2.5;
3592  contacts[0].normal << 0, 0, 1;
3593  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3594 
3595  tf1 = transform;
3596  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
3597  contacts.resize(1);
3598  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
3599  contacts[0].penetration_depth = 2.5;
3600  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
3601  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3602 
3603  tf1 = Transform3<S>::Identity();
3604  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3605  contacts.resize(1);
3606  contacts[0].pos << 0, 0, -2.5;
3607  contacts[0].penetration_depth = 2.5;
3608  contacts[0].normal << 0, 0, -1;
3609  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3610 
3611  tf1 = transform;
3612  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
3613  contacts.resize(1);
3614  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
3615  contacts[0].penetration_depth = 2.5;
3616  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
3617  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
3618 
3619  tf1 = Transform3<S>::Identity();
3620  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
3621  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3622 
3623  tf1 = transform;
3624  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
3625  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3626 
3627  tf1 = Transform3<S>::Identity();
3628  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
3629  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3630 
3631  tf1 = transform;
3632  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
3633  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
3634 }
3635 
3636 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone)
3637 {
3638 // test_shapeIntersection_planecone<float>();
3639  test_shapeIntersection_planecone<double>();
3640 }
3641 
3642 // Shape distance test coverage (libccd)
3643 //
3644 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3645 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
3646 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3647 // | box | O | O | | | | | | | |
3648 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3649 // | sphere |/////| O | | | | | | | |
3650 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3651 // | ellipsoid |/////|////////| O | | | | | | |
3652 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3653 // | capsule |/////|////////|///////////| | | | | | |
3654 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3655 // | cone |/////|////////|///////////|/////////| O | O | | | |
3656 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3657 // | cylinder |/////|////////|///////////|/////////|//////| O | | | |
3658 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3659 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
3660 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3661 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
3662 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3663 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
3664 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
3665 
3666 template <typename S>
3667 void test_shapeDistance_spheresphere()
3668 {
3669  Sphere<S> s1(20);
3670  Sphere<S> s2(10);
3671 
3672  Transform3<S> transform = Transform3<S>::Identity();
3673  //test::generateRandomTransform(extents<S>(), transform);
3674 
3675  bool res;
3676  S dist = -1;
3677  Vector3<S> closest_p1, closest_p2;
3678  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 40, 0))), &dist, &closest_p1, &closest_p2);
3679  EXPECT_TRUE(fabs(dist - 10) < 0.001);
3680  EXPECT_TRUE(res);
3681 
3682  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
3683  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3684  EXPECT_TRUE(res);
3685 
3686  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
3687  EXPECT_TRUE(dist < 0);
3688  EXPECT_FALSE(res);
3689 
3690  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
3691  EXPECT_TRUE(fabs(dist - 10) < 0.001);
3692  EXPECT_TRUE(res);
3693 
3694  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
3695  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3696  EXPECT_TRUE(res);
3697 
3698  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
3699  EXPECT_TRUE(dist < 0);
3700  EXPECT_FALSE(res);
3701 
3702 
3703  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3704  // this is one problem: the precise is low sometimes
3705  EXPECT_TRUE(fabs(dist - 10) < 0.1);
3706  EXPECT_TRUE(res);
3707 
3708  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
3709  EXPECT_TRUE(fabs(dist - 0.1) < 0.06);
3710  EXPECT_TRUE(res);
3711 
3712  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
3713  EXPECT_TRUE(dist < 0);
3714  EXPECT_FALSE(res);
3715 
3716  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
3717  EXPECT_TRUE(fabs(dist - 10) < 0.1);
3718  EXPECT_TRUE(res);
3719 
3720  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
3721  EXPECT_TRUE(fabs(dist - 0.1) < 0.1);
3722  EXPECT_TRUE(res);
3723 
3724  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
3725  EXPECT_TRUE(dist < 0);
3726  EXPECT_FALSE(res);
3727 }
3728 
3729 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_spheresphere)
3730 {
3731 // test_shapeDistance_spheresphere<float>();
3732  test_shapeDistance_spheresphere<double>();
3733 }
3734 
3735 template <typename S>
3736 void test_shapeDistance_boxbox()
3737 {
3738  Box<S> s1(20, 40, 50);
3739  Box<S> s2(10, 10, 10);
3740  Vector3<S> closest_p1, closest_p2;
3741 
3742  Transform3<S> transform = Transform3<S>::Identity();
3743  //test::generateRandomTransform(extents<S>(), transform);
3744 
3745  bool res;
3746  S dist;
3747 
3748  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3749  EXPECT_TRUE(dist < 0);
3750  EXPECT_FALSE(res);
3751 
3752  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3753  EXPECT_TRUE(dist < 0);
3754  EXPECT_FALSE(res);
3755 
3756  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist, &closest_p1, &closest_p2);
3757  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3758  EXPECT_TRUE(res);
3759 
3760  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20.1, 0, 0))), &dist, &closest_p1, &closest_p2);
3761  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
3762  EXPECT_TRUE(res);
3763 
3764  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 20.2, 0))), &dist, &closest_p1, &closest_p2);
3765  EXPECT_TRUE(fabs(dist - 10.2) < 0.001);
3766  EXPECT_TRUE(res);
3767 
3768  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);
3769  EXPECT_TRUE(fabs(dist - 0.1 * 1.414) < 0.001);
3770  EXPECT_TRUE(res);
3771 
3772  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist, &closest_p1, &closest_p2);
3773  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3774  EXPECT_TRUE(res);
3775 
3776  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20.1, 0, 0))), &dist, &closest_p1, &closest_p2);
3777  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
3778  EXPECT_TRUE(res);
3779 
3780  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 20.1, 0))), &dist, &closest_p1, &closest_p2);
3781  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
3782  EXPECT_TRUE(res);
3783 
3784  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);
3785  EXPECT_TRUE(fabs(dist - 0.1 * 1.414) < 0.001);
3786  EXPECT_TRUE(res);
3787 
3788 
3789  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
3790  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3791  EXPECT_TRUE(res);
3792 
3793  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
3794  EXPECT_TRUE(fabs(dist - 5) < 0.001);
3795  EXPECT_TRUE(res);
3796 
3797  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
3798  EXPECT_TRUE(fabs(dist - 5) < 0.001);
3799  EXPECT_TRUE(res);
3800 }
3801 
3802 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxbox)
3803 {
3804 // test_shapeDistance_boxbox<float>();
3805  test_shapeDistance_boxbox<double>();
3806 }
3807 
3808 template <typename S>
3809 void test_shapeDistance_boxsphere()
3810 {
3811  Sphere<S> s1(20);
3812  Box<S> s2(5, 5, 5);
3813 
3814  Transform3<S> transform = Transform3<S>::Identity();
3815  test::generateRandomTransform(extents<S>(), transform);
3816 
3817  bool res;
3818  S dist;
3819 
3820  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3821  EXPECT_TRUE(dist < 0);
3822  EXPECT_FALSE(res);
3823 
3824  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3825  EXPECT_TRUE(dist < 0);
3826  EXPECT_FALSE(res);
3827 
3828  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
3829  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3830  EXPECT_TRUE(res);
3831 
3832  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
3833  EXPECT_TRUE(fabs(dist - 0.1) < 0.05);
3834  EXPECT_TRUE(res);
3835 
3836  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3837  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
3838  EXPECT_TRUE(res);
3839 
3840  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3841  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
3842  EXPECT_TRUE(res);
3843 }
3844 
3845 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxsphere)
3846 {
3847 // test_shapeDistance_boxsphere<float>();
3848  test_shapeDistance_boxsphere<double>();
3849 }
3850 
3851 template <typename S>
3852 void test_shapeDistance_cylindercylinder()
3853 {
3854  Cylinder<S> s1(5, 10);
3855  Cylinder<S> s2(5, 10);
3856 
3857  Transform3<S> transform = Transform3<S>::Identity();
3858  test::generateRandomTransform(extents<S>(), transform);
3859 
3860  bool res;
3861  S dist;
3862 
3863  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3864  EXPECT_TRUE(dist < 0);
3865  EXPECT_FALSE(res);
3866 
3867  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3868  EXPECT_TRUE(dist < 0);
3869  EXPECT_FALSE(res);
3870 
3871  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3872  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3873  EXPECT_TRUE(res);
3874 
3875  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3876  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3877  EXPECT_TRUE(res);
3878 
3879  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3880  EXPECT_TRUE(fabs(dist - 30) < 0.001);
3881  EXPECT_TRUE(res);
3882 
3883  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3884  EXPECT_TRUE(fabs(dist - 30) < 0.001);
3885  EXPECT_TRUE(res);
3886 }
3887 
3888 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_cylindercylinder)
3889 {
3890 // test_shapeDistance_cylindercylinder<float>();
3891  test_shapeDistance_cylindercylinder<double>();
3892 }
3893 
3894 template <typename S>
3895 void test_shapeDistance_conecone()
3896 {
3897  Cone<S> s1(5, 10);
3898  Cone<S> s2(5, 10);
3899 
3900  Transform3<S> transform = Transform3<S>::Identity();
3901  test::generateRandomTransform(extents<S>(), transform);
3902 
3903  bool res;
3904  S dist;
3905 
3906  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3907  EXPECT_TRUE(dist < 0);
3908  EXPECT_FALSE(res);
3909 
3910  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3911  EXPECT_TRUE(dist < 0);
3912  EXPECT_FALSE(res);
3913 
3914  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3915  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3916  EXPECT_TRUE(res);
3917 
3918  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3919  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3920  EXPECT_TRUE(res);
3921 
3922  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
3923  EXPECT_TRUE(fabs(dist - 30) < 1);
3924  EXPECT_TRUE(res);
3925 
3926  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
3927  EXPECT_TRUE(fabs(dist - 30) < 1);
3928  EXPECT_TRUE(res);
3929 }
3930 
3931 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecone)
3932 {
3933 // test_shapeDistance_conecone<float>();
3934  test_shapeDistance_conecone<double>();
3935 }
3936 
3937 template <typename S>
3938 void test_shapeDistance_conecylinder()
3939 {
3940  Cylinder<S> s1(5, 10);
3941  Cone<S> s2(5, 10);
3942 
3943  Transform3<S> transform = Transform3<S>::Identity();
3944  test::generateRandomTransform(extents<S>(), transform);
3945 
3946  bool res;
3947  S dist;
3948 
3949  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
3950  EXPECT_TRUE(dist < 0);
3951  EXPECT_FALSE(res);
3952 
3953  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
3954  EXPECT_TRUE(dist < 0);
3955  EXPECT_FALSE(res);
3956 
3957  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3958  EXPECT_TRUE(fabs(dist - 0.1) < 0.01);
3959  EXPECT_TRUE(res);
3960 
3961  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
3962  EXPECT_TRUE(fabs(dist - 0.1) < 0.02);
3963  EXPECT_TRUE(res);
3964 
3965  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3966  EXPECT_TRUE(fabs(dist - 30) < 0.01);
3967  EXPECT_TRUE(res);
3968 
3969  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
3970  EXPECT_TRUE(fabs(dist - 30) < 0.1);
3971  EXPECT_TRUE(res);
3972 }
3973 
3974 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecylinder)
3975 {
3976 // test_shapeDistance_conecylinder<float>();
3977  test_shapeDistance_conecylinder<double>();
3978 }
3979 
3980 template <typename S>
3981 void test_shapeDistance_ellipsoidellipsoid()
3982 {
3983  Ellipsoid<S> s1(20, 40, 50);
3984  Ellipsoid<S> s2(10, 10, 10);
3985 
3986  Transform3<S> transform = Transform3<S>::Identity();
3987  test::generateRandomTransform(extents<S>(), transform);
3988 
3989  bool res;
3990  S dist = -1;
3991  Vector3<S> closest_p1, closest_p2;
3992 
3993  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist, &closest_p1, &closest_p2);
3994  EXPECT_TRUE(fabs(dist - 10) < 0.001);
3995  EXPECT_TRUE(res);
3996 
3997  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
3998  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
3999  EXPECT_TRUE(res);
4000 
4001  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
4002  EXPECT_TRUE(dist < 0);
4003  EXPECT_FALSE(res);
4004 
4005  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4006  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4007  EXPECT_TRUE(res);
4008 
4009  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4010  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4011  EXPECT_TRUE(res);
4012 
4013  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4014  EXPECT_TRUE(dist < 0);
4015  EXPECT_FALSE(res);
4016 
4017 
4018  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
4019  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4020  EXPECT_TRUE(res);
4021 
4022  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
4023  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4024  EXPECT_TRUE(res);
4025 
4026  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
4027  EXPECT_TRUE(dist < 0);
4028  EXPECT_FALSE(res);
4029 
4030  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
4031  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4032  EXPECT_TRUE(res);
4033 
4034  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
4035  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4036  EXPECT_TRUE(res);
4037 
4038  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
4039  EXPECT_TRUE(dist < 0);
4040  EXPECT_FALSE(res);
4041 }
4042 
4043 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_ellipsoidellipsoid)
4044 {
4045 // test_shapeDistance_ellipsoidellipsoid<float>();
4046  test_shapeDistance_ellipsoidellipsoid<double>();
4047 }
4048 
4049 // Shape intersection test coverage (built-in GJK)
4050 //
4051 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4052 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
4053 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4054 // | box | O | O | | | | | | | |
4055 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4056 // | sphere |/////| O | | | | | | | O |
4057 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4058 // | ellipsoid |/////|////////| O | | | | | | |
4059 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4060 // | capsule |/////|////////|///////////| | | | | | |
4061 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4062 // | cone |/////|////////|///////////|/////////| O | O | | | |
4063 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4064 // | cylinder |/////|////////|///////////|/////////|//////| O | | | |
4065 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4066 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
4067 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4068 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
4069 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4070 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
4071 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4072 
4073 template <typename S>
4074 void test_shapeIntersectionGJK_spheresphere()
4075 {
4076  Sphere<S> s1(20);
4077  Sphere<S> s2(10);
4078 
4079  Transform3<S> tf1;
4080  Transform3<S> tf2;
4081 
4082  Transform3<S> transform = Transform3<S>::Identity();
4083  test::generateRandomTransform(extents<S>(), transform);
4084 
4085  std::vector<ContactPoint<S>> contacts;
4086 
4087  tf1 = Transform3<S>::Identity();
4088  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
4089  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4090 
4091  tf1 = transform;
4092  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
4093  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4094 
4095  tf1 = Transform3<S>::Identity();
4096  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
4097  contacts.resize(1);
4098  contacts[0].normal << 1, 0, 0;
4099  contacts[0].pos << 20, 0, 0;
4100  contacts[0].penetration_depth = 0.0;
4101  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4102 
4103  tf1 = Transform3<S>::Identity();
4104  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
4105  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4106 
4107  tf1 = transform;
4108  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
4109  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4110 
4111  tf1 = Transform3<S>::Identity();
4112  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
4113  contacts.resize(1);
4114  contacts[0].normal << 1, 0, 0;
4115  contacts[0].pos << 20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0;
4116  contacts[0].penetration_depth = 0.1;
4117  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4118 
4119  tf1 = transform;
4120  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
4121  contacts.resize(1);
4122  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4123  contacts[0].pos = transform * Vector3<S>(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0);
4124  contacts[0].penetration_depth = 0.1;
4125  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4126 
4127  tf1 = Transform3<S>::Identity();
4128  tf2 = Transform3<S>::Identity();
4129  contacts.resize(1);
4130  contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
4131  contacts[0].pos.setZero();
4132  contacts[0].penetration_depth = 20.0 + 10.0;
4133  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4134 
4135  tf1 = transform;
4136  tf2 = transform;
4137  contacts.resize(1);
4138  contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
4139  contacts[0].pos = transform * Vector3<S>::Zero();
4140  contacts[0].penetration_depth = 20.0 + 10.0;
4141  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4142 
4143  tf1 = Transform3<S>::Identity();
4144  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
4145  contacts.resize(1);
4146  contacts[0].normal << -1, 0, 0;
4147  contacts[0].pos << -20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0;
4148  contacts[0].penetration_depth = 0.1;
4149  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4150 
4151  tf1 = transform;
4152  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
4153  contacts.resize(1);
4154  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
4155  contacts[0].pos = transform * Vector3<S>(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0);
4156  contacts[0].penetration_depth = 0.1;
4157  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4158 
4159  tf1 = Transform3<S>::Identity();
4160  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.0, 0, 0)));
4161  contacts.resize(1);
4162  contacts[0].normal << -1, 0, 0;
4163  contacts[0].pos << -20, 0, 0;
4164  contacts[0].penetration_depth = 0.0;
4165  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);
4166 
4167  tf1 = Transform3<S>::Identity();
4168  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
4169  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4170 
4171  tf1 = transform;
4172  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
4173  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4174 }
4175 
4176 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheresphere)
4177 {
4178 // test_shapeIntersectionGJK_spheresphere<float>();
4179  test_shapeIntersectionGJK_spheresphere<double>();
4180 }
4181 
4182 template <typename S>
4183 void test_shapeIntersectionGJK_boxbox()
4184 {
4185  Box<S> s1(20, 40, 50);
4186  Box<S> s2(10, 10, 10);
4187 
4188  Transform3<S> tf1;
4189  Transform3<S> tf2;
4190 
4191  Transform3<S> transform = Transform3<S>::Identity();
4192  test::generateRandomTransform(extents<S>(), transform);
4193 
4194  std::vector<ContactPoint<S>> contacts;
4195 
4196  Quaternion<S> q(AngleAxis<S>((S)3.140 / 6, Vector3<S>(0, 0, 1)));
4197 
4198  tf1 = Transform3<S>::Identity();
4199  tf2 = Transform3<S>::Identity();
4200  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
4201  contacts.resize(4);
4202  contacts[0].normal << 1, 0, 0;
4203  contacts[1].normal << 1, 0, 0;
4204  contacts[2].normal << 1, 0, 0;
4205  contacts[3].normal << 1, 0, 0;
4206  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4207 
4208  tf1 = transform;
4209  tf2 = transform;
4210  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
4211  contacts.resize(4);
4212  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4213  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
4214  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
4215  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
4216  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4217 
4218  tf1 = Transform3<S>::Identity();
4219  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(15, 0, 0)));
4220  contacts.resize(4);
4221  contacts[0].normal = Vector3<S>(1, 0, 0);
4222  contacts[1].normal = Vector3<S>(1, 0, 0);
4223  contacts[2].normal = Vector3<S>(1, 0, 0);
4224  contacts[3].normal = Vector3<S>(1, 0, 0);
4225  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4226 
4227  tf1 = transform;
4228  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(15.01, 0, 0)));
4229  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4230 
4231  tf1 = Transform3<S>::Identity();
4232  tf2 = Transform3<S>(q);
4233  contacts.resize(4);
4234  contacts[0].normal = Vector3<S>(1, 0, 0);
4235  contacts[1].normal = Vector3<S>(1, 0, 0);
4236  contacts[2].normal = Vector3<S>(1, 0, 0);
4237  contacts[3].normal = Vector3<S>(1, 0, 0);
4238  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4239 
4240  tf1 = transform;
4241  tf2 = transform * Transform3<S>(q);
4242  contacts.resize(4);
4243  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4244  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
4245  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
4246  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
4247  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4248 }
4249 
4250 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_boxbox)
4251 {
4252 // test_shapeIntersectionGJK_boxbox<float>();
4253  test_shapeIntersectionGJK_boxbox<double>();
4254 }
4255 
4256 template <typename S>
4257 void test_shapeIntersectionGJK_spherebox()
4258 {
4259  Sphere<S> s1(20);
4260  Box<S> s2(5, 5, 5);
4261 
4262  Transform3<S> tf1;
4263  Transform3<S> tf2;
4264 
4265  Transform3<S> transform = Transform3<S>::Identity();
4266  test::generateRandomTransform(extents<S>(), transform);
4267 
4268  std::vector<ContactPoint<S>> contacts;
4269 
4270  tf1 = Transform3<S>::Identity();
4271  tf2 = Transform3<S>::Identity();
4272  // TODO: Need convention for normal when the centers of two objects are at same position.
4273  contacts.resize(1);
4274  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4275 
4276  tf1 = transform;
4277  tf2 = transform;
4278  // TODO: Need convention for normal when the centers of two objects are at same position.
4279  contacts.resize(1);
4280  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4281 
4282  tf1 = Transform3<S>::Identity();
4283  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.5, 0, 0)));
4284  contacts.resize(1);
4285  contacts[0].normal << 1, 0, 0;
4286  // TODO(SeanCurtis-TRI): This osculating contact is considered a collision
4287  // only for a relatively "loose" gjk tolerance. So, for this test to pass, we
4288  // set and restore the default gjk tolerances. We need to determine if this is
4289  // the correct/desired behavior and, if so, fix the defect that smaller
4290  // tolerances prevent this from reporting as a contact. NOTE: this is *not*
4291  // the same tolerance that is passed into the `testShapeIntersection` function
4292  // -- which isn't actually *used*.
4293  {
4294  detail::GJKSolver_indep<S>& solver = solver2<S>();
4295  const S old_gjk_tolerance = solver.gjk_tolerance;
4296  const S old_epa_tolerance = solver.epa_tolerance;
4297 
4298  // The historical tolerances for which this test passes.
4299  solver.gjk_tolerance = 1e-6;
4300  solver.epa_tolerance = 1e-6;
4301 
4302  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false,
4303  false, true, false,
4304  1e-7 // built-in GJK solver requires larger tolerance than libccd
4305  );
4306  // TODO(SeanCurtis-TRI): If testShapeIntersection fails an *assert* this
4307  // code will not get fired off and the static solvers will not get reset.
4308  solver.gjk_tolerance = old_gjk_tolerance;
4309  solver.epa_tolerance = old_epa_tolerance;
4310  }
4311 
4312  tf1 = transform;
4313  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.51, 0, 0)));
4314  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4315 
4316  tf1 = Transform3<S>::Identity();
4317  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
4318  contacts.resize(1);
4319  contacts[0].normal << 1, 0, 0;
4320  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd
4321 
4322  tf1 = transform;
4323  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
4324  contacts.resize(1);
4325  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4326  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4327  // built-in GJK solver returns incorrect normal.
4328 }
4329 
4330 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherebox)
4331 {
4332 // test_shapeIntersectionGJK_spherebox<float>();
4333  test_shapeIntersectionGJK_spherebox<double>();
4334 }
4335 
4336 template <typename S>
4337 void test_shapeIntersectionGJK_spherecapsule()
4338 {
4339  Sphere<S> s1(20);
4340  Capsule<S> s2(5, 10);
4341 
4342  Transform3<S> tf1;
4343  Transform3<S> tf2;
4344 
4345  Transform3<S> transform = Transform3<S>::Identity();
4346  test::generateRandomTransform(extents<S>(), transform);
4347 
4348  std::vector<ContactPoint<S>> contacts;
4349 
4350  tf1 = Transform3<S>::Identity();
4351  tf2 = Transform3<S>::Identity();
4352  // TODO: Need convention for normal when the centers of two objects are at same position.
4353  contacts.resize(1);
4354  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4355 
4356  tf1 = transform;
4357  tf2 = transform;
4358  // TODO: Need convention for normal when the centers of two objects are at same position.
4359  contacts.resize(1);
4360  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4361 
4362  tf1 = Transform3<S>::Identity();
4363  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
4364  contacts.resize(1);
4365  contacts[0].normal << 1, 0, 0;
4366  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4367 
4368  tf1 = transform;
4369  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
4370  contacts.resize(1);
4371  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4372  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4373 
4374  tf1 = Transform3<S>::Identity();
4375  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(25, 0, 0)));
4376  contacts.resize(1);
4377  contacts[0].normal << 1, 0, 0;
4378  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4379 
4380  tf1 = transform;
4381  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25.1, 0, 0)));
4382  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4383 }
4384 
4385 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherecapsule)
4386 {
4387 // test_shapeIntersectionGJK_spherecapsule<float>();
4388  test_shapeIntersectionGJK_spherecapsule<double>();
4389 }
4390 
4391 template <typename S>
4392 void test_shapeIntersectionGJK_cylindercylinder()
4393 {
4394  Cylinder<S> s1(5, 10);
4395  Cylinder<S> s2(5, 10);
4396 
4397  Transform3<S> tf1;
4398  Transform3<S> tf2;
4399 
4400  Transform3<S> transform = Transform3<S>::Identity();
4401  test::generateRandomTransform(extents<S>(), transform);
4402 
4403  std::vector<ContactPoint<S>> contacts;
4404 
4405  tf1 = Transform3<S>::Identity();
4406  tf2 = Transform3<S>::Identity();
4407  // TODO: Need convention for normal when the centers of two objects are at same position.
4408  contacts.resize(1);
4409  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4410 
4411  tf1 = transform;
4412  tf2 = transform;
4413  // TODO: Need convention for normal when the centers of two objects are at same position.
4414  contacts.resize(1);
4415  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4416 
4417  tf1 = Transform3<S>::Identity();
4418  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4419  contacts.resize(1);
4420  contacts[0].normal << 1, 0, 0;
4421  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd
4422 
4423  tf1 = transform;
4424  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4425  contacts.resize(1);
4426  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4427  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4428  // built-in GJK solver returns incorrect normal.
4429 
4430  tf1 = Transform3<S>::Identity();
4431  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
4432  contacts.resize(1);
4433  contacts[0].normal << 1, 0, 0;
4434  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4435 
4436  tf1 = transform;
4437  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
4438  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4439 }
4440 
4441 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercylinder)
4442 {
4443 // test_shapeIntersectionGJK_cylindercylinder<float>();
4444  test_shapeIntersectionGJK_cylindercylinder<double>();
4445 }
4446 
4447 template <typename S>
4448 void test_shapeIntersectionGJK_conecone()
4449 {
4450  Cone<S> s1(5, 10);
4451  Cone<S> s2(5, 10);
4452 
4453  Transform3<S> tf1;
4454  Transform3<S> tf2;
4455 
4456  Transform3<S> transform = Transform3<S>::Identity();
4457  test::generateRandomTransform(extents<S>(), transform);
4458 
4459  std::vector<ContactPoint<S>> contacts;
4460 
4461  tf1 = Transform3<S>::Identity();
4462  tf2 = Transform3<S>::Identity();
4463  // TODO: Need convention for normal when the centers of two objects are at same position.
4464  contacts.resize(1);
4465  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4466 
4467  tf1 = transform;
4468  tf2 = transform;
4469  // TODO: Need convention for normal when the centers of two objects are at same position.
4470  contacts.resize(1);
4471  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4472 
4473  tf1 = Transform3<S>::Identity();
4474  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4475  contacts.resize(1);
4476  contacts[0].normal << 1, 0, 0;
4477  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
4478 
4479  tf1 = transform;
4480  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4481  contacts.resize(1);
4482  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4483  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4484  // built-in GJK solver returns incorrect normal.
4485 
4486  tf1 = Transform3<S>::Identity();
4487  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
4488  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4489 
4490  tf1 = transform;
4491  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
4492  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4493 
4494  tf1 = Transform3<S>::Identity();
4495  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
4496  contacts.resize(1);
4497  contacts[0].normal << 0, 0, 1;
4498  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4499 
4500  tf1 = transform;
4501  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
4502  contacts.resize(1);
4503  // contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
4504  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4505  // built-in GJK solver returns incorrect normal.
4506 }
4507 
4508 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_conecone)
4509 {
4510 // test_shapeIntersectionGJK_conecone<float>();
4511  test_shapeIntersectionGJK_conecone<double>();
4512 }
4513 
4514 template <typename S>
4515 void test_shapeIntersectionGJK_cylindercone()
4516 {
4517  Cylinder<S> s1(5, 10);
4518  Cone<S> s2(5, 10);
4519 
4520  Transform3<S> tf1;
4521  Transform3<S> tf2;
4522 
4523  Transform3<S> transform = Transform3<S>::Identity();
4524  test::generateRandomTransform(extents<S>(), transform);
4525 
4526  std::vector<ContactPoint<S>> contacts;
4527  tf1 = Transform3<S>::Identity();
4528  tf2 = Transform3<S>::Identity();
4529  // TODO: Need convention for normal when the centers of two objects are at same position.
4530  contacts.resize(1);
4531  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4532 
4533  tf1 = transform;
4534  tf2 = transform;
4535  // TODO: Need convention for normal when the centers of two objects are at same position.
4536  contacts.resize(1);
4537  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4538 
4539  tf1 = Transform3<S>::Identity();
4540  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4541  contacts.resize(1);
4542  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4543 
4544  tf1 = transform;
4545  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
4546  contacts.resize(1);
4547  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4548 
4549  tf1 = Transform3<S>::Identity();
4550  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
4551  contacts.resize(1);
4552  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4553 
4554  tf1 = transform;
4555  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
4556  contacts.resize(1);
4557  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4558 
4559  tf1 = Transform3<S>::Identity();
4560  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
4561  contacts.resize(1);
4562  contacts[0].normal << 0, 0, 1;
4563  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4564 
4565  tf1 = transform;
4566  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
4567  contacts.resize(1);
4568  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
4569  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4570  // built-in GJK solver returns incorrect normal.
4571 
4572  tf1 = Transform3<S>::Identity();
4573  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10)));
4574  contacts.resize(1);
4575  contacts[0].normal << 0, 0, 1;
4576  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
4577 
4578  tf1 = transform;
4579  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
4580  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4581 }
4582 
4583 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercone)
4584 {
4585 // test_shapeIntersectionGJK_cylindercone<float>();
4586  test_shapeIntersectionGJK_cylindercone<double>();
4587 }
4588 
4589 template <typename S>
4590 void test_shapeIntersectionGJK_ellipsoidellipsoid()
4591 {
4592  Ellipsoid<S> s1(20, 40, 50);
4593  Ellipsoid<S> s2(10, 10, 10);
4594 
4595  Transform3<S> tf1;
4596  Transform3<S> tf2;
4597 
4598  Transform3<S> transform = Transform3<S>::Identity();
4599  test::generateRandomTransform(extents<S>(), transform);
4600  Transform3<S> identity;
4601 
4602  std::vector<ContactPoint<S>> contacts;
4603 
4604  tf1 = Transform3<S>::Identity();
4605  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
4606  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4607 
4608  tf1 = transform;
4609  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
4610  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4611 
4612  tf1 = Transform3<S>::Identity();
4613  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
4614  contacts.resize(1);
4615  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4616 
4617  tf1 = transform;
4618  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
4619  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4620 
4621  tf1 = Transform3<S>::Identity();
4622  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.99, 0, 0)));
4623  contacts.resize(1);
4624  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4625 
4626  tf1 = transform;
4627  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
4628  contacts.resize(1);
4629  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4630 
4631  tf1 = Transform3<S>::Identity();
4632  tf2 = Transform3<S>::Identity();
4633  contacts.resize(1);
4634  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4635 
4636  tf1 = transform;
4637  tf2 = transform;
4638  contacts.resize(1);
4639  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4640 
4641  tf1 = Transform3<S>::Identity();
4642  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
4643  contacts.resize(1);
4644  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4645 
4646  tf1 = transform;
4647  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
4648  contacts.resize(1);
4649  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4650 
4651  tf1 = Transform3<S>::Identity();
4652  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30, 0, 0)));
4653  contacts.resize(1);
4654  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
4655 
4656  tf1 = transform;
4657  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
4658  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
4659 }
4660 
4661 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_ellipsoidellipsoid)
4662 {
4663 // test_shapeIntersectionGJK_ellipsoidellipsoid<float>();
4664  test_shapeIntersectionGJK_ellipsoidellipsoid<double>();
4665 }
4666 
4667 template <typename S>
4668 void test_shapeIntersectionGJK_spheretriangle()
4669 {
4670  Sphere<S> s(10);
4671  Vector3<S> t[3];
4672  t[0] << 20, 0, 0;
4673  t[1] << -20, 0, 0;
4674  t[2] << 0, 20, 0;
4675 
4676  Transform3<S> transform = Transform3<S>::Identity();
4677  test::generateRandomTransform(extents<S>(), transform);
4678 
4679  // Vector3<S> point;
4680  // S depth;
4681  Vector3<S> normal;
4682  bool res;
4683 
4684  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
4685  EXPECT_TRUE(res);
4686 
4687  res = solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4688  EXPECT_TRUE(res);
4689 
4690  t[0] << 30, 0, 0;
4691  t[1] << 9.9, -20, 0;
4692  t[2] << 9.9, 20, 0;
4693  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
4694  EXPECT_TRUE(res);
4695 
4696  res = solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4697  EXPECT_TRUE(res);
4698 
4699  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, &normal);
4700  EXPECT_TRUE(res);
4701  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
4702 
4703  res = solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
4704  EXPECT_TRUE(res);
4705  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
4706 }
4707 
4708 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheretriangle)
4709 {
4710 // test_shapeIntersectionGJK_spheretriangle<float>();
4711  test_shapeIntersectionGJK_spheretriangle<double>();
4712 }
4713 
4714 template <typename S>
4715 void test_shapeIntersectionGJK_halfspacetriangle()
4716 {
4717  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
4718  Vector3<S> t[3];
4719  t[0] << 20, 0, 0;
4720  t[1] << -20, 0, 0;
4721  t[2] << 0, 20, 0;
4722 
4723  Transform3<S> transform = Transform3<S>::Identity();
4724  test::generateRandomTransform(extents<S>(), transform);
4725 
4726  // Vector3<S> point;
4727  // S depth;
4728  Vector3<S> normal;
4729  bool res;
4730 
4731  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
4732  EXPECT_TRUE(res);
4733 
4734  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4735  EXPECT_TRUE(res);
4736 
4737 
4738  t[0] << 20, 0, 0;
4739  t[1] << -0.1, -20, 0;
4740  t[2] << -0.1, 20, 0;
4741  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
4742  EXPECT_TRUE(res);
4743 
4744  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4745  EXPECT_TRUE(res);
4746 
4747  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
4748  EXPECT_TRUE(res);
4749  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
4750 
4751  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
4752  EXPECT_TRUE(res);
4753  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
4754 }
4755 
4756 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_halfspacetriangle)
4757 {
4758 // test_shapeIntersectionGJK_halfspacetriangle<float>();
4759  test_shapeIntersectionGJK_halfspacetriangle<double>();
4760 }
4761 
4762 template <typename S>
4763 void test_shapeIntersectionGJK_planetriangle()
4764 {
4765  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
4766  Vector3<S> t[3];
4767  t[0] << 20, 0, 0;
4768  t[1] << -20, 0, 0;
4769  t[2] << 0, 20, 0;
4770 
4771  Transform3<S> transform = Transform3<S>::Identity();
4772  test::generateRandomTransform(extents<S>(), transform);
4773 
4774  // Vector3<S> point;
4775  // S depth;
4776  Vector3<S> normal;
4777  bool res;
4778 
4779  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
4780  EXPECT_TRUE(res);
4781 
4782  res = solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4783  EXPECT_TRUE(res);
4784 
4785 
4786  t[0] << 20, 0, 0;
4787  t[1] << -0.1, -20, 0;
4788  t[2] << -0.1, 20, 0;
4789  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
4790  EXPECT_TRUE(res);
4791 
4792  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
4793  EXPECT_TRUE(res);
4794 
4795  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
4796  EXPECT_TRUE(res);
4797  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));
4798 
4799  res = solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
4800  EXPECT_TRUE(res);
4801  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
4802 }
4803 
4804 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_planetriangle)
4805 {
4806 // test_shapeIntersectionGJK_planetriangle<float>();
4807  test_shapeIntersectionGJK_planetriangle<double>();
4808 }
4809 
4810 // Shape distance test coverage (built-in GJK)
4811 //
4812 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4813 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
4814 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4815 // | box | O | O | | | | | | | |
4816 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4817 // | sphere |/////| O | | | | | | | |
4818 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4819 // | ellipsoid |/////|////////| O | | | | | | |
4820 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4821 // | capsule |/////|////////|///////////| | | | | | |
4822 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4823 // | cone |/////|////////|///////////|/////////| O | | | | |
4824 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4825 // | cylinder |/////|////////|///////////|/////////|//////| O | | | |
4826 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4827 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
4828 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4829 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
4830 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4831 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
4832 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
4833 
4834 template <typename S>
4835 void test_shapeDistanceGJK_spheresphere()
4836 {
4837  Sphere<S> s1(20);
4838  Sphere<S> s2(10);
4839 
4840  Transform3<S> transform = Transform3<S>::Identity();
4841  test::generateRandomTransform(extents<S>(), transform);
4842 
4843  bool res;
4844  S dist = -1;
4845 
4846  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
4847  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4848  EXPECT_TRUE(res);
4849 
4850  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
4851  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4852  EXPECT_TRUE(res);
4853 
4854  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
4855  EXPECT_TRUE(dist < 0);
4856  EXPECT_FALSE(res);
4857 
4858  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4859  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4860  EXPECT_TRUE(res);
4861 
4862  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4863  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4864  EXPECT_TRUE(res);
4865 
4866  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
4867  EXPECT_TRUE(dist < 0);
4868  EXPECT_FALSE(res);
4869 
4870 
4871  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
4872  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4873  EXPECT_TRUE(res);
4874 
4875  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
4876  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4877  EXPECT_TRUE(res);
4878 
4879  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
4880  EXPECT_TRUE(dist < 0);
4881  EXPECT_FALSE(res);
4882 
4883  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
4884  EXPECT_TRUE(fabs(dist - 10) < 0.001);
4885  EXPECT_TRUE(res);
4886 
4887  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
4888  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4889  EXPECT_TRUE(res);
4890 
4891  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
4892  EXPECT_TRUE(dist < 0);
4893  EXPECT_FALSE(res);
4894 }
4895 
4896 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_spheresphere)
4897 {
4898 // test_shapeDistanceGJK_spheresphere<float>();
4899  test_shapeDistanceGJK_spheresphere<double>();
4900 }
4901 
4902 template <typename S>
4903 void test_shapeDistanceGJK_boxbox()
4904 {
4905  Box<S> s1(20, 40, 50);
4906  Box<S> s2(10, 10, 10);
4907 
4908  Transform3<S> transform = Transform3<S>::Identity();
4909  test::generateRandomTransform(extents<S>(), transform);
4910 
4911  bool res;
4912  S dist;
4913 
4914  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
4915  EXPECT_TRUE(dist < 0);
4916  EXPECT_FALSE(res);
4917 
4918  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
4919  EXPECT_TRUE(dist < 0);
4920  EXPECT_FALSE(res);
4921 
4922  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
4923  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4924  EXPECT_TRUE(res);
4925 
4926  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
4927  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
4928  EXPECT_TRUE(res);
4929 
4930  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
4931  EXPECT_TRUE(fabs(dist - 5) < 0.001);
4932  EXPECT_TRUE(res);
4933 
4934  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
4935  EXPECT_TRUE(fabs(dist - 5) < 0.001);
4936  EXPECT_TRUE(res);
4937 }
4938 
4939 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxbox)
4940 {
4941 // test_shapeDistanceGJK_boxbox<float>();
4942  test_shapeDistanceGJK_boxbox<double>();
4943 }
4944 
4945 template <typename S>
4946 void test_shapeDistanceGJK_boxsphere()
4947 {
4948  Sphere<S> s1(20);
4949  Box<S> s2(5, 5, 5);
4950 
4951  Transform3<S> transform = Transform3<S>::Identity();
4952  test::generateRandomTransform(extents<S>(), transform);
4953 
4954  bool res;
4955  S dist;
4956 
4957  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
4958  EXPECT_TRUE(dist < 0);
4959  EXPECT_FALSE(res);
4960 
4961  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
4962  EXPECT_TRUE(dist < 0);
4963  EXPECT_FALSE(res);
4964 
4965  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
4966  EXPECT_TRUE(fabs(dist - 0.1) < 0.01);
4967  EXPECT_TRUE(res);
4968 
4969  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
4970  EXPECT_TRUE(fabs(dist - 0.1) < 0.01);
4971  EXPECT_TRUE(res);
4972 
4973  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
4974  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
4975  EXPECT_TRUE(res);
4976 
4977  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
4978  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
4979  EXPECT_TRUE(res);
4980 }
4981 
4982 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxsphere)
4983 {
4984 // test_shapeDistanceGJK_boxsphere<float>();
4985  test_shapeDistanceGJK_boxsphere<double>();
4986 }
4987 
4988 template <typename S>
4989 void test_shapeDistanceGJK_cylindercylinder()
4990 {
4991  Cylinder<S> s1(5, 10);
4992  Cylinder<S> s2(5, 10);
4993 
4994  Transform3<S> transform = Transform3<S>::Identity();
4995  test::generateRandomTransform(extents<S>(), transform);
4996 
4997  bool res;
4998  S dist;
4999 
5000  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
5001  EXPECT_TRUE(dist < 0);
5002  EXPECT_FALSE(res);
5003 
5004  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
5005  EXPECT_TRUE(dist < 0);
5006  EXPECT_FALSE(res);
5007 
5008  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
5009  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
5010  EXPECT_TRUE(res);
5011 
5012  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
5013  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
5014  EXPECT_TRUE(res);
5015 
5016  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
5017  EXPECT_TRUE(fabs(dist - 30) < 0.001);
5018  EXPECT_TRUE(res);
5019 
5020  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
5021  EXPECT_TRUE(fabs(dist - 30) < 0.001);
5022  EXPECT_TRUE(res);
5023 }
5024 
5025 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_cylindercylinder)
5026 {
5027 // test_shapeDistanceGJK_cylindercylinder<float>();
5028  test_shapeDistanceGJK_cylindercylinder<double>();
5029 }
5030 
5031 template <typename S>
5032 void test_shapeDistanceGJK_conecone()
5033 {
5034  Cone<S> s1(5, 10);
5035  Cone<S> s2(5, 10);
5036 
5037  Transform3<S> transform = Transform3<S>::Identity();
5038  test::generateRandomTransform(extents<S>(), transform);
5039 
5040  bool res;
5041  S dist;
5042 
5043  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
5044  EXPECT_TRUE(dist < 0);
5045  EXPECT_FALSE(res);
5046 
5047  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
5048  EXPECT_TRUE(dist < 0);
5049  EXPECT_FALSE(res);
5050 
5051  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
5052  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
5053  EXPECT_TRUE(res);
5054 
5055  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
5056  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
5057  EXPECT_TRUE(res);
5058 
5059  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
5060  EXPECT_TRUE(fabs(dist - 30) < 0.001);
5061  EXPECT_TRUE(res);
5062 
5063  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
5064  EXPECT_TRUE(fabs(dist - 30) < 0.001);
5065  EXPECT_TRUE(res);
5066 }
5067 
5068 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_conecone)
5069 {
5070 // test_shapeDistanceGJK_conecone<float>();
5071  test_shapeDistanceGJK_conecone<double>();
5072 }
5073 
5074 template <typename S>
5075 void test_shapeDistanceGJK_ellipsoidellipsoid()
5076 {
5077  Ellipsoid<S> s1(20, 40, 50);
5078  Ellipsoid<S> s2(10, 10, 10);
5079 
5080  Transform3<S> transform = Transform3<S>::Identity();
5081  test::generateRandomTransform(extents<S>(), transform);
5082 
5083  bool res;
5084  S dist = -1;
5085 
5086  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
5087  EXPECT_TRUE(fabs(dist - 10) < 0.001);
5088  EXPECT_TRUE(res);
5089 
5090  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
5091  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
5092  EXPECT_TRUE(res);
5093 
5094  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
5095  EXPECT_TRUE(dist < 0);
5096  EXPECT_FALSE(res);
5097 
5098  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
5099  EXPECT_TRUE(fabs(dist - 10) < 0.001);
5100  EXPECT_TRUE(res);
5101 
5102  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
5103  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
5104  EXPECT_TRUE(res);
5105 
5106  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
5107  EXPECT_TRUE(dist < 0);
5108  EXPECT_FALSE(res);
5109 
5110  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
5111  EXPECT_TRUE(fabs(dist - 10) < 0.001);
5112  EXPECT_TRUE(res);
5113 
5114  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
5115  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
5116  EXPECT_TRUE(res);
5117 
5118  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
5119  EXPECT_TRUE(dist < 0);
5120  EXPECT_FALSE(res);
5121 
5122  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
5123  EXPECT_TRUE(fabs(dist - 10) < 0.001);
5124  EXPECT_TRUE(res);
5125 
5126  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
5127  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
5128  EXPECT_TRUE(res);
5129 
5130  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
5131  EXPECT_TRUE(dist < 0);
5132  EXPECT_FALSE(res);
5133 }
5134 
5135 GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_ellipsoidellipsoid)
5136 {
5137 // test_shapeDistanceGJK_ellipsoidellipsoid<float>();
5138  test_shapeDistanceGJK_ellipsoidellipsoid<double>();
5139 }
5140 
5141 template<typename Shape1, typename Shape2>
5142 void testReversibleShapeIntersection(const Shape1& s1, const Shape2& s2, typename Shape2::S distance)
5143 {
5144  using S = typename Shape2::S;
5145 
5146  Transform3<S> tf1(Translation3<S>(Vector3<S>(-0.5 * distance, 0.0, 0.0)));
5147  Transform3<S> tf2(Translation3<S>(Vector3<S>(+0.5 * distance, 0.0, 0.0)));
5148 
5149  std::vector<ContactPoint<S>> contactsA;
5150  std::vector<ContactPoint<S>> contactsB;
5151 
5152  bool resA;
5153  bool resB;
5154 
5155  const double tol = 1e-6;
5156 
5157  resA = solver1<S>().shapeIntersect(s1, tf1, s2, tf2, &contactsA);
5158  resB = solver1<S>().shapeIntersect(s2, tf2, s1, tf1, &contactsB);
5159 
5160  // normal should be opposite
5161  for (size_t i = 0; i < contactsB.size(); ++i)
5162  contactsB[i].normal = -contactsB[i].normal;
5163 
5164  EXPECT_TRUE(resA);
5165  EXPECT_TRUE(resB);
5166  EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, GST_LIBCCD,
5167  contactsA, contactsB,
5168  true, true, true, false, tol));
5169 
5170  resA = solver2<S>().shapeIntersect(s1, tf1, s2, tf2, &contactsA);
5171  resB = solver2<S>().shapeIntersect(s2, tf2, s1, tf1, &contactsB);
5172 
5173  // normal should be opposite
5174  for (size_t i = 0; i < contactsB.size(); ++i)
5175  contactsB[i].normal = -contactsB[i].normal;
5176 
5177  EXPECT_TRUE(resA);
5178  EXPECT_TRUE(resB);
5179  EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, GST_INDEP,
5180  contactsA, contactsB,
5181  true, true, true, false, tol));
5182 }
5183 
5184 template <typename S>
5185 void test_reversibleShapeIntersection_allshapes()
5186 {
5187  // This test check whether a shape intersection algorithm is called for the
5188  // reverse case as well. For example, if FCL has sphere-capsule intersection
5189  // algorithm, then this algorithm should be called for capsule-sphere case.
5190 
5191  // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace
5192  Box<S> box(10, 10, 10);
5193  Sphere<S> sphere(5);
5194  Ellipsoid<S> ellipsoid(5, 5, 5);
5195  Capsule<S> capsule(5, 10);
5196  Cone<S> cone(5, 10);
5197  Cylinder<S> cylinder(5, 10);
5198  Plane<S> plane(Vector3<S>::Zero(), 0.0);
5199  Halfspace<S> halfspace(Vector3<S>::Zero(), 0.0);
5200 
5201  // Use sufficiently short distance so that all the primitive shapes can intersect
5202  S distance = 5.0;
5203 
5204  // If new shape intersection algorithm is added for two distinct primitive
5205  // shapes, uncomment associated lines. For example, box-sphere intersection
5206  // algorithm is added, then uncomment box-sphere.
5207 
5208 // testReversibleShapeIntersection(box, sphere, distance);
5209 // testReversibleShapeIntersection(box, ellipsoid, distance);
5210 // testReversibleShapeIntersection(box, capsule, distance);
5211 // testReversibleShapeIntersection(box, cone, distance);
5212 // testReversibleShapeIntersection(box, cylinder, distance);
5213  testReversibleShapeIntersection(box, plane, distance);
5214  testReversibleShapeIntersection(box, halfspace, distance);
5215 
5216 // testReversibleShapeIntersection(sphere, ellipsoid, distance);
5217  testReversibleShapeIntersection(sphere, capsule, distance);
5218 // testReversibleShapeIntersection(sphere, cone, distance);
5219 // testReversibleShapeIntersection(sphere, cylinder, distance);
5220  testReversibleShapeIntersection(sphere, plane, distance);
5221  testReversibleShapeIntersection(sphere, halfspace, distance);
5222 
5223 // testReversibleShapeIntersection(ellipsoid, capsule, distance);
5224 // testReversibleShapeIntersection(ellipsoid, cone, distance);
5225 // testReversibleShapeIntersection(ellipsoid, cylinder, distance);
5226 // testReversibleShapeIntersection(ellipsoid, plane, distance);
5227 // testReversibleShapeIntersection(ellipsoid, halfspace, distance);
5228 
5229 // testReversibleShapeIntersection(capsule, cone, distance);
5230 // testReversibleShapeIntersection(capsule, cylinder, distance);
5231  testReversibleShapeIntersection(capsule, plane, distance);
5232  testReversibleShapeIntersection(capsule, halfspace, distance);
5233 
5234 // testReversibleShapeIntersection(cone, cylinder, distance);
5235  testReversibleShapeIntersection(cone, plane, distance);
5236  testReversibleShapeIntersection(cone, halfspace, distance);
5237 
5238  testReversibleShapeIntersection(cylinder, plane, distance);
5239  testReversibleShapeIntersection(cylinder, halfspace, distance);
5240 
5241  testReversibleShapeIntersection(plane, halfspace, distance);
5242 }
5243 
5244 GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeIntersection_allshapes)
5245 {
5246 // test_reversibleShapeIntersection_allshapes<float>();
5247  test_reversibleShapeIntersection_allshapes<double>();
5248 }
5249 
5250 template<typename Shape1, typename Shape2>
5251 void testReversibleShapeDistance(const Shape1& s1, const Shape2& s2, typename Shape2::S distance)
5252 {
5253  using S = typename Shape2::S;
5254 
5255  Transform3<S> tf1(Translation3<S>(Vector3<S>(-0.5 * distance, 0.0, 0.0)));
5256  Transform3<S> tf2(Translation3<S>(Vector3<S>(+0.5 * distance, 0.0, 0.0)));
5257 
5258  S distA;
5259  S distB;
5260  Vector3<S> p1A;
5261  Vector3<S> p1B;
5262  Vector3<S> p2A;
5263  Vector3<S> p2B;
5264 
5265  bool resA;
5266  bool resB;
5267 
5268  const double tol = 1e-6;
5269 
5270  resA = solver1<S>().shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
5271  resB = solver1<S>().shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
5272 
5273  EXPECT_TRUE(resA);
5274  EXPECT_TRUE(resB);
5275  EXPECT_NEAR(distA, distB, tol); // distances should be same
5276  EXPECT_TRUE(p1A.isApprox(p2B, tol)); // closest points should in reverse order
5277  EXPECT_TRUE(p2A.isApprox(p1B, tol));
5278 
5279  resA = solver2<S>().shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
5280  resB = solver2<S>().shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
5281 
5282  EXPECT_TRUE(resA);
5283  EXPECT_TRUE(resB);
5284  EXPECT_NEAR(distA, distB, tol);
5285  EXPECT_TRUE(p1A.isApprox(p2B, tol));
5286  EXPECT_TRUE(p2A.isApprox(p1B, tol));
5287 }
5288 
5289 template <typename S>
5290 void test_reversibleShapeDistance_allshapes()
5291 {
5292  // This test check whether a shape distance algorithm is called for the
5293  // reverse case as well. For example, if FCL has sphere-capsule distance
5294  // algorithm, then this algorithm should be called for capsule-sphere case.
5295 
5296  // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace
5297  Box<S> box(10, 10, 10);
5298  Sphere<S> sphere(5);
5299  Ellipsoid<S> ellipsoid(5, 5, 5);
5300  Capsule<S> capsule(5, 10);
5301  Cone<S> cone(5, 10);
5302  Cylinder<S> cylinder(5, 10);
5303  Plane<S> plane(Vector3<S>::Zero(), 0.0);
5304  Halfspace<S> halfspace(Vector3<S>::Zero(), 0.0);
5305 
5306  // Use sufficiently long distance so that all the primitive shapes CANNOT intersect
5307  S distance = 15.0;
5308 
5309  // If new shape distance algorithm is added for two distinct primitive
5310  // shapes, uncomment associated lines. For example, box-sphere intersection
5311  // algorithm is added, then uncomment box-sphere.
5312 
5313 // testReversibleShapeDistance(box, sphere, distance);
5314 // testReversibleShapeDistance(box, ellipsoid, distance);
5315 // testReversibleShapeDistance(box, capsule, distance);
5316 // testReversibleShapeDistance(box, cone, distance);
5317 // testReversibleShapeDistance(box, cylinder, distance);
5318 // testReversibleShapeDistance(box, plane, distance);
5319 // testReversibleShapeDistance(box, halfspace, distance);
5320 
5321 // testReversibleShapeDistance(sphere, ellipsoid, distance);
5322  testReversibleShapeDistance(sphere, capsule, distance);
5323 // testReversibleShapeDistance(sphere, cone, distance);
5324 // testReversibleShapeDistance(sphere, cylinder, distance);
5325 // testReversibleShapeDistance(sphere, plane, distance);
5326 // testReversibleShapeDistance(sphere, halfspace, distance);
5327 
5328 // testReversibleShapeDistance(ellipsoid, capsule, distance);
5329 // testReversibleShapeDistance(ellipsoid, cone, distance);
5330 // testReversibleShapeDistance(ellipsoid, cylinder, distance);
5331 // testReversibleShapeDistance(ellipsoid, plane, distance);
5332 // testReversibleShapeDistance(ellipsoid, halfspace, distance);
5333 
5334 // testReversibleShapeDistance(capsule, cone, distance);
5335 // testReversibleShapeDistance(capsule, cylinder, distance);
5336 // testReversibleShapeDistance(capsule, plane, distance);
5337 // testReversibleShapeDistance(capsule, halfspace, distance);
5338 
5339 // testReversibleShapeDistance(cone, cylinder, distance);
5340 // testReversibleShapeDistance(cone, plane, distance);
5341 // testReversibleShapeDistance(cone, halfspace, distance);
5342 
5343 // testReversibleShapeDistance(cylinder, plane, distance);
5344 // testReversibleShapeDistance(cylinder, halfspace, distance);
5345 
5346 // testReversibleShapeDistance(plane, halfspace, distance);
5347 }
5348 
5349 GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeDistance_allshapes)
5350 {
5351 // test_reversibleShapeDistance_allshapes<float>();
5352  test_reversibleShapeDistance_allshapes<double>();
5353 }
5354 
5355 //==============================================================================
5356 int main(int argc, char* argv[])
5357 {
5358  ::testing::InitGoogleTest(&argc, argv);
5359  return RUN_ALL_TESTS();
5360 }
test_shapeIntersectionGJK_spheresphere
void test_shapeIntersectionGJK_spheresphere()
Definition: test_fcl_geometric_shapes.cpp:4074
test_shapeIntersectionGJK_planetriangle
void test_shapeIntersectionGJK_planetriangle()
Definition: test_fcl_geometric_shapes.cpp:4763
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:92
fcl::CollisionRequest::gjk_solver_type
GJKSolverType gjk_solver_type
Enumeration indicating the GJK solver implementation to use.
Definition: collision_request.h:78
fcl::test::Timer
Definition: test_fcl_utility.h:77
fcl::GJKSolverType
GJKSolverType
Type of narrow phase GJK solver.
Definition: gjk_solver_type.h:45
test_gjkcache
void test_gjkcache()
Definition: test_fcl_geometric_shapes.cpp:111
test_shapeIntersectionGJK_spherebox
void test_shapeIntersectionGJK_spherebox()
Definition: test_fcl_geometric_shapes.cpp:4257
fcl::ContactPoint
Minimal contact information returned by collision.
Definition: contact_point.h:48
test_shapeDistance_ellipsoidellipsoid
void test_shapeDistance_ellipsoidellipsoid()
Definition: test_fcl_geometric_shapes.cpp:3981
fcl::CollisionRequest::num_max_contacts
size_t num_max_contacts
The maximum number of contacts that can be returned.
Definition: collision_request.h:59
testShapeIntersection
void testShapeIntersection(const Shape1 &s1, const Transform3< typename Shape1::S > &tf1, const Shape2 &s2, const Transform3< typename Shape1::S > &tf2, GJKSolverType solver_type, bool expected_res, const std::vector< ContactPoint< typename Shape1::S >> &expected_contacts=std::vector< ContactPoint< typename Shape1::S >>(), bool check_position=true, bool check_depth=true, bool check_normal=true, bool check_opposite_normal=false, typename Shape1::S tol=1e-9)
Definition: test_fcl_geometric_shapes.cpp:428
translation_motion.h
fcl::ContactPoint::normal
Vector3< S > normal
Contact normal, pointing from o1 to o2.
Definition: contact_point.h:51
fcl::test::Timer::getElapsedTime
double getElapsedTime()
get elapsed time in milli-second
Definition: test_fcl_utility.cpp:129
fcl::detail::GJKSolver_libccd
collision and distance solver based on libccd library.
Definition: gjk_solver_libccd.h:54
test_shapeIntersection_halfspacecone
void test_shapeIntersection_halfspacecone()
Definition: test_fcl_geometric_shapes.cpp:3173
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
fcl::Quaternion
Eigen::Quaternion< S > Quaternion
Definition: types.h:89
fcl::Sphere::computeVolume
S computeVolume() const override
Definition: sphere-inl.h:88
test_shapeIntersection_spherebox
void test_shapeIntersection_spherebox()
Definition: test_fcl_geometric_shapes.cpp:835
test_shapeDistanceGJK_spheresphere
void test_shapeDistanceGJK_spheresphere()
Definition: test_fcl_geometric_shapes.cpp:4835
fcl::uint32
std::uint32_t uint32
Definition: types.h:63
test_shapeIntersectionGJK_cylindercylinder
void test_shapeIntersectionGJK_cylindercylinder()
Definition: test_fcl_geometric_shapes.cpp:4392
compareContactPointds1
bool compareContactPointds1(const Vector3< S > &c1, const Vector3< S > &c2)
Definition: test_fcl_geometric_shapes.cpp:653
fcl::test::Timer::start
void start()
start timer
Definition: test_fcl_utility.cpp:75
test_shapeDistance_cylindercylinder
void test_shapeDistance_cylindercylinder()
Definition: test_fcl_geometric_shapes.cpp:3852
fcl::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: collision_result-inl.h:76
fcl::test::eulerToMatrix
void eulerToMatrix(S a, S b, S c, Matrix3< S > &R)
Definition: test_fcl_utility.h:313
printComparisonError
void printComparisonError(const std::string &comparison_type, const Shape1 &s1, const Transform3< typename Shape1::S > &tf1, const Shape2 &s2, const Transform3< typename Shape1::S > &tf2, GJKSolverType solver_type, const Vector3< typename Shape1::S > &expected_contact_or_normal, const Vector3< typename Shape1::S > &actual_contact_or_normal, bool check_opposite_normal, typename Shape1::S tol)
Definition: test_fcl_geometric_shapes.cpp:178
unused.h
fcl::detail::GJKSolver_indep::epa_tolerance
S epa_tolerance
the threshold used in EPA to stop iteration
Definition: gjk_solver_indep.h:171
halfspace.h
test_reversibleShapeIntersection_allshapes
void test_reversibleShapeIntersection_allshapes()
Definition: test_fcl_geometric_shapes.cpp:5185
max
static T max(T x, T y)
Definition: svm.cpp:52
test_shapeIntersection_planeellipsoid
void test_shapeIntersection_planeellipsoid()
Definition: test_fcl_geometric_shapes.cpp:2010
test_shapeIntersection_spheresphere
void test_shapeIntersection_spheresphere()
Definition: test_fcl_geometric_shapes.cpp:544
fcl::transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
fcl::ContactPoint::penetration_depth
S penetration_depth
Penetration depth.
Definition: contact_point.h:57
fcl::CollisionResult::clear
void clear()
clear the results obtained
Definition: collision_result-inl.h:125
fcl::TranslationMotion
Definition: bv_motion_bound_visitor.h:57
test_shapeDistance_boxsphere
void test_shapeDistance_boxsphere()
Definition: test_fcl_geometric_shapes.cpp:3809
GTEST_TEST
GTEST_TEST(FCL_GEOMETRIC_SHAPES, sphere_shape)
Definition: test_fcl_geometric_shapes.cpp:104
solver2
detail::GJKSolver_indep< S > & solver2()
Definition: test_fcl_geometric_shapes.cpp:77
test_reversibleShapeDistance_allshapes
void test_reversibleShapeDistance_allshapes()
Definition: test_fcl_geometric_shapes.cpp:5290
test_fcl_utility.h
fcl::CollisionResult
collision result
Definition: collision_request.h:48
test_shapeDistanceGJK_conecone
void test_shapeDistanceGJK_conecone()
Definition: test_fcl_geometric_shapes.cpp:5032
test_shapeIntersection_conecone
void test_shapeIntersection_conecone()
Definition: test_fcl_geometric_shapes.cpp:1035
EXPECT_TRUE
#define EXPECT_TRUE(args)
test_shapeIntersectionGJK_ellipsoidellipsoid
void test_shapeIntersectionGJK_ellipsoidellipsoid()
Definition: test_fcl_geometric_shapes.cpp:4590
test_shapeDistanceGJK_cylindercylinder
void test_shapeDistanceGJK_cylindercylinder()
Definition: test_fcl_geometric_shapes.cpp:4989
fcl::AngleAxis
Eigen::AngleAxis< S > AngleAxis
Definition: types.h:98
testReversibleShapeIntersection
void testReversibleShapeIntersection(const Shape1 &s1, const Shape2 &s2, typename Shape2::S distance)
Definition: test_fcl_geometric_shapes.cpp:5142
plane.h
test_shapeIntersection_halfspacecylinder
void test_shapeIntersection_halfspacecylinder()
Definition: test_fcl_geometric_shapes.cpp:2703
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:71
cone.h
test_shapeIntersection_planecapsule
void test_shapeIntersection_planecapsule()
Definition: test_fcl_geometric_shapes.cpp:2480
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:86
ellipsoid.h
gjk_solver_libccd.h
test_shapeIntersection_planesphere
void test_shapeIntersection_planesphere()
Definition: test_fcl_geometric_shapes.cpp:1490
test_shapeDistanceGJK_ellipsoidellipsoid
void test_shapeDistanceGJK_ellipsoidellipsoid()
Definition: test_fcl_geometric_shapes.cpp:5075
test_shapeIntersection_cylindercylinder
void test_shapeIntersection_cylindercylinder()
Definition: test_fcl_geometric_shapes.cpp:972
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
getContactPointdsFromResult
void getContactPointdsFromResult(std::vector< ContactPoint< S >> &contacts, const CollisionResult< S > &result)
Definition: test_fcl_geometric_shapes.cpp:412
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
fcl::ContactPoint::pos
Vector3< S > pos
Contact position, in world space.
Definition: contact_point.h:54
fcl::detail::GJKSolver_indep::gjk_tolerance
S gjk_tolerance
the threshold used in GJK to stop iteration
Definition: gjk_solver_indep.h:174
fcl::test::getNodeTypeName
std::string getNodeTypeName(NODE_TYPE node_type)
Definition: test_fcl_utility.cpp:135
test_shapeIntersection_halfspacecapsule
void test_shapeIntersection_halfspacecapsule()
Definition: test_fcl_geometric_shapes.cpp:2233
fcl::detail::GJKSolver_indep
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: gjk_solver_indep.h:54
testReversibleShapeDistance
void testReversibleShapeDistance(const Shape1 &s1, const Shape2 &s2, typename Shape2::S distance)
Definition: test_fcl_geometric_shapes.cpp:5251
testBoxBoxContactPointds
void testBoxBoxContactPointds(const Eigen::MatrixBase< Derived > &R)
Definition: test_fcl_geometric_shapes.cpp:689
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_result-inl.h:83
compareContactPointds2
bool compareContactPointds2(const ContactPoint< S > &cp1, const ContactPoint< S > &cp2)
Definition: test_fcl_geometric_shapes.cpp:659
fcl::TranslationMotion::getCurrentTransform
void getCurrentTransform(Transform3< S > &tf_) const override
Definition: translation_motion-inl.h:109
fcl::Halfspace
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d....
Definition: geometry/shape/halfspace.h:60
expected_depth
S expected_depth
Definition: test_half_space_convex.cpp:152
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:95
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
test_shapeDistance_conecone
void test_shapeDistance_conecone()
Definition: test_fcl_geometric_shapes.cpp:3895
checkContactPointds
bool checkContactPointds(const Shape1 &s1, const Transform3< typename Shape1::S > &tf1, const Shape2 &s2, const Transform3< typename Shape1::S > &tf2, GJKSolverType solver_type, const ContactPoint< typename Shape1::S > &expected, const ContactPoint< typename Shape1::S > &actual, bool check_position=false, bool check_depth=false, bool check_normal=false, bool check_opposite_normal=false, typename Shape1::S tol=1e-9)
Definition: test_fcl_geometric_shapes.cpp:232
inspectContactPointds
bool inspectContactPointds(const Shape1 &s1, const Transform3< typename Shape1::S > &tf1, const Shape2 &s2, const Transform3< typename Shape1::S > &tf2, GJKSolverType solver_type, const std::vector< ContactPoint< typename Shape1::S >> &expected_contacts, const std::vector< ContactPoint< typename Shape1::S >> &actual_contacts, bool check_position=false, bool check_depth=false, bool check_normal=false, bool check_opposite_normal=false, typename Shape1::S tol=1e-9)
Definition: test_fcl_geometric_shapes.cpp:277
test_shapeDistance_conecylinder
void test_shapeDistance_conecylinder()
Definition: test_fcl_geometric_shapes.cpp:3938
test_shapeDistanceGJK_boxsphere
void test_shapeDistanceGJK_boxsphere()
Definition: test_fcl_geometric_shapes.cpp:4946
test_shapeDistanceGJK_boxbox
void test_shapeDistanceGJK_boxbox()
Definition: test_fcl_geometric_shapes.cpp:4903
test_shapeIntersection_halfspacetriangle
void test_shapeIntersection_halfspacetriangle()
Definition: test_fcl_geometric_shapes.cpp:1295
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:51
test_shapeIntersection_halfspacesphere
void test_shapeIntersection_halfspacesphere()
Definition: test_fcl_geometric_shapes.cpp:1397
solver1
detail::GJKSolver_libccd< S > & solver1()
Definition: test_fcl_geometric_shapes.cpp:70
test_shapeIntersection_planecylinder
void test_shapeIntersection_planecylinder()
Definition: test_fcl_geometric_shapes.cpp:2950
fcl::CollisionRequest::enable_cached_gjk_guess
bool enable_cached_gjk_guess
If true, uses the provided initial guess for the GJK algorithm.
Definition: collision_request.h:83
fcl::test::Timer::stop
void stop()
stop the timer
Definition: test_fcl_utility.cpp:86
test_shapeIntersection_halfspacebox
void test_shapeIntersection_halfspacebox()
Definition: test_fcl_geometric_shapes.cpp:1575
fcl::TranslationMotion::integrate
bool integrate(S dt) const override
Definition: translation_motion-inl.h:80
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
capsule.h
fcl::CollisionRequest::cached_gjk_guess
Vector3< S > cached_gjk_guess
The initial guess to use in the GJK algorithm.
Definition: collision_request.h:86
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
gjk_solver_indep.h
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
test_shapeIntersection_boxbox
void test_shapeIntersection_boxbox()
Definition: test_fcl_geometric_shapes.cpp:750
main
int main(int argc, char *argv[])
Definition: test_fcl_geometric_shapes.cpp:5356
fcl::CollisionResult::getContact
const Contact< S > & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_result-inl.h:97
test_shapeIntersection_ellipsoidellipsoid
void test_shapeIntersection_ellipsoidellipsoid()
Definition: test_fcl_geometric_shapes.cpp:1173
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
test_shapeIntersectionGJK_cylindercone
void test_shapeIntersectionGJK_cylindercone()
Definition: test_fcl_geometric_shapes.cpp:4515
min
static T min(T x, T y)
Definition: svm.cpp:49
test_shapeDistance_boxbox
void test_shapeDistance_boxbox()
Definition: test_fcl_geometric_shapes.cpp:3736
tolerance
S tolerance()
Definition: test_fcl_geometric_shapes.cpp:87
fcl::test::generateRandomTransform
void generateRandomTransform(S extents[6], Transform3< S > &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
test_shapeIntersection_halfspaceellipsoid
void test_shapeIntersection_halfspaceellipsoid()
Definition: test_fcl_geometric_shapes.cpp:1763
test_shapeIntersection_planebox
void test_shapeIntersection_planebox()
Definition: test_fcl_geometric_shapes.cpp:1673
test_shapeIntersectionGJK_spherecapsule
void test_shapeIntersectionGJK_spherecapsule()
Definition: test_fcl_geometric_shapes.cpp:4337
test_shapeIntersectionGJK_halfspacetriangle
void test_shapeIntersectionGJK_halfspacetriangle()
Definition: test_fcl_geometric_shapes.cpp:4715
test_shapeIntersectionGJK_conecone
void test_shapeIntersectionGJK_conecone()
Definition: test_fcl_geometric_shapes.cpp:4448
test_shapeIntersection_spheretriangle
void test_shapeIntersection_spheretriangle()
Definition: test_fcl_geometric_shapes.cpp:1249
fcl::Plane
Infinite plane.
Definition: geometry/shape/plane.h:51
fcl::Cone
Center at zero cone.
Definition: cone.h:51
test_shapeIntersection_planecone
void test_shapeIntersection_planecone()
Definition: test_fcl_geometric_shapes.cpp:3420
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
fcl::CollisionResult::cached_gjk_guess
Vector3< S > cached_gjk_guess
Definition: collision_result.h:62
extents
std::array< S, 6 > & extents()
Definition: test_fcl_geometric_shapes.cpp:63
test_shapeIntersection_cylindercone
void test_shapeIntersection_cylindercone()
Definition: test_fcl_geometric_shapes.cpp:1100
test_shapeIntersectionGJK_boxbox
void test_shapeIntersectionGJK_boxbox()
Definition: test_fcl_geometric_shapes.cpp:4183
test_sphere_shape
void test_sphere_shape()
Definition: test_fcl_geometric_shapes.cpp:93
fcl::CollisionRequest::enable_contact
bool enable_contact
If true, contact information (e.g., normal, penetration depth, and contact position) will be returned...
Definition: collision_request.h:63
fcl::test::getGJKSolverName
std::string getGJKSolverName(GJKSolverType solver_type)
Definition: test_fcl_utility.cpp:182
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
fcl::Box::side
Vector3< S > side
box side length
Definition: box.h:67
test_shapeDistance_spheresphere
void test_shapeDistance_spheresphere()
Definition: test_fcl_geometric_shapes.cpp:3667
fcl::Capsule< S >
test_shapeIntersection_planetriangle
void test_shapeIntersection_planetriangle()
Definition: test_fcl_geometric_shapes.cpp:1349
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:51
test_shapeIntersectionGJK_spheretriangle
void test_shapeIntersectionGJK_spheretriangle()
Definition: test_fcl_geometric_shapes.cpp:4668
test_shapeIntersection_spherecapsule
void test_shapeIntersection_spherecapsule()
Definition: test_fcl_geometric_shapes.cpp:906
collision.h

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