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 
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 
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 
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 
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 }
Eigen::Vector3d getExtents() const
Get the extents of the OBB.
Definition: obb.cpp:99
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:175
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
Definition of a cylinder.
Definition: bodies.h:357
Definition of a sphere.
Definition: bodies.h:293
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:702
TEST(SphereBoundingBox, Sphere1)
#define M_PI
#define EXPECT_NEAR(a, b, prec)
bool overlaps(const OBB &other) const
Check whether this and the given OBBs have nonempty intersection.
Definition: obb.cpp:164
unsigned int * triangles
The vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1...
Definition: shapes.h:354
EigenSTL::vector_Vector3d computeVertices() const
Compute coordinates of the 8 vertices of this OBB.
Definition: obb.cpp:169
Represents an oriented bounding box.
Definition: obb.h:53
OBB * extendApprox(const OBB &box)
Add the other OBB to this one and compute an approximate enclosing OBB.
Definition: obb.cpp:138
#define EXPECT_FALSE(args)
#define EXPECT_EQ(a, b)
#define M_PI_2
int main(int argc, char **argv)
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:504
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:456
Definition of a box.
Definition: bodies.h:428
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
Definition: shapes.h:347
double uniformReal(double lower_bound, double upper_bound)
Definition of a sphere.
Definition: shapes.h:106
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:243
bool contains(const Eigen::Vector3d &point) const
Check if this OBB contains the given point.
Definition: obb.cpp:159
Eigen::Isometry3d getPose() const
Get the pose of the OBB.
Definition: obb.cpp:119
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin. Padding is not applied to vertices plainly coordinate-wise, but instead the padding value is added to the length of the direction vector between centroid and each vertex.
Definition: shapes.h:281
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
void mergeBoundingBoxesApprox(const std::vector< OBB > &boxes, OBB &mergedBox)
Compute an approximate oriented bounding box to enclose a set of bounding boxes.
shapes::Mesh * createBoxMesh(const Eigen::Vector3d &min, const Eigen::Vector3d &max)
Represents an axis-aligned bounding box.
Definition: aabb.h:45
void mergeBoundingBoxes(const std::vector< AABB > &boxes, AABB &mergedBox)
Compute an axis-aligned bounding box to enclose a set of bounding boxes.
#define EXPECT_TRUE(args)


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