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

hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:58