test_bounding_sphere.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Willow Garage, Inc.
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 
37 #include <geometric_shapes/bodies.h>
38 #include <geometric_shapes/shape_operations.h>
39 #include <geometric_shapes/body_operations.h>
40 #include <boost/filesystem.hpp>
41 #include <gtest/gtest.h>
42 
43 TEST(SphereBoundingSphere, Sphere1)
44 {
45  shapes::Sphere shape(1.0);
46  Eigen::Vector3d center;
47  double radius;
48  computeShapeBoundingSphere(&shape, center, radius);
49 
50  EXPECT_EQ(1.0, radius);
51  EXPECT_EQ(0.0, center.x());
52  EXPECT_EQ(0.0, center.y());
53  EXPECT_EQ(0.0, center.z());
54 }
55 
56 TEST(SphereBoundingSphere, Sphere2)
57 {
58  shapes::Shape* shape = new shapes::Sphere(2.0);
59  Eigen::Vector3d center;
60  double radius;
61  computeShapeBoundingSphere(shape, center, radius);
62 
63  EXPECT_EQ(2.0, radius);
64  EXPECT_EQ(0.0, center.x());
65  EXPECT_EQ(0.0, center.y());
66  EXPECT_EQ(0.0, center.z());
67  delete shape;
68 }
69 
70 TEST(BoxBoundingSphere, Box1)
71 {
72  shapes::Shape* shape = new shapes::Box(2.0, 4.0, 6.0);
73  Eigen::Vector3d center;
74  double radius;
75  computeShapeBoundingSphere(shape, center, radius);
76 
77  EXPECT_EQ(sqrt(14.0), radius);
78  EXPECT_EQ(0.0, center.x());
79  EXPECT_EQ(0.0, center.y());
80  EXPECT_EQ(0.0, center.z());
81 }
82 
83 TEST(BoxBoundingSphere, Box2)
84 {
85  shapes::Shape* shape = new shapes::Box(2.0, 2.0, 2.0);
86  Eigen::Vector3d center;
87  double radius;
88  computeShapeBoundingSphere(shape, center, radius);
89 
90  EXPECT_EQ(sqrt(3.0), radius);
91  EXPECT_EQ(0.0, center.x());
92  EXPECT_EQ(0.0, center.y());
93  EXPECT_EQ(0.0, center.z());
94 }
95 
96 TEST(CylBoundingSphere, Cyl1)
97 {
98  shapes::Shape* shape = new shapes::Cylinder(1.0, 4.0);
99  Eigen::Vector3d center;
100  double radius;
101  computeShapeBoundingSphere(shape, center, radius);
102 
103  EXPECT_EQ(sqrt(1 + 4), radius);
104  EXPECT_EQ(0.0, center.x());
105  EXPECT_EQ(0.0, center.y());
106  EXPECT_EQ(0.0, center.z());
107 }
108 
109 TEST(CylBoundingSphere, Cyl2)
110 {
111  shapes::Shape* shape = new shapes::Cylinder(2.0, 20.0);
112  Eigen::Vector3d center;
113  double radius;
114  computeShapeBoundingSphere(shape, center, radius);
115 
116  EXPECT_EQ(sqrt(4 + 100), radius);
117  EXPECT_EQ(0.0, center.x());
118  EXPECT_EQ(0.0, center.y());
119  EXPECT_EQ(0.0, center.z());
120 }
121 
122 TEST(ConeBoundingSphere, Cone1)
123 {
124  shapes::Shape* shape = new shapes::Cone(20.0, 2.0);
125  Eigen::Vector3d center;
126  double radius;
127  computeShapeBoundingSphere(shape, center, radius);
128 
129  EXPECT_EQ(20.0, radius);
130  EXPECT_EQ(0.0, center.x());
131  EXPECT_EQ(0.0, center.y());
132  EXPECT_EQ(-1.0, center.z());
133 }
134 
135 TEST(ConeBoundingSphere, Cone2)
136 {
137  shapes::Shape* shape = new shapes::Cone(5.0, 5.0);
138  Eigen::Vector3d center;
139  double radius;
140  computeShapeBoundingSphere(shape, center, radius);
141 
142  EXPECT_EQ(5.0, radius);
143  EXPECT_EQ(0.0, center.x());
144  EXPECT_EQ(0.0, center.y());
145  EXPECT_EQ(-2.5, center.z());
146 }
147 
148 TEST(ConeBoundingSphere, Cone3)
149 {
150  double height = 1.0 + 1.0 / sqrt(2);
151  shapes::Shape* shape = new shapes::Cone(1 / sqrt(2), height);
152  Eigen::Vector3d center;
153  double radius;
154  computeShapeBoundingSphere(shape, center, radius);
155 
156  EXPECT_EQ(1.0, radius);
157  EXPECT_EQ(0.0, center.x());
158  EXPECT_EQ(0.0, center.y());
159  EXPECT_EQ(height / 2 - 1, center.z());
160 }
161 
162 TEST(ConeBoundingSphere, Cone4)
163 {
164  shapes::Shape* shape = new shapes::Cone(3, 10);
165  Eigen::Vector3d center;
166  double radius;
167  computeShapeBoundingSphere(shape, center, radius);
168 
169  EXPECT_EQ(109.0 / 20, radius);
170  EXPECT_EQ(0.0, center.x());
171  EXPECT_EQ(0.0, center.y());
172  EXPECT_EQ(5 - (109.0 / 20), center.z());
173 }
174 
175 TEST(MeshBoundingSphere, Mesh1)
176 {
177  shapes::Shape* shape = new shapes::Mesh(8, 12);
178  shapes::Mesh* m = dynamic_cast<shapes::Mesh*>(shape);
179  EXPECT_TRUE(m);
180 
181  // box mesh
182 
183  m->vertices[3 * 0 + 0] = 0;
184  m->vertices[3 * 0 + 1] = 0;
185  m->vertices[3 * 0 + 2] = 0;
186 
187  m->vertices[3 * 1 + 0] = 1;
188  m->vertices[3 * 1 + 1] = 0;
189  m->vertices[3 * 1 + 2] = 0;
190 
191  m->vertices[3 * 2 + 0] = 0;
192  m->vertices[3 * 2 + 1] = 1;
193  m->vertices[3 * 2 + 2] = 0;
194 
195  m->vertices[3 * 3 + 0] = 1;
196  m->vertices[3 * 3 + 1] = 1;
197  m->vertices[3 * 3 + 2] = 0;
198 
199  m->vertices[3 * 4 + 0] = 0;
200  m->vertices[3 * 4 + 1] = 0;
201  m->vertices[3 * 4 + 2] = 1;
202 
203  m->vertices[3 * 5 + 0] = 1;
204  m->vertices[3 * 5 + 1] = 0;
205  m->vertices[3 * 5 + 2] = 1;
206 
207  m->vertices[3 * 6 + 0] = 0;
208  m->vertices[3 * 6 + 1] = 1;
209  m->vertices[3 * 6 + 2] = 1;
210 
211  m->vertices[3 * 7 + 0] = 1;
212  m->vertices[3 * 7 + 1] = 1;
213  m->vertices[3 * 7 + 2] = 1;
214 
215  m->triangles[3 * 0 + 0] = 0;
216  m->triangles[3 * 0 + 1] = 1;
217  m->triangles[3 * 0 + 2] = 2;
218 
219  m->triangles[3 * 1 + 0] = 1;
220  m->triangles[3 * 1 + 1] = 3;
221  m->triangles[3 * 1 + 2] = 2;
222 
223  m->triangles[3 * 2 + 0] = 5;
224  m->triangles[3 * 2 + 1] = 4;
225  m->triangles[3 * 2 + 2] = 6;
226 
227  m->triangles[3 * 3 + 0] = 5;
228  m->triangles[3 * 3 + 1] = 6;
229  m->triangles[3 * 3 + 2] = 7;
230 
231  m->triangles[3 * 4 + 0] = 1;
232  m->triangles[3 * 4 + 1] = 5;
233  m->triangles[3 * 4 + 2] = 3;
234 
235  m->triangles[3 * 5 + 0] = 5;
236  m->triangles[3 * 5 + 1] = 7;
237  m->triangles[3 * 5 + 2] = 3;
238 
239  m->triangles[3 * 6 + 0] = 4;
240  m->triangles[3 * 6 + 1] = 0;
241  m->triangles[3 * 6 + 2] = 2;
242 
243  m->triangles[3 * 7 + 0] = 4;
244  m->triangles[3 * 7 + 1] = 2;
245  m->triangles[3 * 7 + 2] = 6;
246 
247  m->triangles[3 * 8 + 0] = 2;
248  m->triangles[3 * 8 + 1] = 3;
249  m->triangles[3 * 8 + 2] = 6;
250 
251  m->triangles[3 * 9 + 0] = 3;
252  m->triangles[3 * 9 + 1] = 7;
253  m->triangles[3 * 9 + 2] = 6;
254 
255  m->triangles[3 * 10 + 0] = 1;
256  m->triangles[3 * 10 + 1] = 0;
257  m->triangles[3 * 10 + 2] = 4;
258 
259  m->triangles[3 * 11 + 0] = 1;
260  m->triangles[3 * 11 + 1] = 4;
261  m->triangles[3 * 11 + 2] = 5;
262 
263  Eigen::Vector3d center;
264  double radius;
265  computeShapeBoundingSphere(shape, center, radius);
266 
267  EXPECT_EQ(sqrt(0.75), radius);
268  EXPECT_EQ(0.5, center.x());
269  EXPECT_EQ(0.5, center.y());
270  EXPECT_EQ(0.5, center.z());
271 }
272 
273 int main(int argc, char** argv)
274 {
275  testing::InitGoogleTest(&argc, argv);
276  return RUN_ALL_TESTS();
277 }
shapes::Cylinder
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
TEST
TEST(SphereBoundingSphere, Sphere1)
Definition: test_bounding_sphere.cpp:43
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:233
shapes::Cone
Definition of a cone Tip is on positive z axis. Center of base is on negative z axis. Origin is halway between tip and center of base.
Definition: shapes.h:152
EXPECT_EQ
#define EXPECT_EQ(a, b)
shapes::Shape
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
body_operations.h
shapes::Mesh::vertices
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
Definition: shapes.h:226
shapes::Sphere
Definition of a sphere.
Definition: shapes.h:106
main
int main(int argc, char **argv)
Definition: test_bounding_sphere.cpp:273
shapes::computeShapeBoundingSphere
void computeShapeBoundingSphere(const Shape *shape, Eigen::Vector3d &center, double &radius)
Compute a sphere bounding a shape.
Definition: shape_operations.cpp:284
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin.
Definition: shapes.h:196
shapes::Box
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:174
shape_operations.h
EXPECT_TRUE
#define EXPECT_TRUE(args)

geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Fri Jun 7 2019 21:59:29