00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <geometric_shapes/bodies.h>
00038 #include <geometric_shapes/shape_operations.h>
00039 #include <geometric_shapes/body_operations.h>
00040 #include <boost/filesystem.hpp>
00041 #include <gtest/gtest.h>
00042
00043
00044 TEST(SphereBoundingSphere, Sphere1)
00045 {
00046 shapes::Sphere shape(1.0);
00047 Eigen::Vector3d center;
00048 double radius;
00049 computeShapeBoundingSphere(&shape, center, radius);
00050
00051 EXPECT_EQ(1.0, radius);
00052 EXPECT_EQ(0.0, center.x());
00053 EXPECT_EQ(0.0, center.y());
00054 EXPECT_EQ(0.0, center.z());
00055 }
00056
00057 TEST(SphereBoundingSphere, Sphere2)
00058 {
00059 shapes::Shape *shape = new shapes::Sphere(2.0);
00060 Eigen::Vector3d center;
00061 double radius;
00062 computeShapeBoundingSphere(shape, center, radius);
00063
00064 EXPECT_EQ(2.0, radius);
00065 EXPECT_EQ(0.0, center.x());
00066 EXPECT_EQ(0.0, center.y());
00067 EXPECT_EQ(0.0, center.z());
00068 delete shape;
00069 }
00070
00071 TEST(BoxBoundingSphere, Box1)
00072 {
00073 shapes::Shape *shape = new shapes::Box(2.0, 4.0, 6.0);
00074 Eigen::Vector3d center;
00075 double radius;
00076 computeShapeBoundingSphere(shape, center, radius);
00077
00078 EXPECT_EQ(sqrt(14.0), radius);
00079 EXPECT_EQ(0.0, center.x());
00080 EXPECT_EQ(0.0, center.y());
00081 EXPECT_EQ(0.0, center.z());
00082 }
00083
00084 TEST(BoxBoundingSphere, Box2)
00085 {
00086 shapes::Shape *shape = new shapes::Box(2.0, 2.0, 2.0);
00087 Eigen::Vector3d center;
00088 double radius;
00089 computeShapeBoundingSphere(shape, center, radius);
00090
00091 EXPECT_EQ(sqrt(3.0), radius);
00092 EXPECT_EQ(0.0, center.x());
00093 EXPECT_EQ(0.0, center.y());
00094 EXPECT_EQ(0.0, center.z());
00095 }
00096
00097 TEST(CylBoundingSphere, Cyl1)
00098 {
00099 shapes::Shape *shape = new shapes::Cylinder(1.0, 4.0);
00100 Eigen::Vector3d center;
00101 double radius;
00102 computeShapeBoundingSphere(shape, center, radius);
00103
00104 EXPECT_EQ(sqrt(1+4), radius);
00105 EXPECT_EQ(0.0, center.x());
00106 EXPECT_EQ(0.0, center.y());
00107 EXPECT_EQ(0.0, center.z());
00108 }
00109
00110 TEST(CylBoundingSphere, Cyl2)
00111 {
00112 shapes::Shape *shape = new shapes::Cylinder(2.0, 20.0);
00113 Eigen::Vector3d center;
00114 double radius;
00115 computeShapeBoundingSphere(shape, center, radius);
00116
00117 EXPECT_EQ(sqrt(4+100), radius);
00118 EXPECT_EQ(0.0, center.x());
00119 EXPECT_EQ(0.0, center.y());
00120 EXPECT_EQ(0.0, center.z());
00121 }
00122
00123 TEST(ConeBoundingSphere, Cone1)
00124 {
00125 shapes::Shape *shape = new shapes::Cone(20.0, 2.0);
00126 Eigen::Vector3d center;
00127 double radius;
00128 computeShapeBoundingSphere(shape, center, radius);
00129
00130 EXPECT_EQ(20.0, radius);
00131 EXPECT_EQ(0.0, center.x());
00132 EXPECT_EQ(0.0, center.y());
00133 EXPECT_EQ(-1.0, center.z());
00134 }
00135
00136 TEST(ConeBoundingSphere, Cone2)
00137 {
00138 shapes::Shape *shape = new shapes::Cone(5.0, 5.0);
00139 Eigen::Vector3d center;
00140 double radius;
00141 computeShapeBoundingSphere(shape, center, radius);
00142
00143 EXPECT_EQ(5.0, radius);
00144 EXPECT_EQ(0.0, center.x());
00145 EXPECT_EQ(0.0, center.y());
00146 EXPECT_EQ(-2.5, center.z());
00147 }
00148
00149 TEST(ConeBoundingSphere, Cone3)
00150 {
00151 double height = 1.0 + 1.0/sqrt(2);
00152 shapes::Shape *shape = new shapes::Cone(1/sqrt(2), height);
00153 Eigen::Vector3d center;
00154 double radius;
00155 computeShapeBoundingSphere(shape, center, radius);
00156
00157 EXPECT_EQ(1.0, radius);
00158 EXPECT_EQ(0.0, center.x());
00159 EXPECT_EQ(0.0, center.y());
00160 EXPECT_EQ(height/2 - 1, center.z());
00161 }
00162
00163 TEST(ConeBoundingSphere, Cone4)
00164 {
00165 shapes::Shape *shape = new shapes::Cone(3, 10);
00166 Eigen::Vector3d center;
00167 double radius;
00168 computeShapeBoundingSphere(shape, center, radius);
00169
00170 EXPECT_EQ(109.0/20, radius);
00171 EXPECT_EQ(0.0, center.x());
00172 EXPECT_EQ(0.0, center.y());
00173 EXPECT_EQ(5 - (109.0/20), center.z());
00174 }
00175
00176 TEST(MeshBoundingSphere, Mesh1)
00177 {
00178 shapes::Shape *shape = new shapes::Mesh(8, 12);
00179 shapes::Mesh *m = dynamic_cast<shapes::Mesh*>(shape);
00180 EXPECT_TRUE(m);
00181
00182
00183
00184 m->vertices[3*0 + 0] = 0;
00185 m->vertices[3*0 + 1] = 0;
00186 m->vertices[3*0 + 2] = 0;
00187
00188 m->vertices[3*1 + 0] = 1;
00189 m->vertices[3*1 + 1] = 0;
00190 m->vertices[3*1 + 2] = 0;
00191
00192 m->vertices[3*2 + 0] = 0;
00193 m->vertices[3*2 + 1] = 1;
00194 m->vertices[3*2 + 2] = 0;
00195
00196 m->vertices[3*3 + 0] = 1;
00197 m->vertices[3*3 + 1] = 1;
00198 m->vertices[3*3 + 2] = 0;
00199
00200 m->vertices[3*4 + 0] = 0;
00201 m->vertices[3*4 + 1] = 0;
00202 m->vertices[3*4 + 2] = 1;
00203
00204 m->vertices[3*5 + 0] = 1;
00205 m->vertices[3*5 + 1] = 0;
00206 m->vertices[3*5 + 2] = 1;
00207
00208 m->vertices[3*6 + 0] = 0;
00209 m->vertices[3*6 + 1] = 1;
00210 m->vertices[3*6 + 2] = 1;
00211
00212 m->vertices[3*7 + 0] = 1;
00213 m->vertices[3*7 + 1] = 1;
00214 m->vertices[3*7 + 2] = 1;
00215
00216
00217 m->triangles[3*0 + 0] = 0;
00218 m->triangles[3*0 + 1] = 1;
00219 m->triangles[3*0 + 2] = 2;
00220
00221 m->triangles[3*1 + 0] = 1;
00222 m->triangles[3*1 + 1] = 3;
00223 m->triangles[3*1 + 2] = 2;
00224
00225 m->triangles[3*2 + 0] = 5;
00226 m->triangles[3*2 + 1] = 4;
00227 m->triangles[3*2 + 2] = 6;
00228
00229 m->triangles[3*3 + 0] = 5;
00230 m->triangles[3*3 + 1] = 6;
00231 m->triangles[3*3 + 2] = 7;
00232
00233 m->triangles[3*4 + 0] = 1;
00234 m->triangles[3*4 + 1] = 5;
00235 m->triangles[3*4 + 2] = 3;
00236
00237 m->triangles[3*5 + 0] = 5;
00238 m->triangles[3*5 + 1] = 7;
00239 m->triangles[3*5 + 2] = 3;
00240
00241 m->triangles[3*6 + 0] = 4;
00242 m->triangles[3*6 + 1] = 0;
00243 m->triangles[3*6 + 2] = 2;
00244
00245 m->triangles[3*7 + 0] = 4;
00246 m->triangles[3*7 + 1] = 2;
00247 m->triangles[3*7 + 2] = 6;
00248
00249 m->triangles[3*8 + 0] = 2;
00250 m->triangles[3*8 + 1] = 3;
00251 m->triangles[3*8 + 2] = 6;
00252
00253 m->triangles[3*9 + 0] = 3;
00254 m->triangles[3*9 + 1] = 7;
00255 m->triangles[3*9 + 2] = 6;
00256
00257 m->triangles[3*10 + 0] = 1;
00258 m->triangles[3*10 + 1] = 0;
00259 m->triangles[3*10 + 2] = 4;
00260
00261 m->triangles[3*11 + 0] = 1;
00262 m->triangles[3*11 + 1] = 4;
00263 m->triangles[3*11 + 2] = 5;
00264
00265
00266 Eigen::Vector3d center;
00267 double radius;
00268 computeShapeBoundingSphere(shape, center, radius);
00269
00270 EXPECT_EQ(sqrt(0.75), radius);
00271 EXPECT_EQ(0.5, center.x());
00272 EXPECT_EQ(0.5, center.y());
00273 EXPECT_EQ(0.5, center.z());
00274 }
00275
00276 int main(int argc, char **argv)
00277 {
00278 testing::InitGoogleTest(&argc, argv);
00279 return RUN_ALL_TESTS();
00280 }