GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: test/bvh_models.cpp Lines: 241 244 98.8 %
Date: 2024-02-09 12:57:42 Branches: 665 1336 49.8 %

Line Branch Exec Source
1
/*
2
 * Software License Agreement (BSD License)
3
 *
4
 *  Copyright (c) 2015, Open Source Robotics Foundation
5
 *  Copyright (c) 2020, INRIA
6
 *  All rights reserved.
7
 *
8
 *  Redistribution and use in source and binary forms, with or without
9
 *  modification, are permitted provided that the following conditions
10
 *  are met:
11
 *
12
 *   * Redistributions of source code must retain the above copyright
13
 *     notice, this list of conditions and the following disclaimer.
14
 *   * Redistributions in binary form must reproduce the above
15
 *     copyright notice, this list of conditions and the following
16
 *     disclaimer in the documentation and/or other materials provided
17
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
19
 *     contributors may be used to endorse or promote products derived
20
 *     from this software without specific prior written permission.
21
 *
22
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33
 *  POSSIBILITY OF SUCH DAMAGE.
34
 */
35
36
/** \author Jeongseok Lee */
37
38
#define BOOST_TEST_MODULE FCL_BVH_MODELS
39
#include <boost/test/included/unit_test.hpp>
40
#include <boost/filesystem.hpp>
41
42
#include "fcl_resources/config.h"
43
44
#include <hpp/fcl/collision.h>
45
#include <hpp/fcl/BVH/BVH_model.h>
46
#include <hpp/fcl/BVH/BVH_utility.h>
47
#include <hpp/fcl/math/transform.h>
48
#include <hpp/fcl/shape/geometric_shapes.h>
49
#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
50
#include <hpp/fcl/mesh_loader/assimp.h>
51
#include <hpp/fcl/mesh_loader/loader.h>
52
#include "utility.h"
53
#include <iostream>
54
55
using namespace hpp::fcl;
56
57
template <typename BV>
58
16
void testBVHModelPointCloud() {
59

16
  Box box(Vec3f::Ones());
60
16
  double a = box.halfSide[0];
61
16
  double b = box.halfSide[1];
62
16
  double c = box.halfSide[2];
63
16
  std::vector<Vec3f> points(8);
64

16
  points[0] << a, -b, c;
65

16
  points[1] << a, b, c;
66

16
  points[2] << -a, b, c;
67

16
  points[3] << -a, -b, c;
68

16
  points[4] << a, -b, -c;
69

16
  points[5] << a, b, -c;
70

16
  points[6] << -a, b, -c;
71

16
  points[7] << -a, -b, -c;
72
73
  {
74

16
    shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
75
76

30
    if (model->getNodeType() != BV_AABB && model->getNodeType() != BV_KDOP16 &&
77


40
        model->getNodeType() != BV_KDOP18 &&
78

10
        model->getNodeType() != BV_KDOP24) {
79
16
      std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType())
80
                << "' does not support point cloud model. "
81



8
                << "Please see issue #67." << std::endl;
82
8
      return;
83
    }
84
85
    int result;
86
87
8
    result = model->beginModel();
88


8
    BOOST_CHECK_EQUAL(result, BVH_OK);
89
90
72
    for (std::size_t i = 0; i < points.size(); ++i) {
91
64
      result = model->addVertex(points[i]);
92


64
      BOOST_CHECK_EQUAL(result, BVH_OK);
93
    }
94
95
8
    result = model->endModel();
96


8
    BOOST_CHECK_EQUAL(result, BVH_OK);
97
98
8
    model->computeLocalAABB();
99
100


8
    BOOST_CHECK_EQUAL(model->num_vertices, 8);
101


8
    BOOST_CHECK_EQUAL(model->num_tris, 0);
102


8
    BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
103
  }
104
105
  {
106

8
    shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
107
108

14
    if (model->getNodeType() != BV_AABB && model->getNodeType() != BV_KDOP16 &&
109


16
        model->getNodeType() != BV_KDOP18 &&
110

2
        model->getNodeType() != BV_KDOP24) {
111
      std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType())
112
                << "' does not support point cloud model. "
113
                << "Please see issue #67." << std::endl;
114
      return;
115
    }
