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 #include "resources/config.h"
00043
00044 TEST(SpherePointContainment, SimpleInside)
00045 {
00046 shapes::Sphere shape(1.0);
00047 bodies::Body* sphere = new bodies::Sphere(&shape);
00048 sphere->setScale(1.05);
00049 bool contains = sphere->containsPoint(0,0,1.0);
00050 random_numbers::RandomNumberGenerator r;
00051 Eigen::Vector3d p;
00052 EXPECT_TRUE(sphere->samplePointInside(r, 100, p));
00053 EXPECT_TRUE(sphere->containsPoint(p));
00054 EXPECT_TRUE(contains);
00055 delete sphere;
00056 }
00057
00058 TEST(SpherePointContainment, SimpleOutside)
00059 {
00060 shapes::Sphere shape(1.0);
00061 bodies::Body* sphere = new bodies::Sphere(&shape);
00062 sphere->setScale(0.95);
00063 bool contains = sphere->containsPoint(0,0,1.0);
00064 delete sphere;
00065 EXPECT_FALSE(contains);
00066 }
00067
00068 TEST(SpherePointContainment, ComplexInside)
00069 {
00070 shapes::Sphere shape(1.0);
00071 bodies::Body* sphere = new bodies::Sphere(&shape);
00072 sphere->setScale(0.95);
00073 Eigen::Affine3d pose;
00074 pose.setIdentity();
00075 pose.translation() = Eigen::Vector3d(1.0,1.0,1.0);
00076 sphere->setPose(pose);
00077 bool contains = sphere->containsPoint(0.5,1,1.0);
00078 delete sphere;
00079 EXPECT_TRUE(contains);
00080 }
00081
00082 TEST(SpherePointContainment, ComplexOutside)
00083 {
00084 shapes::Sphere shape(1.0);
00085 bodies::Body* sphere = new bodies::Sphere(&shape);
00086 sphere->setScale(0.95);
00087 Eigen::Affine3d pose;
00088 pose.setIdentity();
00089 pose.translation() = Eigen::Vector3d(1.0,1.0,1.0);
00090 sphere->setPose(pose);
00091 bool contains = sphere->containsPoint(0.5,0.0,0.0);
00092 delete sphere;
00093 EXPECT_FALSE(contains);
00094 }
00095
00096 TEST(SphereRayIntersection, SimpleRay1)
00097 {
00098 shapes::Sphere shape(1.0);
00099 bodies::Body* sphere = new bodies::Sphere(&shape);
00100 sphere->setScale(1.05);
00101
00102 Eigen::Vector3d ray_o(5, 0, 0);
00103 Eigen::Vector3d ray_d(-1, 0, 0);
00104 EigenSTL::vector_Vector3d p;
00105 bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
00106
00107 delete sphere;
00108 EXPECT_TRUE(intersect);
00109 EXPECT_EQ(2, (int)p.size());
00110 EXPECT_NEAR(p[0].x(), 1.05, 1e-6);
00111 EXPECT_NEAR(p[1].x(), -1.05, 1e-6);
00112 }
00113
00114 TEST(SphereRayIntersection, SimpleRay2)
00115 {
00116 shapes::Sphere shape(1.0);
00117 bodies::Body* sphere = new bodies::Sphere(&shape);
00118 sphere->setScale(1.05);
00119
00120 Eigen::Vector3d ray_o(5, 0, 0);
00121 Eigen::Vector3d ray_d(1, 0, 0);
00122 EigenSTL::vector_Vector3d p;
00123 bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
00124
00125 delete sphere;
00126 EXPECT_FALSE(intersect);
00127 EXPECT_EQ(0, (int)p.size());
00128 }
00129
00130 TEST(BoxPointContainment, SimpleInside)
00131 {
00132 shapes::Box shape(1.0, 2.0, 3.0);
00133 bodies::Body* box = new bodies::Box(&shape);
00134 box->setScale(0.95);
00135 bool contains = box->containsPoint(0,0,1.0);
00136 EXPECT_TRUE(contains);
00137
00138 random_numbers::RandomNumberGenerator r;
00139 Eigen::Vector3d p;
00140 EXPECT_TRUE(box->samplePointInside(r, 100, p));
00141 EXPECT_TRUE(box->containsPoint(p));
00142
00143 delete box;
00144 }
00145
00146
00147 TEST(BoxPointContainment, SimpleOutside)
00148 {
00149 shapes::Box shape(1.0, 2.0, 3.0);
00150 bodies::Body* box = new bodies::Box(&shape);
00151 box->setScale(0.95);
00152 bool contains = box->containsPoint(0,0,3.0);
00153 delete box;
00154 EXPECT_FALSE(contains);
00155 }
00156
00157
00158 TEST(BoxPointContainment, ComplexInside)
00159 {
00160 shapes::Box shape(1.0, 1.0, 1.0);
00161 bodies::Body* box = new bodies::Box(&shape);
00162 box->setScale(1.01);
00163 Eigen::Affine3d pose(Eigen::AngleAxisd(M_PI/3.0, Eigen::Vector3d::UnitX()));
00164 pose.translation() = Eigen::Vector3d(1.0,1.0,1.0);
00165 box->setPose(pose);
00166
00167 bool contains = box->containsPoint(1.5,1.0,1.5);
00168 EXPECT_TRUE(contains);
00169
00170 random_numbers::RandomNumberGenerator r;
00171 Eigen::Vector3d p;
00172 for (int i = 0 ; i < 100 ; ++i)
00173 {
00174 EXPECT_TRUE(box->samplePointInside(r, 100, p));
00175 EXPECT_TRUE(box->containsPoint(p));
00176 }
00177
00178 delete box;
00179 }
00180
00181 TEST(BoxPointContainment, ComplexOutside)
00182 {
00183 shapes::Box shape(1.0, 1.0, 1.0);
00184 bodies::Body* box = new bodies::Box(&shape);
00185 box->setScale(1.01);
00186 Eigen::Affine3d pose(Eigen::AngleAxisd(M_PI/3.0, Eigen::Vector3d::UnitX()));
00187 pose.translation() = Eigen::Vector3d(1.0,1.0,1.0);
00188 box->setPose(pose);
00189
00190 bool contains = box->containsPoint(1.5,1.5,1.5);
00191 delete box;
00192 EXPECT_FALSE(contains);
00193 }
00194
00195 TEST(BoxRayIntersection, SimpleRay1)
00196 {
00197 shapes::Box shape(1.0, 1.0, 3.0);
00198 bodies::Body* box = new bodies::Box(&shape);
00199 box->setScale(0.95);
00200
00201 Eigen::Vector3d ray_o(10, 0.449, 0);
00202 Eigen::Vector3d ray_d(-1, 0, 0);
00203 EigenSTL::vector_Vector3d p;
00204
00205 bool intersect = box->intersectsRay(ray_o, ray_d, &p);
00206
00207
00208
00209
00210 delete box;
00211 EXPECT_TRUE(intersect);
00212 }
00213
00214
00215 TEST(CylinderPointContainment, SimpleInside)
00216 {
00217 shapes::Cylinder shape(1.0, 4.0);
00218 bodies::Body* cylinder = new bodies::Cylinder(&shape);
00219 cylinder->setScale(1.05);
00220 bool contains = cylinder->containsPoint(0, 0, 2.0);
00221 delete cylinder;
00222 EXPECT_TRUE(contains);
00223 }
00224
00225
00226 TEST(CylinderPointContainment, SimpleOutside)
00227 {
00228 shapes::Cylinder shape(1.0, 4.0);
00229 bodies::Body* cylinder = new bodies::Cylinder(&shape);
00230 cylinder->setScale(0.95);
00231 bool contains = cylinder->containsPoint(0,0,2.0);
00232 delete cylinder;
00233 EXPECT_FALSE(contains);
00234 }
00235
00236 TEST(CylinderPointContainment, CylinderPadding)
00237 {
00238 shapes::Cylinder shape(1.0, 4.0);
00239 bodies::Body* cylinder = new bodies::Cylinder(&shape);
00240 bool contains = cylinder->containsPoint(0,1.01,0);
00241 EXPECT_FALSE(contains);
00242 cylinder->setPadding(.02);
00243 contains = cylinder->containsPoint(0,1.01,0);
00244 EXPECT_TRUE(contains);
00245 cylinder->setPadding(0.0);
00246 bodies::BoundingSphere bsphere;
00247 cylinder->computeBoundingSphere(bsphere);
00248 EXPECT_TRUE(bsphere.radius > 2.0);
00249
00250 random_numbers::RandomNumberGenerator r;
00251 Eigen::Vector3d p;
00252 for (int i = 0 ; i < 1000 ; ++i)
00253 {
00254 EXPECT_TRUE(cylinder->samplePointInside(r, 100, p));
00255 EXPECT_TRUE(cylinder->containsPoint(p));
00256 }
00257 delete cylinder;
00258 }
00259
00260 TEST(MeshPointContainment, Pr2Forearm)
00261 {
00262 shapes::Mesh *ms = shapes::createMeshFromResource("file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/forearm_roll.stl").string());
00263 ASSERT_TRUE(ms != NULL);
00264 bodies::Body *m = new bodies::ConvexMesh(ms);
00265 ASSERT_TRUE(m != NULL);
00266 Eigen::Affine3d t(Eigen::Affine3d::Identity());
00267 t.translation().x() = 1.0;
00268 EXPECT_FALSE(m->cloneAt(t)->containsPoint(-1.0, 0.0, 0.0));
00269
00270 random_numbers::RandomNumberGenerator r;
00271 Eigen::Vector3d p;
00272 bool found = true;
00273 for (int i = 0 ; i < 10 ; ++i) {
00274 if (m->samplePointInside(r, 10000, p))
00275 {
00276 found = true;
00277 EXPECT_TRUE(m->containsPoint(p));
00278 }
00279 }
00280 EXPECT_TRUE(found);
00281
00282 delete m;
00283 delete ms;
00284 }
00285
00286 TEST(MergeBoundingSpheres, MergeTwoSpheres)
00287 {
00288 std::vector<bodies::BoundingSphere> spheres;
00289
00290 bodies::BoundingSphere s1;
00291 s1.center = Eigen::Vector3d(5.0, 0.0, 0.0);
00292 s1.radius = 1.0;
00293
00294 bodies::BoundingSphere s2;
00295 s2.center = Eigen::Vector3d(-5.1, 0.0, 0.0);
00296 s2.radius = 1.0;
00297
00298 spheres.push_back(s1);
00299 spheres.push_back(s2);
00300
00301 bodies::BoundingSphere merged_sphere;
00302 bodies::mergeBoundingSpheres(spheres, merged_sphere);
00303
00304 EXPECT_NEAR(merged_sphere.center[0], -.05, .00001);
00305 EXPECT_EQ(merged_sphere.radius, 6.05);
00306 }
00307
00308 int main(int argc, char **argv)
00309 {
00310 testing::InitGoogleTest(&argc, argv);
00311 return RUN_ALL_TESTS();
00312 }