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); // NOLINT(performance-unnecessary-copy-initialization)
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 }
random_numbers.h
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:1237
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:691
bodies::BoundingSphere::radius
double radius
Definition: bodies.h:128
shapes::Box::size
double size[3]
x, y, z dimensions of the box (axis-aligned)
Definition: shapes.h:270
CHECK_INTERSECTS_TWICE
#define CHECK_INTERSECTS_TWICE(body, origin, direction, intersc1, intersc2, error)
Definition: test_ray_intersection.cpp:97
getRandomPose
Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator &g)
Definition: test_ray_intersection.cpp:46
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:327
main
int main(int argc, char **argv)
Definition: test_ray_intersection.cpp:1373
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:450
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
shapes::Cylinder
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
bodies::BoundingCylinder::radius
double radius
Definition: bodies.h:105
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin....
Definition: shapes.h:281
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:421
bodies::Box::getDimensions
std::vector< double > getDimensions() const override
Get the length & width & height (x, y, z) of the box.
Definition: bodies.cpp:586
body_operations.h
bodies::Body::getPadding
double getPadding() const
Retrieve the current padding.
Definition: bodies.h:186
bodies::BoundingSphere
Definition of a sphere that bounds another object.
Definition: bodies.h:93
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:129
shapes::Box
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
random_numbers::RandomNumberGenerator::quaternion
void quaternion(double value[4])
shapes::Sphere
Definition of a sphere.
Definition: shapes.h:106
bodies::BoundingCylinder::length
double length
Definition: bodies.h:106
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
bodies::BoundingCylinder
Definition of a cylinder.
Definition: bodies.h:102
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bodies::Box
Definition of a box.
Definition: bodies.h:443
bodies::Body::getPose
const Eigen::Isometry3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:212
CHECK_NO_INTERSECTION
#define CHECK_NO_INTERSECTION(body, origin, direction)
Definition: test_ray_intersection.cpp:62
r
S r
distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
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:381
random_numbers::RandomNumberGenerator
shape_operations.h
bodies::Body::getScale
double getScale() const
Retrieve the current scale.
Definition: bodies.h:159
bodies.h
bodies::BoundingSphere::center
Eigen::Vector3d center
Definition: bodies.h:127
M_PI
#define M_PI
CHECK_INTERSECTS_ONCE
#define CHECK_INTERSECTS_ONCE(body, origin, direction, intersection, error)
Definition: test_ray_intersection.cpp:82
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:152
TEST
TEST(SphereRayIntersection, OriginInside)
Definition: test_ray_intersection.cpp:122
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:205
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:415
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:179
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:564
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:247
bodies::Cylinder
Definition of a cylinder.
Definition: bodies.h:380
CHECK_INTERSECTS
#define CHECK_INTERSECTS(body, origin, direction, numIntersections)
Definition: test_ray_intersection.cpp:72
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:506
EXPECT_EQ
#define EXPECT_EQ(a, b)
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:390
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:224
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:191

geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Sun Oct 1 2023 02:40:16