116
117
16
    Matrixx3f all_points((Eigen::DenseIndex)points.size(), 3);
118
72
    for (size_t k = 0; k < points.size(); ++k)
119

64
      all_points.row((Eigen::DenseIndex)k) = points[k].transpose();
120
121
    int result;
122
123
8
    result = model->beginModel();
124


8
    BOOST_CHECK_EQUAL(result, BVH_OK);
125
126
8
    result = model->addVertices(all_points);
127
128
8
    result = model->endModel();
129


8
    BOOST_CHECK_EQUAL(result, BVH_OK);
130
131
8
    model->computeLocalAABB();
132
133


8
    BOOST_CHECK_EQUAL(model->num_vertices, 8);
134


8
    BOOST_CHECK_EQUAL(model->num_tris, 0);
135


8
    BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
136
  }
137
}
138
139
template <typename BV>
140
16
void testBVHModelTriangles() {
141

32
  shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
142

32
  Box box(Vec3f::Ones());
143

16
  AABB aabb(Vec3f(-1, 0, -1), Vec3f(1, 1, 1));
144
145
16
  double a = box.halfSide[0];
146
16
  double b = box.halfSide[1];
147
16
  double c = box.halfSide[2];
148
32
  std::vector<Vec3f> points(8);
149
32
  std::vector<Triangle> tri_indices(12);
150

16
  points[0] << a, -b, c;
151

16
  points[1] << a, b, c;
152

16
  points[2] << -a, b, c;
153

16
  points[3] << -a, -b, c;
154

16
  points[4] << a, -b, -c;
155

16
  points[5] << a, b, -c;
156

16
  points[6] << -a, b, -c;
157

16
  points[7] << -a, -b, -c;
158
159
16
  tri_indices[0].set(0, 4, 1);
160
16
  tri_indices[1].set(1, 4, 5);
161
16
  tri_indices[2].set(2, 6, 3);
162
16
  tri_indices[3].set(3, 6, 7);
163
16
  tri_indices[4].set(3, 0, 2);
164
16
  tri_indices[5].set(2, 0, 1);
165
16
  tri_indices[6].set(6, 5, 7);
166
16
  tri_indices[7].set(7, 5, 4);
167
16
  tri_indices[8].set(1, 5, 2);
168
16
  tri_indices[9].set(2, 5, 6);
169
16
  tri_indices[10].set(3, 7, 0);
170
16
  tri_indices[11].set(0, 7, 4);
171
172
  int result;
173
174
16
  result = model->beginModel();
175


16
  BOOST_CHECK_EQUAL(result, BVH_OK);
176
177
208
  for (std::size_t i = 0; i < tri_indices.size(); ++i) {
178
192
    result =
179
192
        model->addTriangle(points[tri_indices[i][0]], points[tri_indices[i][1]],
180
                           points[tri_indices[i][2]]);
181


192
    BOOST_CHECK_EQUAL(result, BVH_OK);
182
  }
183
184
16
  result = model->endModel();
185


16
  BOOST_CHECK_EQUAL(result, BVH_OK);
186
187
16
  model->computeLocalAABB();
188
189


16
  BOOST_CHECK_EQUAL(model->num_vertices, 12 * 3);
190


16
  BOOST_CHECK_EQUAL(model->num_tris, 12);
191


16
  BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
192
193
16
  Transform3f pose;
194

32
  shared_ptr<BVHModel<BV> > cropped(BVHExtract(*model, pose, aabb));
195



16
  BOOST_REQUIRE(cropped);
196



16
  BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
197


16
  BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
198


16
  BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
199
200

16
  pose.setTranslation(Vec3f(0, 1, 0));
201

16
  cropped.reset(BVHExtract(*model, pose, aabb));
202



16
  BOOST_REQUIRE(cropped);
203



16
  BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
204


16
  BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
205


16
  BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
206
207

16
  pose.setTranslation(Vec3f(0, 0, 0));
208
16
  FCL_REAL sqrt2_2 = std::sqrt(2) / 2;
209

16
  pose.setQuatRotation(Quaternion3f(sqrt2_2, sqrt2_2, 0, 0));
210

16
  cropped.reset(BVHExtract(*model, pose, aabb));
211



16
  BOOST_REQUIRE(cropped);
212



16
  BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
213


16
  BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
214


16
  BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
215
216

16
  pose.setTranslation(-Vec3f(1, 1, 1));
217

16
  pose.setQuatRotation(Quaternion3f::Identity());
218

16
  cropped.reset(BVHExtract(*model, pose, aabb));
219



16
  BOOST_CHECK(!cropped);
220
221


16
  aabb = AABB(Vec3f(-0.1, -0.1, -0.1), Vec3f(0.1, 0.1, 0.1));
222

16
  pose.setTranslation(Vec3f(-0.5, -0.5, 0));
223

16
  cropped.reset(BVHExtract(*model, pose, aabb));
224



16
  BOOST_REQUIRE(cropped);
225


16
  BOOST_CHECK_EQUAL(cropped->num_tris, 2);
226


16
  BOOST_CHECK_EQUAL(cropped->num_vertices, 6);
227
16
}
228
229
template <typename BV>
230
16
void testBVHModelSubModel() {
231

32
  shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
232

32
  Box box(Vec3f::Ones());
233
234
16
  double a = box.halfSide[0];
235
16
  double b = box.halfSide[1];
236
16
  double c = box.halfSide[2];
237
32
  std::vector<Vec3f> points(8);
238
32
  std::vector<Triangle> tri_indices(12);
239

16
  points[0] << a, -b, c;
240

16
  points[1] << a, b, c;
241

16
  points[2] << -a, b, c;
242

16
  points[3] << -a, -b, c;
243

16
  points[4] << a, -b, -c;
244

16
  points[5] << a, b, -c;
245

16
  points[6] << -a, b, -c;
246

16
  points[7] << -a, -b, -c;
247
248
16
  tri_indices[0].set(0, 4, 1);
249
16
  tri_indices[1].set(1, 4, 5);
250
16
  tri_indices[2].set(2, 6, 3);
251
16
  tri_indices[3].set(3, 6, 7);
252
16
  tri_indices[4].set(3, 0, 2);
253
16
  tri_indices[5].set(2, 0, 1);
254
16
  tri_indices[6].set(6, 5, 7);
255
16
  tri_indices[7].set(7, 5, 4);
256
16
  tri_indices[8].set(1, 5, 2);
257
16
  tri_indices[9].set(2, 5, 6);
258
16
  tri_indices[10].set(3, 7, 0);
259
16
  tri_indices[11].set(0, 7, 4);
260
261
  int result;
262
263
16
  result = model->beginModel();
264


16
  BOOST_CHECK_EQUAL(result, BVH_OK);
265
266
16
  result = model->addSubModel(points, tri_indices);
267


16
  BOOST_CHECK_EQUAL(result, BVH_OK);
268
269
16
  result = model->endModel();
270


16
  BOOST_CHECK_EQUAL(result, BVH_OK);
271
272
16
  model->computeLocalAABB();
273
274


16
  BOOST_CHECK_EQUAL(model->num_vertices, 8);
275


16
  BOOST_CHECK_EQUAL(model->num_tris, 12);
276


16
  BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
277
16
}
278
279
template <typename BV>
280
16
void testBVHModel() {
281
16
  testBVHModelTriangles<BV>();
282
16
  testBVHModelPointCloud<BV>();
283
16
  testBVHModelSubModel<BV>();
284
16
}
285
286
















