test_ray_intersection.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, Open Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
38 #include <geometric_shapes/bodies.h>
39 #include <geometric_shapes/shape_operations.h>
40 #include <geometric_shapes/body_operations.h>
41 #include <boost/filesystem.hpp>
42 #include <random_numbers/random_numbers.h>
43 #include <gtest/gtest.h>
44 #include "resources/config.h"
45 
46 Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator& g)
47 {
48  const Eigen::Vector3d t(g.uniformReal(-100, 100), g.uniformReal(-100, 100), g.uniformReal(-100, 100));
49 
50  double quat[4];
51  g.quaternion(quat);
52  const Eigen::Quaterniond r({ quat[3], quat[0], quat[1], quat[2] });
53 
54  return Eigen::Isometry3d::TranslationType(t) * r;
55 }
56 
57 #define EXPECT_VECTORS_EQUAL(v1, v2, error) \
58  EXPECT_NEAR((v1)[0], (v2)[0], (error)); \
59  EXPECT_NEAR((v1)[1], (v2)[1], (error)); \
60  EXPECT_NEAR((v1)[2], (v2)[2], (error));
61 
62 #define CHECK_NO_INTERSECTION(body, origin, direction) \
63  { \
64  EigenSTL::vector_Vector3d intersections; \
65  Eigen::Vector3d o origin; \
66  Eigen::Vector3d d direction; \
67  const auto result = (body).intersectsRay(o, d, &intersections, 2); \
68  EXPECT_FALSE(result); \
69  EXPECT_EQ(0u, intersections.size()); \
70  }
71 
72 #define CHECK_INTERSECTS(body, origin, direction, numIntersections) \
73  { \
74  EigenSTL::vector_Vector3d intersections; \
75  Eigen::Vector3d o origin; \
76  Eigen::Vector3d d direction; \
77  const auto result = (body).intersectsRay(o, d, &intersections, 2); \
78  EXPECT_TRUE(result); \
79  EXPECT_EQ(static_cast<size_t>((numIntersections)), intersections.size()); \
80  }
81 
82 #define CHECK_INTERSECTS_ONCE(body, origin, direction, intersection, error) \
83  { \
84  EigenSTL::vector_Vector3d intersections; \
85  Eigen::Vector3d o origin; \
86  Eigen::Vector3d d direction; \
87  Eigen::Vector3d i intersection; \
88  const auto result = (body).intersectsRay(o, d, &intersections, 2); \
89  EXPECT_TRUE(result); \
90  EXPECT_EQ(1u, intersections.size()); \
91  if (intersections.size() == 1u) \
92  { \
93  EXPECT_VECTORS_EQUAL(intersections.at(0), i, (error)); \
94  } \
95  }
96 
97 #define CHECK_INTERSECTS_TWICE(body, origin, direction, intersc1, intersc2, error) \
98  { \
99  EigenSTL::vector_Vector3d intersections; \
100  Eigen::Vector3d o origin; \
101  Eigen::Vector3d d direction; \
102  Eigen::Vector3d i1 intersc1; \
103  Eigen::Vector3d i2 intersc2; \
104  const auto result = (body).intersectsRay(o, d, &intersections, 2); \
105  EXPECT_TRUE(result); \
106  EXPECT_EQ(2u, intersections.size()); \
107  if (intersections.size() == 2u) \
108  { \
109  if (fabs(static_cast<double>((intersections.at(0) - i1).norm())) < (error)) \
110  { \
111  EXPECT_VECTORS_EQUAL(intersections.at(0), i1, (error)); \
112  EXPECT_VECTORS_EQUAL(intersections.at(1), i2, (error)); \
113  } \
114  else \
115  { \
116  EXPECT_VECTORS_EQUAL(intersections.at(0), i2, (error)); \
117  EXPECT_VECTORS_EQUAL(intersections.at(1), i1, (error)); \
118  } \
119  } \
120  }
121 
122 TEST(SphereRayIntersection, OriginInside)
123 {
124  shapes::Sphere shape(1.0);
125  bodies::Sphere sphere(&shape);
126 
127  // clang-format off
128  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 1, 0, 0), ( 1, 0, 0), 1e-6)
129  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), (-1, 0, 0), (-1, 0, 0), 1e-6)
130  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 1, 0), ( 0, 1, 0), 1e-6)
131  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, -1, 0), ( 0, -1, 0), 1e-6)
132  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1), 1e-6)
133  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1), 1e-6)
134  // clang-format on
135 
136  // scaling
137 
138  sphere.setScale(1.1);
139  // clang-format off
140  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6)
141  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), (-1, 0, 0), (-1.1, 0, 0), 1e-6)
142  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6)
143  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6)
144  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6)
145  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6)
146  // clang-format on
147 
148  // move origin within the sphere
149 
150  // clang-format off
151  CHECK_INTERSECTS_ONCE(sphere, (0.5, 0 , 0 ), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6)
152  CHECK_INTERSECTS_ONCE(sphere, (0.5, 0 , 0 ), (-1, 0, 0), (-1.1, 0, 0), 1e-6)
153  CHECK_INTERSECTS_ONCE(sphere, (0 , 0.5, 0 ), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6)
154  CHECK_INTERSECTS_ONCE(sphere, (0 , 0.5, 0 ), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6)
155  CHECK_INTERSECTS_ONCE(sphere, (0 , 0 , 0.5), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6)
156  CHECK_INTERSECTS_ONCE(sphere, (0 , 0 , 0.5), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6)
157  // clang-format on
158 
159  // move sphere
160 
161  Eigen::Isometry3d pose = Eigen::Translation3d(0.5, 0, 0) * Eigen::Quaterniond::Identity();
162  sphere.setPose(pose);
163  // clang-format off
164  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 1, 0, 0), ( 1.6, 0, 0), 1e-6)
165  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), (-1, 0, 0), (-0.6, 0, 0), 1e-6)
166  // clang-format on
167 
168  pose = Eigen::Translation3d(0, 0.5, 0) * Eigen::Quaterniond::Identity();
169  sphere.setPose(pose);
170  // clang-format off
171  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 1, 0), (0, 1.6, 0), 1e-6)
172  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, -1, 0), (0, -0.6, 0), 1e-6)
173  // clang-format on
174 
175  pose = Eigen::Translation3d(0, 0, 0.5) * Eigen::Quaterniond::Identity();
176  sphere.setPose(pose);
177  // clang-format off
178  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, 1), (0, 0, 1.6), 1e-6)
179  CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, -1), (0, 0, -0.6), 1e-6)
180  // clang-format on
181 
182  // 3D diagonal
183 
184  sphere.setPose(Eigen::Isometry3d::Identity());
185  sphere.setScale(1.0);
186  sphere.setPadding(0.1);
187 
188  const auto sq3 = sqrt(pow(1 + 0.1, 2) / 3);
189  const auto dir3 = Eigen::Vector3d::Ones().normalized();
190  // clang-format off
191  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( dir3), ( sq3, sq3, sq3), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
192  CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0.5, 0.5), ( dir3), ( sq3, sq3, sq3), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
193  CHECK_INTERSECTS_ONCE(sphere, (-0.5, -0.5, -0.5), ( dir3), ( sq3, sq3, sq3), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
194  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), (-dir3), (-sq3, -sq3, -sq3), 1e-4)
195  CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0.5, 0.5), (-dir3), (-sq3, -sq3, -sq3), 1e-4)
196  CHECK_INTERSECTS_ONCE(sphere, (-0.5, -0.5, -0.5), (-dir3), (-sq3, -sq3, -sq3), 1e-4)
197  // clang-format on
198 
199  // 2D diagonal
200 
201  const auto sq2 = sqrt(pow(1 + 0.1, 2) / 2);
202  const auto dir2 = 1 / sqrt(2);
203  // clang-format off
204  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4)
205  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4)
206  CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0.5, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4)
207  CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0.5, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4)
208  CHECK_INTERSECTS_ONCE(sphere, (-0.5, -0.5, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4)
209  CHECK_INTERSECTS_ONCE(sphere, (-0.5, -0.5, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4)
210 
211  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( 0, dir2, dir2), ( 0, sq2, sq2), 1e-4)
212  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( 0, -dir2, -dir2), ( 0, -sq2, -sq2), 1e-4)
213  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0.5, 0.5), ( 0, dir2, dir2), ( 0, sq2, sq2), 1e-4)
214  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0.5, 0.5), ( 0, -dir2, -dir2), ( 0, -sq2, -sq2), 1e-4)
215  CHECK_INTERSECTS_ONCE(sphere, ( 0, -0.5, -0.5), ( 0, dir2, dir2), ( 0, sq2, sq2), 1e-4)
216  CHECK_INTERSECTS_ONCE(sphere, ( 0, -0.5, -0.5), ( 0, -dir2, -dir2), ( 0, -sq2, -sq2), 1e-4)
217 
218  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( dir2, 0, dir2), ( sq2, 0, sq2), 1e-4)
219  CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), (-dir2, 0, -dir2), (-sq2, 0, -sq2), 1e-4)
220  CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0, 0.5), ( dir2, 0, dir2), ( sq2, 0, sq2), 1e-4)
221  CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0, 0.5), (-dir2, 0, -dir2), (-sq2, 0, -sq2), 1e-4)
222  CHECK_INTERSECTS_ONCE(sphere, (-0.5, 0, -0.5), ( dir2, 0, dir2), ( sq2, 0, sq2), 1e-4)
223  CHECK_INTERSECTS_ONCE(sphere, (-0.5, 0, -0.5), (-dir2, 0, -dir2), (-sq2, 0, -sq2), 1e-4)
224  // clang-format on
225 
226  // any random rays that start inside the sphere should have exactly one intersection with it
227  random_numbers::RandomNumberGenerator g(0u);
228  for (size_t i = 0; i < 1000; ++i)
229  {
230  const Eigen::Isometry3d pos = getRandomPose(g);
231  sphere.setPose(pos);
232  sphere.setScale(g.uniformReal(0.5, 100.0));
233  sphere.setPadding(g.uniformReal(-0.1, 100.0));
234 
235  Eigen::Vector3d origin;
236  sphere.samplePointInside(g, 10, origin);
237 
238  // get the scaled sphere
239  bodies::BoundingSphere s;
240  sphere.computeBoundingSphere(s);
241 
242  const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized();
243 
244  EigenSTL::vector_Vector3d intersections;
245  if (sphere.intersectsRay(origin, dir, &intersections, 2))
246  {
247  EXPECT_EQ(1u, intersections.size());
248  if (intersections.size() == 1)
249  {
250  EXPECT_NEAR(s.radius, (s.center - intersections[0]).norm(), 1e-6);
251  // verify the point lies on the ray
252  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[0])), 1e-6);
253  EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir));
254  }
255  }
256  else
257  {
258  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
259  }
260  }
261 }
262 
263 TEST(SphereRayIntersection, OriginOutside)
264 {
265  shapes::Sphere shape(1.0);
266  bodies::Sphere sphere(&shape);
267 
268  // clang-format off
269  CHECK_INTERSECTS_TWICE(sphere, (-2, 0, 0), ( 1, 0, 0), ( 1, 0, 0), (-1, 0, 0), 1e-6)
270  CHECK_INTERSECTS_TWICE(sphere, ( 2, 0, 0), (-1, 0, 0), (-1, 0, 0), ( 1, 0, 0), 1e-6)
271  CHECK_INTERSECTS_TWICE(sphere, ( 0, -2, 0), ( 0, 1, 0), ( 0, 1, 0), ( 0, -1, 0), 1e-6)
272  CHECK_INTERSECTS_TWICE(sphere, ( 0, 2, 0), ( 0, -1, 0), ( 0, -1, 0), ( 0, 1, 0), 1e-6)
273  CHECK_INTERSECTS_TWICE(sphere, ( 0, 0, -2), ( 0, 0, 1), ( 0, 0, 1), ( 0, 0, -1), 1e-6)
274  CHECK_INTERSECTS_TWICE(sphere, ( 0, 0, 2), ( 0, 0, -1), ( 0, 0, -1), ( 0, 0, 1), 1e-6)
275  // clang-format on
276 
277  // test hitting the surface
278  // clang-format off
279  CHECK_INTERSECTS_ONCE(sphere, (-1, -1, 0), ( 0, 1, 0), (-1, 0, 0), 1e-6)
280  CHECK_INTERSECTS_ONCE(sphere, (-1, 1, 0), ( 0, -1, 0), (-1, 0, 0), 1e-6)
281  CHECK_INTERSECTS_ONCE(sphere, ( 1, -1, 0), ( 0, 1, 0), ( 1, 0, 0), 1e-6)
282  CHECK_INTERSECTS_ONCE(sphere, ( 1, 1, 0), ( 0, -1, 0), ( 1, 0, 0), 1e-6)
283  CHECK_INTERSECTS_ONCE(sphere, ( 0, -1, -1), ( 0, 0, 1), ( 0, -1, 0), 1e-6)
284  CHECK_INTERSECTS_ONCE(sphere, ( 0, -1, 1), ( 0, 0, -1), ( 0, -1, 0), 1e-6)
285  CHECK_INTERSECTS_ONCE(sphere, ( 0, 1, -1), ( 0, 0, 1), ( 0, 1, 0), 1e-6)
286  CHECK_INTERSECTS_ONCE(sphere, ( 0, 1, 1), ( 0, 0, -1), ( 0, 1, 0), 1e-6)
287  CHECK_INTERSECTS_ONCE(sphere, (-1, 0, -1), ( 1, 0, 0), ( 0, 0, -1), 1e-6)
288  CHECK_INTERSECTS_ONCE(sphere, ( 1, 0, -1), (-1, 0, 0), ( 0, 0, -1), 1e-6)
289  CHECK_INTERSECTS_ONCE(sphere, (-1, 0, 1), ( 1, 0, 0), ( 0, 0, 1), 1e-6)
290  CHECK_INTERSECTS_ONCE(sphere, ( 1, 0, 1), (-1, 0, 0), ( 0, 0, 1), 1e-6)
291  // clang-format on
292 
293  // test missing the surface
294  // clang-format off
295  CHECK_NO_INTERSECTION(sphere, (-1.1, -1, 0), ( 0, 1, 0))
296  CHECK_NO_INTERSECTION(sphere, (-1.1, 1, 0), ( 0, -1, 0))
297  CHECK_NO_INTERSECTION(sphere, ( 1.1, -1, 0), ( 0, 1, 0))
298  CHECK_NO_INTERSECTION(sphere, ( 1.1, 1, 0), ( 0, -1, 0))
299  CHECK_NO_INTERSECTION(sphere, ( 0, -1.1, -1), ( 0, 0, 1))
300  CHECK_NO_INTERSECTION(sphere, ( 0, -1.1, 1), ( 0, 0, -1))
301  CHECK_NO_INTERSECTION(sphere, ( 0, 1.1, -1), ( 0, 0, 1))
302  CHECK_NO_INTERSECTION(sphere, ( 0, 1.1, 1), ( 0, 0, -1))
303  CHECK_NO_INTERSECTION(sphere, ( -1, 0, -1.1), ( 1, 0, 0))
304  CHECK_NO_INTERSECTION(sphere, ( 1, 0, -1.1), (-1, 0, 0))
305  CHECK_NO_INTERSECTION(sphere, ( -1, 0, 1.1), ( 1, 0, 0))
306  CHECK_NO_INTERSECTION(sphere, ( 1, 0, 1.1), (-1, 0, 0))
307  // clang-format on
308 
309  // generate some random rays outside the sphere and test them
310  random_numbers::RandomNumberGenerator g(0u);
311  for (size_t i = 0; i < 1000; ++i)
312  {
313  const Eigen::Isometry3d pos = getRandomPose(g);
314  sphere.setPose(pos);
315  sphere.setScale(g.uniformReal(0.5, 100.0));
316  sphere.setPadding(g.uniformReal(-0.1, 100.0));
317 
318  // choose a random direction
319  const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized();
320 
321  // get the scaled dimensions of the sphere
322  bodies::BoundingSphere s;
323  sphere.computeBoundingSphere(s);
324 
325  // create origin outside the sphere in the opposite direction than the chosen one
326  const Eigen::Vector3d origin = s.center + -dir * 1.5 * s.radius;
327 
328  // a ray constructed in this way should intersect twice (through the sphere center)
329  EigenSTL::vector_Vector3d intersections;
330  if (sphere.intersectsRay(origin, dir, &intersections, 2))
331  {
332  EXPECT_EQ(2u, intersections.size());
333  if (intersections.size() == 2)
334  {
335  EXPECT_NEAR(s.radius, (s.center - intersections[0]).norm(), 1e-4);
336  EXPECT_NEAR(s.radius, (s.center - intersections[1]).norm(), 1e-4);
337 
338  // verify the point lies on the ray
339  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[0])), 1e-6);
340  EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir));
341  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[1])), 1e-6);
342  EXPECT_LT(0.0, (intersections[1] - origin).normalized().dot(dir));
343  }
344  }
345  else
346  {
347  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
348  }
349 
350  // check that the opposite ray misses
351  CHECK_NO_INTERSECTION(sphere, (origin), (-dir)) // NOLINT(performance-unnecessary-copy-initialization)
352 
353  // shift the ray a bit sideways
354 
355  // choose a perpendicular direction
356  Eigen::Vector3d perpDir = dir.cross(Eigen::Vector3d({ dir.z(), dir.z(), -dir.x() - dir.y() }));
357  if (perpDir.norm() < 1e-6)
358  perpDir = dir.cross(Eigen::Vector3d({ -dir.y() - dir.z(), dir.x(), dir.x() }));
359  perpDir.normalize();
360 
361  // now move origin "sideways" but still only so much that the ray will hit the sphere
362  const Eigen::Vector3d origin2 = origin + g.uniformReal(1e-6, s.radius - 1e-4) * perpDir;
363 
364  intersections.clear();
365  if (sphere.intersectsRay(origin2, dir, &intersections, 2))
366  {
367  EXPECT_EQ(2u, intersections.size());
368  if (intersections.size() == 2)
369  {
370  EXPECT_NEAR(s.radius, (s.center - intersections[0]).norm(), 1e-4);
371  EXPECT_NEAR(s.radius, (s.center - intersections[1]).norm(), 1e-4);
372 
373  // verify the point lies on the ray
374  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin2, dir).distance(intersections[0])), 1e-6);
375  EXPECT_LT(0.0, (intersections[0] - origin2).normalized().dot(dir));
376  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin2, dir).distance(intersections[1])), 1e-6);
377  EXPECT_LT(0.0, (intersections[1] - origin2).normalized().dot(dir));
378  }
379  }
380  else
381  {
382  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
383  }
384  }
385 }
386 
387 TEST(SphereRayIntersection, SimpleRay1)
388 {
389  shapes::Sphere shape(1.0);
390  bodies::Sphere sphere(&shape);
391  sphere.setScale(1.05);
392 
393  CHECK_INTERSECTS_TWICE(sphere, (5, 0, 0), (-1, 0, 0), (1.05, 0, 0), (-1.05, 0, 0), 1e-6)
394 }
395 
396 TEST(SphereRayIntersection, SimpleRay2)
397 {
398  shapes::Sphere shape(1.0);
399  bodies::Sphere sphere(&shape);
400  sphere.setScale(1.05);
401 
402  CHECK_NO_INTERSECTION(sphere, (5, 0, 0), (1, 0, 0))
403 }
404 
405 TEST(CylinderRayIntersection, OriginInside)
406 {
407  shapes::Cylinder shape(1.0, 2.0);
408  bodies::Cylinder cylinder(&shape);
409 
410  // clang-format off
411  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 1, 0, 0), ( 1, 0, 0), 1e-6)
412  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), (-1, 0, 0), (-1, 0, 0), 1e-6)
413  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 1, 0), ( 0, 1, 0), 1e-6)
414  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, -1, 0), ( 0, -1, 0), 1e-6)
415  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1), 1e-6)
416  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1), 1e-6)
417  // clang-format on
418 
419  // scaling
420 
421  cylinder.setScale(1.1);
422  // clang-format off
423  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6)
424  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), (-1, 0, 0), (-1.1, 0, 0), 1e-6)
425  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6)
426  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6)
427  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6)
428  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6)
429  // clang-format on
430 
431  // move origin within the cylinder
432 
433  // clang-format off
434  CHECK_INTERSECTS_ONCE(cylinder, (0.5, 0 , 0 ), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6)
435  CHECK_INTERSECTS_ONCE(cylinder, (0.5, 0 , 0 ), (-1, 0, 0), (-1.1, 0, 0), 1e-6)
436  CHECK_INTERSECTS_ONCE(cylinder, (0 , 0.5, 0 ), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6)
437  CHECK_INTERSECTS_ONCE(cylinder, (0 , 0.5, 0 ), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6)
438  CHECK_INTERSECTS_ONCE(cylinder, (0 , 0 , 0.5), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6)
439  CHECK_INTERSECTS_ONCE(cylinder, (0 , 0 , 0.5), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6)
440  // clang-format on
441 
442  // move cylinder
443 
444  Eigen::Isometry3d pose = Eigen::Translation3d(0.5, 0, 0) * Eigen::Quaterniond::Identity();
445  cylinder.setPose(pose);
446  // clang-format off
447  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 1, 0, 0), ( 1.6, 0, 0), 1e-6)
448  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), (-1, 0, 0), (-0.6, 0, 0), 1e-6)
449  // clang-format on
450 
451  pose = Eigen::Translation3d(0, 0.5, 0) * Eigen::Quaterniond::Identity();
452  cylinder.setPose(pose);
453  // clang-format off
454  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 1, 0), (0, 1.6, 0), 1e-6)
455  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, -1, 0), (0, -0.6, 0), 1e-6)
456  // clang-format on
457 
458  pose = Eigen::Translation3d(0, 0, 0.5) * Eigen::Quaterniond::Identity();
459  cylinder.setPose(pose);
460  // clang-format off
461  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, 1), (0, 0, 1.6), 1e-6)
462  CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, -1), (0, 0, -0.6), 1e-6)
463  // clang-format on
464 
465  // 3D diagonal
466 
467  cylinder.setPose(Eigen::Isometry3d::Identity());
468  cylinder.setScale(1.0);
469  cylinder.setPadding(0.1);
470 
471  // diagonal distance to the base boundary
472  const auto sq2 = sqrt(pow(1 + 0.1, 2) / 2);
473  // direction towards the very "corner" of the cylinder
474  const auto dir3 = Eigen::Vector3d({ sq2, sq2, 1.1 }).normalized();
475  // clang-format off
476  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( dir3), ( sq2, sq2, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
477  CHECK_INTERSECTS_ONCE(cylinder, ( dir3 / 2), ( dir3), ( sq2, sq2, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
478  CHECK_INTERSECTS_ONCE(cylinder, ( -dir3 / 2), ( dir3), ( sq2, sq2, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
479  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), (-dir3), (-sq2, -sq2, -1.1), 1e-4)
480  CHECK_INTERSECTS_ONCE(cylinder, ( dir3 / 2), (-dir3), (-sq2, -sq2, -1.1), 1e-4)
481  CHECK_INTERSECTS_ONCE(cylinder, ( -dir3 / 2), (-dir3), (-sq2, -sq2, -1.1), 1e-4)
482  // clang-format on
483 
484  // 2D diagonal
485 
486  // coordinate of the diagonal direction in the base
487  const auto dir2 = 1 / sqrt(2);
488  // clang-format off
489  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4)
490  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4)
491  CHECK_INTERSECTS_ONCE(cylinder, ( 0.5, 0.5, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4)
492  CHECK_INTERSECTS_ONCE(cylinder, ( 0.5, 0.5, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4)
493  CHECK_INTERSECTS_ONCE(cylinder, (-0.5, -0.5, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4)
494  CHECK_INTERSECTS_ONCE(cylinder, (-0.5, -0.5, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4)
495 
496  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4)
497  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4)
498  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0.5, 0.5), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4)
499  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0.5, 0.5), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4)
500  CHECK_INTERSECTS_ONCE(cylinder, ( 0, -0.5, -0.5), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4)
501  CHECK_INTERSECTS_ONCE(cylinder, ( 0, -0.5, -0.5), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4)
502 
503  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4)
504  CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4)
505  CHECK_INTERSECTS_ONCE(cylinder, ( 0.5, 0, 0.5), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4)
506  CHECK_INTERSECTS_ONCE(cylinder, ( 0.5, 0, 0.5), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4)
507  CHECK_INTERSECTS_ONCE(cylinder, (-0.5, 0, -0.5), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4)
508  CHECK_INTERSECTS_ONCE(cylinder, (-0.5, 0, -0.5), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4)
509  // clang-format on
510 
511  // any random rays that start inside the cylinder should have exactly one intersection with it
512  random_numbers::RandomNumberGenerator g(0u);
513  for (size_t i = 0; i < 1000; ++i)
514  {
515  const Eigen::Isometry3d pos = getRandomPose(g);
516  cylinder.setPose(pos);
517  cylinder.setScale(g.uniformReal(0.5, 100.0));
518  cylinder.setPadding(g.uniformReal(-0.1, 100.0));
519 
520  Eigen::Vector3d origin;
521  cylinder.samplePointInside(g, 10, origin);
522 
523  // get the bounding sphere of the scaled cylinder
524  bodies::BoundingSphere s;
525  cylinder.computeBoundingSphere(s);
526 
527  const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized();
528 
529  EigenSTL::vector_Vector3d intersections;
530  if (cylinder.intersectsRay(origin, dir, &intersections, 2))
531  {
532  EXPECT_EQ(1u, intersections.size());
533  if (intersections.size() == 1)
534  {
535  EXPECT_GE(s.radius, (s.center - intersections[0]).norm());
536  // verify the point lies on the ray
537  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[0])), 1e-6);
538  EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir));
539  }
540  }
541  else
542  {
543  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
544  }
545  }
546 }
547 
548 TEST(CylinderRayIntersection, OriginOutside)
549 {
550  shapes::Cylinder shape(1.0, 2.0);
551  bodies::Cylinder cylinder(&shape);
552  cylinder.setScale(1.5);
553  cylinder.setPadding(0.5);
554 
555  // clang-format off
556  CHECK_INTERSECTS_TWICE(cylinder, (-4, 0, 0), ( 1, 0, 0), ( 2, 0, 0), (-2, 0, 0), 1e-6)
557  CHECK_INTERSECTS_TWICE(cylinder, ( 4, 0, 0), (-1, 0, 0), (-2, 0, 0), ( 2, 0, 0), 1e-6)
558  CHECK_INTERSECTS_TWICE(cylinder, ( 0, -4, 0), ( 0, 1, 0), ( 0, 2, 0), ( 0, -2, 0), 1e-6)
559  CHECK_INTERSECTS_TWICE(cylinder, ( 0, 4, 0), ( 0, -1, 0), ( 0, -2, 0), ( 0, 2, 0), 1e-6)
560  CHECK_INTERSECTS_TWICE(cylinder, ( 0, 0, -4), ( 0, 0, 1), ( 0, 0, 2), ( 0, 0, -2), 1e-6)
561  CHECK_INTERSECTS_TWICE(cylinder, ( 0, 0, 4), ( 0, 0, -1), ( 0, 0, -2), ( 0, 0, 2), 1e-6)
562  // clang-format on
563 
564  // test hitting the surface
565  // clang-format off
566  CHECK_INTERSECTS_ONCE(cylinder, (-2, -2, 0), ( 0, 1, 0), (-2, 0, 0), 1e-6)
567  CHECK_INTERSECTS_ONCE(cylinder, (-2, 2, 0), ( 0, -1, 0), (-2, 0, 0), 1e-6)
568  CHECK_INTERSECTS_ONCE(cylinder, ( 2, -2, 0), ( 0, 1, 0), ( 2, 0, 0), 1e-6)
569  CHECK_INTERSECTS_ONCE(cylinder, ( 2, 2, 0), ( 0, -1, 0), ( 2, 0, 0), 1e-6)
570  CHECK_INTERSECTS_ONCE(cylinder, (-2, -2, 0), ( 1, 0, 0), ( 0, -2, 0), 1e-6)
571  CHECK_INTERSECTS_ONCE(cylinder, ( 2, -2, 0), (-1, 0, 0), ( 0, -2, 0), 1e-6)
572  CHECK_INTERSECTS_ONCE(cylinder, (-2, 2, 0), ( 1, 0, 0), ( 0, 2, 0), 1e-6)
573  CHECK_INTERSECTS_ONCE(cylinder, ( 2, 2, 0), (-1, 0, 0), ( 0, 2, 0), 1e-6)
574  CHECK_INTERSECTS_TWICE(cylinder, ( 0, -2, -3), ( 0, 0, 1), ( 0, -2, -2), ( 0, -2, 2), 1e-6)
575  CHECK_INTERSECTS_TWICE(cylinder, ( 0, -2, 3), ( 0, 0, -1), ( 0, -2, 2), ( 0, -2, -2), 1e-6)
576  CHECK_INTERSECTS_TWICE(cylinder, ( 0, 2, -3), ( 0, 0, 1), ( 0, 2, -2), ( 0, 2, 2), 1e-6)
577  CHECK_INTERSECTS_TWICE(cylinder, ( 0, 2, 3), ( 0, 0, -1), ( 0, 2, 2), ( 0, 2, -2), 1e-6)
578  CHECK_INTERSECTS_TWICE(cylinder, (-2, 0, -3), ( 0, 0, 1), (-2, 0, -2), (-2, 0, 2), 1e-6)
579  CHECK_INTERSECTS_TWICE(cylinder, (-2, 0, 3), ( 0, 0, -1), (-2, 0, 2), (-2, 0, -2), 1e-6)
580  CHECK_INTERSECTS_TWICE(cylinder, ( 2, 0, -3), ( 0, 0, 1), ( 2, 0, -2), ( 2, 0, 2), 1e-6)
581  CHECK_INTERSECTS_TWICE(cylinder, ( 2, 0, 3), ( 0, 0, -1), ( 2, 0, 2), ( 2, 0, -2), 1e-6)
582  // clang-format on
583 
584  // test missing the surface
585  // clang-format off
586  CHECK_NO_INTERSECTION(cylinder, (-2.1, -1, 0), ( 0, 1, 0))
587  CHECK_NO_INTERSECTION(cylinder, (-2.1, 1, 0), ( 0, -1, 0))
588  CHECK_NO_INTERSECTION(cylinder, ( 2.1, -1, 0), ( 0, 1, 0))
589  CHECK_NO_INTERSECTION(cylinder, ( 2.1, 1, 0), ( 0, -1, 0))
590  CHECK_NO_INTERSECTION(cylinder, ( 0, -2.1, -2), ( 0, 0, 1))
591  CHECK_NO_INTERSECTION(cylinder, ( 0, -2.1, 2), ( 0, 0, -1))
592  CHECK_NO_INTERSECTION(cylinder, ( 0, 2.1, -2), ( 0, 0, 1))
593  CHECK_NO_INTERSECTION(cylinder, ( 0, 2.1, 2), ( 0, 0, -1))
594  CHECK_NO_INTERSECTION(cylinder, ( -2, 0, -2.1), ( 1, 0, 0))
595  CHECK_NO_INTERSECTION(cylinder, ( 2, 0, -2.1), (-1, 0, 0))
596  CHECK_NO_INTERSECTION(cylinder, ( -2, 0, 2.1), ( 1, 0, 0))
597  CHECK_NO_INTERSECTION(cylinder, ( 2, 0, 2.1), (-1, 0, 0))
598  // clang-format on
599 
600  // generate some random rays outside the cylinder and test them
601  random_numbers::RandomNumberGenerator g(0u);
602  for (size_t i = 0; i < 1000; ++i)
603  {
604  const Eigen::Isometry3d pos = getRandomPose(g);
605  cylinder.setPose(pos);
606  cylinder.setScale(g.uniformReal(0.5, 100.0));
607  cylinder.setPadding(g.uniformReal(-0.1, 100.0));
608 
609  // choose a random direction
610  const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized();
611 
612  // get the bounding sphere of the cylinder
613  bodies::BoundingSphere s;
614  cylinder.computeBoundingSphere(s);
615 
616  // create origin outside the cylinder in the opposite direction than the chosen one
617  const Eigen::Vector3d origin = s.center + -dir * 1.5 * s.radius;
618 
619  // a ray constructed in this way should intersect twice (through the cylinder center)
620  EigenSTL::vector_Vector3d intersections;
621  if (cylinder.intersectsRay(origin, dir, &intersections, 2))
622  {
623  EXPECT_EQ(2u, intersections.size());
624  if (intersections.size() == 2)
625  {
626  EXPECT_GE(s.radius, (s.center - intersections[0]).norm());
627  EXPECT_GE(s.radius, (s.center - intersections[1]).norm());
628 
629  // verify the point lies on the ray
630  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[0])), 1e-6);
631  EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir));
632  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[1])), 1e-6);
633  EXPECT_LT(0.0, (intersections[1] - origin).normalized().dot(dir));
634  }
635  }
636  else
637  {
638  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
639  }
640 
641  // check that the opposite ray misses
642  CHECK_NO_INTERSECTION(cylinder, (origin), (-dir)) // NOLINT(performance-unnecessary-copy-initialization)
643 
644  // shift the ray a bit sideways
645 
646  // choose a perpendicular direction
647  Eigen::Vector3d perpDir = dir.cross(Eigen::Vector3d({ dir.z(), dir.z(), -dir.x() - dir.y() }));
648  if (perpDir.norm() < 1e-6)
649  perpDir = dir.cross(Eigen::Vector3d({ -dir.y() - dir.z(), dir.x(), dir.x() }));
650  perpDir.normalize();
651 
652  // get the scaled cylinder
653  bodies::BoundingCylinder c;
654  cylinder.computeBoundingCylinder(c);
655 
656  // now move origin "sideways" but still only so much that the ray will hit the cylinder
657  auto minRadius = (std::min)(c.radius, c.length);
658  const Eigen::Vector3d origin2 = origin + g.uniformReal(1e-6, minRadius - 1e-4) * perpDir;
659 
660  intersections.clear();
661  if (cylinder.intersectsRay(origin2, dir, &intersections, 2))
662  {
663  EXPECT_EQ(2u, intersections.size());
664  if (intersections.size() == 2)
665  {
666  EXPECT_GT(s.radius, (s.center - intersections[0]).norm());
667  EXPECT_GT(s.radius, (s.center - intersections[1]).norm());
668  EXPECT_LT(minRadius, (s.center - intersections[0]).norm());
669  EXPECT_LT(minRadius, (s.center - intersections[1]).norm());
670 
671  // verify the point lies on the ray
672  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin2, dir).distance(intersections[0])), 1e-6);
673  EXPECT_LT(0.0, (intersections[0] - origin2).normalized().dot(dir));
674  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin2, dir).distance(intersections[1])), 1e-6);
675  EXPECT_LT(0.0, (intersections[1] - origin2).normalized().dot(dir));
676  }
677  }
678  else
679  {
680  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
681  }
682  }
683 }
684 
685 TEST(CylinderRayIntersection, SimpleRay1)
686 {
687  shapes::Cylinder shape(1.0, 2.0);
688  bodies::Cylinder cylinder(&shape);
689  cylinder.setScale(1.05);
690 
691  CHECK_INTERSECTS_TWICE(cylinder, (5, 0, 0), (-1, 0, 0), (1.05, 0, 0), (-1.05, 0, 0), 1e-6)
692 }
693 
694 TEST(CylinderRayIntersection, SimpleRay2)
695 {
696  shapes::Cylinder shape(1.0, 2.0);
697  bodies::Cylinder cylinder(&shape);
698  cylinder.setScale(1.05);
699 
700  CHECK_NO_INTERSECTION(cylinder, (5, 0, 0), (1, 0, 0))
701 }
702 
703 TEST(BoxRayIntersection, SimpleRay1)
704 {
705  shapes::Box shape(1.0, 1.0, 3.0);
706  bodies::Box box(&shape);
707  box.setScale(0.95);
708 
709  CHECK_INTERSECTS_TWICE(box, (10, 0.449, 0), (-1, 0, 0), (0.475, 0.449, 0), (-0.475, 0.449, 0), 1e-4)
710 }
711 
712 TEST(BoxRayIntersection, SimpleRay2)
713 {
714  shapes::Box shape(0.9, 0.01, 1.2);
715  bodies::Box box(&shape);
716 
717  Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
718  pose.translation() = Eigen::Vector3d(0, 0.005, 0.6);
719  box.setPose(pose);
720 
721  const Eigen::Vector3d ray_d(0, -5.195, -0.77);
722 
723  CHECK_INTERSECTS(box, (0, 5, 1.6), (ray_d.normalized()), 2)
724 }
725 
726 TEST(BoxRayIntersection, SimpleRay3)
727 {
728  shapes::Box shape(0.02, 0.4, 1.2);
729  bodies::Box box(&shape);
730 
731  Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
732  pose.translation() = Eigen::Vector3d(0.45, -0.195, 0.6);
733  box.setPose(pose);
734 
735  const Eigen::Vector3d ray_d(0, 1.8, -0.669);
736 
737  CHECK_NO_INTERSECTION(box, (0, -2, 1.11), (ray_d.normalized()))
738 }
739 
740 TEST(BoxRayIntersection, Regression109)
741 {
742  shapes::Box shape(1.0, 1.0, 1.0);
743  bodies::Box box(&shape);
744 
745  // rotates the box so that the original (0.5, 0.5, 0.5) corner gets elsewhere and is no longer the
746  // maximal corner
747  const auto rot = Eigen::AngleAxisd(M_PI * 2 / 3, Eigen::Vector3d(1.0, -1.0, 1.0).normalized());
748  box.setPose(Eigen::Isometry3d(rot));
749 
750  CHECK_INTERSECTS_TWICE(box, (-2, 0, 0), (1, 0, 0), (0.5, 0, 0), (-0.5, 0, 0), 1e-6)
751 }
752 
753 TEST(BoxRayIntersection, OriginInside)
754 {
755  shapes::Box shape(2.0, 2.0, 2.0);
756  bodies::Box box(&shape);
757 
758  // clang-format off
759  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 1, 0, 0), ( 1, 0, 0), 1e-6)
760  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), (-1, 0, 0), (-1, 0, 0), 1e-6)
761  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 1, 0), ( 0, 1, 0), 1e-6)
762  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, -1, 0), ( 0, -1, 0), 1e-6)
763  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1), 1e-6)
764  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1), 1e-6)
765  // clang-format on
766 
767  // scaling
768 
769  box.setScale(1.1);
770  // clang-format off
771  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6)
772  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), (-1, 0, 0), (-1.1, 0, 0), 1e-6)
773  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6)
774  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6)
775  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6)
776  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6)
777  // clang-format on
778 
779  // move origin within the box
780 
781  // clang-format off
782  CHECK_INTERSECTS_ONCE(box, (0.5, 0 , 0 ), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6)
783  CHECK_INTERSECTS_ONCE(box, (0.5, 0 , 0 ), (-1, 0, 0), (-1.1, 0, 0), 1e-6)
784  CHECK_INTERSECTS_ONCE(box, (0 , 0.5, 0 ), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6)
785  CHECK_INTERSECTS_ONCE(box, (0 , 0.5, 0 ), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6)
786  CHECK_INTERSECTS_ONCE(box, (0 , 0 , 0.5), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6)
787  CHECK_INTERSECTS_ONCE(box, (0 , 0 , 0.5), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6)
788  // clang-format on
789 
790  // move box
791 
792  Eigen::Isometry3d pose(Eigen::Translation3d(0.5, 0, 0));
793  box.setPose(pose);
794  // clang-format off
795  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 1, 0, 0), ( 1.6, 0, 0), 1e-6)
796  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), (-1, 0, 0), (-0.6, 0, 0), 1e-6)
797  // clang-format on
798 
799  pose = Eigen::Translation3d(0, 0.5, 0);
800  box.setPose(pose);
801  // clang-format off
802  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 1, 0), (0, 1.6, 0), 1e-6)
803  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, -1, 0), (0, -0.6, 0), 1e-6)
804  // clang-format on
805 
806  pose = Eigen::Translation3d(0, 0, 0.5);
807  box.setPose(pose);
808  // clang-format off
809  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, 1), (0, 0, 1.6), 1e-6)
810  CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, -1), (0, 0, -0.6), 1e-6)
811  // clang-format on
812 
813  // 3D diagonal
814 
815  box.setPose(Eigen::Isometry3d::Identity());
816  box.setScale(1.0);
817  box.setPadding(0.1);
818 
819  const auto dir3 = Eigen::Vector3d::Ones().normalized();
820  // clang-format off
821  CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( dir3), ( 1.1, 1.1, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
822  CHECK_INTERSECTS_ONCE(box, ( 0.5, 0.5, 0.5), ( dir3), ( 1.1, 1.1, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
823  CHECK_INTERSECTS_ONCE(box, (-0.5, -0.5, -0.5), ( dir3), ( 1.1, 1.1, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
824  CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), (-dir3), (-1.1, -1.1, -1.1), 1e-4)
825  CHECK_INTERSECTS_ONCE(box, ( 0.5, 0.5, 0.5), (-dir3), (-1.1, -1.1, -1.1), 1e-4)
826  CHECK_INTERSECTS_ONCE(box, (-0.5, -0.5, -0.5), (-dir3), (-1.1, -1.1, -1.1), 1e-4)
827  // clang-format on
828 
829  // 2D diagonal
830 
831  const auto dir2 = 1 / sqrt(2);
832  // clang-format off
833  CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( dir2, dir2, 0), ( 1.1, 1.1, 0), 1e-4)
834  CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), (-dir2, -dir2, 0), (-1.1, -1.1, 0), 1e-4)
835  CHECK_INTERSECTS_ONCE(box, ( 0.5, 0.5, 0), ( dir2, dir2, 0), ( 1.1, 1.1, 0), 1e-4)
836  CHECK_INTERSECTS_ONCE(box, ( 0.5, 0.5, 0), (-dir2, -dir2, 0), (-1.1, -1.1, 0), 1e-4)
837  CHECK_INTERSECTS_ONCE(box, (-0.5, -0.5, 0), ( dir2, dir2, 0), ( 1.1, 1.1, 0), 1e-4)
838  CHECK_INTERSECTS_ONCE(box, (-0.5, -0.5, 0), (-dir2, -dir2, 0), (-1.1, -1.1, 0), 1e-4)
839 
840  CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4)
841  CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4)
842  CHECK_INTERSECTS_ONCE(box, ( 0, 0.5, 0.5), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4)
843  CHECK_INTERSECTS_ONCE(box, ( 0, 0.5, 0.5), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4)
844  CHECK_INTERSECTS_ONCE(box, ( 0, -0.5, -0.5), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4)
845  CHECK_INTERSECTS_ONCE(box, ( 0, -0.5, -0.5), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4)
846 
847  CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4)
848  CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4)
849  CHECK_INTERSECTS_ONCE(box, ( 0.5, 0, 0.5), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4)
850  CHECK_INTERSECTS_ONCE(box, ( 0.5, 0, 0.5), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4)
851  CHECK_INTERSECTS_ONCE(box, (-0.5, 0, -0.5), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4)
852  CHECK_INTERSECTS_ONCE(box, (-0.5, 0, -0.5), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4)
853  // clang-format on
854 
855  // any random rays that start inside the box should have exactly one intersection with it
856  random_numbers::RandomNumberGenerator g(0u);
857  for (size_t i = 0; i < 1000; ++i)
858  {
859  const Eigen::Isometry3d pos = getRandomPose(g);
860  box.setPose(pos);
861  box.setScale(g.uniformReal(0.5, 100.0));
862  box.setPadding(g.uniformReal(-0.1, 100.0));
863 
864  // get the scaled dimensions of the box
865  Eigen::Vector3d sizes = { box.getDimensions()[0], box.getDimensions()[1], box.getDimensions()[2] };
866  sizes *= box.getScale();
867  sizes += 2 * box.getPadding() * Eigen::Vector3d::Ones();
868 
869  // radii of the inscribed and bounding spheres
870  const auto minRadius = sizes.minCoeff() / 2;
871  const auto maxRadius = (sizes / 2).norm();
872 
873  const Eigen::Vector3d boxCenter = box.getPose().translation();
874 
875  Eigen::Vector3d origin;
876  box.samplePointInside(g, 10, origin);
877 
878  const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized();
879 
880  EigenSTL::vector_Vector3d intersections;
881  if (box.intersectsRay(origin, dir, &intersections, 2))
882  {
883  EXPECT_EQ(1u, intersections.size());
884  if (intersections.size() == 1)
885  {
886  // just approximate verification that the point is between inscribed and bounding sphere
887  EXPECT_GE(maxRadius, (boxCenter - intersections[0]).norm());
888  EXPECT_LE(minRadius, (boxCenter - intersections[0]).norm());
889  // verify the point lies on the ray
890  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[0])), 1e-6);
891  EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir));
892  }
893  }
894  else
895  {
896  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
897  }
898  }
899 }
900 
901 TEST(BoxRayIntersection, OriginOutsideIntersects)
902 {
903  shapes::Box shape(2.0, 2.0, 2.0);
904  bodies::Box box(&shape);
905 
906  // clang-format off
907  CHECK_INTERSECTS_TWICE(box, (-2, 0, 0), ( 1, 0, 0), ( 1, 0, 0), (-1, 0, 0), 1e-6)
908  CHECK_INTERSECTS_TWICE(box, ( 2, 0, 0), (-1, 0, 0), (-1, 0, 0), ( 1, 0, 0), 1e-6)
909  CHECK_INTERSECTS_TWICE(box, (0, -2, 0), ( 0, 1, 0), ( 0, 1, 0), ( 0, -1, 0), 1e-6)
910  CHECK_INTERSECTS_TWICE(box, (0, 2, 0), ( 0, -1, 0), ( 0, -1, 0), ( 0, 1, 0), 1e-6)
911  CHECK_INTERSECTS_TWICE(box, (0, 0, -2), ( 0, 0, 1), ( 0, 0, 1), ( 0, 0, -1), 1e-6)
912  CHECK_INTERSECTS_TWICE(box, (0, 0, 2), ( 0, 0, -1), ( 0, 0, -1), ( 0, 0, 1), 1e-6)
913  // clang-format on
914 
915  // test hitting the surface
916  // clang-format off
917  CHECK_INTERSECTS_TWICE(box, (-1, -2, 0), ( 0, 1, 0), (-1, -1, 0), (-1, 1, 0), 1e-6)
918  CHECK_INTERSECTS_TWICE(box, (-1, 2, 0), ( 0, -1, 0), (-1, 1, 0), (-1, -1, 0), 1e-6)
919  CHECK_INTERSECTS_TWICE(box, ( 1, -2, 0), ( 0, 1, 0), ( 1, -1, 0), ( 1, 1, 0), 1e-6)
920  CHECK_INTERSECTS_TWICE(box, ( 1, 2, 0), ( 0, -1, 0), ( 1, 1, 0), ( 1, -1, 0), 1e-6)
921  CHECK_INTERSECTS_TWICE(box, ( 0, -1, -2), ( 0, 0, 1), ( 0, -1, -1), ( 0, -1, 1), 1e-6)
922  CHECK_INTERSECTS_TWICE(box, ( 0, -1, 2), ( 0, 0, -1), ( 0, -1, 1), ( 0, -1, -1), 1e-6)
923  CHECK_INTERSECTS_TWICE(box, ( 0, 1, -2), ( 0, 0, 1), ( 0, 1, -1), ( 0, 1, 1), 1e-6)
924  CHECK_INTERSECTS_TWICE(box, ( 0, 1, 2), ( 0, 0, -1), ( 0, 1, 1), ( 0, 1, -1), 1e-6)
925  CHECK_INTERSECTS_TWICE(box, (-2, 0, -1), ( 1, 0, 0), (-1, 0, -1), ( 1, 0, -1), 1e-6)
926  CHECK_INTERSECTS_TWICE(box, ( 2, 0, -1), (-1, 0, 0), ( 1, 0, -1), (-1, 0, -1), 1e-6)
927  CHECK_INTERSECTS_TWICE(box, (-2, 0, 1), ( 1, 0, 0), (-1, 0, 1), ( 1, 0, 1), 1e-6)
928  CHECK_INTERSECTS_TWICE(box, ( 2, 0, 1), (-1, 0, 0), ( 1, 0, 1), (-1, 0, 1), 1e-6)
929  // clang-format on
930 
931  // test missing the surface
932  // clang-format off
933  CHECK_NO_INTERSECTION(box, (-1.1, -2, 0), ( 0, 1, 0))
934  CHECK_NO_INTERSECTION(box, (-1.1, 2, 0), ( 0, -1, 0))
935  CHECK_NO_INTERSECTION(box, ( 1.1, -2, 0), ( 0, 1, 0))
936  CHECK_NO_INTERSECTION(box, ( 1.1, 2, 0), ( 0, -1, 0))
937  CHECK_NO_INTERSECTION(box, ( 0, -1.1, -2), ( 0, 0, 1))
938  CHECK_NO_INTERSECTION(box, ( 0, -1.1, 2), ( 0, 0, -1))
939  CHECK_NO_INTERSECTION(box, ( 0, 1.1, -2), ( 0, 0, 1))
940  CHECK_NO_INTERSECTION(box, ( 0, 1.1, 2), ( 0, 0, -1))
941  CHECK_NO_INTERSECTION(box, ( -2, 0, -1.1), ( 1, 0, 0))
942  CHECK_NO_INTERSECTION(box, ( 2, 0, -1.1), (-1, 0, 0))
943  CHECK_NO_INTERSECTION(box, ( -2, 0, 1.1), ( 1, 0, 0))
944  CHECK_NO_INTERSECTION(box, ( 2, 0, 1.1), (-1, 0, 0))
945  // clang-format on
946 
947  // generate some random rays outside the box and test them
948  random_numbers::RandomNumberGenerator g(0u);
949  for (size_t i = 0; i < 1000; ++i)
950  {
951  const Eigen::Isometry3d pos = getRandomPose(g);
952  box.setPose(pos);
953  box.setScale(g.uniformReal(0.5, 100.0));
954  box.setPadding(g.uniformReal(-0.1, 100.0));
955 
956  // choose a random direction
957  const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized();
958 
959  // get the scaled dimensions of the box
960  Eigen::Vector3d sizes = { box.getDimensions()[0], box.getDimensions()[1], box.getDimensions()[2] };
961  sizes *= box.getScale();
962  sizes += 2 * box.getPadding() * Eigen::Vector3d::Ones();
963 
964  // radii of the inscribed and bounding spheres
965  const auto minRadius = sizes.minCoeff() / 2;
966  const auto maxRadius = (sizes / 2).norm();
967 
968  const Eigen::Vector3d boxCenter = box.getPose().translation();
969 
970  // create origin outside the box in the opposite direction than the chosen one
971  const Eigen::Vector3d origin = boxCenter + -dir * 1.5 * maxRadius;
972 
973  // a ray constructed in this way should intersect twice (through the box center)
974  EigenSTL::vector_Vector3d intersections;
975  if (box.intersectsRay(origin, dir, &intersections, 2))
976  {
977  EXPECT_EQ(2u, intersections.size());
978  if (intersections.size() == 2)
979  {
980  // just approximate verification that the point is between inscribed and bounding sphere
981  EXPECT_GE(maxRadius, (boxCenter - intersections[0]).norm());
982  EXPECT_LE(minRadius, (boxCenter - intersections[0]).norm());
983  EXPECT_GE(maxRadius, (boxCenter - intersections[1]).norm());
984  EXPECT_LE(minRadius, (boxCenter - intersections[1]).norm());
985 
986  // verify the point lies on the ray
987  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[0])), 1e-6);
988  EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir));
989  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[1])), 1e-6);
990  EXPECT_LT(0.0, (intersections[1] - origin).normalized().dot(dir));
991  }
992  }
993  else
994  {
995  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
996  }
997 
998  // check that the opposite ray misses
999  CHECK_NO_INTERSECTION(box, (origin), (-dir)) // NOLINT(performance-unnecessary-copy-initialization)
1000 
1001  // shift the ray a bit sideways
1002 
1003  // choose a perpendicular direction
1004  Eigen::Vector3d perpDir = dir.cross(Eigen::Vector3d({ dir.z(), dir.z(), -dir.x() - dir.y() }));
1005  if (perpDir.norm() < 1e-6)
1006  perpDir = dir.cross(Eigen::Vector3d({ -dir.y() - dir.z(), dir.x(), dir.x() }));
1007  perpDir.normalize();
1008 
1009  // now move origin "sideways" but still only so much that the ray will hit the box
1010  const Eigen::Vector3d origin2 = origin + g.uniformReal(1e-6, minRadius - 1e-4) * perpDir;
1011 
1012  intersections.clear();
1013  if (box.intersectsRay(origin2, dir, &intersections, 2))
1014  {
1015  EXPECT_EQ(2u, intersections.size());
1016  if (intersections.size() == 2)
1017  {
1018  // just approximate verification that the point is between inscribed and bounding sphere
1019  EXPECT_GE(maxRadius, (boxCenter - intersections[0]).norm());
1020  EXPECT_LE(minRadius, (boxCenter - intersections[0]).norm());
1021  EXPECT_GE(maxRadius, (boxCenter - intersections[1]).norm());
1022  EXPECT_LE(minRadius, (boxCenter - intersections[1]).norm());
1023 
1024  // verify the point lies on the ray
1025  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin2, dir).distance(intersections[0])), 1e-6);
1026  EXPECT_LT(0.0, (intersections[0] - origin2).normalized().dot(dir));
1027  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin2, dir).distance(intersections[1])), 1e-6);
1028  EXPECT_LT(0.0, (intersections[1] - origin2).normalized().dot(dir));
1029  }
1030  }
1031  else
1032  {
1033  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
1034  }
1035  }
1036 }
1037 
1038 TEST(ConvexMeshRayIntersection, SimpleRay1)
1039 {
1040  shapes::Box box(1.0, 1.0, 3.0);
1041  shapes::Mesh* shape = shapes::createMeshFromShape(&box);
1042  bodies::ConvexMesh mesh(shape);
1043  delete shape;
1044  mesh.setScale(0.95);
1045 
1046  CHECK_INTERSECTS_TWICE(mesh, (10, 0.449, 0), (-1, 0, 0), (0.475, 0.449, 0), (-0.475, 0.449, 0), 1e-4)
1047 }
1048 
1049 TEST(ConvexMeshRayIntersection, SimpleRay2)
1050 {
1051  shapes::Box box(0.9, 0.01, 1.2);
1052  shapes::Mesh* shape = shapes::createMeshFromShape(&box);
1053  bodies::ConvexMesh mesh(shape);
1054  delete shape;
1055 
1056  Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
1057  pose.translation() = Eigen::Vector3d(0, 0.005, 0.6);
1058  mesh.setPose(pose);
1059 
1060  const Eigen::Vector3d ray_d(0, -5.195, -0.77);
1061 
1062  CHECK_INTERSECTS(mesh, (0, 5, 1.6), (ray_d.normalized()), 2)
1063 }
1064 
1065 TEST(ConvexMeshRayIntersection, SimpleRay3)
1066 {
1067  shapes::Box box(0.02, 0.4, 1.2);
1068  shapes::Mesh* shape = shapes::createMeshFromShape(&box);
1069  bodies::ConvexMesh mesh(shape);
1070  delete shape;
1071 
1072  Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
1073  pose.translation() = Eigen::Vector3d(0.45, -0.195, 0.6);
1074  mesh.setPose(pose);
1075 
1076  const Eigen::Vector3d ray_d(0, 1.8, -0.669);
1077 
1078  CHECK_NO_INTERSECTION(mesh, (0, -2, 1.11), (ray_d.normalized()))
1079 }
1080 
1081 TEST(ConvexMeshRayIntersection, OriginInside)
1082 {
1083  shapes::Box box(2.0, 2.0, 2.0);
1084  shapes::Mesh* shape = shapes::createMeshFromShape(&box);
1085  bodies::ConvexMesh mesh(shape);
1086  delete shape;
1087 
1088  // clang-format off
1089  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 1, 0, 0), ( 1, 0, 0), 1e-6)
1090  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), (-1, 0, 0), (-1, 0, 0), 1e-6)
1091  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 1, 0), ( 0, 1, 0), 1e-6)
1092  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, -1, 0), ( 0, -1, 0), 1e-6)
1093  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1), 1e-6)
1094  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1), 1e-6)
1095  // clang-format on
1096 
1097  // scaling
1098 
1099  mesh.setScale(1.1);
1100  // clang-format off
1101  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6)
1102  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), (-1, 0, 0), (-1.1, 0, 0), 1e-6)
1103  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6)
1104  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6)
1105  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6)
1106  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6)
1107  // clang-format on
1108 
1109  // move origin within the mesh
1110 
1111  // clang-format off
1112  CHECK_INTERSECTS_ONCE(mesh, (0.5, 0 , 0 ), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6)
1113  CHECK_INTERSECTS_ONCE(mesh, (0.5, 0 , 0 ), (-1, 0, 0), (-1.1, 0, 0), 1e-6)
1114  CHECK_INTERSECTS_ONCE(mesh, (0 , 0.5, 0 ), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6)
1115  CHECK_INTERSECTS_ONCE(mesh, (0 , 0.5, 0 ), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6)
1116  CHECK_INTERSECTS_ONCE(mesh, (0 , 0 , 0.5), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6)
1117  CHECK_INTERSECTS_ONCE(mesh, (0 , 0 , 0.5), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6)
1118  // clang-format on
1119 
1120  // move mesh
1121 
1122  Eigen::Isometry3d pose(Eigen::Translation3d(0.5, 0, 0));
1123  mesh.setPose(pose);
1124  // clang-format off
1125  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 1, 0, 0), ( 1.6, 0, 0), 1e-6)
1126  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), (-1, 0, 0), (-0.6, 0, 0), 1e-6)
1127  // clang-format on
1128 
1129  pose = Eigen::Translation3d(0, 0.5, 0);
1130  mesh.setPose(pose);
1131  // clang-format off
1132  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 1, 0), (0, 1.6, 0), 1e-6)
1133  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, -1, 0), (0, -0.6, 0), 1e-6)
1134  // clang-format on
1135 
1136  pose = Eigen::Translation3d(0, 0, 0.5);
1137  mesh.setPose(pose);
1138  // clang-format off
1139  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, 1), (0, 0, 1.6), 1e-6)
1140  CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, -1), (0, 0, -0.6), 1e-6)
1141  // clang-format on
1142 
1143  // 3D diagonal
1144 
1145  mesh.setPose(Eigen::Isometry3d::Identity());
1146  mesh.setScale(1.0);
1147  mesh.setPadding(0.1);
1148 
1149  // half-size of the scaled and padded mesh (mesh scaling isn't "linear" in padding)
1150  const double s = 1.0 + 0.1 / sqrt(3);
1151 
1152  const auto dir3 = Eigen::Vector3d::Ones().normalized();
1153  // clang-format off
1154  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( dir3), ( s, s, s), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
1155  CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0.5, 0.5), ( dir3), ( s, s, s), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
1156  CHECK_INTERSECTS_ONCE(mesh, (-0.5, -0.5, -0.5), ( dir3), ( s, s, s), 1e-4) // NOLINT(performance-unnecessary-copy-initialization)
1157  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), (-dir3), (-s, -s, -s), 1e-4)
1158  CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0.5, 0.5), (-dir3), (-s, -s, -s), 1e-4)
1159  CHECK_INTERSECTS_ONCE(mesh, (-0.5, -0.5, -0.5), (-dir3), (-s, -s, -s), 1e-4)
1160  // clang-format on
1161 
1162  // 2D diagonal
1163 
1164  const auto dir2 = 1 / sqrt(2);
1165  // clang-format off
1166  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( dir2, dir2, 0), ( s, s, 0), 1e-4)
1167  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), (-dir2, -dir2, 0), (-s, -s, 0), 1e-4)
1168  CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0.5, 0), ( dir2, dir2, 0), ( s, s, 0), 1e-4)
1169  CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0.5, 0), (-dir2, -dir2, 0), (-s, -s, 0), 1e-4)
1170  CHECK_INTERSECTS_ONCE(mesh, (-0.5, -0.5, 0), ( dir2, dir2, 0), ( s, s, 0), 1e-4)
1171  CHECK_INTERSECTS_ONCE(mesh, (-0.5, -0.5, 0), (-dir2, -dir2, 0), (-s, -s, 0), 1e-4)
1172 
1173  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( 0, dir2, dir2), ( 0, s, s), 1e-4)
1174  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( 0, -dir2, -dir2), ( 0, -s, -s), 1e-4)
1175  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0.5, 0.5), ( 0, dir2, dir2), ( 0, s, s), 1e-4)
1176  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0.5, 0.5), ( 0, -dir2, -dir2), ( 0, -s, -s), 1e-4)
1177  CHECK_INTERSECTS_ONCE(mesh, ( 0, -0.5, -0.5), ( 0, dir2, dir2), ( 0, s, s), 1e-4)
1178  CHECK_INTERSECTS_ONCE(mesh, ( 0, -0.5, -0.5), ( 0, -dir2, -dir2), ( 0, -s, -s), 1e-4)
1179 
1180  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( dir2, 0, dir2), ( s, 0, s), 1e-4)
1181  CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), (-dir2, 0, -dir2), (-s, 0, -s), 1e-4)
1182  CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0, 0.5), ( dir2, 0, dir2), ( s, 0, s), 1e-4)
1183  CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0, 0.5), (-dir2, 0, -dir2), (-s, 0, -s), 1e-4)
1184  CHECK_INTERSECTS_ONCE(mesh, (-0.5, 0, -0.5), ( dir2, 0, dir2), ( s, 0, s), 1e-4)
1185  CHECK_INTERSECTS_ONCE(mesh, (-0.5, 0, -0.5), (-dir2, 0, -dir2), (-s, 0, -s), 1e-4)
1186  // clang-format on
1187 
1188  // any random rays that start inside the mesh should have exactly one intersection with it
1189  random_numbers::RandomNumberGenerator g(0u);
1190  for (size_t i = 0; i < 1000; ++i)
1191  {
1192  const Eigen::Isometry3d pos = getRandomPose(g);
1193  mesh.setPose(pos);
1194  mesh.setScale(g.uniformReal(0.5, 100.0));
1195  mesh.setPadding(g.uniformReal(-0.1, 100.0));
1196 
1197  // get the scaled dimensions of the mesh
1198  Eigen::Vector3d sizes = { box.size[0], box.size[1], box.size[2] };
1199  sizes *= mesh.getScale();
1200  sizes += 2 * mesh.getPadding() / sqrt(3) * Eigen::Vector3d::Ones();
1201 
1202  // radii of the inscribed and bounding spheres
1203  const auto minRadius = sizes.minCoeff() / 2;
1204  const auto maxRadius = (sizes / 2).norm();
1205 
1206  const Eigen::Vector3d meshCenter = mesh.getPose().translation();
1207 
1208  Eigen::Vector3d origin;
1209  mesh.samplePointInside(g, 10000, origin);
1210 
1211  const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized();
1212 
1213  EigenSTL::vector_Vector3d intersections;
1214  if (mesh.intersectsRay(origin, dir, &intersections, 2))
1215  {
1216  EXPECT_EQ(1u, intersections.size());
1217  if (intersections.size() == 1)
1218  {
1219  // just approximate verification that the point is between inscribed and bounding sphere
1220  EXPECT_GE(maxRadius, (meshCenter - intersections[0]).norm());
1221  EXPECT_LE(minRadius, (meshCenter - intersections[0]).norm());
1222  // verify the point lies on the ray
1223  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[0])), 1e-6);
1224  EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir));
1225  }
1226  }
1227  else
1228  {
1229  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
1230  }
1231  }
1232 }
1233 
1234 TEST(ConvexMeshRayIntersection, OriginOutsideIntersects)
1235 {
1236  shapes::Box box(2.0, 2.0, 2.0);
1237  shapes::Mesh* shape = shapes::createMeshFromShape(&box);
1238  bodies::ConvexMesh mesh(shape);
1239  delete shape;
1240 
1241  // clang-format off
1242  CHECK_INTERSECTS_TWICE(mesh, (-2, 0, 0), ( 1, 0, 0), ( 1, 0, 0), (-1, 0, 0), 1e-6)
1243  CHECK_INTERSECTS_TWICE(mesh, ( 2, 0, 0), (-1, 0, 0), (-1, 0, 0), ( 1, 0, 0), 1e-6)
1244  CHECK_INTERSECTS_TWICE(mesh, (0, -2, 0), ( 0, 1, 0), ( 0, 1, 0), ( 0, -1, 0), 1e-6)
1245  CHECK_INTERSECTS_TWICE(mesh, (0, 2, 0), ( 0, -1, 0), ( 0, -1, 0), ( 0, 1, 0), 1e-6)
1246  CHECK_INTERSECTS_TWICE(mesh, (0, 0, -2), ( 0, 0, 1), ( 0, 0, 1), ( 0, 0, -1), 1e-6)
1247  CHECK_INTERSECTS_TWICE(mesh, (0, 0, 2), ( 0, 0, -1), ( 0, 0, -1), ( 0, 0, 1), 1e-6)
1248  // clang-format on
1249 
1250  // test hitting the surface
1251  // clang-format off
1252  CHECK_INTERSECTS_TWICE(mesh, (-1, -2, 0), ( 0, 1, 0), (-1, -1, 0), (-1, 1, 0), 1e-6)
1253  CHECK_INTERSECTS_TWICE(mesh, (-1, 2, 0), ( 0, -1, 0), (-1, 1, 0), (-1, -1, 0), 1e-6)
1254  CHECK_INTERSECTS_TWICE(mesh, ( 1, -2, 0), ( 0, 1, 0), ( 1, -1, 0), ( 1, 1, 0), 1e-6)
1255  CHECK_INTERSECTS_TWICE(mesh, ( 1, 2, 0), ( 0, -1, 0), ( 1, 1, 0), ( 1, -1, 0), 1e-6)
1256  CHECK_INTERSECTS_TWICE(mesh, ( 0, -1, -2), ( 0, 0, 1), ( 0, -1, -1), ( 0, -1, 1), 1e-6)
1257  CHECK_INTERSECTS_TWICE(mesh, ( 0, -1, 2), ( 0, 0, -1), ( 0, -1, 1), ( 0, -1, -1), 1e-6)
1258  CHECK_INTERSECTS_TWICE(mesh, ( 0, 1, -2), ( 0, 0, 1), ( 0, 1, -1), ( 0, 1, 1), 1e-6)
1259  CHECK_INTERSECTS_TWICE(mesh, ( 0, 1, 2), ( 0, 0, -1), ( 0, 1, 1), ( 0, 1, -1), 1e-6)
1260  CHECK_INTERSECTS_TWICE(mesh, (-2, 0, -1), ( 1, 0, 0), (-1, 0, -1), ( 1, 0, -1), 1e-6)
1261  CHECK_INTERSECTS_TWICE(mesh, ( 2, 0, -1), (-1, 0, 0), ( 1, 0, -1), (-1, 0, -1), 1e-6)
1262  CHECK_INTERSECTS_TWICE(mesh, (-2, 0, 1), ( 1, 0, 0), (-1, 0, 1), ( 1, 0, 1), 1e-6)
1263  CHECK_INTERSECTS_TWICE(mesh, ( 2, 0, 1), (-1, 0, 0), ( 1, 0, 1), (-1, 0, 1), 1e-6)
1264  // clang-format on
1265 
1266  // test missing the surface
1267  // clang-format off
1268  CHECK_NO_INTERSECTION(mesh, (-1.1, -2, 0), ( 0, 1, 0))
1269  CHECK_NO_INTERSECTION(mesh, (-1.1, 2, 0), ( 0, -1, 0))
1270  CHECK_NO_INTERSECTION(mesh, ( 1.1, -2, 0), ( 0, 1, 0))
1271  CHECK_NO_INTERSECTION(mesh, ( 1.1, 2, 0), ( 0, -1, 0))
1272  CHECK_NO_INTERSECTION(mesh, ( 0, -1.1, -2), ( 0, 0, 1))
1273  CHECK_NO_INTERSECTION(mesh, ( 0, -1.1, 2), ( 0, 0, -1))
1274  CHECK_NO_INTERSECTION(mesh, ( 0, 1.1, -2), ( 0, 0, 1))
1275  CHECK_NO_INTERSECTION(mesh, ( 0, 1.1, 2), ( 0, 0, -1))
1276  CHECK_NO_INTERSECTION(mesh, ( -2, 0, -1.1), ( 1, 0, 0))
1277  CHECK_NO_INTERSECTION(mesh, ( 2, 0, -1.1), (-1, 0, 0))
1278  CHECK_NO_INTERSECTION(mesh, ( -2, 0, 1.1), ( 1, 0, 0))
1279  CHECK_NO_INTERSECTION(mesh, ( 2, 0, 1.1), (-1, 0, 0))
1280  // clang-format on
1281 
1282  // generate some random rays outside the mesh and test them
1283  random_numbers::RandomNumberGenerator g(0u);
1284  for (size_t i = 0; i < 1000; ++i)
1285  {
1286  const Eigen::Isometry3d pos = getRandomPose(g);
1287  mesh.setPose(pos);
1288  mesh.setScale(g.uniformReal(0.5, 100.0));
1289  mesh.setPadding(g.uniformReal(-0.1, 100.0));
1290 
1291  // choose a random direction
1292  const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized();
1293 
1294  // get the scaled dimensions of the mesh
1295  Eigen::Vector3d sizes = { box.size[0], box.size[1], box.size[2] };
1296  sizes *= mesh.getScale();
1297  sizes += 2 * mesh.getPadding() / sqrt(3) * Eigen::Vector3d::Ones();
1298 
1299  // radii of the inscribed and bounding spheres
1300  const auto minRadius = sizes.minCoeff() / 2;
1301  const auto maxRadius = (sizes / 2).norm();
1302 
1303  const Eigen::Vector3d meshCenter = mesh.getPose().translation();
1304 
1305  // create origin outside the mesh in the opposite direction than the chosen one
1306  const Eigen::Vector3d origin = meshCenter + -dir * 1.5 * maxRadius;
1307 
1308  // a ray constructed in this way should intersect twice (through the mesh center)
1309  EigenSTL::vector_Vector3d intersections;
1310  if (mesh.intersectsRay(origin, dir, &intersections, 2))
1311  {
1312  EXPECT_EQ(2u, intersections.size());
1313  if (intersections.size() == 2)
1314  {
1315  // just approximate verification that the point is between inscribed and bounding sphere
1316  EXPECT_GE(maxRadius, (meshCenter - intersections[0]).norm());
1317  EXPECT_LE(minRadius, (meshCenter - intersections[0]).norm());
1318  EXPECT_GE(maxRadius, (meshCenter - intersections[1]).norm());
1319  EXPECT_LE(minRadius, (meshCenter - intersections[1]).norm());
1320 
1321  // verify the point lies on the ray
1322  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[0])), 1e-6);
1323  EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir));
1324  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin, dir).distance(intersections[1])), 1e-6);
1325  EXPECT_LT(0.0, (intersections[1] - origin).normalized().dot(dir));
1326  }
1327  }
1328  else
1329  {
1330  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
1331  }
1332 
1333  // check that the opposite ray misses
1334  CHECK_NO_INTERSECTION(mesh, (origin), (-dir)) // NOLINT(performance-unnecessary-copy-initialization)
1335 
1336  // shift the ray a bit sideways
1337 
1338  // choose a perpendicular direction
1339  Eigen::Vector3d perpDir = dir.cross(Eigen::Vector3d({ dir.z(), dir.z(), -dir.x() - dir.y() }));
1340  if (perpDir.norm() < 1e-6)
1341  perpDir = dir.cross(Eigen::Vector3d({ -dir.y() - dir.z(), dir.x(), dir.x() }));
1342  perpDir.normalize();
1343 
1344  // now move origin "sideways" but still only so much that the ray will hit the mesh
1345  const Eigen::Vector3d origin2 = origin + g.uniformReal(1e-6, minRadius - 1e-4) * perpDir;
1346 
1347  intersections.clear();
1348  if (mesh.intersectsRay(origin2, dir, &intersections, 2))
1349  {
1350  EXPECT_EQ(2u, intersections.size());
1351  if (intersections.size() == 2)
1352  {
1353  // just approximate verification that the point is between inscribed and bounding sphere
1354  EXPECT_GE(maxRadius, (meshCenter - intersections[0]).norm());
1355  EXPECT_LE(minRadius, (meshCenter - intersections[0]).norm());
1356  EXPECT_GE(maxRadius, (meshCenter - intersections[1]).norm());
1357  EXPECT_LE(minRadius, (meshCenter - intersections[1]).norm());
1358 
1359  // verify the point lies on the ray
1360  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin2, dir).distance(intersections[0])), 1e-6);
1361  EXPECT_LT(0.0, (intersections[0] - origin2).normalized().dot(dir));
1362  EXPECT_NEAR(0.0, (Eigen::ParametrizedLine<double, 3>(origin2, dir).distance(intersections[1])), 1e-6);
1363  EXPECT_LT(0.0, (intersections[1] - origin2).normalized().dot(dir));
1364  }
1365  }
1366  else
1367  {
1368  GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str());
1369  }
1370  }
1371 }
1372 
1373 int main(int argc, char** argv)
1374 {
1375  testing::InitGoogleTest(&argc, argv);
1376  return RUN_ALL_TESTS();
1377 }
CHECK_INTERSECTS_TWICE
#define CHECK_INTERSECTS_TWICE(body, origin, direction, intersc1, intersc2, error)
Definition: test_ray_intersection.cpp:97
bodies::Box::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:714
bodies::Body::samplePointInside
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:168
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:175
shapes::Cylinder
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bodies::ConvexMesh::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:1231
bodies::Cylinder
Definition of a cylinder.
Definition: bodies.h:357
random_numbers::RandomNumberGenerator::quaternion
void quaternion(double value[4])
shapes::Box::size
double size[3]
x, y, z dimensions of the box (axis-aligned)
Definition: shapes.h:270
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:293
getRandomPose
Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator &g)
Definition: test_ray_intersection.cpp:46
main
int main(int argc, char **argv)
Definition: test_ray_intersection.cpp:1373
bodies::Sphere::samplePointInside
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:263
M_PI
#define M_PI
bodies::Body::setScale
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1...
Definition: bodies.h:122
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
bodies::Cylinder::samplePointInside
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:409
bodies::BoundingSphere::center
Eigen::Vector3d center
Definition: bodies.h:63
bodies::BoundingSphere
Definition of a sphere that bounds another object.
Definition: bodies.h:61
EXPECT_EQ
#define EXPECT_EQ(a, b)
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:504
body_operations.h
bodies::Box
Definition of a box.
Definition: bodies.h:428
bodies::BoundingCylinder
Definition of a cylinder.
Definition: bodies.h:70
bodies::Cylinder::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:449
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
bodies::Sphere::computeBoundingSphere
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:230
bodies::Cylinder::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:478
random_numbers.h
shapes::Sphere
Definition of a sphere.
Definition: shapes.h:106
bodies::Cylinder::computeBoundingSphere
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:443
bodies::Body::getScale
double getScale() const
Retrieve the current scale.
Definition: bodies.h:129
bodies::Body::getPadding
double getPadding() const
Retrieve the current padding.
Definition: bodies.h:156
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin. Padding is not applied to vertices plainly coordinate-wise, but instead the padding value is added to the length of the direction vector between centroid and each vertex.
Definition: shapes.h:281
CHECK_NO_INTERSECTION
#define CHECK_NO_INTERSECTION(body, origin, direction)
Definition: test_ray_intersection.cpp:62
shapes::Box
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
shape_operations.h
CHECK_INTERSECTS_ONCE
#define CHECK_INTERSECTS_ONCE(body, origin, direction, intersection, error)
Definition: test_ray_intersection.cpp:82
bodies::Box::samplePointInside
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:584
bodies::Body::getPose
const Eigen::Isometry3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:182
CHECK_INTERSECTS
#define CHECK_INTERSECTS(body, origin, direction, numIntersections)
Definition: test_ray_intersection.cpp:72
bodies::Sphere::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:286
shapes::createMeshFromShape
Mesh * createMeshFromShape(const Shape *shape)
Construct a mesh from a primitive shape that is NOT already a mesh. This call allocates a new object...
Definition: mesh_operations.cpp:358
r
r
TEST
TEST(SphereRayIntersection, OriginInside)
Definition: test_ray_intersection.cpp:122
bodies::Body::setPadding
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...
Definition: bodies.h:149
bodies::Box::getDimensions
std::vector< double > getDimensions() const override
Get the length & width & height (x, y, z) of the box.
Definition: bodies.cpp:606
random_numbers::RandomNumberGenerator

geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Fri Apr 14 2023 02:14:40