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  EXPECT_EQ(shape->type, shapes::MESH);
179  shapes::Mesh* m = static_cast<shapes::Mesh*>(shape);
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::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
shapes::Cylinder
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
shapes::Shape
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin....
Definition: shapes.h:281
body_operations.h
shapes::MESH
@ MESH
Definition: shapes.h:69
shapes::Box
Definition of a box Aligned with the XYZ axes.
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::Shape::type
ShapeType type
The type of the shape.
Definition: shapes.h:102
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
shape_operations.h
bodies.h
TEST
TEST(SphereBoundingSphere, Sphere1)
Definition: test_bounding_sphere.cpp:43
shapes::computeShapeBoundingSphere
void computeShapeBoundingSphere(const Shape *shape, Eigen::Vector3d &center, double &radius)
Compute a sphere bounding a shape.
Definition: shape_operations.cpp:312
shapes::Cone
Definition of a cone Tip is on positive z axis. Center of base is on negative z axis....
Definition: shapes.h:178
EXPECT_EQ
#define EXPECT_EQ(a, b)

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