test_loaded_meshes.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, Jorge Santos
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
40 #include <boost/filesystem.hpp>
41 #include <gtest/gtest.h>
42 #include "resources/config.h"
43 
51 {
52 public:
54  {
55  }
56 
57  void SetUp() override
58  {
59  // BOX
60  shapes::Box box(1.0, 1.0, 1.0);
63  "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/cube.stl").string()));
64 
67  }
68 
69  void TearDown() override
70  {
71  for (size_t i = 0; i < shape_meshes.size(); ++i)
72  {
73  delete shape_meshes[i];
74  delete loaded_meshes[i];
75 
76  delete shape_convex_meshes[i];
77  delete loaded_convex_meshes[i];
78  }
79  }
80 
81  ~CompareMeshVsPrimitive() override
82  {
83  }
84 
85 protected:
87 
88  std::vector<shapes::Mesh*> shape_meshes;
89  std::vector<shapes::Mesh*> loaded_meshes;
90 
91  std::vector<bodies::Body*> shape_convex_meshes;
92  std::vector<bodies::Body*> loaded_convex_meshes;
93 };
94 
96 {
97  // Any point inside a mesh must be inside the other too
98  for (size_t i = 0; i < shape_meshes.size(); ++i)
99  {
100  bodies::Body* shape_cms = shape_convex_meshes[i];
101  bodies::Body* loaded_cms = loaded_convex_meshes[i];
102 
103  Eigen::Vector3d p;
104  bool found = false;
105  for (int i = 0; i < 100; ++i)
106  {
107  if ((shape_cms->samplePointInside(rng, 10000, p)) || (loaded_cms->samplePointInside(rng, 10000, p)))
108  {
109  found = true;
110  EXPECT_EQ(shape_cms->containsPoint(p), loaded_cms->containsPoint(p));
111  }
112  }
113  EXPECT_TRUE(found) << "No point inside the meshes was found (very unlikely)";
114  }
115 }
116 
118 {
119  // Random rays must intersect both meshes nearly at the same points
120  for (size_t i = 0; i < shape_meshes.size(); ++i)
121  {
122  bodies::Body* shape_cms = shape_convex_meshes[i];
123  bodies::Body* loaded_cms = loaded_convex_meshes[i];
124 
125  bool intersects = false;
126  for (int i = 0; i < 100; ++i)
127  {
128  Eigen::Vector3d ray_o(rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0));
129  Eigen::Vector3d ray_d(rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0));
130  EigenSTL::vector_Vector3d vi1, vi2;
131  shape_cms->intersectsRay(ray_o, ray_d, &vi1);
132  loaded_cms->intersectsRay(ray_o, ray_d, &vi2);
133 
134  // DEBUG printing
135  // if (vi1.size() != vi2.size() && vi1.size() > 0 && vi2.size() > 0)
136  // {
137  // std::cout << vi1.size() << " " << vi2.size() << "\n";
138  // std::cout << ray_o.x() << " "<< ray_o.y() << " "<< ray_o.z()
139  // << "\n" << ray_d.x() << " "<< ray_d.y() << " "<< ray_d.z() << "\n";
140  // }
141 
142  EXPECT_EQ(vi1.size(), vi2.size());
143  if (!vi1.empty() && !vi2.empty())
144  {
145  EXPECT_NEAR(vi1[0].x(), vi2[0].x(), 0.01);
146  EXPECT_NEAR(vi1[0].y(), vi2[0].y(), 0.01);
147  EXPECT_NEAR(vi1[0].z(), vi2[0].z(), 0.01);
148 
149  intersects = true;
150  }
151  }
152 
153  EXPECT_TRUE(intersects) << "No ray intersects the meshes (very unlikely)";
154  }
155 }
156 
158 {
159  // Bounding spheres must be nearly identical
160  for (size_t i = 0; i < shape_meshes.size(); ++i)
161  {
162  shapes::Mesh* shape_ms = shape_meshes[i];
163  shapes::Mesh* loaded_ms = loaded_meshes[i];
164 
165  shapes::Sphere shape(1.0);
166  Eigen::Vector3d center1, center2;
167  double radius1, radius2;
168  computeShapeBoundingSphere(shape_ms, center1, radius1);
169  computeShapeBoundingSphere(loaded_ms, center2, radius2);
170 
171  EXPECT_NEAR(radius1, radius2, 0.001);
172  EXPECT_NEAR(center1.x(), center2.x(), 0.001);
173  EXPECT_NEAR(center1.y(), center2.y(), 0.001);
174  EXPECT_NEAR(center1.z(), center2.z(), 0.001);
175  }
176 }
177 
179 {
180  // For a simple shape as a cube, we expect that both meshes have the same number of vertex and triangles
181  // But that was not the case before fixing issue #38!
182  // These tests don't apply to curve shapes because the number of elements depends on how smooth they where
183  // created. So ensure that "back()" gives a pointer to box meshes!
184  EXPECT_EQ(shape_meshes.back()->vertex_count, loaded_meshes.back()->vertex_count);
185 }
186 
187 TEST_F(CompareMeshVsPrimitive, BoxTriangleCount)
188 {
189  EXPECT_EQ(shape_meshes.back()->triangle_count, loaded_meshes.back()->triangle_count);
190 }
191 
192 int main(int argc, char** argv)
193 {
194  testing::InitGoogleTest(&argc, argv);
195  return RUN_ALL_TESTS();
196 }
CompareMeshVsPrimitive::SetUp
void SetUp() override
Definition: test_loaded_meshes.cpp:89
CompareMeshVsPrimitive::shape_meshes
std::vector< shapes::Mesh * > shape_meshes
Definition: test_loaded_meshes.cpp:120
TEST_F
TEST_F(CompareMeshVsPrimitive, ContainsPoint)
Definition: test_loaded_meshes.cpp:95
bodies::Body::intersectsRay
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const =0
Check if a ray intersects the body, and find the set of intersections, in order, along the ray....
CompareMeshVsPrimitive::CompareMeshVsPrimitive
CompareMeshVsPrimitive()
Definition: test_loaded_meshes.cpp:85
CompareMeshVsPrimitive::rng
random_numbers::RandomNumberGenerator rng
Definition: test_loaded_meshes.cpp:118
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
bodies::Body::containsPoint
bool containsPoint(double x, double y, double z, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.h:244
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin....
Definition: shapes.h:281
testing::Test
bodies::Body
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:121
EXPECT_TRUE
#define EXPECT_TRUE(condition)
body_operations.h
main
int main(int argc, char **argv)
Definition: test_loaded_meshes.cpp:192
CompareMeshVsPrimitive::loaded_convex_meshes
std::vector< bodies::Body * > loaded_convex_meshes
Definition: test_loaded_meshes.cpp:124
bodies::Body::samplePointInside
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:129
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
CompareMeshVsPrimitive::shape_convex_meshes
std::vector< bodies::Body * > shape_convex_meshes
Definition: test_loaded_meshes.cpp:123
shapes::Box
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
CompareMeshVsPrimitive::loaded_meshes
std::vector< shapes::Mesh * > loaded_meshes
Definition: test_loaded_meshes.cpp:121
shapes::Sphere
Definition of a sphere.
Definition: shapes.h:106
y
double y
Definition: mesh_operations.cpp:202
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
CompareMeshVsPrimitive::~CompareMeshVsPrimitive
~CompareMeshVsPrimitive() override
Definition: test_loaded_meshes.cpp:113
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
Load a mesh from a resource that contains a mesh that can be loaded by assimp.
Definition: mesh_operations.cpp:234
CompareMeshVsPrimitive
Definition: test_loaded_meshes.cpp:50
CompareMeshVsPrimitive::TearDown
void TearDown() override
Definition: test_loaded_meshes.cpp:101
random_numbers::RandomNumberGenerator
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
shape_operations.h
bodies.h
x
double x
Definition: mesh_operations.cpp:202
shapes::computeShapeBoundingSphere
void computeShapeBoundingSphere(const Shape *shape, Eigen::Vector3d &center, double &radius)
Compute a sphere bounding a shape.
Definition: shape_operations.cpp:312
gtest.h
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:506
z
double z
Definition: mesh_operations.cpp:202
shapes::createMeshFromShape
Mesh * createMeshFromShape(const Shape *shape)
Construct a mesh from a primitive shape that is NOT already a mesh. This call allocates a new object.
Definition: mesh_operations.cpp:390


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Fri Apr 1 2022 02:40:31