test/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-2015, 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 #define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES
39 #include <boost/test/included/unit_test.hpp>
40 
42 #include <hpp/fcl/collision.h>
43 #include <hpp/fcl/distance.h>
44 #include "utility.h"
45 #include <iostream>
46 #include <hpp/fcl/internal/tools.h>
48 
49 using namespace hpp::fcl;
50 
51 FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
52 
56 
57 int line;
58 #define SET_LINE line = __LINE__
59 #define FCL_CHECK(cond) \
60  BOOST_CHECK_MESSAGE(cond, "from line " << line << ": " #cond)
61 #define FCL_CHECK_EQUAL(a, b) \
62  BOOST_CHECK_MESSAGE((a) == (b), "from line " << line << ": " #a "[" << (a) \
63  << "] != " #b "[" << (b) \
64  << "].")
65 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
66 
67 namespace hpp {
68 namespace fcl {
69 std::ostream& operator<<(std::ostream& os, const ShapeBase&) {
70  return os << "a_shape";
71 }
72 
73 std::ostream& operator<<(std::ostream& os, const Box& b) {
74  return os << "Box(" << 2 * b.halfSide.transpose() << ')';
75 }
76 } // namespace fcl
77 } // namespace hpp
78 
79 template <typename S1, typename S2>
80 void printComparisonError(const std::string& comparison_type, const S1& s1,
81  const Transform3f& tf1, const S2& s2,
82  const Transform3f& tf2,
83  const Vec3f& contact_or_normal,
84  const Vec3f& expected_contact_or_normal,
85  bool check_opposite_normal, FCL_REAL tol) {
86  std::cout << "Disagreement between " << comparison_type << " and expected_"
87  << comparison_type << " for " << getNodeTypeName(s1.getNodeType())
88  << " and " << getNodeTypeName(s2.getNodeType()) << ".\n"
89  << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl
90  << "tf1.translation: " << tf1.getTranslation().transpose()
91  << std::endl
92  << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl
93  << "tf2.translation: " << tf2.getTranslation().transpose()
94  << std::endl
95  << comparison_type << ": " << contact_or_normal.transpose()
96  << std::endl
97  << "expected_" << comparison_type << ": "
98  << expected_contact_or_normal.transpose();
99 
100  if (check_opposite_normal)
101  std::cout << " or " << -expected_contact_or_normal.transpose();
102 
103  std::cout << std::endl
104  << "difference: "
105  << (contact_or_normal - expected_contact_or_normal).norm()
106  << std::endl
107  << "tolerance: " << tol << std::endl;
108 }
109 
110 template <typename S1, typename S2>
111 void printComparisonError(const std::string& comparison_type, const S1& s1,
112  const Transform3f& tf1, const S2& s2,
113  const Transform3f& tf2, FCL_REAL depth,
114  FCL_REAL expected_depth, FCL_REAL tol) {
115  std::cout << "Disagreement between " << comparison_type << " and expected_"
116  << comparison_type << " for " << getNodeTypeName(s1.getNodeType())
117  << " and " << getNodeTypeName(s2.getNodeType()) << ".\n"
118  << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl
119  << "tf1.translation: " << tf1.getTranslation() << std::endl
120  << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl
121  << "tf2.translation: " << tf2.getTranslation() << std::endl
122  << "depth: " << depth << std::endl
123  << "expected_depth: " << expected_depth << std::endl
124  << "difference: " << std::fabs(depth - expected_depth) << std::endl
125  << "tolerance: " << tol << std::endl;
126 }
127 
128 template <typename S1, typename S2>
129 void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2,
130  const Transform3f& tf2, const Vec3f& contact,
131  Vec3f* expected_point, FCL_REAL depth,
132  FCL_REAL* expected_depth, const Vec3f& normal,
133  Vec3f* expected_normal, bool check_opposite_normal,
134  FCL_REAL tol) {
135  if (expected_point) {
136  bool contact_equal = isEqual(contact, *expected_point, tol);
137  FCL_CHECK(contact_equal);
138  if (!contact_equal)
139  printComparisonError("contact", s1, tf1, s2, tf2, contact,
140  *expected_point, false, tol);
141  }
142 
143  if (expected_depth) {
144  bool depth_equal = std::fabs(depth - *expected_depth) < tol;
145  FCL_CHECK(depth_equal);
146  if (!depth_equal)
147  printComparisonError("depth", s1, tf1, s2, tf2, depth, *expected_depth,
148  tol);
149  }
150 
151  if (expected_normal) {
152  bool normal_equal = isEqual(normal, *expected_normal, tol);
153 
154  if (!normal_equal && check_opposite_normal)
155  normal_equal = isEqual(normal, -(*expected_normal), tol);
156 
157  FCL_CHECK(normal_equal);
158  if (!normal_equal)
159  printComparisonError("normal", s1, tf1, s2, tf2, normal, *expected_normal,
160  check_opposite_normal, tol);
161  }
162 }
163 
164 template <typename S1, typename S2>
165 void testShapeIntersection(const S1& s1, const Transform3f& tf1, const S2& s2,
166  const Transform3f& tf2, bool expect_collision,
167  Vec3f* expected_point = NULL,
168  FCL_REAL* expected_depth = NULL,
169  Vec3f* expected_normal = NULL,
170  bool check_opposite_normal = false,
171  FCL_REAL tol = 1e-9) {
172  CollisionRequest request;
173  CollisionResult result;
174 
175  Vec3f contact;
176  Vec3f normal; // normal direction should be from object 1 to object 2
177  bool collision;
178  bool check_failed = false;
179 
180  request.enable_contact = false;
181  result.clear();
182  collision = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
183  FCL_CHECK_EQUAL(collision, expect_collision);
184  check_failed = check_failed || (collision != expect_collision);
185 
186  request.enable_contact = true;
187  result.clear();
188  collision = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
189  FCL_CHECK_EQUAL(collision, expect_collision);
190  check_failed = check_failed || (collision != expect_collision);
191 
192  if (check_failed) {
193  BOOST_TEST_MESSAGE("Failure occured between " << s1 << " and " << s2
194  << " at transformations\n"
195  << tf1 << '\n'
196  << tf2);
197  }
198 
199  if (expect_collision) {
200  FCL_CHECK_EQUAL(result.numContacts(), 1);
201  if (result.numContacts() == 1) {
202  Contact contact = result.getContact(0);
203  compareContact(s1, tf1, s2, tf2, contact.pos, expected_point,
204  contact.penetration_depth, expected_depth, contact.normal,
205  expected_normal, check_opposite_normal, tol);
206  }
207  }
208 }
209 
210 BOOST_AUTO_TEST_CASE(box_to_bvh) {
211  Box shape(1, 1, 1);
212  BVHModel<OBB> bvh;
213  generateBVHModel(bvh, shape, Transform3f());
214 }
215 
216 BOOST_AUTO_TEST_CASE(sphere_to_bvh) {
217  Sphere shape(1);
218  BVHModel<OBB> bvh;
219  generateBVHModel(bvh, shape, Transform3f(), 10, 10);
220  generateBVHModel(bvh, shape, Transform3f(), 50);
221 }
222 
223 BOOST_AUTO_TEST_CASE(cylinder_to_bvh) {
224  Cylinder shape(1, 1);
225  BVHModel<OBB> bvh;
226  generateBVHModel(bvh, shape, Transform3f(), 10, 10);
227  generateBVHModel(bvh, shape, Transform3f(), 50);
228 }
229 
230 BOOST_AUTO_TEST_CASE(cone_to_bvh) {
231  Cone shape(1, 1);
232  BVHModel<OBB> bvh;
233  generateBVHModel(bvh, shape, Transform3f(), 10, 10);
234  generateBVHModel(bvh, shape, Transform3f(), 50);
235 }
236 
237 BOOST_AUTO_TEST_CASE(shapeIntersection_cylinderbox) {
238  Cylinder s1(0.029, 0.1);
239  Box s2(1.6, 0.6, 0.025);
240 
242  Quaternion3f(0.5279170511703305, -0.50981118132505521,
243  -0.67596178682051911, 0.0668715876735793),
244  Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
245 
247  Quaternion3f(0.70738826916719977, 0, 0, 0.70682518110536596),
248  Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
249 
250  GJKSolver solver;
252  Vec3f p1, p2, normal;
253  bool res = solver.shapeDistance(s1, tf1, s2, tf2, distance, p1, p2, normal);
254  BOOST_CHECK((res && distance > 0) || (!res && distance <= 0));
255  // If objects are not colliding, p2 should be outside the cylinder and
256  // p1 should be outside the box
257  Vec3f p2Loc(tf1.inverse().transform(p2));
258  bool p2_in_cylinder((fabs(p2Loc[2]) <= s1.halfLength) &&
259  (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius));
260  Vec3f p1Loc(tf2.inverse().transform(p1));
261  bool p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all();
262  std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl;
263  std::cout << "p1 in box = (" << p1Loc.transpose() << ")" << std::endl;
264 
265  BOOST_CHECK((res && !p2_in_cylinder && !p1_in_box) ||
266  (!res && p2_in_cylinder && p1_in_box));
267 
268  res = solver.shapeDistance(s2, tf2, s1, tf1, distance, p2, p1, normal);
269  BOOST_CHECK((res && distance > 0) || (!res && distance <= 0));
270  // If objects are not colliding, p2 should be outside the cylinder and
271  // p1 should be outside the box
272 
273  p2Loc = tf1.inverse().transform(p2);
274  p2_in_cylinder = (fabs(p2Loc[2]) <= s1.halfLength) &&
275  (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius);
276  p1Loc = tf2.inverse().transform(p1);
277  p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all();
278 
279  std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl;
280  std::cout << "p1 in box = (" << p1.transpose() << ")" << std::endl;
281 
282  BOOST_CHECK((res && !p2_in_cylinder && !p1_in_box) ||
283  (!res && p2_in_cylinder && p1_in_box));
284 
285  s1 = Cylinder(0.06, 0.1);
286  tf1.setTranslation(
287  Vec3f(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293));
288  tf1.setQuatRotation(Quaternion3f(0.52613359459338371, 0.32189408354839893,
289  0.70415587451837913, -0.35175580165512249));
290  res = solver.shapeDistance(s1, tf1, s2, tf2, distance, p1, p2, normal);
291 }
292 
293 BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere) {
294  Sphere s1(20);
295  Sphere s2(10);
296 
299 
301  generateRandomTransform(extents, transform);
302 
303  // Vec3f point;
304  // FCL_REAL depth;
305  Vec3f normal;
306 
307  tf1 = Transform3f();
308  tf2 = Transform3f(Vec3f(40, 0, 0));
309  SET_LINE;
310  testShapeIntersection(s1, tf1, s2, tf2, false);
311 
312  tf1 = transform;
313  tf2 = transform * Transform3f(Vec3f(40, 0, 0));
314  SET_LINE;
315  testShapeIntersection(s1, tf1, s2, tf2, false);
316 
317  tf1 = Transform3f();
318  tf2 = Transform3f(Vec3f(30, 0, 0));
319  normal << 1, 0, 0;
320  SET_LINE;
321  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
322 
323  tf1 = Transform3f();
324  tf2 = Transform3f(Vec3f(30.01, 0, 0));
325  SET_LINE;
326  testShapeIntersection(s1, tf1, s2, tf2, false);
327 
328  tf1 = transform;
329  tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
330  normal = transform.getRotation() * Vec3f(1, 0, 0);
331  SET_LINE;
332  testShapeIntersection(s1, tf1, s2, tf2, false);
333 
334  tf1 = Transform3f();
335  tf2 = Transform3f(Vec3f(29.9, 0, 0));
336  normal << 1, 0, 0;
337  SET_LINE;
338  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
339 
340  tf1 = transform;
341  tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
342  normal = transform.getRotation() * Vec3f(1, 0, 0);
343  SET_LINE;
344  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
345 
346  tf1 = Transform3f();
347  tf2 = Transform3f();
348  normal.setZero(); // If the centers of two sphere are at the same position,
349  // the normal is (0, 0, 0)
350  SET_LINE;
351  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
352 
353  tf1 = transform;
354  tf2 = transform;
355  normal.setZero(); // If the centers of two sphere are at the same position,
356  // the normal is (0, 0, 0)
357  SET_LINE;
358  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
359 
360  tf1 = Transform3f();
361  tf2 = Transform3f(Vec3f(-29.9, 0, 0));
362  normal << -1, 0, 0;
363  SET_LINE;
364  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
365 
366  tf1 = transform;
367  tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
368  normal = transform.getRotation() * Vec3f(-1, 0, 0);
369  SET_LINE;
370  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
371 
372  tf1 = Transform3f();
373  tf2 = Transform3f(Vec3f(-30.0, 0, 0));
374  normal << -1, 0, 0;
375  SET_LINE;
376  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
377 
378  tf1 = Transform3f();
379  tf2 = Transform3f(Vec3f(-30.01, 0, 0));
380  normal << -1, 0, 0;
381  SET_LINE;
382  testShapeIntersection(s1, tf1, s2, tf2, false);
383 
384  tf1 = transform;
385  tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
386  SET_LINE;
387  testShapeIntersection(s1, tf1, s2, tf2, false);
388 }
389 
390 bool compareContactPoints(const Vec3f& c1, const Vec3f& c2) {
391  return c1[2] < c2[2];
392 } // Ascending order
393 
395  Box s1(100, 100, 100);
396  Box s2(10, 20, 30);
397 
398  // Vertices of s2
399  std::vector<Vec3f> vertices(8);
400  vertices[0] << 1, 1, 1;
401  vertices[1] << 1, 1, -1;
402  vertices[2] << 1, -1, 1;
403  vertices[3] << 1, -1, -1;
404  vertices[4] << -1, 1, 1;
405  vertices[5] << -1, 1, -1;
406  vertices[6] << -1, -1, 1;
407  vertices[7] << -1, -1, -1;
408 
409  for (std::size_t i = 0; i < 8; ++i) {
410  vertices[i].array() *= s2.halfSide.array();
411  }
412 
413  Transform3f tf1 = Transform3f(Vec3f(0, 0, -50));
415 
416  Vec3f normal;
417  Vec3f point(0., 0., 0.);
418  double distance;
419 
420  // Make sure the two boxes are colliding
421  solver1.gjk_tolerance = 1e-5;
422  solver1.epa_tolerance = 1e-5;
423  bool res =
424  solver1.shapeIntersect(s1, tf1, s2, tf2, distance, true, &point, &normal);
425  FCL_CHECK(res);
426 
427  // Compute global vertices
428  for (std::size_t i = 0; i < 8; ++i) vertices[i] = tf2.transform(vertices[i]);
429 
430  // Sort the vertices so that the lowest vertex along z-axis comes first
431  std::sort(vertices.begin(), vertices.end(), compareContactPoints);
432 
433  // The lowest vertex along z-axis should be the contact point
434  FCL_CHECK(normal.isApprox(Vec3f(0, 0, 1), 1e-6));
435  FCL_CHECK(vertices[0].head<2>().isApprox(point.head<2>(), 1e-6));
436  FCL_CHECK(vertices[0][2] <= point[2] && point[2] < 0);
437 }
438 
439 BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) {
440  Box s1(20, 40, 50);
441  Box s2(10, 10, 10);
442 
445 
447  generateRandomTransform(extents, transform);
448 
449  // Vec3f point;
450  // FCL_REAL depth;
451  Vec3f normal;
452 
453  Quaternion3f q;
454  q = AngleAxis((FCL_REAL)3.140 / 6, UnitZ);
455 
456  tf1 = Transform3f();
457  tf2 = Transform3f();
458  // TODO: Need convention for normal when the centers of two objects are at
459  // same position. The current result is (1, 0, 0).
460  normal << 1, 0, 0;
461  SET_LINE;
462  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
463 
464  tf1 = transform;
465  tf2 = transform;
466  // TODO: Need convention for normal when the centers of two objects are at
467  // same position. The current result is (1, 0, 0).
468  normal = transform.getRotation() * Vec3f(1, 0, 0);
469  SET_LINE;
470  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
471 
472  tf1 = Transform3f();
473  tf2 = Transform3f(Vec3f(15, 0, 0));
474  normal << 1, 0, 0;
475  SET_LINE;
476  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
477  1e-8);
478 
479  tf1 = Transform3f();
480  tf2 = Transform3f(Vec3f(15.01, 0, 0));
481  SET_LINE;
482  testShapeIntersection(s1, tf1, s2, tf2, false);
483 
484  tf1 = Transform3f();
485  tf2 = Transform3f(q);
486  normal << 1, 0, 0;
487  SET_LINE;
488  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
489 
490  tf1 = transform;
491  tf2 = transform * Transform3f(q);
492  normal = transform.getRotation() * Vec3f(1, 0, 0);
493  SET_LINE;
494  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
495 
496  int numTests = 100;
497  for (int i = 0; i < numTests; ++i) {
498  Transform3f tf;
500  SET_LINE;
502  }
503 }
504 
505 BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) {
506  Sphere s1(20);
507  Box s2(5, 5, 5);
508 
511 
513  generateRandomTransform(extents, transform);
514 
515  // Vec3f point;
516  // FCL_REAL depth;
517  Vec3f normal;
518 
519  tf1 = Transform3f();
520  tf2 = Transform3f();
521  // TODO: Need convention for normal when the centers of two objects are at
522  // same position. The current result is (-1, 0, 0).
523  normal << -1, 0, 0;
524  SET_LINE;
525  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
526 
527  tf1 = transform;
528  tf2 = transform;
529  // TODO: Need convention for normal when the centers of two objects are at
530  // same position.
531  SET_LINE;
532  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
533 
534  tf1 = Transform3f();
535  tf2 = Transform3f(Vec3f(22.50001, 0, 0));
536  SET_LINE;
537  testShapeIntersection(s1, tf1, s2, tf2, false);
538 
539  tf1 = transform;
540  tf2 = transform * Transform3f(Vec3f(22.501, 0, 0));
541  SET_LINE;
542  testShapeIntersection(s1, tf1, s2, tf2, false);
543 
544  tf1 = Transform3f();
545  tf2 = Transform3f(Vec3f(22.4, 0, 0));
546  normal << 1, 0, 0;
547  SET_LINE;
548  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
549  tol_gjk);
550 
551  tf1 = transform;
552  tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
553  normal = transform.getRotation() * Vec3f(1, 0, 0);
554  SET_LINE;
555  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
556  tol_gjk);
557 }
558 
559 BOOST_AUTO_TEST_CASE(shapeDistance_spherebox) {
560  hpp::fcl::Matrix3f rotSphere;
561  rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
562  hpp::fcl::Vec3f trSphere(0.0, 0.0, 0.0);
563 
564  hpp::fcl::Matrix3f rotBox;
565  rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
566  hpp::fcl::Vec3f trBox(0.0, 5.0, 3.0);
567 
569  hpp::fcl::Box box(10, 2, 10);
570 
572  distance(&sphere, Transform3f(rotSphere, trSphere), &box,
573  Transform3f(rotBox, trBox), DistanceRequest(true), result);
574 
576  BOOST_CHECK_CLOSE(result.min_distance, 3., eps);
577  EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3f(0, 1, 0), eps);
578  EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3f(0, 4, 0), eps);
579  EIGEN_VECTOR_IS_APPROX(result.normal, Vec3f(0, 1, 0), eps);
580 }
581 
582 BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) {
583  Sphere s1(20);
584  Capsule s2(5, 10);
585 
588 
590  generateRandomTransform(extents, transform);
591 
592  // Vec3f point;
593  // FCL_REAL depth;
594  Vec3f normal;
595 
596  tf1 = Transform3f();
597  tf2 = Transform3f();
598  // TODO: Need convention for normal when the centers of two objects are at
599  // same position.
600  SET_LINE;
601  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
602 
603  tf1 = transform;
604  tf2 = transform;
605  // TODO: Need convention for normal when the centers of two objects are at
606  // same position.
607  SET_LINE;
608  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
609 
610  tf1 = Transform3f();
611  tf2 = Transform3f(Vec3f(24.9, 0, 0));
612  normal << 1, 0, 0;
613  SET_LINE;
614  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
615 
616  tf1 = transform;
617  tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
618  normal = transform.getRotation() * Vec3f(1, 0, 0);
619  SET_LINE;
620  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
621 
622  tf1 = Transform3f();
623  tf2 = Transform3f(Vec3f(25, 0, 0));
624  normal << 1, 0, 0;
625  SET_LINE;
626  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
627 
628  tf1 = transform;
629  tf2 = transform * Transform3f(Vec3f(24.999999, 0, 0));
630  normal = transform.getRotation() * Vec3f(1, 0, 0);
631  SET_LINE;
632  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
633 
634  tf1 = Transform3f();
635  tf2 = Transform3f(Vec3f(25.1, 0, 0));
636  normal << 1, 0, 0;
637  SET_LINE;
638  testShapeIntersection(s1, tf1, s2, tf2, false);
639 
640  tf1 = transform;
641  tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
642  normal = transform.getRotation() * Vec3f(1, 0, 0);
643  SET_LINE;
644  testShapeIntersection(s1, tf1, s2, tf2, false);
645 }
646 
647 BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) {
648  Cylinder s1(5, 15);
649  Cylinder s2(5, 15);
650 
653 
655  generateRandomTransform(extents, transform);
656 
657  // Vec3f point;
658  // FCL_REAL depth;
659  Vec3f normal;
660 
661  tf1 = Transform3f();
662  tf2 = Transform3f();
663  // TODO: Need convention for normal when the centers of two objects are at
664  // same position.
665  SET_LINE;
666  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
667 
668  tf1 = transform;
669  tf2 = transform;
670  // TODO: Need convention for normal when the centers of two objects are at
671  // same position.
672  SET_LINE;
673  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
674 
675  tf1 = Transform3f();
676  tf2 = Transform3f(Vec3f(0, 9.9, 0));
677  normal << 0, 1, 0;
678  SET_LINE;
679  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
680  tol_gjk);
681 
682  tf1 = Transform3f();
683  tf2 = Transform3f(Vec3f(9.9, 0, 0));
684  normal << 1, 0, 0;
685  SET_LINE;
686  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
687  tol_gjk);
688 
689  tf1 = transform;
690  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
691  normal = transform.getRotation() * Vec3f(1, 0, 0);
692  SET_LINE;
693  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
694  tol_gjk);
695 
696  tf1 = Transform3f();
697  tf2 = Transform3f(Vec3f(10.01, 0, 0));
698  SET_LINE;
699  testShapeIntersection(s1, tf1, s2, tf2, false);
700 
701  tf1 = transform;
702  tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
703  SET_LINE;
704  testShapeIntersection(s1, tf1, s2, tf2, false);
705 }
706 
707 /*
708 BOOST_AUTO_TEST_CASE(shapeIntersection_conecone)
709 {
710  Cone s1(5, 10);
711  Cone s2(5, 10);
712 
713  Transform3f tf1;
714  Transform3f tf2;
715 
716  Transform3f transform;
717  generateRandomTransform(extents, transform);
718 
719  // Vec3f point;
720  // FCL_REAL depth;
721  Vec3f normal;
722 
723  tf1 = Transform3f();
724  tf2 = Transform3f();
725  // TODO: Need convention for normal when the centers of two objects are at
726 same position. SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL,
727 NULL, NULL);
728 
729  tf1 = transform;
730  tf2 = transform;
731  // TODO: Need convention for normal when the centers of two objects are at
732 same position. SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL,
733 NULL, NULL);
734 
735  tf1 = Transform3f();
736  tf2 = Transform3f(Vec3f(9.9, 0, 0));
737  normal << 1, 0, 0;
738  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal,
739 false, tol_gjk);
740 
741  tf1 = transform;
742  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
743  normal = transform.getRotation() * Vec3f(1, 0, 0);
744  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal,
745 false, tol_gjk);
746 
747  tf1 = Transform3f();
748  tf2 = Transform3f(Vec3f(10.001, 0, 0));
749  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false);
750 
751  tf1 = transform;
752  tf2 = transform * Transform3f(Vec3f(10.001, 0, 0));
753  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false);
754 
755  tf1 = Transform3f();
756  tf2 = Transform3f(Vec3f(0, 0, 9.9));
757  normal << 0, 0, 1;
758  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
759 
760  tf1 = transform;
761  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
762  normal = transform.getRotation() * Vec3f(0, 0, 1);
763  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
764 }
765 */
766 
767 /*
768 BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder)
769 {
770  Cylinder s1(5, 10);
771  Cone s2(5, 10);
772 
773  Transform3f tf1;
774  Transform3f tf2;
775 
776  Transform3f transform;
777  generateRandomTransform(extents, transform);
778 
779  // Vec3f point;
780  // FCL_REAL depth;
781  Vec3f normal;
782 
783  tf1 = Transform3f();
784  tf2 = Transform3f();
785  // TODO: Need convention for normal when the centers of two objects are at
786 same position. SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL,
787 NULL, NULL);
788 
789  tf1 = transform;
790  tf2 = transform;
791  // TODO: Need convention for normal when the centers of two objects are at
792 same position. SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL,
793 NULL, NULL);
794 
795  tf1 = Transform3f();
796  tf2 = Transform3f(Vec3f(9.9, 0, 0));
797  normal << 1, 0, 0;
798  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal,
799 false, 0.061);
800 
801  tf1 = transform;
802  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
803  normal = transform.getRotation() * Vec3f(1, 0, 0);
804  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal,
805 false, 0.46);
806 
807  tf1 = Transform3f();
808  tf2 = Transform3f(Vec3f(10.01, 0, 0));
809  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false);
810 
811  tf1 = transform;
812  tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
813  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false);
814 
815  tf1 = Transform3f();
816  tf2 = Transform3f(Vec3f(0, 0, 9.9));
817  normal << 0, 0, 1;
818  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
819 
820  tf1 = transform;
821  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
822  normal = transform.getRotation() * Vec3f(0, 0, 1);
823  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
824 
825  tf1 = Transform3f();
826  tf2 = Transform3f(Vec3f(0, 0, 10.01));
827  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false);
828 
829  tf1 = transform;
830  tf2 = transform * Transform3f(Vec3f(0, 0, 10.01));
831  SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false);
832 }
833 */
834 
835 BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle) {
836  Sphere s(10);
837  Vec3f t[3];
838  t[0] << 20, 0, 0;
839  t[1] << -20, 0, 0;
840  t[2] << 0, 20, 0;
841 
843  generateRandomTransform(extents, transform);
844 
845  Vec3f c1, c2, normal;
847  bool res;
848 
849  res =
850  solver1.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2],
851  Transform3f(), distance, c1, c2, normal);
852  BOOST_CHECK(res);
853 
854  res = solver1.shapeTriangleInteraction(s, transform, t[0], t[1], t[2],
855  transform, distance, c1, c2, normal);
856  BOOST_CHECK(res);
857 
858  t[0] << 30, 0, 0;
859  t[1] << 9.9, -20, 0;
860  t[2] << 9.9, 20, 0;
861  res =
862  solver1.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2],
863  Transform3f(), distance, c1, c2, normal);
864  BOOST_CHECK(res);
865 
866  res = solver1.shapeTriangleInteraction(s, transform, t[0], t[1], t[2],
867  transform, distance, c1, c2, normal);
868  BOOST_CHECK(res);
869 
870  res =
871  solver1.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2],
872  Transform3f(), distance, c1, c2, normal);
873  BOOST_CHECK(res);
874  BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
875 
876  res = solver1.shapeTriangleInteraction(s, transform, t[0], t[1], t[2],
877  transform, distance, c1, c2, normal);
878  BOOST_CHECK(res);
879  BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
880 }
881 
882 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle) {
883  Halfspace hs(Vec3f(1, 0, 0), 0);
884  Vec3f t[3];
885  t[0] << 20, 0, 0;
886  t[1] << -20, 0, 0;
887  t[2] << 0, 20, 0;
888 
890  generateRandomTransform(extents, transform);
891 
892  Vec3f c1, c2;
894  Vec3f normal;
895  bool res;
896 
897  res =
898  solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
899  Transform3f(), distance, c1, c2, normal);
900  BOOST_CHECK(res);
901 
902  res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
903  transform, distance, c1, c2, normal);
904  BOOST_CHECK(res);
905 
906  t[0] << 20, 0, 0;
907  t[1] << 0, -20, 0;
908  t[2] << 0, 20, 0;
909  res =
910  solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
911  Transform3f(), distance, c1, c2, normal);
912  BOOST_CHECK(res);
913 
914  // These tests fail because of numerical precision errors. The points t[1] and
915  // t[2] lies on the border of the half-space. The normals should be good, when
916  // computed (i.e. res == true)
917  res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
918  transform, distance, c1, c2, normal);
919  // BOOST_CHECK(res);
920  if (res)
921  BOOST_CHECK(
922  isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
923 
924  res =
925  solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
926  Transform3f(), distance, c1, c2, normal);
927  // BOOST_CHECK(res);
928  if (res) BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
929 
930  res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
931  transform, distance, c1, c2, normal);
932  // BOOST_CHECK(res);
933  if (res)
934  BOOST_CHECK(
935  isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
936 }
937 
938 BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle) {
939  Plane hs(Vec3f(1, 0, 0), 0);
940  Vec3f t[3];
941  t[0] << 20, 0, 0;
942  t[1] << -20, 0, 0;
943  t[2] << 0, 20, 0;
944 
946  generateRandomTransform(extents, transform);
947 
948  Vec3f c1, c2;
950  Vec3f normal;
951  bool res;
952 
953  res =
954  solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
955  Transform3f(), distance, c1, c2, normal);
956  BOOST_CHECK(res);
957 
958  res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
959  transform, distance, c1, c2, normal);
960  BOOST_CHECK(res);
961 
962  t[0] << 20, 0, 0;
963  t[1] << -0.1, -20, 0;
964  t[2] << -0.1, 20, 0;
965 
966  res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
967  transform, distance, c1, c2, normal);
968  BOOST_CHECK(res);
969 
970  res =
971  solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
972  Transform3f(), distance, c1, c2, normal);
973  BOOST_CHECK(res);
974  BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
975 
976  res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
977  transform, distance, c1, c2, normal);
978  BOOST_CHECK(res);
979  BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
980 }
981 
982 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
983  Sphere s(10);
984  Halfspace hs(Vec3f(1, 0, 0), 0);
985 
988 
990  generateRandomTransform(extents, transform);
991 
992  Vec3f contact;
993  FCL_REAL depth;
994  Vec3f normal;
995 
996  tf1 = Transform3f();
997  tf2 = Transform3f();
998  contact << -5, 0, 0;
999  depth = 10;
1000  normal << -1, 0, 0;
1001  SET_LINE;
1002  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1003 
1004  tf1 = transform;
1005  tf2 = transform;
1006  contact = transform.transform(Vec3f(-5, 0, 0));
1007  depth = 10;
1008  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1009  SET_LINE;
1010  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1011 
1012  tf1 = Transform3f();
1013  tf2 = Transform3f(Vec3f(5, 0, 0));
1014  contact << -2.5, 0, 0;
1015  depth = 15;
1016  normal << -1, 0, 0;
1017  SET_LINE;
1018  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1019 
1020  tf1 = transform;
1021  tf2 = transform * Transform3f(Vec3f(5, 0, 0));
1022  contact = transform.transform(Vec3f(-2.5, 0, 0));
1023  depth = 15;
1024  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1025  SET_LINE;
1026  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1027 
1028  tf1 = Transform3f();
1029  tf2 = Transform3f(Vec3f(-5, 0, 0));
1030  contact << -7.5, 0, 0;
1031  depth = 5;
1032  normal << -1, 0, 0;
1033  SET_LINE;
1034  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1035 
1036  tf1 = transform;
1037  tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
1038  contact = transform.transform(Vec3f(-7.5, 0, 0));
1039  depth = 5;
1040  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1041  SET_LINE;
1042  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1043 
1044  tf1 = Transform3f();
1045  tf2 = Transform3f(Vec3f(-10.1, 0, 0));
1046  SET_LINE;
1047  testShapeIntersection(s, tf1, hs, tf2, false);
1048 
1049  tf1 = transform;
1050  tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
1051  SET_LINE;
1052  testShapeIntersection(s, tf1, hs, tf2, false);
1053 
1054  tf1 = Transform3f();
1055  tf2 = Transform3f(Vec3f(10.1, 0, 0));
1056  contact << 0.05, 0, 0;
1057  depth = 20.1;
1058  normal << -1, 0, 0;
1059  SET_LINE;
1060  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1061 
1062  tf1 = transform;
1063  tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
1064  contact = transform.transform(Vec3f(0.05, 0, 0));
1065  depth = 20.1;
1066  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1067  SET_LINE;
1068  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1069 }
1070 
1071 BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) {
1072  Sphere s(10);
1073  Plane hs(Vec3f(1, 0, 0), 0);
1074 
1075  Transform3f tf1;
1076  Transform3f tf2;
1077 
1079  generateRandomTransform(extents, transform);
1080 
1081  Vec3f contact;
1082  FCL_REAL depth;
1083  Vec3f normal;
1084 
1085  tf1 = Transform3f();
1086  tf2 = Transform3f();
1087  contact.setZero();
1088  depth = 10;
1089  normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
1090  SET_LINE;
1091  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1092 
1093  tf1 = transform;
1094  tf2 = transform;
1095  contact = transform.transform(Vec3f(0, 0, 0));
1096  depth = 10;
1097  normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
1098  SET_LINE;
1099  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
1100 
1101  tf1 = Transform3f();
1102  tf2 = Transform3f(Vec3f(5, 0, 0));
1103  contact << 5, 0, 0;
1104  depth = 5;
1105  normal << 1, 0, 0;
1106  SET_LINE;
1107  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1108 
1109  tf1 = transform;
1110  tf2 = transform * Transform3f(Vec3f(5, 0, 0));
1111  contact = transform.transform(Vec3f(5, 0, 0));
1112  depth = 5;
1113  normal = transform.getRotation() * Vec3f(1, 0, 0);
1114  SET_LINE;
1115  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1116 
1117  tf1 = Transform3f();
1118  tf2 = Transform3f(Vec3f(-5, 0, 0));
1119  contact << -5, 0, 0;
1120  depth = 5;
1121  normal << -1, 0, 0;
1122  SET_LINE;
1123  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1124 
1125  tf1 = transform;
1126  tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
1127  contact = transform.transform(Vec3f(-5, 0, 0));
1128  depth = 5;
1129  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1130  SET_LINE;
1131  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1132 
1133  tf1 = Transform3f();
1134  tf2 = Transform3f(Vec3f(-10.1, 0, 0));
1135  SET_LINE;
1136  testShapeIntersection(s, tf1, hs, tf2, false);
1137 
1138  tf1 = transform;
1139  tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
1140  SET_LINE;
1141  testShapeIntersection(s, tf1, hs, tf2, false);
1142 
1143  tf1 = Transform3f();
1144  tf2 = Transform3f(Vec3f(10.1, 0, 0));
1145  SET_LINE;
1146  testShapeIntersection(s, tf1, hs, tf2, false);
1147 
1148  tf1 = transform;
1149  tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
1150  SET_LINE;
1151  testShapeIntersection(s, tf1, hs, tf2, false);
1152 }
1153 
1154 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
1155  Box s(5, 10, 20);
1156  Halfspace hs(Vec3f(1, 0, 0), 0);
1157 
1158  Transform3f tf1;
1159  Transform3f tf2;
1160 
1162  generateRandomTransform(extents, transform);
1163 
1164  Vec3f contact;
1165  FCL_REAL depth;
1166  Vec3f normal;
1167 
1168  tf1 = Transform3f();
1169  tf2 = Transform3f();
1170  contact << -1.25, 0, 0;
1171  depth = 2.5;
1172  normal << -1, 0, 0;
1173  SET_LINE;
1174  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1175 
1176  tf1 = transform;
1177  tf2 = transform;
1178  contact = transform.transform(Vec3f(-1.25, 0, 0));
1179  depth = 2.5;
1180  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1181  SET_LINE;
1182  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1183 
1184  tf1 = Transform3f();
1185  tf2 = Transform3f(Vec3f(1.25, 0, 0));
1186  contact << -0.625, 0, 0;
1187  depth = 3.75;
1188  normal << -1, 0, 0;
1189  SET_LINE;
1190  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1191 
1192  tf1 = transform;
1193  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
1194  contact = transform.transform(Vec3f(-0.625, 0, 0));
1195  depth = 3.75;
1196  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1197  SET_LINE;
1198  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1199 
1200  tf1 = Transform3f();
1201  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
1202  contact << -1.875, 0, 0;
1203  depth = 1.25;
1204  normal << -1, 0, 0;
1205  SET_LINE;
1206  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1207 
1208  tf1 = transform;
1209  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
1210  contact = transform.transform(Vec3f(-1.875, 0, 0));
1211  depth = 1.25;
1212  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1213  SET_LINE;
1214  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1215 
1216  tf1 = Transform3f();
1217  tf2 = Transform3f(Vec3f(2.51, 0, 0));
1218  contact << 0.005, 0, 0;
1219  depth = 5.01;
1220  normal << -1, 0, 0;
1221  SET_LINE;
1222  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1223 
1224  tf1 = transform;
1225  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
1226  contact = transform.transform(Vec3f(0.005, 0, 0));
1227  depth = 5.01;
1228  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1229  SET_LINE;
1230  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1231 
1232  tf1 = Transform3f();
1233  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
1234  SET_LINE;
1235  testShapeIntersection(s, tf1, hs, tf2, false);
1236 
1237  tf1 = transform;
1238  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
1239  SET_LINE;
1240  testShapeIntersection(s, tf1, hs, tf2, false);
1241 
1242  tf1 = Transform3f(transform.getRotation());
1243  tf2 = Transform3f();
1244  SET_LINE;
1245  testShapeIntersection(s, tf1, hs, tf2, true);
1246 }
1247 
1248 BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) {
1249  Box s(5, 10, 20);
1250  Plane hs(Vec3f(1, 0, 0), 0);
1251 
1252  Transform3f tf1;
1253  Transform3f tf2;
1254 
1256  generateRandomTransform(extents, transform);
1257 
1258  Vec3f contact;
1259  FCL_REAL depth;
1260  Vec3f normal;
1261 
1262  tf1 = Transform3f();
1263  tf2 = Transform3f();
1264  contact << 0, 0, 0;
1265  depth = 2.5;
1266  normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
1267  SET_LINE;
1268  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1269 
1270  tf1 = transform;
1271  tf2 = transform;
1272  contact = transform.transform(Vec3f(0, 0, 0));
1273  depth = 2.5;
1274  normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
1275  SET_LINE;
1276  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1277 
1278  tf1 = Transform3f();
1279  tf2 = Transform3f(Vec3f(1.25, 0, 0));
1280  contact << 1.25, 0, 0;
1281  depth = 1.25;
1282  normal << 1, 0, 0;
1283  SET_LINE;
1284  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1285 
1286  tf1 = transform;
1287  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
1288  contact = transform.transform(Vec3f(1.25, 0, 0));
1289  depth = 1.25;
1290  normal = transform.getRotation() * Vec3f(1, 0, 0);
1291  SET_LINE;
1292  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1293 
1294  tf1 = Transform3f();
1295  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
1296  contact << -1.25, 0, 0;
1297  depth = 1.25;
1298  normal << -1, 0, 0;
1299  SET_LINE;
1300  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1301 
1302  tf1 = transform;
1303  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
1304  contact = transform.transform(Vec3f(-1.25, 0, 0));
1305  depth = 1.25;
1306  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1307  SET_LINE;
1308  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1309 
1310  tf1 = Transform3f();
1311  tf2 = Transform3f(Vec3f(2.51, 0, 0));
1312  SET_LINE;
1313  testShapeIntersection(s, tf1, hs, tf2, false);
1314 
1315  tf1 = transform;
1316  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
1317  SET_LINE;
1318  testShapeIntersection(s, tf1, hs, tf2, false);
1319 
1320  tf1 = Transform3f();
1321  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
1322  SET_LINE;
1323  testShapeIntersection(s, tf1, hs, tf2, false);
1324 
1325  tf1 = transform;
1326  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
1327  SET_LINE;
1328  testShapeIntersection(s, tf1, hs, tf2, false);
1329 
1330  tf1 = Transform3f(transform.getRotation());
1331  tf2 = Transform3f();
1332  SET_LINE;
1333  testShapeIntersection(s, tf1, hs, tf2, true);
1334 }
1335 
1336 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
1337  Capsule s(5, 10);
1338  Halfspace hs(Vec3f(1, 0, 0), 0);
1339 
1340  Transform3f tf1;
1341  Transform3f tf2;
1342 
1344  generateRandomTransform(extents, transform);
1345 
1346  Vec3f contact;
1347  FCL_REAL depth;
1348  Vec3f normal;
1349 
1350  tf1 = Transform3f();
1351  tf2 = Transform3f();
1352  contact << -2.5, 0, 0;
1353  depth = 5;
1354  normal << -1, 0, 0;
1355  SET_LINE;
1356  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1357 
1358  tf1 = transform;
1359  tf2 = transform;
1360  contact = transform.transform(Vec3f(-2.5, 0, 0));
1361  depth = 5;
1362  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1363  SET_LINE;
1364  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1365 
1366  tf1 = Transform3f();
1367  tf2 = Transform3f(Vec3f(2.5, 0, 0));
1368  contact << -1.25, 0, 0;
1369  depth = 7.5;
1370  normal << -1, 0, 0;
1371  SET_LINE;
1372  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1373 
1374  tf1 = transform;
1375  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
1376  contact = transform.transform(Vec3f(-1.25, 0, 0));
1377  depth = 7.5;
1378  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1379  SET_LINE;
1380  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1381 
1382  tf1 = Transform3f();
1383  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
1384  contact << -3.75, 0, 0;
1385  depth = 2.5;
1386  normal << -1, 0, 0;
1387  SET_LINE;
1388  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1389 
1390  tf1 = transform;
1391  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
1392  contact = transform.transform(Vec3f(-3.75, 0, 0));
1393  depth = 2.5;
1394  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1395  SET_LINE;
1396  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1397 
1398  tf1 = Transform3f();
1399  tf2 = Transform3f(Vec3f(5.1, 0, 0));
1400  contact << 0.05, 0, 0;
1401  depth = 10.1;
1402  normal << -1, 0, 0;
1403  SET_LINE;
1404  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1405 
1406  tf1 = transform;
1407  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
1408  contact = transform.transform(Vec3f(0.05, 0, 0));
1409  depth = 10.1;
1410  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1411  SET_LINE;
1412  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1413 
1414  tf1 = Transform3f();
1415  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
1416  SET_LINE;
1417  testShapeIntersection(s, tf1, hs, tf2, false);
1418 
1419  tf1 = transform;
1420  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
1421  SET_LINE;
1422  testShapeIntersection(s, tf1, hs, tf2, false);
1423 
1424  hs = Halfspace(Vec3f(0, 1, 0), 0);
1425 
1426  tf1 = Transform3f();
1427  tf2 = Transform3f();
1428  contact << 0, -2.5, 0;
1429  depth = 5;
1430  normal << 0, -1, 0;
1431  SET_LINE;
1432  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1433 
1434  tf1 = transform;
1435  tf2 = transform;
1436  contact = transform.transform(Vec3f(0, -2.5, 0));
1437  depth = 5;
1438  normal = transform.getRotation() * Vec3f(0, -1, 0);
1439  SET_LINE;
1440  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1441 
1442  tf1 = Transform3f();
1443  tf2 = Transform3f(Vec3f(0, 2.5, 0));
1444  contact << 0, -1.25, 0;
1445  depth = 7.5;
1446  normal << 0, -1, 0;
1447  SET_LINE;
1448  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1449 
1450  tf1 = transform;
1451  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
1452  contact = transform.transform(Vec3f(0, -1.25, 0));
1453  depth = 7.5;
1454  normal = transform.getRotation() * Vec3f(0, -1, 0);
1455  SET_LINE;
1456  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1457 
1458  tf1 = Transform3f();
1459  tf2 = Transform3f(Vec3f(0, -2.5, 0));
1460  contact << 0, -3.75, 0;
1461  depth = 2.5;
1462  normal << 0, -1, 0;
1463  SET_LINE;
1464  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1465 
1466  tf1 = transform;
1467  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
1468  contact = transform.transform(Vec3f(0, -3.75, 0));
1469  depth = 2.5;
1470  normal = transform.getRotation() * Vec3f(0, -1, 0);
1471  SET_LINE;
1472  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1473 
1474  tf1 = Transform3f();
1475  tf2 = Transform3f(Vec3f(0, 5.1, 0));
1476  contact << 0, 0.05, 0;
1477  depth = 10.1;
1478  normal << 0, -1, 0;
1479  SET_LINE;
1480  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1481 
1482  tf1 = transform;
1483  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
1484  contact = transform.transform(Vec3f(0, 0.05, 0));
1485  depth = 10.1;
1486  normal = transform.getRotation() * Vec3f(0, -1, 0);
1487  SET_LINE;
1488  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1489 
1490  tf1 = Transform3f();
1491  tf2 = Transform3f(Vec3f(0, -5.1, 0));
1492  SET_LINE;
1493  testShapeIntersection(s, tf1, hs, tf2, false);
1494 
1495  tf1 = transform;
1496  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
1497  SET_LINE;
1498  testShapeIntersection(s, tf1, hs, tf2, false);
1499 
1500  hs = Halfspace(Vec3f(0, 0, 1), 0);
1501 
1502  tf1 = Transform3f();
1503  tf2 = Transform3f();
1504  contact << 0, 0, -5;
1505  depth = 10;
1506  normal << 0, 0, -1;
1507  SET_LINE;
1508  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1509 
1510  tf1 = transform;
1511  tf2 = transform;
1512  contact = transform.transform(Vec3f(0, 0, -5));
1513  depth = 10;
1514  normal = transform.getRotation() * Vec3f(0, 0, -1);
1515  SET_LINE;
1516  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1517 
1518  tf1 = Transform3f();
1519  tf2 = Transform3f(Vec3f(0, 0, 2.5));
1520  contact << 0, 0, -3.75;
1521  depth = 12.5;
1522  normal << 0, 0, -1;
1523  SET_LINE;
1524  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1525 
1526  tf1 = transform;
1527  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
1528  contact = transform.transform(Vec3f(0, 0, -3.75));
1529  depth = 12.5;
1530  normal = transform.getRotation() * Vec3f(0, 0, -1);
1531  SET_LINE;
1532  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1533 
1534  tf1 = Transform3f();
1535  tf2 = Transform3f(Vec3f(0, 0, -2.5));
1536  contact << 0, 0, -6.25;
1537  depth = 7.5;
1538  normal << 0, 0, -1;
1539  SET_LINE;
1540  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1541 
1542  tf1 = transform;
1543  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
1544  contact = transform.transform(Vec3f(0, 0, -6.25));
1545  depth = 7.5;
1546  normal = transform.getRotation() * Vec3f(0, 0, -1);
1547  SET_LINE;
1548  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1549 
1550  tf1 = Transform3f();
1551  tf2 = Transform3f(Vec3f(0, 0, 10.1));
1552  contact << 0, 0, 0.05;
1553  depth = 20.1;
1554  normal << 0, 0, -1;
1555  SET_LINE;
1556  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1557 
1558  tf1 = transform;
1559  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
1560  contact = transform.transform(Vec3f(0, 0, 0.05));
1561  depth = 20.1;
1562  normal = transform.getRotation() * Vec3f(0, 0, -1);
1563  SET_LINE;
1564  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1565 
1566  tf1 = Transform3f();
1567  tf2 = Transform3f(Vec3f(0, 0, -10.1));
1568  SET_LINE;
1569  testShapeIntersection(s, tf1, hs, tf2, false);
1570 
1571  tf1 = transform;
1572  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
1573  SET_LINE;
1574  testShapeIntersection(s, tf1, hs, tf2, false);
1575 }
1576 
1577 BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
1578  Capsule s(5, 10);
1579  Plane hs(Vec3f(1, 0, 0), 0);
1580 
1581  Transform3f tf1;
1582  Transform3f tf2;
1583 
1585  generateRandomTransform(extents, transform);
1586 
1587  Vec3f contact;
1588  FCL_REAL depth;
1589  Vec3f normal;
1590 
1591  tf1 = Transform3f();
1592  tf2 = Transform3f();
1593  contact << 0, 0, 0;
1594  depth = 5;
1595  normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
1596  SET_LINE;
1597  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true);
1598 
1599  tf1 = transform;
1600  tf2 = transform;
1601  contact = transform.transform(Vec3f(0, 0, 0));
1602  depth = 5;
1603  normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
1604  SET_LINE;
1605  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true);
1606 
1607  tf1 = Transform3f();
1608  tf2 = Transform3f(Vec3f(2.5, 0, 0));
1609  contact << 2.5, 0, 0;
1610  depth = 2.5;
1611  normal << 1, 0, 0;
1612  SET_LINE;
1613  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
1614 
1615  tf1 = transform;
1616  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
1617  contact = transform.transform(Vec3f(2.5, 0, 0));
1618  depth = 2.5;
1619  normal = transform.getRotation() * Vec3f(1, 0, 0);
1620  SET_LINE;
1621  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
1622 
1623  tf1 = Transform3f();
1624  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
1625  contact << -2.5, 0, 0;
1626  depth = 2.5;
1627  normal << -1, 0, 0;
1628  SET_LINE;
1629  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
1630 
1631  tf1 = transform;
1632  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
1633  contact = transform.transform(Vec3f(-2.5, 0, 0));
1634  depth = 2.5;
1635  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1636  SET_LINE;
1637  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
1638 
1639  tf1 = Transform3f();
1640  tf2 = Transform3f(Vec3f(5.1, 0, 0));
1641  SET_LINE;
1642  testShapeIntersection(s, tf1, hs, tf2, false);
1643 
1644  tf1 = transform;
1645  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
1646  SET_LINE;
1647  testShapeIntersection(s, tf1, hs, tf2, false);
1648 
1649  tf1 = Transform3f();
1650  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
1651  SET_LINE;
1652  testShapeIntersection(s, tf1, hs, tf2, false);
1653 
1654  tf1 = transform;
1655  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
1656  SET_LINE;
1657  testShapeIntersection(s, tf1, hs, tf2, false);
1658 
1659  hs = Plane(Vec3f(0, 1, 0), 0);
1660 
1661  tf1 = Transform3f();
1662  tf2 = Transform3f();
1663  contact << 0, 0, 0;
1664  depth = 5;
1665  normal << 0, 1, 0; // (0, 1, 0) or (0, -1, 0)
1666  SET_LINE;
1667  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true);
1668 
1669  tf1 = transform;
1670  tf2 = transform;
1671  contact = transform.transform(Vec3f(0, 0, 0));
1672  depth = 5;
1673  normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0)
1674  SET_LINE;
1675  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true);
1676 
1677  tf1 = Transform3f();
1678  tf2 = Transform3f(Vec3f(0, 2.5, 0));
1679  contact << 0, 2.5, 0;
1680  depth = 2.5;
1681  normal << 0, 1, 0;
1682  SET_LINE;
1683  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
1684 
1685  tf1 = transform;
1686  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
1687  contact = transform.transform(Vec3f(0, 2.5, 0));
1688  depth = 2.5;
1689  normal = transform.getRotation() * Vec3f(0, 1, 0);
1690  SET_LINE;
1691  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
1692 
1693  tf1 = Transform3f();
1694  tf2 = Transform3f(Vec3f(0, -2.5, 0));
1695  contact << 0, -2.5, 0;
1696  depth = 2.5;
1697  normal << 0, -1, 0;
1698  SET_LINE;
1699  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
1700 
1701  tf1 = transform;
1702  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
1703  contact = transform.transform(Vec3f(0, -2.5, 0));
1704  depth = 2.5;
1705  normal = transform.getRotation() * Vec3f(0, -1, 0);
1706  SET_LINE;
1707  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
1708 
1709  tf1 = Transform3f();
1710  tf2 = Transform3f(Vec3f(0, 5.1, 0));
1711  SET_LINE;
1712  testShapeIntersection(s, tf1, hs, tf2, false);
1713 
1714  tf1 = transform;
1715  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
1716  SET_LINE;
1717  testShapeIntersection(s, tf1, hs, tf2, false);
1718 
1719  tf1 = Transform3f();
1720  tf2 = Transform3f(Vec3f(0, -5.1, 0));
1721  SET_LINE;
1722  testShapeIntersection(s, tf1, hs, tf2, false);
1723 
1724  tf1 = transform;
1725  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
1726  SET_LINE;
1727  testShapeIntersection(s, tf1, hs, tf2, false);
1728 
1729  hs = Plane(Vec3f(0, 0, 1), 0);
1730 
1731  tf1 = Transform3f();
1732  tf2 = Transform3f();
1733  contact << 0, 0, 0;
1734  depth = 10;
1735  normal << 0, 0, 1; // (0, 0, 1) or (0, 0, -1)
1736  SET_LINE;
1737  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true);
1738 
1739  tf1 = transform;
1740  tf2 = transform;
1741  contact = transform.transform(Vec3f(0, 0, 0));
1742  depth = 10;
1743  normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1)
1744  SET_LINE;
1745  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true);
1746 
1747  tf1 = Transform3f();
1748  tf2 = Transform3f(Vec3f(0, 0, 2.5));
1749  contact << 0, 0, 2.5;
1750  depth = 7.5;
1751  normal << 0, 0, 1;
1752  SET_LINE;
1753  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
1754 
1755  tf1 = transform;
1756  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
1757  contact = transform.transform(Vec3f(0, 0, 2.5));
1758  depth = 7.5;
1759  normal = transform.getRotation() * Vec3f(0, 0, 1);
1760  SET_LINE;
1761  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
1762 
1763  tf1 = Transform3f();
1764  tf2 = Transform3f(Vec3f(0, 0, -2.5));
1765  contact << 0, 0, -2.5;
1766  depth = 7.5;
1767  normal << 0, 0, -1;
1768  SET_LINE;
1769  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
1770 
1771  tf1 = transform;
1772  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
1773  contact = transform.transform(Vec3f(0, 0, -2.5));
1774  depth = 7.5;
1775  normal = transform.getRotation() * Vec3f(0, 0, -1);
1776  SET_LINE;
1777  testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
1778 
1779  tf1 = Transform3f();
1780  tf2 = Transform3f(Vec3f(0, 0, 10.1));
1781  SET_LINE;
1782  testShapeIntersection(s, tf1, hs, tf2, false);
1783 
1784  tf1 = transform;
1785  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
1786  SET_LINE;
1787  testShapeIntersection(s, tf1, hs, tf2, false);
1788 
1789  tf1 = Transform3f();
1790  tf2 = Transform3f(Vec3f(0, 0, -10.1));
1791  SET_LINE;
1792  testShapeIntersection(s, tf1, hs, tf2, false);
1793 
1794  tf1 = transform;
1795  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
1796  SET_LINE;
1797  testShapeIntersection(s, tf1, hs, tf2, false);
1798 }
1799 
1800 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
1801  Cylinder s(5, 10);
1802  Halfspace hs(Vec3f(1, 0, 0), 0);
1803 
1804  Transform3f tf1;
1805  Transform3f tf2;
1806 
1808  generateRandomTransform(extents, transform);
1809 
1810  Vec3f contact;
1811  FCL_REAL depth;
1812  Vec3f normal;
1813 
1814  tf1 = Transform3f();
1815  tf2 = Transform3f();
1816  contact << -2.5, 0, 0;
1817  depth = 5;
1818  normal << -1, 0, 0;
1819  SET_LINE;
1820  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1821 
1822  tf1 = transform;
1823  tf2 = transform;
1824  contact = transform.transform(Vec3f(-2.5, 0, 0));
1825  depth = 5;
1826  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1827  SET_LINE;
1828  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1829 
1830  tf1 = Transform3f();
1831  tf2 = Transform3f(Vec3f(2.5, 0, 0));
1832  contact << -1.25, 0, 0;
1833  depth = 7.5;
1834  normal << -1, 0, 0;
1835  SET_LINE;
1836  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1837 
1838  tf1 = transform;
1839  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
1840  contact = transform.transform(Vec3f(-1.25, 0, 0));
1841  depth = 7.5;
1842  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1843  SET_LINE;
1844  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1845 
1846  tf1 = Transform3f();
1847  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
1848  contact << -3.75, 0, 0;
1849  depth = 2.5;
1850  normal << -1, 0, 0;
1851  SET_LINE;
1852  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1853 
1854  tf1 = transform;
1855  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
1856  contact = transform.transform(Vec3f(-3.75, 0, 0));
1857  depth = 2.5;
1858  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1859  SET_LINE;
1860  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1861 
1862  tf1 = Transform3f();
1863  tf2 = Transform3f(Vec3f(5.1, 0, 0));
1864  contact << 0.05, 0, 0;
1865  depth = 10.1;
1866  normal << -1, 0, 0;
1867  SET_LINE;
1868  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1869 
1870  tf1 = transform;
1871  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
1872  contact = transform.transform(Vec3f(0.05, 0, 0));
1873  depth = 10.1;
1874  normal = transform.getRotation() * Vec3f(-1, 0, 0);
1875  SET_LINE;
1876  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1877 
1878  tf1 = Transform3f();
1879  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
1880  SET_LINE;
1881  testShapeIntersection(s, tf1, hs, tf2, false);
1882 
1883  tf1 = transform;
1884  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
1885  SET_LINE;
1886  testShapeIntersection(s, tf1, hs, tf2, false);
1887 
1888  hs = Halfspace(Vec3f(0, 1, 0), 0);
1889 
1890  tf1 = Transform3f();
1891  tf2 = Transform3f();
1892  contact << 0, -2.5, 0;
1893  depth = 5;
1894  normal << 0, -1, 0;
1895  SET_LINE;
1896  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1897 
1898  tf1 = transform;
1899  tf2 = transform;
1900  contact = transform.transform(Vec3f(0, -2.5, 0));
1901  depth = 5;
1902  normal = transform.getRotation() * Vec3f(0, -1, 0);
1903  SET_LINE;
1904  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1905 
1906  tf1 = Transform3f();
1907  tf2 = Transform3f(Vec3f(0, 2.5, 0));
1908  contact << 0, -1.25, 0;
1909  depth = 7.5;
1910  normal << 0, -1, 0;
1911  SET_LINE;
1912  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1913 
1914  tf1 = transform;
1915  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
1916  contact = transform.transform(Vec3f(0, -1.25, 0));
1917  depth = 7.5;
1918  normal = transform.getRotation() * Vec3f(0, -1, 0);
1919  SET_LINE;
1920  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1921 
1922  tf1 = Transform3f();
1923  tf2 = Transform3f(Vec3f(0, -2.5, 0));
1924  contact << 0, -3.75, 0;
1925  depth = 2.5;
1926  normal << 0, -1, 0;
1927  SET_LINE;
1928  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1929 
1930  tf1 = transform;
1931  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
1932  contact = transform.transform(Vec3f(0, -3.75, 0));
1933  depth = 2.5;
1934  normal = transform.getRotation() * Vec3f(0, -1, 0);
1935  SET_LINE;
1936  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1937 
1938  tf1 = Transform3f();
1939  tf2 = Transform3f(Vec3f(0, 5.1, 0));
1940  contact << 0, 0.05, 0;
1941  depth = 10.1;
1942  normal << 0, -1, 0;
1943  SET_LINE;
1944  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1945 
1946  tf1 = transform;
1947  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
1948  contact = transform.transform(Vec3f(0, 0.05, 0));
1949  depth = 10.1;
1950  normal = transform.getRotation() * Vec3f(0, -1, 0);
1951  SET_LINE;
1952  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1953 
1954  tf1 = Transform3f();
1955  tf2 = Transform3f(Vec3f(0, -5.1, 0));
1956  SET_LINE;
1957  testShapeIntersection(s, tf1, hs, tf2, false);
1958 
1959  tf1 = transform;
1960  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
1961  SET_LINE;
1962  testShapeIntersection(s, tf1, hs, tf2, false);
1963 
1964  hs = Halfspace(Vec3f(0, 0, 1), 0);
1965 
1966  tf1 = Transform3f();
1967  tf2 = Transform3f();
1968  contact << 0, 0, -2.5;
1969  depth = 5;
1970  normal << 0, 0, -1;
1971  SET_LINE;
1972  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1973 
1974  tf1 = transform;
1975  tf2 = transform;
1976  contact = transform.transform(Vec3f(0, 0, -2.5));
1977  depth = 5;
1978  normal = transform.getRotation() * Vec3f(0, 0, -1);
1979  SET_LINE;
1980  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1981 
1982  tf1 = Transform3f();
1983  tf2 = Transform3f(Vec3f(0, 0, 2.5));
1984  contact << 0, 0, -1.25;
1985  depth = 7.5;
1986  normal << 0, 0, -1;
1987  SET_LINE;
1988  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1989 
1990  tf1 = transform;
1991  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
1992  contact = transform.transform(Vec3f(0, 0, -1.25));
1993  depth = 7.5;
1994  normal = transform.getRotation() * Vec3f(0, 0, -1);
1995  SET_LINE;
1996  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
1997 
1998  tf1 = Transform3f();
1999  tf2 = Transform3f(Vec3f(0, 0, -2.5));
2000  contact << 0, 0, -3.75;
2001  depth = 2.5;
2002  normal << 0, 0, -1;
2003  SET_LINE;
2004  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2005 
2006  tf1 = transform;
2007  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
2008  contact = transform.transform(Vec3f(0, 0, -3.75));
2009  depth = 2.5;
2010  normal = transform.getRotation() * Vec3f(0, 0, -1);
2011  SET_LINE;
2012  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2013 
2014  tf1 = Transform3f();
2015  tf2 = Transform3f(Vec3f(0, 0, 5.1));
2016  contact << 0, 0, 0.05;
2017  depth = 10.1;
2018  normal << 0, 0, -1;
2019  SET_LINE;
2020  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2021 
2022  tf1 = transform;
2023  tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
2024  contact = transform.transform(Vec3f(0, 0, 0.05));
2025  depth = 10.1;
2026  normal = transform.getRotation() * Vec3f(0, 0, -1);
2027  SET_LINE;
2028  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2029 
2030  tf1 = Transform3f();
2031  tf2 = Transform3f(Vec3f(0, 0, -5.1));
2032  SET_LINE;
2033  testShapeIntersection(s, tf1, hs, tf2, false);
2034 
2035  tf1 = transform;
2036  tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
2037  SET_LINE;
2038  testShapeIntersection(s, tf1, hs, tf2, false);
2039 }
2040 
2041 BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) {
2042  Cylinder s(5, 10);
2043  Plane hs(Vec3f(1, 0, 0), 0);
2044 
2045  Transform3f tf1;
2046  Transform3f tf2;
2047 
2049  generateRandomTransform(extents, transform);
2050 
2051  Vec3f contact;
2052  FCL_REAL depth;
2053  Vec3f normal;
2054 
2055  tf1 = Transform3f();
2056  tf2 = Transform3f();
2057  contact << 0, 0, 0;
2058  depth = 5;
2059  normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
2060  SET_LINE;
2061  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2062 
2063  tf1 = transform;
2064  tf2 = transform;
2065  contact = transform.transform(Vec3f(0, 0, 0));
2066  depth = 5;
2067  normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
2068  SET_LINE;
2069  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2070 
2071  tf1 = Transform3f();
2072  tf2 = Transform3f(Vec3f(2.5, 0, 0));
2073  contact << 2.5, 0, 0;
2074  depth = 2.5;
2075  normal << 1, 0, 0;
2076  SET_LINE;
2077  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2078 
2079  tf1 = transform;
2080  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
2081  contact = transform.transform(Vec3f(2.5, 0, 0));
2082  depth = 2.5;
2083  normal = transform.getRotation() * Vec3f(1, 0, 0);
2084  SET_LINE;
2085  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2086 
2087  tf1 = Transform3f();
2088  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
2089  contact << -2.5, 0, 0;
2090  depth = 2.5;
2091  normal << -1, 0, 0;
2092  SET_LINE;
2093  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2094 
2095  tf1 = transform;
2096  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
2097  contact = transform.transform(Vec3f(-2.5, 0, 0));
2098  depth = 2.5;
2099  normal = transform.getRotation() * Vec3f(-1, 0, 0);
2100  SET_LINE;
2101  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2102 
2103  tf1 = Transform3f();
2104  tf2 = Transform3f(Vec3f(5.1, 0, 0));
2105  SET_LINE;
2106  testShapeIntersection(s, tf1, hs, tf2, false);
2107 
2108  tf1 = transform;
2109  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
2110  SET_LINE;
2111  testShapeIntersection(s, tf1, hs, tf2, false);
2112 
2113  tf1 = Transform3f();
2114  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
2115  SET_LINE;
2116  testShapeIntersection(s, tf1, hs, tf2, false);
2117 
2118  tf1 = transform;
2119  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
2120  SET_LINE;
2121  testShapeIntersection(s, tf1, hs, tf2, false);
2122 
2123  hs = Plane(Vec3f(0, 1, 0), 0);
2124 
2125  tf1 = Transform3f();
2126  tf2 = Transform3f();
2127  contact << 0, 0, 0;
2128  depth = 5;
2129  normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0)
2130  SET_LINE;
2131  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2132 
2133  tf1 = transform;
2134  tf2 = transform;
2135  contact = transform.transform(Vec3f(0, 0, 0));
2136  depth = 5;
2137  normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
2138  SET_LINE;
2139  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2140 
2141  tf1 = Transform3f();
2142  tf2 = Transform3f(Vec3f(0, 2.5, 0));
2143  contact << 0, 2.5, 0;
2144  depth = 2.5;
2145  normal << 0, 1, 0;
2146  SET_LINE;
2147  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2148 
2149  tf1 = transform;
2150  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
2151  contact = transform.transform(Vec3f(0, 2.5, 0));
2152  depth = 2.5;
2153  normal = transform.getRotation() * Vec3f(0, 1, 0);
2154  SET_LINE;
2155  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2156 
2157  tf1 = Transform3f();
2158  tf2 = Transform3f(Vec3f(0, -2.5, 0));
2159  contact << 0, -2.5, 0;
2160  depth = 2.5;
2161  normal << 0, -1, 0;
2162  SET_LINE;
2163  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2164 
2165  tf1 = transform;
2166  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
2167  contact = transform.transform(Vec3f(0, -2.5, 0));
2168  depth = 2.5;
2169  normal = transform.getRotation() * Vec3f(0, -1, 0);
2170  SET_LINE;
2171  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2172 
2173  tf1 = Transform3f();
2174  tf2 = Transform3f(Vec3f(0, 5.1, 0));
2175  SET_LINE;
2176  testShapeIntersection(s, tf1, hs, tf2, false);
2177 
2178  tf1 = transform;
2179  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
2180  SET_LINE;
2181  testShapeIntersection(s, tf1, hs, tf2, false);
2182 
2183  tf1 = Transform3f();
2184  tf2 = Transform3f(Vec3f(0, -5.1, 0));
2185  SET_LINE;
2186  testShapeIntersection(s, tf1, hs, tf2, false);
2187 
2188  tf1 = transform;
2189  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
2190  SET_LINE;
2191  testShapeIntersection(s, tf1, hs, tf2, false);
2192 
2193  hs = Plane(Vec3f(0, 0, 1), 0);
2194 
2195  tf1 = Transform3f();
2196  tf2 = Transform3f();
2197  contact << 0, 0, 0;
2198  depth = 5;
2199  normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0)
2200  SET_LINE;
2201  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2202 
2203  tf1 = transform;
2204  tf2 = transform;
2205  contact = transform.transform(Vec3f(0, 0, 0));
2206  depth = 5;
2207  normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
2208  SET_LINE;
2209  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2210 
2211  tf1 = Transform3f();
2212  tf2 = Transform3f(Vec3f(0, 0, 2.5));
2213  contact << 0, 0, 2.5;
2214  depth = 2.5;
2215  normal << 0, 0, 1;
2216  SET_LINE;
2217  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2218 
2219  tf1 = transform;
2220  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
2221  contact = transform.transform(Vec3f(0, 0, 2.5));
2222  depth = 2.5;
2223  normal = transform.getRotation() * Vec3f(0, 0, 1);
2224  SET_LINE;
2225  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2226 
2227  tf1 = Transform3f();
2228  tf2 = Transform3f(Vec3f(0, 0, -2.5));
2229  contact << 0, 0, -2.5;
2230  depth = 2.5;
2231  normal << 0, 0, -1;
2232  SET_LINE;
2233  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2234 
2235  tf1 = transform;
2236  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
2237  contact = transform.transform(Vec3f(0, 0, -2.5));
2238  depth = 2.5;
2239  normal = transform.getRotation() * Vec3f(0, 0, -1);
2240  SET_LINE;
2241  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2242 
2243  tf1 = Transform3f();
2244  tf2 = Transform3f(Vec3f(0, 0, 10.1));
2245  SET_LINE;
2246  testShapeIntersection(s, tf1, hs, tf2, false);
2247 
2248  tf1 = transform;
2249  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
2250  SET_LINE;
2251  testShapeIntersection(s, tf1, hs, tf2, false);
2252 
2253  tf1 = Transform3f();
2254  tf2 = Transform3f(Vec3f(0, 0, -10.1));
2255  SET_LINE;
2256  testShapeIntersection(s, tf1, hs, tf2, false);
2257 
2258  tf1 = transform;
2259  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
2260  SET_LINE;
2261  testShapeIntersection(s, tf1, hs, tf2, false);
2262 }
2263 
2264 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
2265  Cone s(5, 10);
2266  Halfspace hs(Vec3f(1, 0, 0), 0);
2267 
2268  Transform3f tf1;
2269  Transform3f tf2;
2270 
2272  generateRandomTransform(extents, transform);
2273 
2274  Vec3f contact;
2275  FCL_REAL depth;
2276  Vec3f normal;
2277 
2278  tf1 = Transform3f();
2279  tf2 = Transform3f();
2280  contact << -2.5, 0, -5;
2281  depth = 5;
2282  normal << -1, 0, 0;
2283  SET_LINE;
2284  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2285 
2286  tf1 = transform;
2287  tf2 = transform;
2288  contact = transform.transform(Vec3f(-2.5, 0, -5));
2289  depth = 5;
2290  normal = transform.getRotation() * Vec3f(-1, 0, 0);
2291  SET_LINE;
2292  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2293 
2294  tf1 = Transform3f();
2295  tf2 = Transform3f(Vec3f(2.5, 0, 0));
2296  contact << -1.25, 0, -5;
2297  depth = 7.5;
2298  normal << -1, 0, 0;
2299  SET_LINE;
2300  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2301 
2302  tf1 = transform;
2303  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
2304  contact = transform.transform(Vec3f(-1.25, 0, -5));
2305  depth = 7.5;
2306  normal = transform.getRotation() * Vec3f(-1, 0, 0);
2307  SET_LINE;
2308  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2309 
2310  tf1 = Transform3f();
2311  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
2312  contact << -3.75, 0, -5;
2313  depth = 2.5;
2314  normal << -1, 0, 0;
2315  SET_LINE;
2316  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2317 
2318  tf1 = transform;
2319  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
2320  contact = transform.transform(Vec3f(-3.75, 0, -5));
2321  depth = 2.5;
2322  normal = transform.getRotation() * Vec3f(-1, 0, 0);
2323  SET_LINE;
2324  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2325 
2326  tf1 = Transform3f();
2327  tf2 = Transform3f(Vec3f(5.1, 0, 0));
2328  contact << 0.05, 0, -5;
2329  depth = 10.1;
2330  normal << -1, 0, 0;
2331  SET_LINE;
2332  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2333 
2334  tf1 = transform;
2335  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
2336  contact = transform.transform(Vec3f(0.05, 0, -5));
2337  depth = 10.1;
2338  normal = transform.getRotation() * Vec3f(-1, 0, 0);
2339  SET_LINE;
2340  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2341 
2342  tf1 = Transform3f();
2343  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
2344  SET_LINE;
2345  testShapeIntersection(s, tf1, hs, tf2, false);
2346 
2347  tf1 = transform;
2348  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
2349  SET_LINE;
2350  testShapeIntersection(s, tf1, hs, tf2, false);
2351 
2352  hs = Halfspace(Vec3f(0, 1, 0), 0);
2353 
2354  tf1 = Transform3f();
2355  tf2 = Transform3f();
2356  contact << 0, -2.5, -5;
2357  depth = 5;
2358  normal << 0, -1, 0;
2359  SET_LINE;
2360  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2361 
2362  tf1 = transform;
2363  tf2 = transform;
2364  contact = transform.transform(Vec3f(0, -2.5, -5));
2365  depth = 5;
2366  normal = transform.getRotation() * Vec3f(0, -1, 0);
2367  SET_LINE;
2368  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2369 
2370  tf1 = Transform3f();
2371  tf2 = Transform3f(Vec3f(0, 2.5, 0));
2372  contact << 0, -1.25, -5;
2373  depth = 7.5;
2374  normal << 0, -1, 0;
2375  SET_LINE;
2376  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2377 
2378  tf1 = transform;
2379  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
2380  contact = transform.transform(Vec3f(0, -1.25, -5));
2381  depth = 7.5;
2382  normal = transform.getRotation() * Vec3f(0, -1, 0);
2383  SET_LINE;
2384  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2385 
2386  tf1 = Transform3f();
2387  tf2 = Transform3f(Vec3f(0, -2.5, 0));
2388  contact << 0, -3.75, -5;
2389  depth = 2.5;
2390  normal << 0, -1, 0;
2391  SET_LINE;
2392  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2393 
2394  tf1 = transform;
2395  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
2396  contact = transform.transform(Vec3f(0, -3.75, -5));
2397  depth = 2.5;
2398  normal = transform.getRotation() * Vec3f(0, -1, 0);
2399  SET_LINE;
2400  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2401 
2402  tf1 = Transform3f();
2403  tf2 = Transform3f(Vec3f(0, 5.1, 0));
2404  contact << 0, 0.05, -5;
2405  depth = 10.1;
2406  normal << 0, -1, 0;
2407  SET_LINE;
2408  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2409 
2410  tf1 = transform;
2411  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
2412  contact = transform.transform(Vec3f(0, 0.05, -5));
2413  depth = 10.1;
2414  normal = transform.getRotation() * Vec3f(0, -1, 0);
2415  SET_LINE;
2416  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2417 
2418  tf1 = Transform3f();
2419  tf2 = Transform3f(Vec3f(0, -5.1, 0));
2420  SET_LINE;
2421  testShapeIntersection(s, tf1, hs, tf2, false);
2422 
2423  tf1 = transform;
2424  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
2425  SET_LINE;
2426  testShapeIntersection(s, tf1, hs, tf2, false);
2427 
2428  hs = Halfspace(Vec3f(0, 0, 1), 0);
2429 
2430  tf1 = Transform3f();
2431  tf2 = Transform3f();
2432  contact << 0, 0, -2.5;
2433  depth = 5;
2434  normal << 0, 0, -1;
2435  SET_LINE;
2436  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2437 
2438  tf1 = transform;
2439  tf2 = transform;
2440  contact = transform.transform(Vec3f(0, 0, -2.5));
2441  depth = 5;
2442  normal = transform.getRotation() * Vec3f(0, 0, -1);
2443  SET_LINE;
2444  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2445 
2446  tf1 = Transform3f();
2447  tf2 = Transform3f(Vec3f(0, 0, 2.5));
2448  contact << 0, 0, -1.25;
2449  depth = 7.5;
2450  normal << 0, 0, -1;
2451  SET_LINE;
2452  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2453 
2454  tf1 = transform;
2455  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
2456  contact = transform.transform(Vec3f(0, 0, -1.25));
2457  depth = 7.5;
2458  normal = transform.getRotation() * Vec3f(0, 0, -1);
2459  SET_LINE;
2460  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2461 
2462  tf1 = Transform3f();
2463  tf2 = Transform3f(Vec3f(0, 0, -2.5));
2464  contact << 0, 0, -3.75;
2465  depth = 2.5;
2466  normal << 0, 0, -1;
2467  SET_LINE;
2468  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2469 
2470  tf1 = transform;
2471  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
2472  contact = transform.transform(Vec3f(0, 0, -3.75));
2473  depth = 2.5;
2474  normal = transform.getRotation() * Vec3f(0, 0, -1);
2475  SET_LINE;
2476  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2477 
2478  tf1 = Transform3f();
2479  tf2 = Transform3f(Vec3f(0, 0, 5.1));
2480  contact << 0, 0, 0.05;
2481  depth = 10.1;
2482  normal << 0, 0, -1;
2483  SET_LINE;
2484  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2485 
2486  tf1 = transform;
2487  tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
2488  contact = transform.transform(Vec3f(0, 0, 0.05));
2489  depth = 10.1;
2490  normal = transform.getRotation() * Vec3f(0, 0, -1);
2491  SET_LINE;
2492  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2493 
2494  tf1 = Transform3f();
2495  tf2 = Transform3f(Vec3f(0, 0, -5.1));
2496  SET_LINE;
2497  testShapeIntersection(s, tf1, hs, tf2, false);
2498 
2499  tf1 = transform;
2500  tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
2501  SET_LINE;
2502  testShapeIntersection(s, tf1, hs, tf2, false);
2503 }
2504 
2505 BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) {
2506  Cone s(5, 10);
2507  Plane hs(Vec3f(1, 0, 0), 0);
2508 
2509  Transform3f tf1;
2510  Transform3f tf2;
2511 
2513  generateRandomTransform(extents, transform);
2514 
2515  Vec3f contact;
2516  FCL_REAL depth;
2517  Vec3f normal;
2518 
2519  tf1 = Transform3f();
2520  tf2 = Transform3f();
2521  contact << 0, 0, 0;
2522  depth = 5;
2523  normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
2524  SET_LINE;
2525  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2526 
2527  tf1 = transform;
2528  tf2 = transform;
2529  contact = transform.transform(Vec3f(0, 0, 0));
2530  depth = 5;
2531  normal =
2532  transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
2533  SET_LINE;
2534  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2535 
2536  tf1 = Transform3f();
2537  tf2 = Transform3f(Vec3f(2.5, 0, 0));
2538  contact << 2.5, 0, -2.5;
2539  depth = 2.5;
2540  normal << 1, 0, 0;
2541  SET_LINE;
2542  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2543 
2544  tf1 = transform;
2545  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
2546  contact = transform.transform(Vec3f(2.5, 0, -2.5));
2547  depth = 2.5;
2548  normal = transform.getRotation() * Vec3f(1, 0, 0);
2549  SET_LINE;
2550  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2551 
2552  tf1 = Transform3f();
2553  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
2554  contact << -2.5, 0, -2.5;
2555  depth = 2.5;
2556  normal << -1, 0, 0;
2557  SET_LINE;
2558  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2559 
2560  tf1 = transform;
2561  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
2562  contact = transform.transform(Vec3f(-2.5, 0, -2.5));
2563  depth = 2.5;
2564  normal = transform.getRotation() * Vec3f(-1, 0, 0);
2565  SET_LINE;
2566  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2567 
2568  tf1 = Transform3f();
2569  tf2 = Transform3f(Vec3f(5.1, 0, 0));
2570  SET_LINE;
2571  testShapeIntersection(s, tf1, hs, tf2, false);
2572 
2573  tf1 = transform;
2574  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
2575  SET_LINE;
2576  testShapeIntersection(s, tf1, hs, tf2, false);
2577 
2578  tf1 = Transform3f();
2579  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
2580  SET_LINE;
2581  testShapeIntersection(s, tf1, hs, tf2, false);
2582 
2583  tf1 = transform;
2584  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
2585  SET_LINE;
2586  testShapeIntersection(s, tf1, hs, tf2, false);
2587 
2588  hs = Plane(Vec3f(0, 1, 0), 0);
2589 
2590  tf1 = Transform3f();
2591  tf2 = Transform3f();
2592  contact << 0, 0, 0;
2593  depth = 5;
2594  normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0)
2595  SET_LINE;
2596  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2597 
2598  tf1 = transform;
2599  tf2 = transform;
2600  contact = transform.transform(Vec3f(0, 0, 0));
2601  depth = 5;
2602  normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
2603  SET_LINE;
2604  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2605 
2606  tf1 = Transform3f();
2607  tf2 = Transform3f(Vec3f(0, 2.5, 0));
2608  contact << 0, 2.5, -2.5;
2609  depth = 2.5;
2610  normal << 0, 1, 0;
2611  SET_LINE;
2612  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2613 
2614  tf1 = transform;
2615  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
2616  contact = transform.transform(Vec3f(0, 2.5, -2.5));
2617  depth = 2.5;
2618  normal = transform.getRotation() * Vec3f(0, 1, 0);
2619  SET_LINE;
2620  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2621 
2622  tf1 = Transform3f();
2623  tf2 = Transform3f(Vec3f(0, -2.5, 0));
2624  contact << 0, -2.5, -2.5;
2625  depth = 2.5;
2626  normal << 0, -1, 0;
2627  SET_LINE;
2628  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2629 
2630  tf1 = transform;
2631  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
2632  contact = transform.transform(Vec3f(0, -2.5, -2.5));
2633  depth = 2.5;
2634  normal = transform.getRotation() * Vec3f(0, -1, 0);
2635  SET_LINE;
2636  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2637 
2638  tf1 = Transform3f();
2639  tf2 = Transform3f(Vec3f(0, 5.1, 0));
2640  SET_LINE;
2641  testShapeIntersection(s, tf1, hs, tf2, false);
2642 
2643  tf1 = transform;
2644  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
2645  SET_LINE;
2646  testShapeIntersection(s, tf1, hs, tf2, false);
2647 
2648  tf1 = Transform3f();
2649  tf2 = Transform3f(Vec3f(0, -5.1, 0));
2650  SET_LINE;
2651  testShapeIntersection(s, tf1, hs, tf2, false);
2652 
2653  tf1 = transform;
2654  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
2655  SET_LINE;
2656  testShapeIntersection(s, tf1, hs, tf2, false);
2657 
2658  hs = Plane(Vec3f(0, 0, 1), 0);
2659 
2660  tf1 = Transform3f();
2661  tf2 = Transform3f();
2662  contact << 0, 0, 0;
2663  depth = 5;
2664  normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0)
2665  SET_LINE;
2666  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2667 
2668  tf1 = transform;
2669  tf2 = transform;
2670  contact = transform.transform(Vec3f(0, 0, 0));
2671  depth = 5;
2672  normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
2673  SET_LINE;
2674  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
2675 
2676  tf1 = Transform3f();
2677  tf2 = Transform3f(Vec3f(0, 0, 2.5));
2678  contact << 0, 0, 2.5;
2679  depth = 2.5;
2680  normal << 0, 0, 1;
2681  SET_LINE;
2682  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2683 
2684  tf1 = transform;
2685  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
2686  contact = transform.transform(Vec3f(0, 0, 2.5));
2687  depth = 2.5;
2688  normal = transform.getRotation() * Vec3f(0, 0, 1);
2689  SET_LINE;
2690  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2691 
2692  tf1 = Transform3f();
2693  tf2 = Transform3f(Vec3f(0, 0, -2.5));
2694  contact << 0, 0, -2.5;
2695  depth = 2.5;
2696  normal << 0, 0, -1;
2697  SET_LINE;
2698  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2699 
2700  tf1 = transform;
2701  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
2702  contact = transform.transform(Vec3f(0, 0, -2.5));
2703  depth = 2.5;
2704  normal = transform.getRotation() * Vec3f(0, 0, -1);
2705  SET_LINE;
2706  testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
2707 
2708  tf1 = Transform3f();
2709  tf2 = Transform3f(Vec3f(0, 0, 10.1));
2710  SET_LINE;
2711  testShapeIntersection(s, tf1, hs, tf2, false);
2712 
2713  tf1 = transform;
2714  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
2715  SET_LINE;
2716  testShapeIntersection(s, tf1, hs, tf2, false);
2717 
2718  tf1 = Transform3f();
2719  tf2 = Transform3f(Vec3f(0, 0, -10.1));
2720  SET_LINE;
2721  testShapeIntersection(s, tf1, hs, tf2, false);
2722 
2723  tf1 = transform;
2724  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
2725  SET_LINE;
2726  testShapeIntersection(s, tf1, hs, tf2, false);
2727 }
2728 
2729 BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere) {
2730  Sphere s1(20);
2731  Sphere s2(10);
2732 
2734  // generateRandomTransform(extents, transform);
2735 
2736  bool res;
2737  FCL_REAL dist = -1;
2738  Vec3f closest_p1, closest_p2, normal;
2739  res =
2740  solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 40, 0)),
2741  dist, closest_p1, closest_p2, normal);
2742  BOOST_CHECK(fabs(dist - 10) < 0.001);
2743  BOOST_CHECK(res);
2744 
2745  res = solver1.shapeDistance(s1, Transform3f(), s2,
2746  Transform3f(Vec3f(30.1, 0, 0)), dist, closest_p1,
2747  closest_p2, normal);
2748  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2749  BOOST_CHECK(res);
2750 
2751  res = solver1.shapeDistance(s1, Transform3f(), s2,
2752  Transform3f(Vec3f(29.9, 0, 0)), dist, closest_p1,
2753  closest_p2, normal);
2754  BOOST_CHECK(dist < 0);
2755  BOOST_CHECK_FALSE(res);
2756 
2757  res =
2758  solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(),
2759  dist, closest_p1, closest_p2, normal);
2760  BOOST_CHECK(fabs(dist - 10) < 0.001);
2761  BOOST_CHECK(res);
2762 
2763  res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2,
2764  Transform3f(), dist, closest_p1, closest_p2,
2765  normal);
2766  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2767  BOOST_CHECK(res);
2768 
2769  res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2,
2770  Transform3f(), dist, closest_p1, closest_p2,
2771  normal);
2772  BOOST_CHECK(dist < 0);
2773  BOOST_CHECK_FALSE(res);
2774 
2775  res = solver1.shapeDistance(s1, transform, s2,
2776  transform * Transform3f(Vec3f(40, 0, 0)), dist,
2777  closest_p1, closest_p2, normal);
2778  // this is one problem: the precise is low sometimes
2779  BOOST_CHECK(fabs(dist - 10) < 0.1);
2780  BOOST_CHECK(res);
2781 
2782  res = solver1.shapeDistance(s1, transform, s2,
2783  transform * Transform3f(Vec3f(30.1, 0, 0)), dist,
2784  closest_p1, closest_p2, normal);
2785  BOOST_CHECK(fabs(dist - 0.1) < 0.06);
2786  BOOST_CHECK(res);
2787 
2788  res = solver1.shapeDistance(s1, transform, s2,
2789  transform * Transform3f(Vec3f(29.9, 0, 0)), dist,
2790  closest_p1, closest_p2, normal);
2791  BOOST_CHECK(dist < 0);
2792  BOOST_CHECK_FALSE(res);
2793 
2794  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2,
2795  transform, dist, closest_p1, closest_p2, normal);
2796  BOOST_CHECK(fabs(dist - 10) < 0.1);
2797  BOOST_CHECK(res);
2798 
2799  res =
2800  solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2,
2801  transform, dist, closest_p1, closest_p2, normal);
2802  BOOST_CHECK(fabs(dist - 0.1) < 0.1);
2803  BOOST_CHECK(res);
2804 
2805  res =
2806  solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2,
2807  transform, dist, closest_p1, closest_p2, normal);
2808  BOOST_CHECK(dist < 0);
2809  BOOST_CHECK_FALSE(res);
2810 }
2811 
2812 BOOST_AUTO_TEST_CASE(shapeDistance_boxbox) {
2813  Box s1(20, 40, 50);
2814  Box s2(10, 10, 10);
2815  Vec3f closest_p1, closest_p2, normal;
2816 
2818  // generateRandomTransform(extents, transform);
2819 
2820  bool res;
2821  FCL_REAL dist;
2822 
2823  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
2824  closest_p1, closest_p2, normal);
2825  BOOST_CHECK(dist <= 0);
2826  BOOST_CHECK_FALSE(res);
2827 
2828  res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
2829  closest_p2, normal);
2830  BOOST_CHECK(dist <= 0);
2831  BOOST_CHECK_FALSE(res);
2832 
2833  res = solver1.shapeDistance(s2, Transform3f(), s2,
2834  Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
2835  closest_p2, normal);
2836  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2837  BOOST_CHECK(res);
2838 
2839  res = solver1.shapeDistance(s2, Transform3f(), s2,
2840  Transform3f(Vec3f(20.1, 0, 0)), dist, closest_p1,
2841  closest_p2, normal);
2842  BOOST_CHECK(fabs(dist - 10.1) < 0.001);
2843  BOOST_CHECK(res);
2844 
2845  res = solver1.shapeDistance(s2, Transform3f(), s2,
2846  Transform3f(Vec3f(0, 20.2, 0)), dist, closest_p1,
2847  closest_p2, normal);
2848  BOOST_CHECK(fabs(dist - 10.2) < 0.001);
2849  BOOST_CHECK(res);
2850 
2851  res = solver1.shapeDistance(s2, Transform3f(), s2,
2852  Transform3f(Vec3f(10.1, 10.1, 0)), dist,
2853  closest_p1, closest_p2, normal);
2854  BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
2855  BOOST_CHECK(res);
2856 
2857  res = solver2.shapeDistance(s2, Transform3f(), s2,
2858  Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
2859  closest_p2, normal);
2860  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2861  BOOST_CHECK(res);
2862 
2863  res = solver2.shapeDistance(s2, Transform3f(), s2,
2864  Transform3f(Vec3f(20.1, 0, 0)), dist, closest_p1,
2865  closest_p2, normal);
2866  BOOST_CHECK(fabs(dist - 10.1) < 0.001);
2867  BOOST_CHECK(res);
2868 
2869  res = solver2.shapeDistance(s2, Transform3f(), s2,
2870  Transform3f(Vec3f(0, 20.1, 0)), dist, closest_p1,
2871  closest_p2, normal);
2872  BOOST_CHECK(fabs(dist - 10.1) < 0.001);
2873  BOOST_CHECK(res);
2874 
2875  res = solver2.shapeDistance(s2, Transform3f(), s2,
2876  Transform3f(Vec3f(10.1, 10.1, 0)), dist,
2877  closest_p1, closest_p2, normal);
2878  BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
2879  BOOST_CHECK(res);
2880 
2881  res = solver1.shapeDistance(s1, transform, s2,
2882  transform * Transform3f(Vec3f(15.1, 0, 0)), dist,
2883  closest_p1, closest_p2, normal);
2884  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2885  BOOST_CHECK(res);
2886 
2887  res =
2888  solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)),
2889  dist, closest_p1, closest_p2, normal);
2890  BOOST_CHECK(fabs(dist - 5) < 0.001);
2891  BOOST_CHECK(res);
2892 
2893  res = solver1.shapeDistance(s1, transform, s2,
2894  transform * Transform3f(Vec3f(20, 0, 0)), dist,
2895  closest_p1, closest_p2, normal);
2896  BOOST_CHECK(fabs(dist - 5) < 0.001);
2897  BOOST_CHECK(res);
2898 }
2899 
2900 BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere) {
2901  Sphere s1(20);
2902  Box s2(5, 5, 5);
2903  Vec3f closest_p1, closest_p2, normal;
2904 
2906  generateRandomTransform(extents, transform);
2907 
2908  bool res;
2909  FCL_REAL dist;
2910 
2911  int N = 10;
2912  for (int i = 0; i < N + 1; ++i) {
2913  FCL_REAL dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N);
2914  res = solver1.shapeDistance(s1, Transform3f(Vec3f(dbox, 0., 0.)), s2,
2915  Transform3f(), dist, closest_p1, closest_p2,
2916  normal);
2917  BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6);
2918  EIGEN_VECTOR_IS_APPROX(normal, -Vec3f(1, 0, 0), 1e-6);
2919 
2920  res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
2921  closest_p2, normal);
2922  res = solver1.shapeDistance(
2923  s1, transform * Transform3f(Vec3f(dbox, 0., 0.)), s2, transform, dist,
2924  closest_p1, closest_p2, normal);
2925  BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6);
2926  EIGEN_VECTOR_IS_APPROX(normal, -transform.getRotation().col(0), 1e-6);
2927  }
2928 
2929  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
2930  closest_p1, closest_p2, normal);
2931  BOOST_CHECK(dist <= 0);
2932  BOOST_CHECK_FALSE(res);
2933 
2934  res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
2935  closest_p2, normal);
2936  BOOST_CHECK(dist <= 0);
2937  BOOST_CHECK_FALSE(res);
2938 
2939  res = solver1.shapeDistance(s1, Transform3f(), s2,
2940  Transform3f(Vec3f(22.6, 0, 0)), dist, closest_p1,
2941  closest_p2, normal);
2942  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2943  BOOST_CHECK(res);
2944 
2945  res = solver1.shapeDistance(s1, transform, s2,
2946  transform * Transform3f(Vec3f(22.6, 0, 0)), dist,
2947  closest_p1, closest_p2, normal);
2948  BOOST_CHECK(fabs(dist - 0.1) < 0.05);
2949  BOOST_CHECK(res);
2950 
2951  res =
2952  solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)),
2953  dist, closest_p1, closest_p2, normal);
2954  BOOST_CHECK(fabs(dist - 17.5) < 0.001);
2955  BOOST_CHECK(res);
2956 
2957  res = solver1.shapeDistance(s1, transform, s2,
2958  transform * Transform3f(Vec3f(40, 0, 0)), dist,
2959  closest_p1, closest_p2, normal);
2960  BOOST_CHECK(fabs(dist - 17.5) < 0.001);
2961  BOOST_CHECK(res);
2962 }
2963 
2964 BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder) {
2965  Cylinder s1(5, 10);
2966  Cylinder s2(5, 10);
2967  Vec3f closest_p1, closest_p2, normal;
2968 
2970  generateRandomTransform(extents, transform);
2971 
2972  bool res;
2973  FCL_REAL dist;
2974 
2975  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
2976  closest_p1, closest_p2, normal);
2977  BOOST_CHECK(dist <= 0);
2978  BOOST_CHECK_FALSE(res);
2979 
2980  res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
2981  closest_p2, normal);
2982  BOOST_CHECK(dist <= 0);
2983  BOOST_CHECK_FALSE(res);
2984 
2985  res = solver1.shapeDistance(s1, Transform3f(), s2,
2986  Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
2987  closest_p2, normal);
2988  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2989  BOOST_CHECK(res);
2990 
2991  res = solver1.shapeDistance(s1, transform, s2,
2992  transform * Transform3f(Vec3f(10.1, 0, 0)), dist,
2993  closest_p1, closest_p2, normal);
2994  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2995  BOOST_CHECK(res);
2996 
2997  res =
2998  solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)),
2999  dist, closest_p1, closest_p2, normal);
3000  BOOST_CHECK(fabs(dist - 30) < 0.001);
3001  BOOST_CHECK(res);
3002 
3003  res = solver1.shapeDistance(s1, transform, s2,
3004  transform * Transform3f(Vec3f(40, 0, 0)), dist,
3005  closest_p1, closest_p2, normal);
3006  BOOST_CHECK(fabs(dist - 30) < 0.001);
3007  BOOST_CHECK(res);
3008 }
3009 
3010 BOOST_AUTO_TEST_CASE(shapeDistance_conecone) {
3011  Cone s1(5, 10);
3012  Cone s2(5, 10);
3013  Vec3f closest_p1, closest_p2, normal;
3014 
3016  generateRandomTransform(extents, transform);
3017 
3018  bool res;
3019  FCL_REAL dist;
3020 
3021  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3022  closest_p1, closest_p2, normal);
3023  BOOST_CHECK(dist <= 0);
3024  BOOST_CHECK_FALSE(res);
3025 
3026  res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3027  closest_p2, normal);
3028  BOOST_CHECK(dist <= 0);
3029  BOOST_CHECK_FALSE(res);
3030 
3031  res = solver1.shapeDistance(s1, Transform3f(), s2,
3032  Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
3033  closest_p2, normal);
3034  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3035  BOOST_CHECK(res);
3036 
3037  res = solver1.shapeDistance(s1, transform, s2,
3038  transform * Transform3f(Vec3f(10.1, 0, 0)), dist,
3039  closest_p1, closest_p2, normal);
3040  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3041  BOOST_CHECK(res);
3042 
3043  res =
3044  solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)),
3045  dist, closest_p1, closest_p2, normal);
3046  BOOST_CHECK(fabs(dist - 30) < 1);
3047  BOOST_CHECK(res);
3048 
3049  res = solver1.shapeDistance(s1, transform, s2,
3050  transform * Transform3f(Vec3f(0, 0, 40)), dist,
3051  closest_p1, closest_p2, normal);
3052  BOOST_CHECK(fabs(dist - 30) < 1);
3053  BOOST_CHECK(res);
3054 }
3055 
3056 BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder) {
3057  Cylinder s1(5, 10);
3058  Cone s2(5, 10);
3059  Vec3f closest_p1, closest_p2, normal;
3060 
3062  generateRandomTransform(extents, transform);
3063 
3064  bool res;
3065  FCL_REAL dist;
3066 
3067  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3068  closest_p1, closest_p2, normal);
3069  BOOST_CHECK(dist <= 0);
3070  BOOST_CHECK_FALSE(res);
3071 
3072  res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3073  closest_p2, normal);
3074  BOOST_CHECK(dist <= 0);
3075  BOOST_CHECK_FALSE(res);
3076 
3077  res = solver1.shapeDistance(s1, Transform3f(), s2,
3078  Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
3079  closest_p2, normal);
3080  BOOST_CHECK(fabs(dist - 0.1) < 0.01);
3081  BOOST_CHECK(res);
3082 
3083  res = solver1.shapeDistance(s1, transform, s2,
3084  transform * Transform3f(Vec3f(10.1, 0, 0)), dist,
3085  closest_p1, closest_p2, normal);
3086  BOOST_CHECK(fabs(dist - 0.1) < 0.02);
3087  BOOST_CHECK(res);
3088 
3089  res =
3090  solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)),
3091  dist, closest_p1, closest_p2, normal);
3092  BOOST_CHECK(fabs(dist - 30) < 0.01);
3093  BOOST_CHECK(res);
3094 
3095  res = solver1.shapeDistance(s1, transform, s2,
3096  transform * Transform3f(Vec3f(40, 0, 0)), dist,
3097  closest_p1, closest_p2, normal);
3098  BOOST_CHECK(fabs(dist - 30) < 0.1);
3099  BOOST_CHECK(res);
3100 }
3101 
3102 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) {
3103  Sphere s1(20);
3104  Sphere s2(10);
3105 
3106  Transform3f tf1;
3107  Transform3f tf2;
3108 
3110  generateRandomTransform(extents, transform);
3111 
3112  // Vec3f contact;
3113  // FCL_REAL depth;
3114  Vec3f normal;
3115 
3116  tf1 = Transform3f();
3117  tf2 = Transform3f(Vec3f(40, 0, 0));
3118  SET_LINE;
3119  testShapeIntersection(s1, tf1, s2, tf2, false);
3120 
3121  tf1 = transform;
3122  tf2 = transform * Transform3f(Vec3f(40, 0, 0));
3123  SET_LINE;
3124  testShapeIntersection(s1, tf1, s2, tf2, false);
3125 
3126  tf1 = Transform3f();
3127  tf2 = Transform3f(Vec3f(30, 0, 0));
3128  normal << 1, 0, 0;
3129  SET_LINE;
3130  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3131 
3132  tf1 = Transform3f();
3133  tf2 = Transform3f(Vec3f(30.01, 0, 0));
3134  SET_LINE;
3135  testShapeIntersection(s1, tf1, s2, tf2, false);
3136 
3137  tf1 = transform;
3138  tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
3139  normal = transform.getRotation() * Vec3f(1, 0, 0);
3140  SET_LINE;
3141  testShapeIntersection(s1, tf1, s2, tf2, false);
3142 
3143  tf1 = Transform3f();
3144  tf2 = Transform3f(Vec3f(29.9, 0, 0));
3145  normal << 1, 0, 0;
3146  SET_LINE;
3147  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3148 
3149  tf1 = transform;
3150  tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
3151  normal = transform.getRotation() * Vec3f(1, 0, 0);
3152  SET_LINE;
3153  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3154 
3155  tf1 = Transform3f();
3156  tf2 = Transform3f();
3157  normal.setZero(); // If the centers of two sphere are at the same position,
3158  // the normal is (0, 0, 0)
3159  SET_LINE;
3160  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3161 
3162  tf1 = transform;
3163  tf2 = transform;
3164  normal.setZero(); // If the centers of two sphere are at the same position,
3165  // the normal is (0, 0, 0)
3166  SET_LINE;
3167  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3168 
3169  tf1 = Transform3f();
3170  tf2 = Transform3f(Vec3f(-29.9, 0, 0));
3171  normal << -1, 0, 0;
3172  SET_LINE;
3173  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3174 
3175  tf1 = transform;
3176  tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
3177  normal = transform.getRotation() * Vec3f(-1, 0, 0);
3178  SET_LINE;
3179  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3180 
3181  tf1 = Transform3f();
3182  tf2 = Transform3f(Vec3f(-30.0, 0, 0));
3183  normal << -1, 0, 0;
3184  SET_LINE;
3185  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3186 
3187  tf1 = Transform3f();
3188  tf2 = Transform3f(Vec3f(-30.01, 0, 0));
3189  normal << -1, 0, 0;
3190  SET_LINE;
3191  testShapeIntersection(s1, tf1, s2, tf2, false);
3192 
3193  tf1 = transform;
3194  tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
3195  SET_LINE;
3196  testShapeIntersection(s1, tf1, s2, tf2, false);
3197 }
3198 
3199 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) {
3200  Box s1(20, 40, 50);
3201  Box s2(10, 10, 10);
3202 
3203  Transform3f tf1;
3204  Transform3f tf2;
3205 
3207  generateRandomTransform(extents, transform);
3208 
3209  // Vec3f point;
3210  // FCL_REAL depth;
3211  Vec3f normal;
3212 
3213  Quaternion3f q;
3214  q = AngleAxis((FCL_REAL)3.140 / 6, UnitZ);
3215 
3216  tf1 = Transform3f();
3217  tf2 = Transform3f();
3218  // TODO: Need convention for normal when the centers of two objects are at
3219  // same position. The current result is (1, 0, 0).
3220  normal << 1, 0, 0;
3221  SET_LINE;
3222  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
3223 
3224  tf1 = transform;
3225  tf2 = transform;
3226  // TODO: Need convention for normal when the centers of two objects are at
3227  // same position. The current result is (1, 0, 0).
3228  normal = transform.getRotation() * Vec3f(1, 0, 0);
3229  SET_LINE;
3230  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
3231 
3232  tf1 = Transform3f();
3233  tf2 = Transform3f(Vec3f(15, 0, 0));
3234  normal << 1, 0, 0;
3235  SET_LINE;
3236  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3237  1e-8);
3238 
3239  tf1 = transform;
3240  tf2 = transform * Transform3f(Vec3f(15.01, 0, 0));
3241  SET_LINE;
3242  testShapeIntersection(s1, tf1, s2, tf2, false);
3243 
3244  tf1 = Transform3f();
3245  tf2 = Transform3f(q);
3246  normal << 1, 0, 0;
3247  SET_LINE;
3248  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
3249 
3250  tf1 = transform;
3251  tf2 = transform * Transform3f(q);
3252  normal = transform.getRotation() * Vec3f(1, 0, 0);
3253  SET_LINE;
3254  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
3255 }
3256 
3257 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) {
3258  Sphere s1(20);
3259  Box s2(5, 5, 5);
3260 
3261  Transform3f tf1;
3262  Transform3f tf2;
3263 
3265  generateRandomTransform(extents, transform);
3266 
3267  // Vec3f point;
3268  // FCL_REAL depth;
3269  Vec3f normal;
3270 
3271  tf1 = Transform3f();
3272  tf2 = Transform3f();
3273  // TODO: Need convention for normal when the centers of two objects are at
3274  // same position.
3275  SET_LINE;
3276  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3277 
3278  tf1 = transform;
3279  tf2 = transform;
3280  // TODO: Need convention for normal when the centers of two objects are at
3281  // same position.
3282  SET_LINE;
3283  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3284 
3285  tf1 = Transform3f();
3286  tf2 = Transform3f(Vec3f(22.5, 0, 0));
3287  normal << 1, 0, 0;
3288  SET_LINE;
3290  s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3291  1e-7); // built-in GJK solver requires larger tolerance than libccd
3292 
3293  tf1 = transform;
3294  tf2 = transform * Transform3f(Vec3f(22.51, 0, 0));
3295  SET_LINE;
3296  testShapeIntersection(s1, tf1, s2, tf2, false);
3297 
3298  tf1 = Transform3f();
3299  tf2 = Transform3f(Vec3f(22.4, 0, 0));
3300  normal << 1, 0, 0;
3301  SET_LINE;
3303  s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3304  1e-2); // built-in GJK solver requires larger tolerance than libccd
3305 
3306  tf1 = transform;
3307  tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
3308  normal = transform.getRotation() * Vec3f(1, 0, 0);
3309  SET_LINE;
3310  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3311  // built-in GJK solver returns incorrect normal.
3312  // testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3313 }
3314 
3315 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) {
3316  Sphere s1(20);
3317  Capsule s2(5, 10);
3318 
3319  Transform3f tf1;
3320  Transform3f tf2;
3321 
3323  generateRandomTransform(extents, transform);
3324 
3325  // Vec3f point;
3326  // FCL_REAL depth;
3327  Vec3f normal;
3328 
3329  tf1 = Transform3f();
3330  tf2 = Transform3f();
3331  // TODO: Need convention for normal when the centers of two objects are at
3332  // same position.
3333  SET_LINE;
3334  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3335 
3336  tf1 = transform;
3337  tf2 = transform;
3338  // TODO: Need convention for normal when the centers of two objects are at
3339  // same position.
3340  SET_LINE;
3341  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3342 
3343  tf1 = Transform3f();
3344  tf2 = Transform3f(Vec3f(24.9, 0, 0));
3345  normal << 1, 0, 0;
3346  SET_LINE;
3347  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3348 
3349  tf1 = transform;
3350  tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
3351  normal = transform.getRotation() * Vec3f(1, 0, 0);
3352  SET_LINE;
3353  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3354 
3355  tf1 = Transform3f();
3356  tf2 = Transform3f(Vec3f(25, 0, 0));
3357  normal << 1, 0, 0;
3358  SET_LINE;
3359  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
3360 
3361  tf1 = transform;
3362  tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
3363  normal = transform.getRotation() * Vec3f(1, 0, 0);
3364  SET_LINE;
3365  testShapeIntersection(s1, tf1, s2, tf2, false);
3366 }
3367 
3368 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) {
3369  Cylinder s1(5, 10);
3370  Cylinder s2(5, 10);
3371 
3372  Transform3f tf1;
3373  Transform3f tf2;
3374 
3376  generateRandomTransform(extents, transform);
3377 
3378  // Vec3f point;
3379  // FCL_REAL depth;
3380  Vec3f normal;
3381 
3382  tf1 = Transform3f();
3383  tf2 = Transform3f();
3384  // TODO: Need convention for normal when the centers of two objects are at
3385  // same position.
3386  SET_LINE;
3387  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3388 
3389  tf1 = transform;
3390  tf2 = transform;
3391  // TODO: Need convention for normal when the centers of two objects are at
3392  // same position.
3393  SET_LINE;
3394  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3395 
3396  tf1 = Transform3f();
3397  tf2 = Transform3f(Vec3f(9.9, 0, 0));
3398  normal << 1, 0, 0;
3399  SET_LINE;
3400  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3401  tol_gjk);
3402 
3403  tf1 = transform;
3404  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
3405  normal = transform.getRotation() * Vec3f(1, 0, 0);
3406  SET_LINE;
3407  testShapeIntersection(s1, tf1, s2, tf2, true);
3408  SET_LINE;
3409  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3410  tol_gjk);
3411 
3412  tf1 = Transform3f();
3413  tf2 = Transform3f(Vec3f(10, 0, 0));
3414  normal << 1, 0, 0;
3415  SET_LINE;
3416  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3417  tol_gjk);
3418 
3419  tf1 = transform;
3420  tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
3421  SET_LINE;
3422  testShapeIntersection(s1, tf1, s2, tf2, false);
3423 }
3424 
3425 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) {
3426  Cone s1(5, 10);
3427  Cone s2(5, 10);
3428 
3429  Transform3f tf1;
3430  Transform3f tf2;
3431 
3433  generateRandomTransform(extents, transform);
3434 
3435  // Vec3f point;
3436  // FCL_REAL depth;
3437  Vec3f normal;
3438 
3439  tf1 = Transform3f();
3440  tf2 = Transform3f();
3441  // TODO: Need convention for normal when the centers of two objects are at
3442  // same position.
3443  SET_LINE;
3444  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3445 
3446  tf1 = transform;
3447  tf2 = transform;
3448  // TODO: Need convention for normal when the centers of two objects are at
3449  // same position.
3450  SET_LINE;
3451  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3452 
3453  tf1 = Transform3f();
3454  // z=0 is a singular points. Two normals could be returned.
3455  tf2 = Transform3f(Vec3f(9.9, 0, 0.00001));
3456  normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
3457  .normalized();
3458  SET_LINE;
3459  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3460  tol_gjk);
3461 
3462  tf1 = transform * tf1;
3463  tf2 = transform * tf2;
3464  normal = transform.getRotation() * normal;
3465  SET_LINE;
3466  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3467  tol_gjk);
3468 
3469  tf1 = Transform3f();
3470  tf2 = Transform3f(Vec3f(10.1, 0, 0));
3471  SET_LINE;
3472  testShapeIntersection(s1, tf1, s2, tf2, false);
3473 
3474  tf1 = transform;
3475  tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
3476  SET_LINE;
3477  testShapeIntersection(s1, tf1, s2, tf2, false);
3478 
3479  tf1 = Transform3f();
3480  tf2 = Transform3f(Vec3f(0, 0, 9.9));
3481  normal << 0, 0, 1;
3482  SET_LINE;
3483  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3484  tol_gjk);
3485 
3486  tf1 = transform;
3487  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
3488  normal = transform.getRotation() * Vec3f(0, 0, 1);
3489  SET_LINE;
3490  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3491  tol_gjk);
3492 }
3493 
3494 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) {
3495  Cylinder s1(5, 10);
3496  Cone s2(5, 10);
3497 
3498  Transform3f tf1;
3499  Transform3f tf2;
3500 
3502  generateRandomTransform(extents, transform);
3503 
3504  // Vec3f point;
3505  // FCL_REAL depth;
3506  Vec3f normal;
3507 
3508  tf1 = Transform3f();
3509  tf2 = Transform3f();
3510  // TODO: Need convention for normal when the centers of two objects are at
3511  // same position.
3512  SET_LINE;
3513  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3514 
3515  tf1 = transform;
3516  tf2 = transform;
3517  // TODO: Need convention for normal when the centers of two objects are at
3518  // same position.
3519  SET_LINE;
3520  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3521 
3522  tf1 = Transform3f();
3523  tf2 = Transform3f(Vec3f(9.9, 0, 0));
3524  SET_LINE;
3525  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3526 
3527  tf1 = transform;
3528  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
3529  SET_LINE;
3530  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3531 
3532  tf1 = Transform3f();
3533  tf2 = Transform3f(Vec3f(10, 0, 0));
3534  SET_LINE;
3535  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3536 
3537  tf1 = transform;
3538  tf2 = transform * Transform3f(Vec3f(10, 0, 0));
3539  SET_LINE;
3540  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
3541 
3542  tf1 = Transform3f();
3543  tf2 = Transform3f(Vec3f(0, 0, 9.9));
3544  normal << 0, 0, 1;
3545  SET_LINE;
3546  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3547  tol_gjk);
3548 
3549  tf1 = transform;
3550  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
3551  normal = transform.getRotation() * Vec3f(0, 0, 1);
3552  SET_LINE;
3553  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3554  tol_gjk);
3555 
3556  tf1 = Transform3f();
3557  tf2 = Transform3f(Vec3f(0, 0, 10));
3558  normal << 0, 0, 1;
3559  SET_LINE;
3560  testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false,
3561  tol_gjk);
3562 
3563  tf1 = transform;
3564  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
3565  SET_LINE;
3566  testShapeIntersection(s1, tf1, s2, tf2, false);
3567 }
3568 
3569 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle) {
3570  Sphere s(10);
3571  Vec3f t[3];
3572  t[0] << 20, 0, 0;
3573  t[1] << -20, 0, 0;
3574  t[2] << 0, 20, 0;
3575 
3577  generateRandomTransform(extents, transform);
3578 
3579  Vec3f c1, c2;
3581  Vec3f normal;
3582  bool res;
3583 
3584  res =
3585  solver2.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2],
3586  Transform3f(), distance, c1, c2, normal);
3587  BOOST_CHECK(res);
3588 
3589  res = solver2.shapeTriangleInteraction(s, transform, t[0], t[1], t[2],
3590  transform, distance, c1, c2, normal);
3591  BOOST_CHECK(res);
3592 
3593  t[0] << 30, 0, 0;
3594  t[1] << 9.9, -20, 0;
3595  t[2] << 9.9, 20, 0;
3596  res =
3597  solver2.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2],
3598  Transform3f(), distance, c1, c2, normal);
3599  BOOST_CHECK(res);
3600 
3601  res = solver2.shapeTriangleInteraction(s, transform, t[0], t[1], t[2],
3602  transform, distance, c1, c2, normal);
3603  BOOST_CHECK(res);
3604 
3605  res =
3606  solver2.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2],
3607  Transform3f(), distance, c1, c2, normal);
3608  BOOST_CHECK(res);
3609  BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
3610 
3611  res = solver2.shapeTriangleInteraction(s, transform, t[0], t[1], t[2],
3612  transform, distance, c1, c2, normal);
3613  BOOST_CHECK(res);
3614  BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
3615 }
3616 
3617 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle) {
3618  Halfspace hs(Vec3f(1, 0, 0), 0);
3619  Vec3f t[3];
3620  t[0] << 20, 0, 0;
3621  t[1] << -20, 0, 0;
3622  t[2] << 0, 20, 0;
3623 
3625  generateRandomTransform(extents, transform);
3626 
3627  Vec3f c1, c2;
3629  Vec3f normal;
3630  bool res;
3631 
3632  res =
3633  solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
3634  Transform3f(), distance, c1, c2, normal);
3635  BOOST_CHECK(res);
3636 
3637  res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
3638  transform, distance, c1, c2, normal);
3639  BOOST_CHECK(res);
3640 
3641  t[0] << 20, 0, 0;
3642  t[1] << 0, -20, 0;
3643  t[2] << 0, 20, 0;
3644  res =
3645  solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
3646  Transform3f(), distance, c1, c2, normal);
3647  BOOST_CHECK(res);
3648 
3649  res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
3650  transform, distance, c1, c2, normal);
3651  BOOST_CHECK(res);
3652 
3653  res =
3654  solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
3655  Transform3f(), distance, c1, c2, normal);
3656  BOOST_CHECK(res);
3657  BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
3658 
3659  res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
3660  transform, distance, c1, c2, normal);
3661  BOOST_CHECK(res);
3662  BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
3663 }
3664 
3665 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle) {
3666  Plane hs(Vec3f(1, 0, 0), 0);
3667  Vec3f t[3];
3668  t[0] << 20, 0, 0;
3669  t[1] << -20, 0, 0;
3670  t[2] << 0, 20, 0;
3671 
3673  generateRandomTransform(extents, transform);
3674 
3675  Vec3f c1, c2;
3677  Vec3f normal;
3678  bool res;
3679 
3680  res =
3681  solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
3682  Transform3f(), distance, c1, c2, normal);
3683  BOOST_CHECK(res);
3684 
3685  res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
3686  transform, distance, c1, c2, normal);
3687  BOOST_CHECK(res);
3688 
3689  t[0] << 20, 0, 0;
3690  t[1] << -0.1, -20, 0;
3691  t[2] << -0.1, 20, 0;
3692  res =
3693  solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
3694  Transform3f(), distance, c1, c2, normal);
3695  BOOST_CHECK(res);
3696 
3697  res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
3698  transform, distance, c1, c2, normal);
3699  BOOST_CHECK(res);
3700 
3701  res =
3702  solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2],
3703  Transform3f(), distance, c1, c2, normal);
3704  BOOST_CHECK(res);
3705  BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
3706 
3707  res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2],
3708  transform, distance, c1, c2, normal);
3709  BOOST_CHECK(res);
3710  BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
3711 }
3712 
3713 BOOST_AUTO_TEST_CASE(spheresphere) {
3714  Sphere s1(20);
3715  Sphere s2(10);
3716  Vec3f closest_p1, closest_p2, normal;
3717 
3719  generateRandomTransform(extents, transform);
3720 
3721  bool res;
3722  FCL_REAL dist = -1;
3723 
3724  res =
3725  solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)),
3726  dist, closest_p1, closest_p2, normal);
3727  BOOST_CHECK(fabs(dist - 10) < 0.001);
3728  BOOST_CHECK(res);
3729 
3730  res = solver2.shapeDistance(s1, Transform3f(), s2,
3731  Transform3f(Vec3f(30.1, 0, 0)), dist, closest_p1,
3732  closest_p2, normal);
3733  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3734  BOOST_CHECK(res);
3735 
3736  res = solver2.shapeDistance(s1, Transform3f(), s2,
3737  Transform3f(Vec3f(29.9, 0, 0)), dist, closest_p1,
3738  closest_p2, normal);
3739  BOOST_CHECK(dist <= 0);
3740  BOOST_CHECK_FALSE(res);
3741 
3742  res =
3743  solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(),
3744  dist, closest_p1, closest_p2, normal);
3745  BOOST_CHECK(fabs(dist - 10) < 0.001);
3746  BOOST_CHECK(res);
3747 
3748  res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2,
3749  Transform3f(), dist, closest_p1, closest_p2,
3750  normal);
3751  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3752  BOOST_CHECK(res);
3753 
3754  res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2,
3755  Transform3f(), dist, closest_p1, closest_p2,
3756  normal);
3757  BOOST_CHECK(dist <= 0);
3758  BOOST_CHECK_FALSE(res);
3759 
3760  res = solver2.shapeDistance(s1, transform, s2,
3761  transform * Transform3f(Vec3f(40, 0, 0)), dist,
3762  closest_p1, closest_p2, normal);
3763  BOOST_CHECK(fabs(dist - 10) < 0.001);
3764  BOOST_CHECK(res);
3765 
3766  res = solver2.shapeDistance(s1, transform, s2,
3767  transform * Transform3f(Vec3f(30.1, 0, 0)), dist,
3768  closest_p1, closest_p2, normal);
3769  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3770  BOOST_CHECK(res);
3771 
3772  res = solver2.shapeDistance(s1, transform, s2,
3773  transform * Transform3f(Vec3f(29.9, 0, 0)), dist,
3774  closest_p1, closest_p2, normal);
3775  BOOST_CHECK(dist <= 0);
3776  BOOST_CHECK_FALSE(res);
3777 
3778  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2,
3779  transform, dist, closest_p1, closest_p2, normal);
3780  BOOST_CHECK(fabs(dist - 10) < 0.001);
3781  BOOST_CHECK(res);
3782 
3783  res =
3784  solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2,
3785  transform, dist, closest_p1, closest_p2, normal);
3786  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3787  BOOST_CHECK(res);
3788 
3789  res =
3790  solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2,
3791  transform, dist, closest_p1, closest_p2, normal);
3792  BOOST_CHECK(dist <= 0);
3793  BOOST_CHECK_FALSE(res);
3794 }
3795 
3797  Box s1(20, 40, 50);
3798  Box s2(10, 10, 10);
3799  Vec3f closest_p1, closest_p2, normal;
3800 
3802  generateRandomTransform(extents, transform);
3803 
3804  bool res;
3805  FCL_REAL dist;
3806 
3807  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3808  closest_p1, closest_p2, normal);
3809  BOOST_CHECK(dist <= 0);
3810  BOOST_CHECK_FALSE(res);
3811 
3812  res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3813  closest_p2, normal);
3814  BOOST_CHECK(dist <= 0);
3815  BOOST_CHECK_FALSE(res);
3816 
3817  res = solver2.shapeDistance(s1, Transform3f(), s2,
3818  Transform3f(Vec3f(15.1, 0, 0)), dist, closest_p1,
3819  closest_p2, normal);
3820  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3821  BOOST_CHECK(res);
3822 
3823  res = solver2.shapeDistance(s1, transform, s2,
3824  transform * Transform3f(Vec3f(15.1, 0, 0)), dist,
3825  closest_p1, closest_p2, normal);
3826  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3827  BOOST_CHECK(res);
3828 
3829  res =
3830  solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)),
3831  dist, closest_p1, closest_p2, normal);
3832  BOOST_CHECK(fabs(dist - 5) < 0.001);
3833  BOOST_CHECK(res);
3834 
3835  res = solver2.shapeDistance(s1, transform, s2,
3836  transform * Transform3f(Vec3f(20, 0, 0)), dist,
3837  closest_p1, closest_p2, normal);
3838  BOOST_CHECK(fabs(dist - 5) < 0.001);
3839  BOOST_CHECK(res);
3840 }
3841 
3843  Sphere s1(20);
3844  Box s2(5, 5, 5);
3845  Vec3f closest_p1, closest_p2, normal;
3846 
3848  generateRandomTransform(extents, transform);
3849 
3850  bool res;
3851  FCL_REAL dist;
3852 
3853  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3854  closest_p1, closest_p2, normal);
3855  BOOST_CHECK(dist <= 0);
3856  BOOST_CHECK_FALSE(res);
3857 
3858  res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3859  closest_p2, normal);
3860  BOOST_CHECK(dist <= 0);
3861  BOOST_CHECK_FALSE(res);
3862 
3863  res = solver2.shapeDistance(s1, Transform3f(), s2,
3864  Transform3f(Vec3f(22.6, 0, 0)), dist, closest_p1,
3865  closest_p2, normal);
3866  BOOST_CHECK(fabs(dist - 0.1) < 0.01);
3867  BOOST_CHECK(res);
3868 
3869  res = solver2.shapeDistance(s1, transform, s2,
3870  transform * Transform3f(Vec3f(22.6, 0, 0)), dist,
3871  closest_p1, closest_p2, normal);
3872  BOOST_CHECK(fabs(dist - 0.1) < 0.01);
3873  BOOST_CHECK(res);
3874 
3875  res =
3876  solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)),
3877  dist, closest_p1, closest_p2, normal);
3878  BOOST_CHECK(fabs(dist - 17.5) < 0.001);
3879  BOOST_CHECK(res);
3880 
3881  res = solver2.shapeDistance(s1, transform, s2,
3882  transform * Transform3f(Vec3f(40, 0, 0)), dist,
3883  closest_p1, closest_p2, normal);
3884  BOOST_CHECK(fabs(dist - 17.5) < 0.001);
3885  BOOST_CHECK(res);
3886 }
3887 
3888 BOOST_AUTO_TEST_CASE(cylindercylinder) {
3889  Cylinder s1(5, 10);
3890  Cylinder s2(5, 10);
3891  Vec3f closest_p1, closest_p2, normal;
3892 
3894  generateRandomTransform(extents, transform);
3895 
3896  bool res;
3897  FCL_REAL dist;
3898 
3899  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3900  closest_p1, closest_p2, normal);
3901  BOOST_CHECK(dist <= 0);
3902  BOOST_CHECK_FALSE(res);
3903 
3904  res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3905  closest_p2, normal);
3906  BOOST_CHECK(dist <= 0);
3907  BOOST_CHECK_FALSE(res);
3908 
3909  res = solver2.shapeDistance(s1, Transform3f(), s2,
3910  Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
3911  closest_p2, normal);
3912  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3913  BOOST_CHECK(res);
3914 
3915  res = solver2.shapeDistance(s1, transform, s2,
3916  transform * Transform3f(Vec3f(10.1, 0, 0)), dist,
3917  closest_p1, closest_p2, normal);
3918  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3919  BOOST_CHECK(res);
3920 
3921  res =
3922  solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)),
3923  dist, closest_p1, closest_p2, normal);
3924  BOOST_CHECK(fabs(dist - 30) < 0.001);
3925  BOOST_CHECK(res);
3926 
3927  res = solver2.shapeDistance(s1, transform, s2,
3928  transform * Transform3f(Vec3f(40, 0, 0)), dist,
3929  closest_p1, closest_p2, normal);
3930  BOOST_CHECK(fabs(dist - 30) < 0.001);
3931  BOOST_CHECK(res);
3932 }
3933 
3935  Cone s1(5, 10);
3936  Cone s2(5, 10);
3937  Vec3f closest_p1, closest_p2, normal;
3938 
3940  generateRandomTransform(extents, transform);
3941 
3942  bool res;
3943  FCL_REAL dist;
3944 
3945  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3946  closest_p1, closest_p2, normal);
3947  BOOST_CHECK(dist <= 0);
3948  BOOST_CHECK_FALSE(res);
3949 
3950  res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3951  closest_p2, normal);
3952  BOOST_CHECK(dist <= 0);
3953  BOOST_CHECK_FALSE(res);
3954 
3955  res = solver2.shapeDistance(s1, Transform3f(), s2,
3956  Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
3957  closest_p2, normal);
3958  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3959  BOOST_CHECK(res);
3960 
3961  res = solver2.shapeDistance(s1, transform, s2,
3962  transform * Transform3f(Vec3f(10.1, 0, 0)), dist,
3963  closest_p1, closest_p2, normal);
3964  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3965  BOOST_CHECK(res);
3966 
3967  res =
3968  solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)),
3969  dist, closest_p1, closest_p2, normal);
3970  BOOST_CHECK(fabs(dist - 30) < 0.001);
3971  BOOST_CHECK(res);
3972 
3973  res = solver2.shapeDistance(s1, transform, s2,
3974  transform * Transform3f(Vec3f(0, 0, 40)), dist,
3975  closest_p1, closest_p2, normal);
3976  BOOST_CHECK(fabs(dist - 30) < 0.001);
3977  BOOST_CHECK(res);
3978 }
3979 
3980 template <typename S1, typename S2>
3981 void testReversibleShapeDistance(const S1& s1, const S2& s2,
3982  FCL_REAL distance) {
3983  Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
3984  Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
3985 
3986  FCL_REAL distA;
3987  FCL_REAL distB;
3988  Vec3f p1A;
3989  Vec3f p1B;
3990  Vec3f p2A;
3991  Vec3f p2B;
3992  Vec3f normalA, normalB;
3993 
3994  bool resA;
3995  bool resB;
3996 
3997  const double tol = 1e-6;
3998 
3999  resA = solver1.shapeDistance(s1, tf1, s2, tf2, distA, p1A, p2A, normalA);
4000  resB = solver1.shapeDistance(s2, tf2, s1, tf1, distB, p1B, p2B, normalB);
4001 
4002  BOOST_CHECK(resA);
4003  BOOST_CHECK(resB);
4004  BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same
4005  BOOST_CHECK(
4006  isEqual(p1A, p2B, tol)); // closest points should in reverse order
4007  BOOST_CHECK(isEqual(p2A, p1B, tol));
4008 
4009  resA = solver2.shapeDistance(s1, tf1, s2, tf2, distA, p1A, p2A, normalA);
4010  resB = solver2.shapeDistance(s2, tf2, s1, tf1, distB, p1B, p2B, normalB);
4011 
4012  BOOST_CHECK(resA);
4013  BOOST_CHECK(resB);
4014  BOOST_CHECK_CLOSE(distA, distB, tol);
4015  BOOST_CHECK(isEqual(p1A, p2B, tol));
4016  BOOST_CHECK(isEqual(p2A, p1B, tol));
4017 }
4018 
4019 BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) {
4020  // This test check whether a shape distance algorithm is called for the
4021  // reverse case as well. For example, if FCL has sphere-capsule distance
4022  // algorithm, then this algorithm should be called for capsule-sphere case.
4023 
4024  // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone,
4025  // cylinder, plane, halfspace
4026  Box box(10, 10, 10);
4027  Sphere sphere(5);
4028  Capsule capsule(5, 10);
4029  Cone cone(5, 10);
4030  Cylinder cylinder(5, 10);
4031  Plane plane(Vec3f(0, 0, 0), 0.0);
4032  Halfspace halfspace(Vec3f(0, 0, 0), 0.0);
4033 
4034  // Use sufficiently long distance so that all the primitive shapes CANNOT
4035  // intersect
4036  FCL_REAL distance = 15.0;
4037 
4038  // If new shape distance algorithm is added for two distinct primitive
4039  // shapes, uncomment associated lines. For example, box-sphere intersection
4040  // algorithm is added, then uncomment box-sphere.
4041 
4042  // testReversibleShapeDistance(box, sphere, distance);
4043  // testReversibleShapeDistance(box, capsule, distance);
4044  // testReversibleShapeDistance(box, cone, distance);
4045  // testReversibleShapeDistance(box, cylinder, distance);
4046  // testReversibleShapeDistance(box, plane, distance);
4047  // testReversibleShapeDistance(box, halfspace, distance);
4048 
4049  SET_LINE;
4050  testReversibleShapeDistance(sphere, capsule, distance);
4051  // testReversibleShapeDistance(sphere, cone, distance);
4052  // testReversibleShapeDistance(sphere, cylinder, distance);
4053  // testReversibleShapeDistance(sphere, plane, distance);
4054  // testReversibleShapeDistance(sphere, halfspace, distance);
4055 
4056  // testReversibleShapeDistance(capsule, cone, distance);
4057  // testReversibleShapeDistance(capsule, cylinder, distance);
4058  // testReversibleShapeDistance(capsule, plane, distance);
4059  // testReversibleShapeDistance(capsule, halfspace, distance);
4060 
4061  // testReversibleShapeDistance(cone, cylinder, distance);
4062  // testReversibleShapeDistance(cone, plane, distance);
4063  // testReversibleShapeDistance(cone, halfspace, distance);
4064 
4065  // testReversibleShapeDistance(cylinder, plane, distance);
4066  // testReversibleShapeDistance(cylinder, halfspace, distance);
4067 
4068  // testReversibleShapeDistance(plane, halfspace, distance);
4069 }
Quaternion3f getQuatRotation() const
get quaternion
Definition: transform.h:118
Vec3f halfSide
box side half-length
FCL_REAL tol_gjk
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
request to the distance computation
BOOST_AUTO_TEST_CASE(box_to_bvh)
q
tuple p1
Definition: octree.py:55
GJKSolver solver1
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
FCL_REAL halfLength
Half Length along z axis.
Cylinder along Z axis. The cylinder is defined at its centroid.
Main namespace.
t
void setQuatRotation(const Quaternion3f &q_)
set transform from rotation
Definition: transform.h:147
static FCL_REAL epsilon
Definition: simple.cpp:12
Vec3f nearest_points[2]
nearest points
tuple tf2
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Transform3f inverse()
inverse transform
Definition: transform.h:165
size_t numContacts() const
number of contacts found
const FCL_REAL eps
Definition: obb.cpp:93
bool compareContactPoints(const Vec3f &c1, const Vec3f &c2)
void compareContact(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, const Vec3f &contact, Vec3f *expected_point, FCL_REAL depth, FCL_REAL *expected_depth, const Vec3f &normal, Vec3f *expected_normal, bool check_opposite_normal, FCL_REAL tol)
void printComparisonError(const std::string &comparison_type, const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, const Vec3f &contact_or_normal, const Vec3f &expected_contact_or_normal, bool check_opposite_normal, FCL_REAL tol)
#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)
Definition: utility.h:59
#define FCL_CHECK_EQUAL(a, b)
#define SET_LINE
Base class for all basic geometric shapes.
void testBoxBoxContactPoints(const Matrix3f &R)
request to the collision algorithm
void clear()
clear the results obtained
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
FCL_REAL radius
Radius of the cone.
FCL_REAL penetration_depth
penetration depth
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
double FCL_REAL
Definition: data_types.h:65
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Center at zero point, axis aligned box.
Vec3f normal
In case both objects are in collision, store the normal.
static std::ostream & operator<<(std::ostream &o, const Quaternion3f &q)
Definition: transform.h:48
void testReversibleShapeDistance(const S1 &s1, const S2 &s2, FCL_REAL distance)
FCL_REAL extents[6]
std::string getNodeTypeName(NODE_TYPE node_type)
Definition: utility.cpp:325
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Cone The base of the cone is at and the top is at .
Vec3f pos
contact position, in world space
GJKSolver solver2
FCL_REAL radius
Radius of the sphere.
FCL_REAL radius
Radius of the cylinder.
bool shapeDistance(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
distance computation between two shapes
Definition: narrowphase.h:261
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:46
Center at zero point sphere.
Capsule It is where is the distance between the point x and the capsule segment AB...
Eigen::AngleAxis< FCL_REAL > AngleAxis
Definition: utility.h:123
bool shapeTriangleInteraction(const S &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
intersection checking between one shape and a triangle with transformation
Definition: narrowphase.h:178
res
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
bool shapeIntersect(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance_lower_bound, bool enable_penetration, Vec3f *contact_points, Vec3f *normal) const
intersection checking between two shapes
Definition: narrowphase.h:104
const Contact & getContact(size_t i) const
get the i-th contact calculated
#define BOOST_CHECK_FALSE(p)
bool enable_contact
whether the contact information (normal, penetration depth and contact position) will return ...
FCL_REAL halfLength
Half Length along z axis.
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
c2
const Vec3f UnitZ
Definition: utility.cpp:89
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
Contact information returned by collision.
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:488
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon() *100)
Definition: tools.h:209
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:485
void generateRandomTransform(FCL_REAL extents[6], Transform3f &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
Definition: utility.cpp:208
Vec3f normal
contact normal, pointing from o1 to o2
#define FCL_CHECK(cond)
tuple tf1
void testShapeIntersection(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, bool expect_collision, Vec3f *expected_point=NULL, FCL_REAL *expected_depth=NULL, Vec3f *expected_normal=NULL, bool check_opposite_normal=false, FCL_REAL tol=1e-9)


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01