geometric_shapes: test_point_inclusion.cpp Source 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 <gtest/gtest.h>
00039 
00040 TEST(SpherePointContainment, SimpleInside)
00041 {
00042     shapes::Sphere shape(1.0);
00043     bodies::Body* sphere = new bodies::Sphere(&shape);
00044     sphere->setScale(1.05);
00045     bool contains = sphere->containsPoint(0,0,1.0);
00046     delete sphere;
00047     EXPECT_TRUE(contains);
00048 }
00049 
00050 TEST(SpherePointContainment, SimpleOutside)
00051 { 
00052     shapes::Sphere shape(1.0);
00053     bodies::Body* sphere = new bodies::Sphere(&shape);
00054     sphere->setScale(0.95);
00055     bool contains = sphere->containsPoint(0,0,1.0);
00056     delete sphere;
00057     EXPECT_FALSE(contains);
00058 }
00059 
00060 TEST(SpherePointContainment, ComplexInside)
00061 { 
00062     shapes::Sphere shape(1.0);
00063     bodies::Body* sphere = new bodies::Sphere(&shape);
00064     sphere->setScale(0.95);
00065     btTransform pose;
00066     pose.setIdentity();    
00067     pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
00068     sphere->setPose(pose);
00069     bool contains = sphere->containsPoint(0.5,1,1.0);
00070     delete sphere;
00071     EXPECT_TRUE(contains);
00072 }
00073 
00074 TEST(SpherePointContainment, ComplexOutside)
00075 {   
00076     shapes::Sphere shape(1.0);
00077     bodies::Body* sphere = new bodies::Sphere(&shape);
00078     sphere->setScale(0.95);
00079     btTransform pose;
00080     pose.setIdentity();    
00081     pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
00082     sphere->setPose(pose);
00083     bool contains = sphere->containsPoint(0.5,0.0,0.0);
00084     delete sphere;
00085     EXPECT_FALSE(contains);
00086 }
00087 
00088 TEST(SphereRayIntersection, SimpleRay1)
00089 {
00090     shapes::Sphere shape(1.0);
00091     bodies::Body* sphere = new bodies::Sphere(&shape);
00092     sphere->setScale(1.05);
00093 
00094     btVector3 ray_o(5, 0, 0);
00095     btVector3 ray_d(-1, 0, 0);
00096     std::vector<btVector3> p;
00097     bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
00098     
00099     delete sphere;
00100     EXPECT_TRUE(intersect);
00101     EXPECT_EQ(2, (int)p.size());
00102     EXPECT_NEAR(p[0].x(), 1.05, 1e-12);
00103     EXPECT_NEAR(p[1].x(), -1.05, 1e-12);
00104 }
00105 
00106 TEST(SphereRayIntersection, SimpleRay2)
00107 {
00108     shapes::Sphere shape(1.0);
00109     bodies::Body* sphere = new bodies::Sphere(&shape);
00110     sphere->setScale(1.05);
00111 
00112     btVector3 ray_o(5, 0, 0);
00113     btVector3 ray_d(1, 0, 0);
00114     std::vector<btVector3> p;
00115     bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
00116     
00117     delete sphere;
00118     EXPECT_FALSE(intersect);
00119     EXPECT_EQ(0, (int)p.size());
00120 }
00121 
00122 TEST(BoxPointContainment, SimpleInside)
00123 {    
00124     shapes::Box shape(1.0, 2.0, 3.0);
00125     bodies::Body* box = new bodies::Box(&shape);
00126     box->setScale(0.95);
00127     bool contains = box->containsPoint(0,0,1.0);
00128     delete box;
00129     EXPECT_TRUE(contains);
00130 }
00131 
00132 
00133 TEST(BoxPointContainment, SimpleOutside)
00134 {
00135     shapes::Box shape(1.0, 2.0, 3.0);
00136     bodies::Body* box = new bodies::Box(&shape);
00137     box->setScale(0.95);
00138     bool contains = box->containsPoint(0,0,3.0);
00139     delete box;
00140     EXPECT_FALSE(contains);
00141 }
00142 
00143 
00144 TEST(BoxPointContainment, ComplexInside)
00145 {  
00146     shapes::Box shape(1.0, 1.0, 1.0);
00147     bodies::Body* box = new bodies::Box(&shape);
00148     box->setScale(1.01);
00149     btTransform pose;
00150     pose.setIdentity();    
00151     pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
00152     btQuaternion quat(btVector3(btScalar(1), btScalar(0), btScalar(0)), M_PI/3.0);
00153     pose.setRotation(quat);
00154     box->setPose(pose);    
00155 
00156     bool contains = box->containsPoint(1.5,1.0,1.5);
00157     delete box;
00158     EXPECT_TRUE(contains);
00159 }
00160 
00161 TEST(BoxPointContainment, ComplexOutside)
00162 {    
00163     shapes::Box shape(1.0, 1.0, 1.0);
00164     bodies::Body* box = new bodies::Box(&shape);
00165     box->setScale(1.01);
00166     btTransform pose;
00167     pose.setIdentity();    
00168     pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
00169     btQuaternion quat(btVector3(btScalar(1), btScalar(0), btScalar(0)), M_PI/3.0);
00170     pose.setRotation(quat);
00171     box->setPose(pose);    
00172 
00173     bool contains = box->containsPoint(1.5,1.5,1.5);
00174     delete box;
00175     EXPECT_FALSE(contains);
00176 }
00177 
00178 TEST(BoxRayIntersection, SimpleRay1)
00179 {
00180     shapes::Box shape(1.0, 1.0, 3.0);
00181     bodies::Body* box = new bodies::Box(&shape);
00182     box->setScale(0.95);
00183     
00184     btVector3 ray_o(10, 0.449, 0);
00185     btVector3 ray_d(-1, 0, 0);
00186     std::vector<btVector3> p;
00187     
00188     bool intersect = box->intersectsRay(ray_o, ray_d, &p);
00189     
00190     //    for (unsigned int i = 0; i < p.size() ; ++i)
00191     //  printf("intersection at %f, %f, %f\n", p[i].x(), p[i].y(), p[i].z());
00192     
00193     delete box;
00194     EXPECT_TRUE(intersect);
00195 }
00196 
00197 
00198 TEST(CylinderPointContainment, SimpleInside)
00199 {
00200     shapes::Cylinder shape(1.0, 4.0);
00201     bodies::Body* cylinder = new bodies::Cylinder(&shape);
00202     cylinder->setScale(1.05);
00203     bool contains = cylinder->containsPoint(0, 0, 2.0);
00204     delete cylinder;
00205     EXPECT_TRUE(contains);
00206 }
00207 
00208 
00209 TEST(CylinderPointContainment, SimpleOutside)
00210 {
00211     shapes::Cylinder shape(1.0, 4.0);
00212     bodies::Body* cylinder = new bodies::Cylinder(&shape);
00213     cylinder->setScale(0.95);
00214     bool contains = cylinder->containsPoint(0,0,2.0);
00215     delete cylinder;
00216     EXPECT_FALSE(contains);
00217 }
00218 
00219 TEST(CylinderPointContainment, CylinderPadding)
00220 {
00221   shapes::Cylinder shape(1.0, 4.0);
00222   bodies::Body* cylinder = new bodies::Cylinder(&shape);
00223   bool contains = cylinder->containsPoint(0,1.01,0);
00224   EXPECT_FALSE(contains);
00225   cylinder->setPadding(.02);
00226   contains = cylinder->containsPoint(0,1.01,0);
00227   EXPECT_TRUE(contains);
00228   cylinder->setPadding(0.0);
00229   bodies::BoundingSphere bsphere;
00230   cylinder->computeBoundingSphere(bsphere);
00231   EXPECT_TRUE(bsphere.radius > 2.0);
00232 }
00233 
00234 int main(int argc, char **argv)
00235 { 
00236     testing::InitGoogleTest(&argc, argv);
00237     return RUN_ALL_TESTS();
00238 }