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 
41 #include <hpp/fcl/narrowphase/narrowphase.h>
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>
47 #include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
48 
49 using namespace hpp::fcl;
50 
51 FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
52 
53 FCL_REAL tol_gjk = 0.01;
54 GJKSolver solver1;
55 GJKSolver solver2;
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 
241  Transform3f tf1(
242  Quaternion3f(0.5279170511703305, -0.50981118132505521,
243  -0.67596178682051911, 0.0668715876735793),
244  Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
245 
246  Transform3f tf2(
247  Quaternion3f(0.70738826916719977, 0, 0, 0.70682518110536596),
248  Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
249 
250  GJKSolver solver;
251  FCL_REAL distance;
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 
297  Transform3f tf1;
298  Transform3f tf2;
299 
300  Transform3f transform;
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 
394 void testBoxBoxContactPoints(const Matrix3f& R) {
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));
414  Transform3f tf2 = Transform3f(R);
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 
443  Transform3f tf1;
444  Transform3f tf2;
445 
446  Transform3f transform;
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;
499  generateRandomTransform(extents, tf);
500  SET_LINE;
501  testBoxBoxContactPoints(tf.getRotation());
502  }
503 }
504 
505 BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) {
506  Sphere s1(20);
507  Box s2(5, 5, 5);
508 
509  Transform3f tf1;
510  Transform3f tf2;
511 
512  Transform3f transform;
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 
568  hpp::fcl::Sphere sphere(1);
569  hpp::fcl::Box box(10, 2, 10);
570 
571  hpp::fcl::DistanceResult result;
572  distance(&sphere, Transform3f(rotSphere, trSphere), &box,
573  Transform3f(rotBox, trBox), DistanceRequest(true), result);
574 
575  FCL_REAL eps = Eigen::NumTraits<FCL_REAL>::epsilon();
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 
586  Transform3f tf1;
587  Transform3f tf2;
588 
589  Transform3f transform;
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 
651  Transform3f tf1;
652  Transform3f tf2;
653 
654  Transform3f transform;
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 
842  Transform3f transform;
843  generateRandomTransform(extents, transform);
844 
845  Vec3f c1, c2, normal;
846  FCL_REAL distance;
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 
889  Transform3f transform;
890  generateRandomTransform(extents, transform);
891 
892  Vec3f c1, c2;
893  FCL_REAL distance;
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 
945  Transform3f transform;
946  generateRandomTransform(extents, transform);
947 
948  Vec3f c1, c2;
949  FCL_REAL distance;
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 
986  Transform3f tf1;
987  Transform3f tf2;
988 
989  Transform3f transform;
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 
1078  Transform3f transform;
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 
1161  Transform3f transform;
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 
1255  Transform3f transform;
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 
1343  Transform3f transform;
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 
1584  Transform3f transform;
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 
1807  Transform3f transform;
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 
2048  Transform3f transform;
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 
2271  Transform3f transform;
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 
2512  Transform3f transform;
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 
2733  Transform3f transform;
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 
2817  Transform3f transform;
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 
2905  Transform3f transform;
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 
2969  Transform3f transform;
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 
3015  Transform3f transform;
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 
3061  Transform3f transform;
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 
3109  Transform3f transform;
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 
3206  Transform3f transform;
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 
3264  Transform3f transform;
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;
3289  testShapeIntersection(
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;
3302  testShapeIntersection(
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 
3322  Transform3f transform;
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 
3375  Transform3f transform;
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 
3432  Transform3f transform;
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 
3501  Transform3f transform;
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 
3576  Transform3f transform;
3577  generateRandomTransform(extents, transform);
3578 
3579  Vec3f c1, c2;
3580  FCL_REAL distance;
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 
3624  Transform3f transform;
3625  generateRandomTransform(extents, transform);
3626 
3627  Vec3f c1, c2;
3628  FCL_REAL distance;
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 
3672  Transform3f transform;
3673  generateRandomTransform(extents, transform);
3674 
3675  Vec3f c1, c2;
3676  FCL_REAL distance;
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 
3718  Transform3f transform;
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 
3796 BOOST_AUTO_TEST_CASE(boxbox) {
3797  Box s1(20, 40, 50);
3798  Box s2(10, 10, 10);
3799  Vec3f closest_p1, closest_p2, normal;
3800 
3801  Transform3f transform;
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 
3842 BOOST_AUTO_TEST_CASE(boxsphere) {
3843  Sphere s1(20);
3844  Box s2(5, 5, 5);
3845  Vec3f closest_p1, closest_p2, normal;
3846 
3847  Transform3f transform;
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 
3893  Transform3f transform;
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 
3934 BOOST_AUTO_TEST_CASE(conecone) {
3935  Cone s1(5, 10);
3936  Cone s2(5, 10);
3937  Vec3f closest_p1, closest_p2, normal;
3938 
3939  Transform3f transform;
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 }
hpp::fcl::Cylinder::radius
FCL_REAL radius
Radius of the cylinder.
Definition: shape/geometric_shapes.h:522
hpp::fcl::isEqual
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
solver1
GJKSolver solver1
Definition: test/geometric_shapes.cpp:54
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::transform
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
Definition: geometric_shapes_utility.cpp:248
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::DistanceResult::nearest_points
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:427
FCL_CHECK
#define FCL_CHECK(cond)
Definition: test/geometric_shapes.cpp:59
collision_manager.sphere
sphere
Definition: collision_manager.py:4
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
compareContactPoints
bool compareContactPoints(const Vec3f &c1, const Vec3f &c2)
Definition: test/geometric_shapes.cpp:390
hpp::fcl::CollisionResult::getContact
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_data.h:345
hpp::fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_data.h:342
utility.h
eps
const FCL_REAL eps
Definition: obb.cpp:93
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
hpp::fcl::GJKSolver::shapeTriangleInteraction
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
hpp::fcl::distance
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
R
R
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: shape/geometric_shapes.h:148
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
narrowphase.h
printComparisonError
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)
Definition: test/geometric_shapes.cpp:80
hpp::fcl::GJKSolver::shapeDistance
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_VECTOR_IS_APPROX
#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)
Definition: utility.h:59
hpp::fcl::Cylinder::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:528
SET_LINE
#define SET_LINE
Definition: test/geometric_shapes.cpp:58
hpp::fcl::collide
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,...
Definition: src/collision.cpp:63
res
res
distance.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::Sphere::radius
FCL_REAL radius
Radius of the sphere.
Definition: shape/geometric_shapes.h:206
hpp::fcl::operator<<
static std::ostream & operator<<(std::ostream &o, const Quaternion3f &q)
Definition: transform.h:48
epsilon
static FCL_REAL epsilon
Definition: simple.cpp:12
hpp::fcl::DistanceResult::normal
Vec3f normal
In case both objects are in collision, store the normal.
Definition: collision_data.h:430
testBoxBoxContactPoints
void testBoxBoxContactPoints(const Matrix3f &R)
Definition: test/geometric_shapes.cpp:394
hpp::fcl::getNodeTypeName
std::string getNodeTypeName(NODE_TYPE node_type)
Definition: utility.cpp:325
hpp::fcl::GJKSolver
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
hpp::fcl::AngleAxis
Eigen::AngleAxis< FCL_REAL > AngleAxis
Definition: utility.h:123
compareContact
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)
Definition: test/geometric_shapes.cpp:129
hpp::fcl::CollisionRequest::enable_contact
bool enable_contact
whether the contact information (normal, penetration depth and contact position) will return
Definition: collision_data.h:241
hpp::fcl::Contact::normal
Vec3f normal
contact normal, pointing from o1 to o2
Definition: collision_data.h:74
hpp::fcl::GJKSolver::epa_tolerance
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:485
hpp::fcl::CollisionResult::clear
void clear()
clear the results obtained
Definition: collision_data.h:377
FCL_CHECK_EQUAL
#define FCL_CHECK_EQUAL(a, b)
Definition: test/geometric_shapes.cpp:61
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
t
tuple t
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: shape/geometric_shapes.h:333
q
q
BOOST_CHECK_FALSE
#define BOOST_CHECK_FALSE(p)
Definition: test/geometric_shapes.cpp:65
hpp::fcl::Cone::radius
FCL_REAL radius
Radius of the cone.
Definition: shape/geometric_shapes.h:427
hpp::fcl::DistanceResult::min_distance
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0.
Definition: collision_data.h:424
hpp::fcl
Definition: broadphase_bruteforce.h:45
octree.p1
tuple p1
Definition: octree.py:54
testReversibleShapeDistance
void testReversibleShapeDistance(const S1 &s1, const S2 &s2, FCL_REAL distance)
Definition: test/geometric_shapes.cpp:3981
collision
Definition: python_unit/collision.py:1
solver2
GJKSolver solver2
Definition: test/geometric_shapes.cpp:55
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::Halfspace
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: shape/geometric_shapes.h:729
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
hpp::fcl::UnitZ
const Vec3f UnitZ
Definition: utility.cpp:89
geometric_shape_to_BVH_model.h
hpp::fcl::Cone::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:433
hpp::fcl::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:50
tools.h
c2
c2
testShapeIntersection
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)
Definition: test/geometric_shapes.cpp:165
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape/geometric_shapes.h:51
hpp::fcl::generateRandomTransform
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
hpp::fcl::Contact::penetration_depth
FCL_REAL penetration_depth
penetration depth
Definition: collision_data.h:80
hpp::fcl::GJKSolver::gjk_tolerance
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:488
hpp::fcl::Contact::pos
Vec3f pos
contact position, in world space
Definition: collision_data.h:77
line
int line
Definition: test/geometric_shapes.cpp:57
hpp::fcl::GJKSolver::shapeIntersect
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
hpp::fcl::Contact
Contact information returned by collision.
Definition: collision_data.h:54
hpp::fcl::Quaternion3f
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:46
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(box_to_bvh)
Definition: test/geometric_shapes.cpp:210
tol_gjk
FCL_REAL tol_gjk
Definition: test/geometric_shapes.cpp:53
collision.h
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
hpp::fcl::Plane
Infinite plane.
Definition: shape/geometric_shapes.h:810

hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:13