38 #define BOOST_TEST_MODULE FCL_BVH_MODELS
39 #include <boost/test/included/unit_test.hpp>
40 #include <boost/filesystem.hpp>
42 #include "fcl_resources/config.h"
57 template <
typename BV>
60 double a =
box.halfSide[0];
61 double b =
box.halfSide[1];
62 double c =
box.halfSide[2];
63 std::vector<Vec3f> points(8);
64 points[0] <<
a, -
b,
c;
66 points[2] << -
a,
b,
c;
67 points[3] << -
a, -
b,
c;
68 points[4] <<
a, -
b, -
c;
69 points[5] <<
a,
b, -
c;
70 points[6] << -
a,
b, -
c;
71 points[7] << -
a, -
b, -
c;
79 std::cout <<
"Abort test since '" <<
getNodeTypeName(model->getNodeType())
80 <<
"' does not support point cloud model. "
81 <<
"Please see issue #67." << std::endl;
87 result = model->beginModel();
88 BOOST_CHECK_EQUAL(result,
BVH_OK);
90 for (std::size_t i = 0; i < points.size(); ++i) {
91 result = model->addVertex(points[i]);
92 BOOST_CHECK_EQUAL(result,
BVH_OK);
95 result = model->endModel();
96 BOOST_CHECK_EQUAL(result,
BVH_OK);
98 model->computeLocalAABB();
100 BOOST_CHECK_EQUAL(model->num_vertices, 8);
101 BOOST_CHECK_EQUAL(model->num_tris, 0);
111 std::cout <<
"Abort test since '" <<
getNodeTypeName(model->getNodeType())
112 <<
"' does not support point cloud model. "
113 <<
"Please see issue #67." << std::endl;
117 Matrixx3f all_points((Eigen::DenseIndex)points.size(), 3);
118 for (
size_t k = 0; k < points.size(); ++k)
119 all_points.row((Eigen::DenseIndex)k) = points[k].transpose();
123 result = model->beginModel();
124 BOOST_CHECK_EQUAL(result,
BVH_OK);
126 result = model->addVertices(all_points);
128 result = model->endModel();
129 BOOST_CHECK_EQUAL(result,
BVH_OK);
131 model->computeLocalAABB();
133 BOOST_CHECK_EQUAL(model->num_vertices, 8);
134 BOOST_CHECK_EQUAL(model->num_tris, 0);
139 template <
typename BV>
145 double a =
box.halfSide[0];
146 double b =
box.halfSide[1];
147 double c =
box.halfSide[2];
148 std::vector<Vec3f> points(8);
149 std::vector<Triangle> tri_indices(12);
150 points[0] <<
a, -
b,
c;
151 points[1] <<
a,
b,
c;
152 points[2] << -
a,
b,
c;
153 points[3] << -
a, -
b,
c;
154 points[4] <<
a, -
b, -
c;
155 points[5] <<
a,
b, -
c;
156 points[6] << -
a,
b, -
c;
157 points[7] << -
a, -
b, -
c;
159 tri_indices[0].set(0, 4, 1);
160 tri_indices[1].set(1, 4, 5);
161 tri_indices[2].set(2, 6, 3);
162 tri_indices[3].set(3, 6, 7);
163 tri_indices[4].set(3, 0, 2);
164 tri_indices[5].set(2, 0, 1);
165 tri_indices[6].set(6, 5, 7);
166 tri_indices[7].set(7, 5, 4);
167 tri_indices[8].set(1, 5, 2);
168 tri_indices[9].set(2, 5, 6);
169 tri_indices[10].set(3, 7, 0);
170 tri_indices[11].set(0, 7, 4);
174 result = model->beginModel();
175 BOOST_CHECK_EQUAL(result,
BVH_OK);
177 for (std::size_t i = 0; i < tri_indices.size(); ++i) {
179 model->addTriangle(points[tri_indices[i][0]], points[tri_indices[i][1]],
180 points[tri_indices[i][2]]);
181 BOOST_CHECK_EQUAL(result,
BVH_OK);
184 result = model->endModel();
185 BOOST_CHECK_EQUAL(result,
BVH_OK);
187 model->computeLocalAABB();
189 BOOST_CHECK_EQUAL(model->num_vertices, 12 * 3);
190 BOOST_CHECK_EQUAL(model->num_tris, 12);
194 shared_ptr<BVHModel<BV> > cropped(
BVHExtract(*model, pose, aabb));
195 BOOST_REQUIRE(cropped);
197 BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
198 BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
201 cropped.reset(
BVHExtract(*model, pose, aabb));
202 BOOST_REQUIRE(cropped);
204 BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
205 BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
208 FCL_REAL sqrt2_2 = std::sqrt(2) / 2;
210 cropped.reset(
BVHExtract(*model, pose, aabb));
211 BOOST_REQUIRE(cropped);
213 BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
214 BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
218 cropped.reset(
BVHExtract(*model, pose, aabb));
219 BOOST_CHECK(!cropped);
223 cropped.reset(
BVHExtract(*model, pose, aabb));
224 BOOST_REQUIRE(cropped);
225 BOOST_CHECK_EQUAL(cropped->num_tris, 2);
226 BOOST_CHECK_EQUAL(cropped->num_vertices, 6);
229 template <
typename BV>
234 double a =
box.halfSide[0];
235 double b =
box.halfSide[1];
236 double c =
box.halfSide[2];
237 std::vector<Vec3f> points(8);
238 std::vector<Triangle> tri_indices(12);
239 points[0] <<
a, -
b,
c;
240 points[1] <<
a,
b,
c;
241 points[2] << -
a,
b,
c;
242 points[3] << -
a, -
b,
c;
243 points[4] <<
a, -
b, -
c;
244 points[5] <<
a,
b, -
c;
245 points[6] << -
a,
b, -
c;
246 points[7] << -
a, -
b, -
c;
248 tri_indices[0].set(0, 4, 1);
249 tri_indices[1].set(1, 4, 5);
250 tri_indices[2].set(2, 6, 3);
251 tri_indices[3].set(3, 6, 7);
252 tri_indices[4].set(3, 0, 2);
253 tri_indices[5].set(2, 0, 1);
254 tri_indices[6].set(6, 5, 7);
255 tri_indices[7].set(7, 5, 4);
256 tri_indices[8].set(1, 5, 2);
257 tri_indices[9].set(2, 5, 6);
258 tri_indices[10].set(3, 7, 0);
259 tri_indices[11].set(0, 7, 4);
263 result = model->beginModel();
264 BOOST_CHECK_EQUAL(result,
BVH_OK);
266 result = model->addSubModel(points, tri_indices);
267 BOOST_CHECK_EQUAL(result,
BVH_OK);
269 result = model->endModel();
270 BOOST_CHECK_EQUAL(result,
BVH_OK);
272 model->computeLocalAABB();
274 BOOST_CHECK_EQUAL(model->num_vertices, 8);
275 BOOST_CHECK_EQUAL(model->num_tris, 12);
279 template <
typename BV>
281 testBVHModelTriangles<BV>();
282 testBVHModelPointCloud<BV>();
283 testBVHModelSubModel<BV>();
287 testBVHModel<AABB>();
290 testBVHModel<kIOS>();
291 testBVHModel<OBBRSS>();
292 testBVHModel<KDOP<16> >();
293 testBVHModel<KDOP<18> >();
294 testBVHModel<KDOP<24> >();
297 template <
class BoundingVolume>
300 std::string env = (
path /
"env.obj").
string(),
301 rob = (
path /
"rob.obj").
string();
304 typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
305 PolyhedronPtr_t
P1(
new Polyhedron_t),
P2;
308 scale.setConstant(1);
311 scale.setConstant(-1);
314 P2 = dynamic_pointer_cast<Polyhedron_t>(geom);
317 BOOST_CHECK_EQUAL(
P1->num_tris,
P2->num_tris);
318 BOOST_CHECK_EQUAL(
P1->num_vertices,
P2->num_vertices);
319 BOOST_CHECK_EQUAL(
P1->getNumBVs(),
P2->getNumBVs());
322 BOOST_CHECK_EQUAL(geom, geom2);
325 template <
class BoundingVolume>
328 std::string env = (
path /
"staircases_koroibot_hr.dae").
string();
331 typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
332 PolyhedronPtr_t
P1(
new Polyhedron_t),
P2;
335 scale.setConstant(1);
345 collide(&stairs, &obj, request, result);
350 testLoadPolyhedron<AABB>();
351 testLoadPolyhedron<OBB>();
352 testLoadPolyhedron<RSS>();
353 testLoadPolyhedron<kIOS>();
354 testLoadPolyhedron<OBBRSS>();
355 testLoadPolyhedron<KDOP<16> >();
356 testLoadPolyhedron<KDOP<18> >();
357 testLoadPolyhedron<KDOP<24> >();
361 testLoadGerardBauzil<OBB>();
362 testLoadGerardBauzil<RSS>();
363 testLoadGerardBauzil<kIOS>();
364 testLoadGerardBauzil<OBBRSS>();
369 const std::string
filename = (
path /
"illformated_mesh.dae").
string();
382 box_bvh_model.
convex->computeLocalAABB();
383 std::shared_ptr<ConvexBase> convex_copy(box_bvh_model.
convex->clone());
384 BOOST_CHECK(*convex_copy.get() == *box_bvh_model.
convex.get());