test_bounding_box.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 Open Robotics 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 
37 #include <geometric_shapes/bodies.h>
38 #include <geometric_shapes/body_operations.h>
39 #include <random_numbers/random_numbers.h>
40 #include <gtest/gtest.h>
41 
42 // The magic numbers in this test were verified visually using Blender
43 
44 TEST(SphereBoundingBox, Sphere1)
45 {
46  shapes::Sphere shape(1.0);
47  bodies::Sphere body(&shape);
48  bodies::AABB bbox;
49  body.computeBoundingBox(bbox);
50  bodies::OBB obbox;
51  body.computeBoundingBox(obbox);
52 
53  EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4);
54  EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4);
55  EXPECT_NEAR(-1.0, bbox.min().z(), 1e-4);
56  EXPECT_NEAR(1.0, bbox.max().x(), 1e-4);
57  EXPECT_NEAR(1.0, bbox.max().y(), 1e-4);
58  EXPECT_NEAR(1.0, bbox.max().z(), 1e-4);
59 
60  EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4);
61  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
62  EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4);
63  EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4);
64  EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4);
65  EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4);
66 
67  EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
68 }
69 
70 TEST(SphereBoundingBox, Sphere2)
71 {
72  shapes::Sphere shape(2.0);
73  bodies::Sphere body(&shape);
74  Eigen::Isometry3d pose;
75  pose.setIdentity();
76  pose.translation() = Eigen::Vector3d(1, 2, 3);
77  body.setPose(pose);
78  bodies::AABB bbox;
79  body.computeBoundingBox(bbox);
80  bodies::OBB obbox;
81  body.computeBoundingBox(obbox);
82 
83  EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4);
84  EXPECT_NEAR(0.0, bbox.min().y(), 1e-4);
85  EXPECT_NEAR(1.0, bbox.min().z(), 1e-4);
86  EXPECT_NEAR(3.0, bbox.max().x(), 1e-4);
87  EXPECT_NEAR(4.0, bbox.max().y(), 1e-4);
88  EXPECT_NEAR(5.0, bbox.max().z(), 1e-4);
89 
90  EXPECT_NEAR(4.0, obbox.getExtents().x(), 1e-4);
91  EXPECT_NEAR(4.0, obbox.getExtents().y(), 1e-4);
92  EXPECT_NEAR(4.0, obbox.getExtents().z(), 1e-4);
93 
94  EXPECT_TRUE(obbox.getPose().isApprox(pose, 1e-4));
95 
96  pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
97  body.setPose(pose);
98  body.computeBoundingBox(bbox);
99  body.computeBoundingBox(obbox);
100 
101  EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4);
102  EXPECT_NEAR(0.0, bbox.min().y(), 1e-4);
103  EXPECT_NEAR(1.0, bbox.min().z(), 1e-4);
104  EXPECT_NEAR(3.0, bbox.max().x(), 1e-4);
105  EXPECT_NEAR(4.0, bbox.max().y(), 1e-4);
106  EXPECT_NEAR(5.0, bbox.max().z(), 1e-4);
107 
108  EXPECT_NEAR(4.0, obbox.getExtents().x(), 1e-4);
109  EXPECT_NEAR(4.0, obbox.getExtents().y(), 1e-4);
110  EXPECT_NEAR(4.0, obbox.getExtents().z(), 1e-4);
111 
112  // oriented bounding box doesn't rotate with the sphere
113  EXPECT_TRUE(obbox.getPose().translation().isApprox(pose.translation(), 1e-4));
114  EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
115 
116  // verify the bounding box is rotation-invariant
117 
118  random_numbers::RandomNumberGenerator gen;
119  double quatData[4];
120  Eigen::Quaterniond quat;
121 
122  for (size_t i = 0; i < 10; ++i)
123  {
124  gen.quaternion(quatData);
125  quat.x() = quatData[0];
126  quat.y() = quatData[1];
127  quat.z() = quatData[2];
128  quat.w() = quatData[3];
129  pose.linear() = quat.toRotationMatrix();
130  body.setPose(pose);
131  bodies::AABB bbox2;
132  bodies::OBB obbox2;
133  body.computeBoundingBox(bbox2);
134  body.computeBoundingBox(obbox2);
135 
136  EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4);
137  EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4);
138  EXPECT_NEAR(bbox2.min().z(), bbox.min().z(), 1e-4);
139  EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4);
140  EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4);
141  EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4);
142 
143  EXPECT_NEAR(obbox.getExtents().x(), obbox2.getExtents().x(), 1e-4);
144  EXPECT_NEAR(obbox.getExtents().y(), obbox2.getExtents().y(), 1e-4);
145  EXPECT_NEAR(obbox.getExtents().z(), obbox2.getExtents().z(), 1e-4);
146 
147  EXPECT_TRUE(obbox2.getPose().isApprox(obbox.getPose(), 1e-4));
148  }
149 }
150 
151 TEST(BoxBoundingBox, Box1)
152 {
153  shapes::Box shape(1.0, 2.0, 3.0);
154  bodies::Box body(&shape);
155  bodies::AABB bbox;
156  body.computeBoundingBox(bbox);
157  bodies::OBB obbox;
158  body.computeBoundingBox(obbox);
159 
160  EXPECT_NEAR(-0.5, bbox.min().x(), 1e-4);
161  EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4);
162  EXPECT_NEAR(-1.5, bbox.min().z(), 1e-4);
163  EXPECT_NEAR(0.5, bbox.max().x(), 1e-4);
164  EXPECT_NEAR(1.0, bbox.max().y(), 1e-4);
165  EXPECT_NEAR(1.5, bbox.max().z(), 1e-4);
166 
167  EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4);
168  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
169  EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4);
170  EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4);
171  EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4);
172  EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4);
173 
174  EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
175 }
176 
177 TEST(BoxBoundingBox, Box2)
178 {
179  shapes::Box shape(1.0, 2.0, 3.0);
180  bodies::Box body(&shape);
181  Eigen::Isometry3d pose;
182  pose.setIdentity();
183  pose.translation() = Eigen::Vector3d(1, 2, 3);
184  body.setPose(pose);
185  bodies::AABB bbox;
186  body.computeBoundingBox(bbox);
187  bodies::OBB obbox;
188  body.computeBoundingBox(obbox);
189 
190  EXPECT_NEAR(0.5, bbox.min().x(), 1e-4);
191  EXPECT_NEAR(1.0, bbox.min().y(), 1e-4);
192  EXPECT_NEAR(1.5, bbox.min().z(), 1e-4);
193  EXPECT_NEAR(1.5, bbox.max().x(), 1e-4);
194  EXPECT_NEAR(3.0, bbox.max().y(), 1e-4);
195  EXPECT_NEAR(4.5, bbox.max().z(), 1e-4);
196 
197  EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4);
198  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
199  EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4);
200  EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4);
201  EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4);
202  EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4);
203 
204  EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
205 
206  pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
207  body.setPose(pose);
208  body.computeBoundingBox(bbox);
209  body.computeBoundingBox(obbox);
210 
211  EXPECT_NEAR(-0.7767, bbox.min().x(), 1e-4);
212  EXPECT_NEAR(0.8452, bbox.min().y(), 1e-4);
213  EXPECT_NEAR(1.4673, bbox.min().z(), 1e-4);
214  EXPECT_NEAR(2.7767, bbox.max().x(), 1e-4);
215  EXPECT_NEAR(3.1547, bbox.max().y(), 1e-4);
216  EXPECT_NEAR(4.5326, bbox.max().z(), 1e-4);
217 
218  EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4);
219  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
220  EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4);
221  EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4);
222  EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4);
223  EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4);
224 
225  EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4));
226 }
227 
228 TEST(CylinderBoundingBox, Cylinder1)
229 {
230  shapes::Cylinder shape(1.0, 2.0);
231  bodies::Cylinder body(&shape);
232  bodies::AABB bbox;
233  body.computeBoundingBox(bbox);
234  bodies::OBB obbox;
235  body.computeBoundingBox(obbox);
236 
237  EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4);
238  EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4);
239  EXPECT_NEAR(-1.0, bbox.min().z(), 1e-4);
240  EXPECT_NEAR(1.0, bbox.max().x(), 1e-4);
241  EXPECT_NEAR(1.0, bbox.max().y(), 1e-4);
242  EXPECT_NEAR(1.0, bbox.max().z(), 1e-4);
243 
244  EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4);
245  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
246  EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4);
247  EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4);
248  EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4);
249  EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4);
250 
251  EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
252 }
253 
254 TEST(CylinderBoundingBox, Cylinder2)
255 {
256  shapes::Cylinder shape(1.0, 2.0);
257  bodies::Cylinder body(&shape);
258  Eigen::Isometry3d pose;
259  pose.setIdentity();
260  pose.translation() = Eigen::Vector3d(1, 2, 3);
261  body.setPose(pose);
262  bodies::AABB bbox;
263  body.computeBoundingBox(bbox);
264  bodies::OBB obbox;
265  body.computeBoundingBox(obbox);
266 
267  EXPECT_NEAR(0.0, bbox.min().x(), 1e-4);
268  EXPECT_NEAR(1.0, bbox.min().y(), 1e-4);
269  EXPECT_NEAR(2.0, bbox.min().z(), 1e-4);
270  EXPECT_NEAR(2.0, bbox.max().x(), 1e-4);
271  EXPECT_NEAR(3.0, bbox.max().y(), 1e-4);
272  EXPECT_NEAR(4.0, bbox.max().z(), 1e-4);
273 
274  EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4);
275  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
276  EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4);
277  EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4);
278  EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4);
279  EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4);
280 
281  EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
282 
283  pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
284  body.setPose(pose);
285  body.computeBoundingBox(bbox);
286  body.computeBoundingBox(obbox);
287 
288  EXPECT_NEAR(-0.3238, bbox.min().x(), 1e-4);
289  EXPECT_NEAR(0.7862, bbox.min().y(), 1e-4);
290  EXPECT_NEAR(1.7239, bbox.min().z(), 1e-4);
291  EXPECT_NEAR(2.3238, bbox.max().x(), 1e-4);
292  EXPECT_NEAR(3.2138, bbox.max().y(), 1e-4);
293  EXPECT_NEAR(4.2761, bbox.max().z(), 1e-4);
294 
295  EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4);
296  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
297  EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4);
298  EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4);
299  EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4);
300  EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4);
301 
302  EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4));
303 
304  // verify the bounding box is yaw-invariant
305 
306  random_numbers::RandomNumberGenerator gen(0);
307  const auto rollPitch =
308  Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitY());
309 
310  pose.linear() = rollPitch.toRotationMatrix();
311  body.setPose(pose);
312  body.computeBoundingBox(bbox);
313 
314  bodies::AABB bbox2;
315  bodies::OBB obbox2;
316  for (size_t i = 0; i < 10; ++i)
317  {
318  const auto angle = gen.uniformReal(-M_PI, M_PI);
319  const auto yaw = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
320  pose.linear() = (rollPitch * yaw).toRotationMatrix();
321  body.setPose(pose);
322  body.computeBoundingBox(bbox2);
323  body.computeBoundingBox(obbox2);
324 
325  EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4);
326  EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4);
327  EXPECT_NEAR(bbox2.min().z(), bbox.min().z(), 1e-4);
328  EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4);
329  EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4);
330  EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4);
331 
332  EXPECT_NEAR(2.0, obbox2.getExtents().x(), 1e-4);
333  EXPECT_NEAR(2.0, obbox2.getExtents().y(), 1e-4);
334  EXPECT_NEAR(2.0, obbox2.getExtents().z(), 1e-4);
335  EXPECT_NEAR(1.0, obbox2.getPose().translation().x(), 1e-4);
336  EXPECT_NEAR(2.0, obbox2.getPose().translation().y(), 1e-4);
337  EXPECT_NEAR(3.0, obbox2.getPose().translation().z(), 1e-4);
338 
339  // oriented bounding boxes are not yaw-invariant
340  EXPECT_TRUE(obbox2.getPose().linear().isApprox(pose.linear(), 1e-4));
341  }
342 }
343 
344 shapes::Mesh* createBoxMesh(const Eigen::Vector3d& min, const Eigen::Vector3d& max)
345 {
346  shapes::Mesh* m = new shapes::Mesh(8, 12);
347 
348  m->vertices[3 * 0 + 0] = min.x();
349  m->vertices[3 * 0 + 1] = min.y();
350  m->vertices[3 * 0 + 2] = min.z();
351 
352  m->vertices[3 * 1 + 0] = max.x();
353  m->vertices[3 * 1 + 1] = min.y();
354  m->vertices[3 * 1 + 2] = min.z();
355 
356  m->vertices[3 * 2 + 0] = min.x();
357  m->vertices[3 * 2 + 1] = max.y();
358  m->vertices[3 * 2 + 2] = min.z();
359 
360  m->vertices[3 * 3 + 0] = max.x();
361  m->vertices[3 * 3 + 1] = max.y();
362  m->vertices[3 * 3 + 2] = min.z();
363 
364  m->vertices[3 * 4 + 0] = min.x();
365  m->vertices[3 * 4 + 1] = min.y();
366  m->vertices[3 * 4 + 2] = max.z();
367 
368  m->vertices[3 * 5 + 0] = max.x();
369  m->vertices[3 * 5 + 1] = min.y();
370  m->vertices[3 * 5 + 2] = max.z();
371 
372  m->vertices[3 * 6 + 0] = min.x();
373  m->vertices[3 * 6 + 1] = max.y();
374  m->vertices[3 * 6 + 2] = max.z();
375 
376  m->vertices[3 * 7 + 0] = max.x();
377  m->vertices[3 * 7 + 1] = max.y();
378  m->vertices[3 * 7 + 2] = max.z();
379 
380  m->triangles[3 * 0 + 0] = 0;
381  m->triangles[3 * 0 + 1] = 1;
382  m->triangles[3 * 0 + 2] = 2;
383 
384  m->triangles[3 * 1 + 0] = 1;
385  m->triangles[3 * 1 + 1] = 3;
386  m->triangles[3 * 1 + 2] = 2;
387 
388  m->triangles[3 * 2 + 0] = 5;
389  m->triangles[3 * 2 + 1] = 4;
390  m->triangles[3 * 2 + 2] = 6;
391 
392  m->triangles[3 * 3 + 0] = 5;
393  m->triangles[3 * 3 + 1] = 6;
394  m->triangles[3 * 3 + 2] = 7;
395 
396  m->triangles[3 * 4 + 0] = 1;
397  m->triangles[3 * 4 + 1] = 5;
398  m->triangles[3 * 4 + 2] = 3;
399 
400  m->triangles[3 * 5 + 0] = 5;
401  m->triangles[3 * 5 + 1] = 7;
402  m->triangles[3 * 5 + 2] = 3;
403 
404  m->triangles[3 * 6 + 0] = 4;
405  m->triangles[3 * 6 + 1] = 0;
406  m->triangles[3 * 6 + 2] = 2;
407 
408  m->triangles[3 * 7 + 0] = 4;
409  m->triangles[3 * 7 + 1] = 2;
410  m->triangles[3 * 7 + 2] = 6;
411 
412  m->triangles[3 * 8 + 0] = 2;
413  m->triangles[3 * 8 + 1] = 3;
414  m->triangles[3 * 8 + 2] = 6;
415 
416  m->triangles[3 * 9 + 0] = 3;
417  m->triangles[3 * 9 + 1] = 7;
418  m->triangles[3 * 9 + 2] = 6;
419 
420  m->triangles[3 * 10 + 0] = 1;
421  m->triangles[3 * 10 + 1] = 0;
422  m->triangles[3 * 10 + 2] = 4;
423 
424  m->triangles[3 * 11 + 0] = 1;
425  m->triangles[3 * 11 + 1] = 4;
426  m->triangles[3 * 11 + 2] = 5;
427 
428  return m;
429 }
430 
431 TEST(MeshBoundingBox, Mesh1)
432 {
433  shapes::Mesh* m = createBoxMesh({ -1, -1, -1 }, { 1, 1, 1 });
434 
435  bodies::ConvexMesh body(m);
436  bodies::AABB bbox;
437  body.computeBoundingBox(bbox);
438  bodies::OBB obbox;
439  body.computeBoundingBox(obbox);
440 
441  EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4);
442  EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4);
443  EXPECT_NEAR(-1.0, bbox.min().z(), 1e-4);
444  EXPECT_NEAR(1.0, bbox.max().x(), 1e-4);
445  EXPECT_NEAR(1.0, bbox.max().y(), 1e-4);
446  EXPECT_NEAR(1.0, bbox.max().z(), 1e-4);
447 
448  EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4);
449  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
450  EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4);
451  EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4);
452  EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4);
453  EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4);
454 
455  EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
456  delete m;
457 }
458 
459 TEST(MeshBoundingBox, Mesh2)
460 {
461  shapes::Mesh* m = createBoxMesh({ -0.5, -1.0, -1.5 }, { 0.5, 1.0, 1.5 });
462 
463  bodies::ConvexMesh body(m);
464  Eigen::Isometry3d pose;
465  pose.setIdentity();
466  pose.translation() = Eigen::Vector3d(1, 2, 3);
467  body.setPose(pose);
468  bodies::AABB bbox;
469  body.computeBoundingBox(bbox);
470  bodies::OBB obbox;
471  body.computeBoundingBox(obbox);
472 
473  EXPECT_NEAR(0.5, bbox.min().x(), 1e-4);
474  EXPECT_NEAR(1.0, bbox.min().y(), 1e-4);
475  EXPECT_NEAR(1.5, bbox.min().z(), 1e-4);
476  EXPECT_NEAR(1.5, bbox.max().x(), 1e-4);
477  EXPECT_NEAR(3.0, bbox.max().y(), 1e-4);
478  EXPECT_NEAR(4.5, bbox.max().z(), 1e-4);
479 
480  EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4);
481  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
482  EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4);
483  EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4);
484  EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4);
485  EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4);
486 
487  EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
488 
489  pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
490  body.setPose(pose);
491  body.computeBoundingBox(bbox);
492  body.computeBoundingBox(obbox);
493 
494  EXPECT_NEAR(-0.7767, bbox.min().x(), 1e-4);
495  EXPECT_NEAR(0.8452, bbox.min().y(), 1e-4);
496  EXPECT_NEAR(1.4673, bbox.min().z(), 1e-4);
497  EXPECT_NEAR(2.7767, bbox.max().x(), 1e-4);
498  EXPECT_NEAR(3.1547, bbox.max().y(), 1e-4);
499  EXPECT_NEAR(4.5326, bbox.max().z(), 1e-4);
500 
501  EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4);
502  EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4);
503  EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4);
504  EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4);
505  EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4);
506  EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4);
507 
508  EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4));
509 
510  delete m;
511 }
512 
513 TEST(MergeBoundingBoxes, Merge1)
514 {
515  std::vector<bodies::AABB> boxes;
516  boxes.emplace_back(Eigen::Vector3d(-1, -1, -1), Eigen::Vector3d(0, 0, 0));
517  boxes.emplace_back(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(1, 1, 1));
518 
519  bodies::AABB bbox;
520  bodies::mergeBoundingBoxes(boxes, bbox);
521 
522  EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4);
523  EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4);
524  EXPECT_NEAR(-1.0, bbox.min().z(), 1e-4);
525  EXPECT_NEAR(1.0, bbox.max().x(), 1e-4);
526  EXPECT_NEAR(1.0, bbox.max().y(), 1e-4);
527  EXPECT_NEAR(1.0, bbox.max().z(), 1e-4);
528 }
529 
530 TEST(MergeBoundingBoxes, OBBInvalid)
531 {
532  auto pose = Eigen::Isometry3d::Identity();
533  pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix();
534 
535  bodies::OBB b1; // uninitialized OBB, extents == 0 and invalid rotation
536  pose.translation() = -0.6 * Eigen::Vector3d::Ones();
537  bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1));
538 
539  // invalid b1 extended with valid b2 should equal b2
540  b1.extendApprox(b2);
541 
542  EXPECT_TRUE(b1.overlaps(b2));
543  EXPECT_TRUE(b2.overlaps(b1));
544 
545  EXPECT_NEAR(0.1, b1.getExtents().x(), 1e-12);
546  EXPECT_NEAR(0.1, b1.getExtents().y(), 1e-12);
547  EXPECT_NEAR(0.1, b1.getExtents().z(), 1e-12);
548  EXPECT_NEAR(-0.6, b1.getPose().translation().x(), 1e-12);
549  EXPECT_NEAR(-0.6, b1.getPose().translation().y(), 1e-12);
550  EXPECT_NEAR(-0.6, b1.getPose().translation().z(), 1e-12);
551  EXPECT_TRUE(b1.getPose().linear().isApprox(pose.linear(), 1e-12));
552 }
553 
554 TEST(MergeBoundingBoxes, OBBContains1)
555 {
556  auto pose = Eigen::Isometry3d::Identity();
557 
558  pose.translation() = -0.5 * Eigen::Vector3d::Ones();
559  bodies::OBB b1(pose, Eigen::Vector3d(1, 1, 1));
560  pose.translation() = -0.6 * Eigen::Vector3d::Ones();
561  pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix();
562  bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1));
563 
564  EXPECT_TRUE(b1.contains(b2));
565  EXPECT_FALSE(b2.contains(b1));
566  EXPECT_TRUE(b1.overlaps(b2));
567  EXPECT_TRUE(b2.overlaps(b1));
568 
569  // b1 contains whole b2, so the extended b1 should be equal to the original b1
570  b1.extendApprox(b2);
571 
572  EXPECT_TRUE(b1.contains(b2));
573  EXPECT_FALSE(b2.contains(b1));
574  EXPECT_TRUE(b1.overlaps(b2));
575  EXPECT_TRUE(b2.overlaps(b1));
576 
577  auto u = b1.computeVertices();
578  auto v = b2.computeVertices();
579  EXPECT_NEAR(1, b1.getExtents().x(), 1e-12);
580  EXPECT_NEAR(1, b1.getExtents().y(), 1e-12);
581  EXPECT_NEAR(1, b1.getExtents().z(), 1e-12);
582  EXPECT_NEAR(-0.5, b1.getPose().translation().x(), 1e-12);
583  EXPECT_NEAR(-0.5, b1.getPose().translation().y(), 1e-12);
584  EXPECT_NEAR(-0.5, b1.getPose().translation().z(), 1e-12);
585  EXPECT_TRUE(b1.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-12));
586 }
587 
588 TEST(MergeBoundingBoxes, OBBContains2)
589 {
590  auto pose = Eigen::Isometry3d::Identity();
591 
592  pose.translation() = -0.5 * Eigen::Vector3d::Ones();
593  bodies::OBB b1(pose, Eigen::Vector3d(1, 1, 1));
594  pose.translation() = -0.6 * Eigen::Vector3d::Ones();
595  pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix();
596  bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1));
597 
598  EXPECT_TRUE(b1.contains(b2));
599  EXPECT_FALSE(b2.contains(b1));
600  EXPECT_TRUE(b1.overlaps(b2));
601  EXPECT_TRUE(b2.overlaps(b1));
602 
603  // b1 contains whole b2, so the extended b2 should be equal to the original b1
604  b2.extendApprox(b1);
605 
606  EXPECT_TRUE(b1.overlaps(b2));
607  EXPECT_TRUE(b2.overlaps(b1));
608 
609  EXPECT_NEAR(1, b2.getExtents().x(), 1e-12);
610  EXPECT_NEAR(1, b2.getExtents().y(), 1e-12);
611  EXPECT_NEAR(1, b2.getExtents().z(), 1e-12);
612  EXPECT_NEAR(-0.5, b2.getPose().translation().x(), 1e-12);
613  EXPECT_NEAR(-0.5, b2.getPose().translation().y(), 1e-12);
614  EXPECT_NEAR(-0.5, b2.getPose().translation().z(), 1e-12);
615  EXPECT_TRUE(b2.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-12));
616 }
617 
618 TEST(MergeBoundingBoxes, OBBApprox1)
619 {
620  std::vector<bodies::OBB> boxes;
621  auto pose = Eigen::Isometry3d::Identity();
622 
623  pose.translation() = -0.5 * Eigen::Vector3d::Ones();
624  boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1));
625  pose.translation() = 0.5 * Eigen::Vector3d::Ones();
626  boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1));
627 
628  bodies::OBB bbox;
629  // check that the empty constructor constructs a valid empty OBB
630  EXPECT_EQ(0.0, bbox.getPose().translation().x());
631  EXPECT_EQ(0.0, bbox.getPose().translation().y());
632  EXPECT_EQ(0.0, bbox.getPose().translation().z());
633  EXPECT_EQ(0.0, bbox.getExtents().x());
634  EXPECT_EQ(0.0, bbox.getExtents().y());
635  EXPECT_EQ(0.0, bbox.getExtents().z());
636  EXPECT_TRUE(bbox.getPose().rotation().isApprox(Eigen::Matrix3d::Identity()));
637 
638  bodies::mergeBoundingBoxesApprox(boxes, bbox);
639 
640  // the resulting bounding box might not be tight, so we only do some sanity checks
641 
642  EXPECT_GE(2.1, bbox.getExtents().x());
643  EXPECT_GE(2.1, bbox.getExtents().y());
644  EXPECT_GE(2.1, bbox.getExtents().z());
645  EXPECT_GE(0.1, bbox.getPose().translation().x());
646  EXPECT_GE(0.1, bbox.getPose().translation().y());
647  EXPECT_GE(0.1, bbox.getPose().translation().z());
648  EXPECT_LE(2.0, bbox.getExtents().x());
649  EXPECT_LE(2.0, bbox.getExtents().y());
650  EXPECT_LE(2.0, bbox.getExtents().z());
651  EXPECT_LE(-0.1, bbox.getPose().translation().x());
652  EXPECT_LE(-0.1, bbox.getPose().translation().y());
653  EXPECT_LE(-0.1, bbox.getPose().translation().z());
654 
655  EXPECT_TRUE(bbox.contains(boxes[0].getPose().translation()));
656  EXPECT_TRUE(bbox.contains(boxes[1].getPose().translation()));
657  EXPECT_TRUE(bbox.overlaps(boxes[0]));
658  EXPECT_TRUE(bbox.overlaps(boxes[1]));
659 }
660 
661 int main(int argc, char** argv)
662 {
663  testing::InitGoogleTest(&argc, argv);
664  return RUN_ALL_TESTS();
665 }
random_numbers.h
bodies::OBB::getExtents
Eigen::Vector3d getExtents() const
Get the extents of the OBB.
Definition: obb.cpp:99
shapes::Mesh::triangles
unsigned int * triangles
The vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1,...
Definition: shapes.h:355
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:327
bodies::mergeBoundingBoxesApprox
void mergeBoundingBoxesApprox(const std::vector< OBB > &boxes, OBB &mergedBox)
Compute an approximate oriented bounding box to enclose a set of bounding boxes.
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::Cylinder::computeBoundingBox
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:428
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin....
Definition: shapes.h:281
bodies::mergeBoundingBoxes
void mergeBoundingBoxes(const std::vector< AABB > &boxes, AABB &mergedBox)
Compute an axis-aligned bounding box to enclose a set of bounding boxes.
bodies::OBB::extendApprox
OBB * extendApprox(const OBB &box)
Add the other OBB to this one and compute an approximate enclosing OBB.
Definition: obb.cpp:138
body_operations.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
bodies::Sphere::computeBoundingBox
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:204
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::Box::computeBoundingBox
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:679
bodies::OBB
Represents an oriented bounding box.
Definition: obb.h:85
bodies::OBB::contains
bool contains(const Eigen::Vector3d &point) const
Check if this OBB contains the given point.
Definition: obb.cpp:159
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
M_PI_2
#define M_PI_2
main
int main(int argc, char **argv)
Definition: test_bounding_box.cpp:661
bodies::Box
Definition of a box.
Definition: bodies.h:443
shapes::Mesh::vertices
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,...
Definition: shapes.h:348
TEST
TEST(SphereBoundingBox, Sphere1)
Definition: test_bounding_box.cpp:44
random_numbers::RandomNumberGenerator
bodies::OBB::getPose
Eigen::Isometry3d getPose() const
Get the pose of the OBB.
Definition: obb.cpp:119
bodies.h
M_PI
#define M_PI
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:205
bodies::OBB::computeVertices
EigenSTL::vector_Vector3d computeVertices() const
Compute coordinates of the 8 vertices of this OBB.
Definition: obb.cpp:169
bodies::Cylinder
Definition of a cylinder.
Definition: bodies.h:380
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:506
createBoxMesh
shapes::Mesh * createBoxMesh(const Eigen::Vector3d &min, const Eigen::Vector3d &max)
Definition: test_bounding_box.cpp:344
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
bodies::OBB::overlaps
bool overlaps(const OBB &other) const
Check whether this and the given OBBs have nonempty intersection.
Definition: obb.cpp:164
bodies::AABB
Represents an axis-aligned bounding box.
Definition: aabb.h:77

geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Tue Aug 13 2024 02:40:57