test_fcl_bvh_models.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, Open Source Robotics Foundation
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 Open Source Robotics Foundation 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 
37 #include <gtest/gtest.h>
38 
39 #include "fcl/config.h"
41 #include "test_fcl_utility.h"
42 #include <iostream>
43 
44 using namespace fcl;
45 
46 template<typename BV>
48 {
49  using S = typename BV::S;
50 
51  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
52 
53  Box<S> box;
54  auto a = box.side[0];
55  auto b = box.side[1];
56  auto c = box.side[2];
57  std::vector<Vector3<S>> points(8);
58  points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
59  points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
60  points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
61  points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
62  points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
63  points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
64  points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
65  points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
66 
67  int result;
68 
69  result = model->beginModel();
70  EXPECT_EQ(result, BVH_OK);
71 
72  for (std::size_t i = 0; i < points.size(); ++i)
73  {
74  result = model->addVertex(points[i]);
75  EXPECT_EQ(result, BVH_OK);
76  }
77 
78  result = model->endModel();
79  EXPECT_EQ(result, BVH_OK);
80 
81  model->computeLocalAABB();
82 
83  EXPECT_EQ(model->num_vertices, 8);
84  EXPECT_EQ(model->num_tris, 0);
85  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
86 }
87 
88 template<typename BV>
90 {
91  using S = typename BV::S;
92 
93  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
94  Box<S> box;
95 
96  auto a = box.side[0];
97  auto b = box.side[1];
98  auto c = box.side[2];
99  std::vector<Vector3<S>> points(8);
100  std::vector<Triangle> tri_indices(12);
101  points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
102  points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
103  points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
104  points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
105  points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
106  points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
107  points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
108  points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
109 
110  tri_indices[0].set(0, 4, 1);
111  tri_indices[1].set(1, 4, 5);
112  tri_indices[2].set(2, 6, 3);
113  tri_indices[3].set(3, 6, 7);
114  tri_indices[4].set(3, 0, 2);
115  tri_indices[5].set(2, 0, 1);
116  tri_indices[6].set(6, 5, 7);
117  tri_indices[7].set(7, 5, 4);
118  tri_indices[8].set(1, 5, 2);
119  tri_indices[9].set(2, 5, 6);
120  tri_indices[10].set(3, 7, 0);
121  tri_indices[11].set(0, 7, 4);
122 
123  int result;
124 
125  result = model->beginModel();
126  EXPECT_EQ(result, BVH_OK);
127 
128  for (std::size_t i = 0; i < tri_indices.size(); ++i)
129  {
130  result = model->addTriangle(points[tri_indices[i][0]], points[tri_indices[i][1]], points[tri_indices[i][2]]);
131  EXPECT_EQ(result, BVH_OK);
132  }
133 
134  result = model->endModel();
135  EXPECT_EQ(result, BVH_OK);
136 
137  model->computeLocalAABB();
138 
139  EXPECT_EQ(model->num_vertices, 12 * 3);
140  EXPECT_EQ(model->num_tris, 12);
141  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
142 }
143 
144 template<typename BV>
146 {
147  using S = typename BV::S;
148 
149  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
150  Box<S> box;
151 
152  auto a = box.side[0];
153  auto b = box.side[1];
154  auto c = box.side[2];
155  std::vector<Vector3<S>> points(8);
156  std::vector<Triangle> tri_indices(12);
157  points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
158  points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
159  points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
160  points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
161  points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
162  points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
163  points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
164  points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
165 
166  tri_indices[0].set(0, 4, 1);
167  tri_indices[1].set(1, 4, 5);
168  tri_indices[2].set(2, 6, 3);
169  tri_indices[3].set(3, 6, 7);
170  tri_indices[4].set(3, 0, 2);
171  tri_indices[5].set(2, 0, 1);
172  tri_indices[6].set(6, 5, 7);
173  tri_indices[7].set(7, 5, 4);
174  tri_indices[8].set(1, 5, 2);
175  tri_indices[9].set(2, 5, 6);
176  tri_indices[10].set(3, 7, 0);
177  tri_indices[11].set(0, 7, 4);
178 
179  int result;
180 
181  result = model->beginModel();
182  EXPECT_EQ(result, BVH_OK);
183 
184  result = model->addSubModel(points, tri_indices);
185  EXPECT_EQ(result, BVH_OK);
186 
187  result = model->endModel();
188  EXPECT_EQ(result, BVH_OK);
189 
190  model->computeLocalAABB();
191 
192  EXPECT_EQ(model->num_vertices, 8);
193  EXPECT_EQ(model->num_tris, 12);
194  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
195 }
196 
197 template<typename BV>
199 {
200  testBVHModelTriangles<BV>();
201  testBVHModelPointCloud<BV>();
202  testBVHModelSubModel<BV>();
203 }
204 
205 GTEST_TEST(FCL_BVH_MODELS, building_bvh_models)
206 {
207 // testBVHModel<AABB<float>>();
208 // testBVHModel<OBB<float>>();
209 // testBVHModel<RSS<float>>();
210 // testBVHModel<kIOS<float>>();
211 // testBVHModel<OBBRSS<float>>();
212 // testBVHModel<KDOP<float, 16> >();
213 // testBVHModel<KDOP<float, 18> >();
214 // testBVHModel<KDOP<float, 24> >();
215 
216  testBVHModel<AABB<double>>();
217  testBVHModel<OBB<double>>();
218  testBVHModel<RSS<double>>();
219  testBVHModel<kIOS<double>>();
220  testBVHModel<OBBRSS<double>>();
221  testBVHModel<KDOP<double, 16> >();
222  testBVHModel<KDOP<double, 18> >();
223  testBVHModel<KDOP<double, 24> >();
224 }
225 
226 //==============================================================================
227 int main(int argc, char* argv[])
228 {
229  ::testing::InitGoogleTest(&argc, argv);
230  return RUN_ALL_TESTS();
231 }
fcl::BVH_BUILD_STATE_PROCESSED
@ BVH_BUILD_STATE_PROCESSED
after beginModel(), state for adding geometry primitives
Definition: BVH_internal.h:54
testBVHModel
void testBVHModel()
Definition: test_fcl_bvh_models.cpp:198
main
int main(int argc, char *argv[])
Definition: test_fcl_bvh_models.cpp:227
BVH_model.h
test_fcl_utility.h
testBVHModelSubModel
void testBVHModelSubModel()
Definition: test_fcl_bvh_models.cpp:145
testBVHModelTriangles
void testBVHModelTriangles()
Definition: test_fcl_bvh_models.cpp:89
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
GTEST_TEST
GTEST_TEST(FCL_BVH_MODELS, building_bvh_models)
Definition: test_fcl_bvh_models.cpp:205
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
testBVHModelPointCloud
void testBVHModelPointCloud()
Definition: test_fcl_bvh_models.cpp:47
fcl::BVH_OK
@ BVH_OK
Definition: BVH_internal.h:63
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
EXPECT_EQ
#define EXPECT_EQ(a, b)
fcl::Box::side
Vector3< S > side
box side length
Definition: box.h:67


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49