test_bounding_cylinder.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(SphereBoundingCylinder, Sphere1)
45 {
46  shapes::Sphere shape(1.0);
47  bodies::Sphere body(&shape);
49  body.computeBoundingCylinder(bcyl);
50 
51  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
52  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
53  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
54  // orientation can be any
55  EXPECT_NEAR(1.0, bcyl.radius, 1e-4);
56  EXPECT_NEAR(2.0, bcyl.length, 1e-4);
57 
58  body.setScale(2.0);
59  body.setPadding(1.0);
60  body.computeBoundingCylinder(bcyl);
61 
62  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
63  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
64  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
65  // orientation can be any
66  EXPECT_NEAR(3.0, bcyl.radius, 1e-4);
67  EXPECT_NEAR(6.0, bcyl.length, 1e-4);
68 }
69 
70 TEST(SphereBoundingCylinder, Sphere2)
71 {
72  shapes::Sphere shape(2.0);
73  bodies::Sphere body(&shape);
74  Eigen::Isometry3d pose(Eigen::Isometry3d::TranslationType(1, 2, 3));
75  body.setPose(pose);
77  body.computeBoundingCylinder(bcyl);
78 
79  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
80  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
81  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
82  // orientation can be any
83  EXPECT_NEAR(2.0, bcyl.radius, 1e-4);
84  EXPECT_NEAR(4.0, bcyl.length, 1e-4);
85 
86  // verify the bounding cylinder is rotation-invariant
87 
88  pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
89  body.setPose(pose);
90  body.computeBoundingCylinder(bcyl);
91 
92  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
93  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
94  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
95  // orientation can be any
96  EXPECT_NEAR(2.0, bcyl.radius, 1e-4);
97  EXPECT_NEAR(4.0, bcyl.length, 1e-4);
98 
100  double quatData[4];
101  Eigen::Quaterniond quat;
102 
103  for (size_t i = 0; i < 10; ++i)
104  {
105  gen.quaternion(quatData);
106  quat.x() = quatData[0];
107  quat.y() = quatData[1];
108  quat.z() = quatData[2];
109  quat.w() = quatData[3];
110  pose.linear() = quat.toRotationMatrix();
111  body.setPose(pose);
113  body.computeBoundingCylinder(bcyl2);
114 
115  EXPECT_NEAR(1.0, bcyl2.pose.translation().x(), 1e-4);
116  EXPECT_NEAR(2.0, bcyl2.pose.translation().y(), 1e-4);
117  EXPECT_NEAR(3.0, bcyl2.pose.translation().z(), 1e-4);
118  // orientation can be any
119  EXPECT_NEAR(2.0, bcyl2.radius, 1e-4);
120  EXPECT_NEAR(4.0, bcyl2.length, 1e-4);
121  }
122 }
123 
124 TEST(BoxBoundingCylinder, Box1)
125 {
126  shapes::Box shape(1.0, 2.0, 3.0);
127  bodies::Box body(&shape);
129  body.computeBoundingCylinder(bcyl);
130 
131  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
132  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
133  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
134  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
135  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
136  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
137  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
138  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
139  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
140 
141  body.setScale(2.0);
142  body.setPadding(1.0);
143  body.computeBoundingCylinder(bcyl);
144 
145  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
146  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
147  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
148  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
149  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
150  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
151  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
152  EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4);
153  EXPECT_NEAR(8.0, bcyl.length, 1e-4);
154 
155  // test that the rotational axis of the cylinder sticks with the longest dimension
156 
157  shape = shapes::Box(2.0, 3.0, 1.0);
158  body = bodies::Box(&shape);
159  body.computeBoundingCylinder(bcyl);
160 
161  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
162  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
163  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
164  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
165  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
166  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
167  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
168  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
169  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
170 
171  body.setScale(2.0);
172  body.setPadding(1.0);
173  body.computeBoundingCylinder(bcyl);
174 
175  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
176  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
177  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
178  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
179  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
180  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
181  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
182  EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4);
183  EXPECT_NEAR(8.0, bcyl.length, 1e-4);
184 
185  shape = shapes::Box(3.0, 1.0, 2.0);
186  body = bodies::Box(&shape);
187  body.computeBoundingCylinder(bcyl);
188 
189  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
190  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
191  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
192  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
193  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
194  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
195  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
196  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
197  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
198 
199  body.setScale(2.0);
200  body.setPadding(1.0);
201  body.computeBoundingCylinder(bcyl);
202 
203  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
204  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
205  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
206  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
207  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
208  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
209  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
210  EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4);
211  EXPECT_NEAR(8.0, bcyl.length, 1e-4);
212 }
213 
214 TEST(BoxBoundingCylinder, Box2)
215 {
216  shapes::Box shape(1.0, 2.0, 3.0);
217  bodies::Box body(&shape);
218  Eigen::Isometry3d pose;
219  pose.setIdentity();
220  pose.translation() = Eigen::Vector3d(1, 2, 3);
221  body.setPose(pose);
223  body.computeBoundingCylinder(bcyl);
224 
225  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
226  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
227  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
228  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
229  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
230  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
231  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
232  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
233  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
234 
235  Eigen::AngleAxisd rot(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
236  pose *= rot;
237  body.setPose(pose);
238  body.computeBoundingCylinder(bcyl);
239 
240  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
241  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
242  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
243  EXPECT_NEAR(Eigen::Quaterniond(rot).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
244  EXPECT_NEAR(Eigen::Quaterniond(rot).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
245  EXPECT_NEAR(Eigen::Quaterniond(rot).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
246  EXPECT_NEAR(Eigen::Quaterniond(rot).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
247  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
248  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
249 }
250 
251 TEST(CylinderBoundingCylinder, Cylinder1)
252 {
253  shapes::Cylinder shape(1.0, 2.0);
254  bodies::Cylinder body(&shape);
256  body.computeBoundingCylinder(bcyl);
257 
258  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
259  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
260  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
261  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
262  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
263  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
264  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
265  EXPECT_NEAR(1.0, bcyl.radius, 1e-4);
266  EXPECT_NEAR(2.0, bcyl.length, 1e-4);
267 
268  body.setScale(2.0);
269  body.setPadding(1.0);
270  body.computeBoundingCylinder(bcyl);
271 
272  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
273  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
274  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
275  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
276  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
277  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
278  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
279  EXPECT_NEAR(3.0, bcyl.radius, 1e-4);
280  EXPECT_NEAR(6.0, bcyl.length, 1e-4);
281 }
282 
283 TEST(CylinderBoundingCylinder, Cylinder2)
284 {
285  shapes::Cylinder shape(1.0, 2.0);
286  bodies::Cylinder body(&shape);
287  Eigen::Isometry3d pose;
288  pose.setIdentity();
289  pose.translation() = Eigen::Vector3d(1, 2, 3);
290  body.setPose(pose);
292  body.computeBoundingCylinder(bcyl);
293 
294  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
295  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
296  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
297  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
298  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
299  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
300  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
301  EXPECT_NEAR(1.0, bcyl.radius, 1e-4);
302  EXPECT_NEAR(2.0, bcyl.length, 1e-4);
303 
304  pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).toRotationMatrix();
305  body.setPose(pose);
306  body.computeBoundingCylinder(bcyl);
307 
308  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
309  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
310  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
311  EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
312  EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
313  EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
314  EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
315  EXPECT_NEAR(1.0, bcyl.radius, 1e-4);
316  EXPECT_NEAR(2.0, bcyl.length, 1e-4);
317 }
318 
319 shapes::Mesh* createBoxMesh(const Eigen::Vector3d& min, const Eigen::Vector3d& max)
320 {
321  shapes::Mesh* m = new shapes::Mesh(8, 12);
322 
323  m->vertices[3 * 0 + 0] = min.x();
324  m->vertices[3 * 0 + 1] = min.y();
325  m->vertices[3 * 0 + 2] = min.z();
326 
327  m->vertices[3 * 1 + 0] = max.x();
328  m->vertices[3 * 1 + 1] = min.y();
329  m->vertices[3 * 1 + 2] = min.z();
330 
331  m->vertices[3 * 2 + 0] = min.x();
332  m->vertices[3 * 2 + 1] = max.y();
333  m->vertices[3 * 2 + 2] = min.z();
334 
335  m->vertices[3 * 3 + 0] = max.x();
336  m->vertices[3 * 3 + 1] = max.y();
337  m->vertices[3 * 3 + 2] = min.z();
338 
339  m->vertices[3 * 4 + 0] = min.x();
340  m->vertices[3 * 4 + 1] = min.y();
341  m->vertices[3 * 4 + 2] = max.z();
342 
343  m->vertices[3 * 5 + 0] = max.x();
344  m->vertices[3 * 5 + 1] = min.y();
345  m->vertices[3 * 5 + 2] = max.z();
346 
347  m->vertices[3 * 6 + 0] = min.x();
348  m->vertices[3 * 6 + 1] = max.y();
349  m->vertices[3 * 6 + 2] = max.z();
350 
351  m->vertices[3 * 7 + 0] = max.x();
352  m->vertices[3 * 7 + 1] = max.y();
353  m->vertices[3 * 7 + 2] = max.z();
354 
355  m->triangles[3 * 0 + 0] = 0;
356  m->triangles[3 * 0 + 1] = 1;
357  m->triangles[3 * 0 + 2] = 2;
358 
359  m->triangles[3 * 1 + 0] = 1;
360  m->triangles[3 * 1 + 1] = 3;
361  m->triangles[3 * 1 + 2] = 2;
362 
363  m->triangles[3 * 2 + 0] = 5;
364  m->triangles[3 * 2 + 1] = 4;
365  m->triangles[3 * 2 + 2] = 6;
366 
367  m->triangles[3 * 3 + 0] = 5;
368  m->triangles[3 * 3 + 1] = 6;
369  m->triangles[3 * 3 + 2] = 7;
370 
371  m->triangles[3 * 4 + 0] = 1;
372  m->triangles[3 * 4 + 1] = 5;
373  m->triangles[3 * 4 + 2] = 3;
374 
375  m->triangles[3 * 5 + 0] = 5;
376  m->triangles[3 * 5 + 1] = 7;
377  m->triangles[3 * 5 + 2] = 3;
378 
379  m->triangles[3 * 6 + 0] = 4;
380  m->triangles[3 * 6 + 1] = 0;
381  m->triangles[3 * 6 + 2] = 2;
382 
383  m->triangles[3 * 7 + 0] = 4;
384  m->triangles[3 * 7 + 1] = 2;
385  m->triangles[3 * 7 + 2] = 6;
386 
387  m->triangles[3 * 8 + 0] = 2;
388  m->triangles[3 * 8 + 1] = 3;
389  m->triangles[3 * 8 + 2] = 6;
390 
391  m->triangles[3 * 9 + 0] = 3;
392  m->triangles[3 * 9 + 1] = 7;
393  m->triangles[3 * 9 + 2] = 6;
394 
395  m->triangles[3 * 10 + 0] = 1;
396  m->triangles[3 * 10 + 1] = 0;
397  m->triangles[3 * 10 + 2] = 4;
398 
399  m->triangles[3 * 11 + 0] = 1;
400  m->triangles[3 * 11 + 1] = 4;
401  m->triangles[3 * 11 + 2] = 5;
402 
403  return m;
404 }
405 
406 TEST(MeshBoundingCylinder, Mesh1)
407 {
408  shapes::Mesh* m = createBoxMesh({ -0.5, -1.0, -1.5 }, { 0.5, 1.0, 1.5 });
409 
410  double radiusScaled;
411  double lengthScaled;
412 
413  bodies::ConvexMesh body(m);
415  body.computeBoundingCylinder(bcyl);
416 
417  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
418  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
419  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
420  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
421  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
422  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
423  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
424  // for meshes without padding, the bounding cylinder should be tight
425  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
426  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
427 
428  body.setScale(2.0);
429  body.setPadding(1.0);
430  body.computeBoundingCylinder(bcyl);
431 
432  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
433  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
434  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
435  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
436  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
437  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
438  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
439  // computation of bounding cylinder with padding is only approximate; the approximation error in both radius and
440  // length is anywhere from zero to padding_ (or 2*padding_ for length)
441  radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
442  EXPECT_LE(radiusScaled, bcyl.radius);
443  EXPECT_GE(radiusScaled + 1.0, bcyl.radius);
444  lengthScaled = 6.0;
445  EXPECT_LE(lengthScaled, bcyl.length);
446  EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length);
447 
448  delete m;
449 
450  // test that the rotational axis of the cylinder sticks with the longest dimension
451 
452  m = createBoxMesh({ -1.0, -1.5, -0.5 }, { 1.0, 1.5, 0.5 });
453  bodies::ConvexMesh body2(m);
454  body2.computeBoundingCylinder(bcyl);
455 
456  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
457  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
458  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
459  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
460  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
461  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
462  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
463  // for meshes without padding, the bounding cylinder should be tight
464  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
465  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
466 
467  body2.setScale(2.0);
468  body2.setPadding(1.0);
469  body2.computeBoundingCylinder(bcyl);
470 
471  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
472  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
473  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
474  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
475  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
476  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
477  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
478  radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
479  EXPECT_LE(radiusScaled, bcyl.radius);
480  EXPECT_GE(radiusScaled + 1.0, bcyl.radius);
481  lengthScaled = 6.0;
482  EXPECT_LE(lengthScaled, bcyl.length);
483  EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length);
484 
485  delete m;
486 
487  m = createBoxMesh({ -1.5, -0.5, -1.0 }, { 1.5, 0.5, 1.0 });
488  bodies::ConvexMesh body3(m);
489  body3.computeBoundingCylinder(bcyl);
490 
491  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
492  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
493  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
494  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
495  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
496  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
497  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
498  // for meshes without padding, the bounding cylinder should be tight
499  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
500  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
501 
502  body3.setScale(2.0);
503  body3.setPadding(1.0);
504  body3.computeBoundingCylinder(bcyl);
505 
506  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
507  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
508  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
509  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
510  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
511  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
512  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
513  radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
514  EXPECT_LE(radiusScaled, bcyl.radius);
515  EXPECT_GE(radiusScaled + 1.0, bcyl.radius);
516  lengthScaled = 6.0;
517  EXPECT_LE(lengthScaled, bcyl.length);
518  EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length);
519 
520  delete m;
521 }
522 
523 TEST(MeshBoundingCylinder, Mesh2)
524 {
525  shapes::Mesh* m = createBoxMesh({ -0.5, -1.0, -1.5 }, { 0.5, 1.0, 1.5 });
526 
527  bodies::ConvexMesh body(m);
528  Eigen::Isometry3d pose;
529  pose.setIdentity();
530  pose.translation() = Eigen::Vector3d(1, 2, 3);
531  body.setPose(pose);
533  body.computeBoundingCylinder(bcyl);
534 
535  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
536  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
537  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
538  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
539  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
540  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
541  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
542  // for meshes without padding, the bounding cylinder should be tight
543  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
544  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
545 
546  Eigen::AngleAxisd rot(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
547  pose *= rot;
548  body.setPose(pose);
549  body.computeBoundingCylinder(bcyl);
550 
551  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
552  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
553  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
554  EXPECT_NEAR(Eigen::Quaterniond(rot).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
555  EXPECT_NEAR(Eigen::Quaterniond(rot).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
556  EXPECT_NEAR(Eigen::Quaterniond(rot).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
557  EXPECT_NEAR(Eigen::Quaterniond(rot).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
558  // for meshes without padding, the bounding cylinder should be tight
559  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
560  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
561 
562  delete m;
563 }
564 
565 int main(int argc, char** argv)
566 {
567  testing::InitGoogleTest(&argc, argv);
568  return RUN_ALL_TESTS();
569 }
shapes::Mesh * createBoxMesh(const Eigen::Vector3d &min, const Eigen::Vector3d &max)
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
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:1159
Definition of a sphere.
Definition: bodies.h:293
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1...
Definition: bodies.h:122
#define EXPECT_NEAR(a, b, prec)
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
double y
#define M_PI_2
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:504
Definition of a box.
Definition: bodies.h:428
Definition of a cylinder.
Definition: bodies.h:70
TEST(SphereBoundingCylinder, Sphere1)
double z
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
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:449
Definition of a sphere.
Definition: shapes.h:106
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:236
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
Eigen::Isometry3d pose
Definition: bodies.h:72
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
int main(int argc, char **argv)
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:671
double x
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...
Definition: bodies.h:149


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