4
BOOST_AUTO_TEST_CASE(building_bvh_models) {
287
2
  testBVHModel<AABB>();
288
2
  testBVHModel<OBB>();
289
2
  testBVHModel<RSS>();
290
2
  testBVHModel<kIOS>();
291
2
  testBVHModel<OBBRSS>();
292
2
  testBVHModel<KDOP<16> >();
293
2
  testBVHModel<KDOP<18> >();
294
2
  testBVHModel<KDOP<24> >();
295
2
}
296
297
template <class BoundingVolume>
298
16
void testLoadPolyhedron() {
299
32
  boost::filesystem::path path(TEST_RESOURCES_DIR);
300

48
  std::string env = (path / "env.obj").string(),
301

48
              rob = (path / "rob.obj").string();
302
303
  typedef BVHModel<BoundingVolume> Polyhedron_t;
304
  typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
305

32
  PolyhedronPtr_t P1(new Polyhedron_t), P2;
306
307
16
  Vec3f scale;
308
16
  scale.setConstant(1);
309
16
  loadPolyhedronFromResource(env, scale, P1);
310
311
16
  scale.setConstant(-1);
312
32
  CachedMeshLoader loader(P1->getNodeType());
313
32
  CollisionGeometryPtr_t geom = loader.load(env, scale);
314
16
  P2 = dynamic_pointer_cast<Polyhedron_t>(geom);
315



16
  BOOST_REQUIRE(P2);
316
317


16
  BOOST_CHECK_EQUAL(P1->num_tris, P2->num_tris);
318


16
  BOOST_CHECK_EQUAL(P1->num_vertices, P2->num_vertices);
319


16
  BOOST_CHECK_EQUAL(P1->getNumBVs(), P2->getNumBVs());
320
321
48
  CollisionGeometryPtr_t geom2 = loader.load(env, scale);
322


16
  BOOST_CHECK_EQUAL(geom, geom2);
323
16
}
324
325
template <class BoundingVolume>
326
8
void testLoadGerardBauzil() {
327
16
  boost::filesystem::path path(TEST_RESOURCES_DIR);
328

24
  std::string env = (path / "staircases_koroibot_hr.dae").string();
329
330
  typedef BVHModel<BoundingVolume> Polyhedron_t;
331
  typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
332

16
  PolyhedronPtr_t P1(new Polyhedron_t), P2;
333
334
8
  Vec3f scale;
335
8
  scale.setConstant(1);
336
8
  loadPolyhedronFromResource(env, scale, P1);
337

16
  CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27));
