test_point_inclusion.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
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     //    for (unsigned int i = 0; i < p.size() ; ++i)
00208     //        printf("intersection at %f, %f, %f\n", p[i].x(), p[i].y(), p[i].z());
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 }


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Thu Jun 6 2019 20:13:53