test_loaded_meshes.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2016, Jorge Santos
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 
00051 class CompareMeshVsPrimitive : public ::testing::Test
00052 {
00053 public:
00054   CompareMeshVsPrimitive()
00055   {
00056   }
00057 
00058   void SetUp()
00059   {
00060     // BOX
00061     shapes::Box box(1.0, 1.0, 1.0);
00062     shape_meshes.push_back(shapes::createMeshFromShape(&box));
00063     loaded_meshes.push_back(shapes::createMeshFromResource(
00064         "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/cube.stl").string()));
00065 
00066     shape_convex_meshes.push_back(new bodies::ConvexMesh(shape_meshes.back()));
00067     loaded_convex_meshes.push_back(new bodies::ConvexMesh(loaded_meshes.back()));
00068   }
00069 
00070   void TearDown()
00071   {
00072     for (int i = 0; i < shape_meshes.size(); ++i)
00073     {
00074       delete shape_meshes[i];
00075       delete loaded_meshes[i];
00076 
00077       delete shape_convex_meshes[i];
00078       delete loaded_convex_meshes[i];
00079     }
00080   }
00081 
00082   ~CompareMeshVsPrimitive()
00083   {
00084   }
00085 
00086 protected:
00087   random_numbers::RandomNumberGenerator rng;
00088 
00089   std::vector<shapes::Mesh*> shape_meshes;
00090   std::vector<shapes::Mesh*> loaded_meshes;
00091 
00092   std::vector<bodies::Body*> shape_convex_meshes;
00093   std::vector<bodies::Body*> loaded_convex_meshes;
00094 };
00095 
00096 TEST_F(CompareMeshVsPrimitive, ContainsPoint)
00097 {
00098   // Any point inside a mesh must be inside the other too
00099   for (int i = 0; i < shape_meshes.size(); ++i)
00100   {
00101     shapes::Mesh *shape_ms = shape_meshes[i];
00102     shapes::Mesh *loaded_ms = loaded_meshes[i];
00103 
00104     bodies::Body *shape_cms = shape_convex_meshes[i];
00105     bodies::Body *loaded_cms = loaded_convex_meshes[i];
00106 
00107     Eigen::Vector3d p;
00108     bool found = false;
00109     for (int i = 0; i < 100; ++i)
00110     {
00111       if ((shape_cms->samplePointInside(rng, 10000, p)) ||
00112           (loaded_cms->samplePointInside(rng, 10000, p)))
00113       {
00114         found = true;
00115         EXPECT_EQ(shape_cms->containsPoint(p), loaded_cms->containsPoint(p));
00116       }
00117     }
00118     EXPECT_TRUE(found) << "No point inside the meshes was found (very unlikely)";
00119   }
00120 }
00121 
00122 TEST_F(CompareMeshVsPrimitive, IntersectsRay)
00123 {
00124   // Random rays must intersect both meshes nearly at the same points
00125   for (int i = 0; i < shape_meshes.size(); ++i)
00126   {
00127     shapes::Mesh *shape_ms = shape_meshes[i];
00128     shapes::Mesh *loaded_ms = loaded_meshes[i];
00129 
00130     bodies::Body *shape_cms = shape_convex_meshes[i];
00131     bodies::Body *loaded_cms = loaded_convex_meshes[i];
00132 
00133     bool intersects = false;
00134     for (int i = 0; i < 100; ++i)
00135     {
00136       Eigen::Vector3d ray_o(rng.uniformReal(-1.0, +1.0),
00137                             rng.uniformReal(-1.0, +1.0),
00138                             rng.uniformReal(-1.0, +1.0));
00139       Eigen::Vector3d ray_d(rng.uniformReal(-1.0, +1.0),
00140                             rng.uniformReal(-1.0, +1.0),
00141                             rng.uniformReal(-1.0, +1.0));
00142       EigenSTL::vector_Vector3d vi1, vi2;
00143       shape_cms->intersectsRay(ray_o, ray_d, &vi1);
00144       loaded_cms->intersectsRay(ray_o, ray_d, &vi2);
00145 
00146   // DEBUG printing
00147   //    if (vi1.size() != vi2.size() && vi1.size() > 0 && vi2.size() > 0)
00148   //    {
00149   //        std::cout << vi1.size() << "   " << vi2.size() << "\n";
00150   //        std::cout << ray_o.x() << "  "<< ray_o.y() << "  "<< ray_o.z()
00151   //          << "\n" << ray_d.x() << "  "<< ray_d.y() << "  "<< ray_d.z() << "\n";
00152   //    }
00153 
00154       EXPECT_EQ(vi1.size(), vi2.size());
00155       if (vi1.size() > 0 && vi2.size() > 0)
00156       {
00157         EXPECT_NEAR(vi1[0].x(), vi2[0].x(), 0.01);
00158         EXPECT_NEAR(vi1[0].y(), vi2[0].y(), 0.01);
00159         EXPECT_NEAR(vi1[0].z(), vi2[0].z(), 0.01);
00160 
00161         intersects = true;
00162       }
00163     }
00164 
00165     EXPECT_TRUE(intersects) << "No ray intersects the meshes (very unlikely)";
00166   }
00167 }
00168 
00169 TEST_F(CompareMeshVsPrimitive, BoundingSphere)
00170 {
00171   // Bounding spheres must be nearly identical
00172   for (int i = 0; i < shape_meshes.size(); ++i)
00173   {
00174     shapes::Mesh *shape_ms = shape_meshes[i];
00175     shapes::Mesh *loaded_ms = loaded_meshes[i];
00176 
00177     bodies::Body *shape_cms = shape_convex_meshes[i];
00178     bodies::Body *loaded_cms = loaded_convex_meshes[i];
00179 
00180     shapes::Sphere shape(1.0);
00181     Eigen::Vector3d center1, center2;
00182     double radius1, radius2;
00183     computeShapeBoundingSphere(shape_ms, center1, radius1);
00184     computeShapeBoundingSphere(loaded_ms, center2, radius2);
00185 
00186     EXPECT_NEAR(radius1,     radius2,     0.001);
00187     EXPECT_NEAR(center1.x(), center2.x(), 0.001);
00188     EXPECT_NEAR(center1.y(), center2.y(), 0.001);
00189     EXPECT_NEAR(center1.z(), center2.z(), 0.001);
00190   }
00191 }
00192 
00193 TEST_F(CompareMeshVsPrimitive, BoxVertexCount)
00194 {
00195   // For a simple shape as a cube, we expect that both meshes have the same number of vertex and triangles
00196   // But that was not the case before fixing issue #38!
00197   // These tests don't apply to curve shapes because the number of elements depends on how smooth they where
00198   // created. So ensure that "back()" gives a pointer to box meshes!
00199   EXPECT_EQ(shape_meshes.back()->vertex_count, loaded_meshes.back()->vertex_count);
00200 }
00201 
00202 TEST_F(CompareMeshVsPrimitive, BoxTriangleCount)
00203 {
00204   EXPECT_EQ(shape_meshes.back()->triangle_count, loaded_meshes.back()->triangle_count);
00205 }
00206 
00207 
00208 int main(int argc, char **argv)
00209 {
00210   testing::InitGoogleTest(&argc, argv);
00211   return RUN_ALL_TESTS();
00212 }


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