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 
50 class CompareMeshVsPrimitive : public ::testing::Test
51 {
52 public:
54  {
55  }
56 
57  void SetUp()
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()
70  {
71  for (int 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 
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 (int i = 0; i < shape_meshes.size(); ++i)
99  {
100  shapes::Mesh* shape_ms = shape_meshes[i];
101  shapes::Mesh* loaded_ms = loaded_meshes[i];
102 
103  bodies::Body* shape_cms = shape_convex_meshes[i];
104  bodies::Body* loaded_cms = loaded_convex_meshes[i];
105 
106  Eigen::Vector3d p;
107  bool found = false;
108  for (int i = 0; i < 100; ++i)
109  {
110  if ((shape_cms->samplePointInside(rng, 10000, p)) || (loaded_cms->samplePointInside(rng, 10000, p)))
111  {
112  found = true;
113  EXPECT_EQ(shape_cms->containsPoint(p), loaded_cms->containsPoint(p));
114  }
115  }
116  EXPECT_TRUE(found) << "No point inside the meshes was found (very unlikely)";
117  }
118 }
119 
121 {
122  // Random rays must intersect both meshes nearly at the same points
123  for (int i = 0; i < shape_meshes.size(); ++i)
124  {
125  shapes::Mesh* shape_ms = shape_meshes[i];
126  shapes::Mesh* loaded_ms = loaded_meshes[i];
127 
128  bodies::Body* shape_cms = shape_convex_meshes[i];
129  bodies::Body* loaded_cms = loaded_convex_meshes[i];
130 
131  bool intersects = false;
132  for (int i = 0; i < 100; ++i)
133  {
134  Eigen::Vector3d ray_o(rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0));
135  Eigen::Vector3d ray_d(rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0));
136  EigenSTL::vector_Vector3d vi1, vi2;
137  shape_cms->intersectsRay(ray_o, ray_d, &vi1);
138  loaded_cms->intersectsRay(ray_o, ray_d, &vi2);
139 
140  // DEBUG printing
141  // if (vi1.size() != vi2.size() && vi1.size() > 0 && vi2.size() > 0)
142  // {
143  // std::cout << vi1.size() << " " << vi2.size() << "\n";
144  // std::cout << ray_o.x() << " "<< ray_o.y() << " "<< ray_o.z()
145  // << "\n" << ray_d.x() << " "<< ray_d.y() << " "<< ray_d.z() << "\n";
146  // }
147 
148  EXPECT_EQ(vi1.size(), vi2.size());
149  if (vi1.size() > 0 && vi2.size() > 0)
150  {
151  EXPECT_NEAR(vi1[0].x(), vi2[0].x(), 0.01);
152  EXPECT_NEAR(vi1[0].y(), vi2[0].y(), 0.01);
153  EXPECT_NEAR(vi1[0].z(), vi2[0].z(), 0.01);
154 
155  intersects = true;
156  }
157  }
158 
159  EXPECT_TRUE(intersects) << "No ray intersects the meshes (very unlikely)";
160  }
161 }
162 
164 {
165  // Bounding spheres must be nearly identical
166  for (int i = 0; i < shape_meshes.size(); ++i)
167  {
168  shapes::Mesh* shape_ms = shape_meshes[i];
169  shapes::Mesh* loaded_ms = loaded_meshes[i];
170 
171  bodies::Body* shape_cms = shape_convex_meshes[i];
172  bodies::Body* loaded_cms = loaded_convex_meshes[i];
173 
174  shapes::Sphere shape(1.0);
175  Eigen::Vector3d center1, center2;
176  double radius1, radius2;
177  computeShapeBoundingSphere(shape_ms, center1, radius1);
178  computeShapeBoundingSphere(loaded_ms, center2, radius2);
179 
180  EXPECT_NEAR(radius1, radius2, 0.001);
181  EXPECT_NEAR(center1.x(), center2.x(), 0.001);
182  EXPECT_NEAR(center1.y(), center2.y(), 0.001);
183  EXPECT_NEAR(center1.z(), center2.z(), 0.001);
184  }
185 }
186 
188 {
189  // For a simple shape as a cube, we expect that both meshes have the same number of vertex and triangles
190  // But that was not the case before fixing issue #38!
191  // These tests don't apply to curve shapes because the number of elements depends on how smooth they where
192  // created. So ensure that "back()" gives a pointer to box meshes!
193  EXPECT_EQ(shape_meshes.back()->vertex_count, loaded_meshes.back()->vertex_count);
194 }
195 
196 TEST_F(CompareMeshVsPrimitive, BoxTriangleCount)
197 {
198  EXPECT_EQ(shape_meshes.back()->triangle_count, loaded_meshes.back()->triangle_count);
199 }
200 
201 int main(int argc, char** argv)
202 {
203  testing::InitGoogleTest(&argc, argv);
204  return RUN_ALL_TESTS();
205 }
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const =0
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bool containsPoint(double x, double y, double z, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.h:154
TEST_F(CompareMeshVsPrimitive, ContainsPoint)
#define EXPECT_NEAR(a, b, prec)
double y
#define EXPECT_EQ(a, b)
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:397
std::vector< shapes::Mesh * > shape_meshes
double z
double uniformReal(double lower_bound, double upper_bound)
Definition of a sphere.
Definition: shapes.h:106
int main(int argc, char **argv)
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:115
void computeShapeBoundingSphere(const Shape *shape, Eigen::Vector3d &center, double &radius)
Compute a sphere bounding a shape.
Mesh * createMeshFromResource(const std::string &resource)
Load a mesh from a resource that contains a mesh that can be loaded by assimp.
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin.
Definition: shapes.h:196
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:174
random_numbers::RandomNumberGenerator rng
std::vector< bodies::Body * > loaded_convex_meshes
Mesh * createMeshFromShape(const Shape *shape)
Construct a mesh from a primitive shape that is NOT already a mesh. This call allocates a new object...
std::vector< bodies::Body * > shape_convex_meshes
double x
std::vector< shapes::Mesh * > loaded_meshes
#define EXPECT_TRUE(args)
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:88


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Jun 10 2019 13:22:04