338

8
  Transform3f pos(Vec3f(-1.33, 1.36, .14));
339
16
  CollisionObject obj(cylinder, pos);
340
16
  CollisionObject stairs(P1);
341
342
8
  CollisionRequest request;
343
16
  CollisionResult result;
344
345
8
  collide(&stairs, &obj, request, result);
346



8
  BOOST_CHECK(result.isCollision());
347
8
}
348
349
















4
BOOST_AUTO_TEST_CASE(load_polyhedron) {
350
2
  testLoadPolyhedron<AABB>();
351
2
  testLoadPolyhedron<OBB>();
352
2
  testLoadPolyhedron<RSS>();
353
2
  testLoadPolyhedron<kIOS>();
354
2
  testLoadPolyhedron<OBBRSS>();
355
2
  testLoadPolyhedron<KDOP<16> >();
356
2
  testLoadPolyhedron<KDOP<18> >();
357
2
  testLoadPolyhedron<KDOP<24> >();
358
2
}
359
360
















4
BOOST_AUTO_TEST_CASE(gerard_bauzil) {
361
2
  testLoadGerardBauzil<OBB>();
362
2
  testLoadGerardBauzil<RSS>();
363
2
  testLoadGerardBauzil<kIOS>();
364
2
  testLoadGerardBauzil<OBBRSS>();
365
2
}
366
367
















4
BOOST_AUTO_TEST_CASE(load_illformated_mesh) {
368
4
  boost::filesystem::path path(TEST_RESOURCES_DIR);
369

6
  const std::string filename = (path / "illformated_mesh.dae").string();
370
371
4
  MeshLoader loader;
372








2
  BOOST_CHECK_NO_THROW(loader.load(filename));
373
2
}
374
375
















4
BOOST_AUTO_TEST_CASE(test_convex) {
376

2
  Box* box_ptr = new hpp::fcl::Box(1, 1, 1);
377
4
  CollisionGeometryPtr_t b1(box_ptr);
378
4
  BVHModel<OBBRSS> box_bvh_model = BVHModel<OBBRSS>();
379

2
  generateBVHModel(box_bvh_model, *box_ptr, Transform3f());
380
2
  box_bvh_model.buildConvexRepresentation(false);
381
382
2
  box_bvh_model.convex->computeLocalAABB();
383

4
  std::shared_ptr<ConvexBase> convex_copy(box_bvh_model.convex->clone());
384



2
  BOOST_CHECK(*convex_copy.get() == *box_bvh_model.convex.get());
385
2
}