bvh_models.cpp
Go to the documentation of this file.
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 
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 void testBVHModelPointCloud() {
59  Box box(Vec3f::Ones());
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;
65  points[1] << 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;
72 
73  {
74  shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
75 
76  if (model->getNodeType() != BV_AABB && model->getNodeType() != BV_KDOP16 &&
77  model->getNodeType() != BV_KDOP18 &&
78  model->getNodeType() != BV_KDOP24) {
79  std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType())
80  << "' does not support point cloud model. "
81  << "Please see issue #67." << std::endl;
82  return;
83  }
84 
85  int result;
86 
87  result = model->beginModel();
88  BOOST_CHECK_EQUAL(result, BVH_OK);
89 
90  for (std::size_t i = 0; i < points.size(); ++i) {
91  result = model->addVertex(points[i]);
92  BOOST_CHECK_EQUAL(result, BVH_OK);
93  }
94 
95  result = model->endModel();
96  BOOST_CHECK_EQUAL(result, BVH_OK);
97 
98  model->computeLocalAABB();
99 
100  BOOST_CHECK_EQUAL(model->num_vertices, 8);
101  BOOST_CHECK_EQUAL(model->num_tris, 0);
102  BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
103  }
104 
105  {
106  shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
107 
108  if (model->getNodeType() != BV_AABB && model->getNodeType() != BV_KDOP16 &&
109  model->getNodeType() != BV_KDOP18 &&
110  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  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();
120 
121  int result;
122 
123  result = model->beginModel();
124  BOOST_CHECK_EQUAL(result, BVH_OK);
125 
126  result = model->addVertices(all_points);
127 
128  result = model->endModel();
129  BOOST_CHECK_EQUAL(result, BVH_OK);
130 
131  model->computeLocalAABB();
132 
133  BOOST_CHECK_EQUAL(model->num_vertices, 8);
134  BOOST_CHECK_EQUAL(model->num_tris, 0);
135  BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
136  }
137 }
138 
139 template <typename BV>
140 void testBVHModelTriangles() {
141  shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
142  Box box(Vec3f::Ones());
143  AABB aabb(Vec3f(-1, 0, -1), Vec3f(1, 1, 1));
144 
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;
158 
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);
171 
172  int result;
173 
174  result = model->beginModel();
175  BOOST_CHECK_EQUAL(result, BVH_OK);
176 
177  for (std::size_t i = 0; i < tri_indices.size(); ++i) {
178  result =
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);
182  }
183 
184  result = model->endModel();
185  BOOST_CHECK_EQUAL(result, BVH_OK);
186 
187  model->computeLocalAABB();
188 
189  BOOST_CHECK_EQUAL(model->num_vertices, 12 * 3);
190  BOOST_CHECK_EQUAL(model->num_tris, 12);
191  BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
192 
193  Transform3f pose;
194  shared_ptr<BVHModel<BV> > cropped(BVHExtract(*model, pose, aabb));
195  BOOST_REQUIRE(cropped);
196  BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
197  BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
198  BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
199 
200  pose.setTranslation(Vec3f(0, 1, 0));
201  cropped.reset(BVHExtract(*model, pose, aabb));
202  BOOST_REQUIRE(cropped);
203  BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
204  BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
205  BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
206 
207  pose.setTranslation(Vec3f(0, 0, 0));
208  FCL_REAL sqrt2_2 = std::sqrt(2) / 2;
209  pose.setQuatRotation(Quaternion3f(sqrt2_2, sqrt2_2, 0, 0));
210  cropped.reset(BVHExtract(*model, pose, aabb));
211  BOOST_REQUIRE(cropped);
212  BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
213  BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
214  BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
215 
216  pose.setTranslation(-Vec3f(1, 1, 1));
217  pose.setQuatRotation(Quaternion3f::Identity());
218  cropped.reset(BVHExtract(*model, pose, aabb));
219  BOOST_CHECK(!cropped);
220 
221  aabb = AABB(Vec3f(-0.1, -0.1, -0.1), Vec3f(0.1, 0.1, 0.1));
222  pose.setTranslation(Vec3f(-0.5, -0.5, 0));
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);
227 }
228 
229 template <typename BV>
230 void testBVHModelSubModel() {
231  shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
232  Box box(Vec3f::Ones());
233 
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;
247 
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);
260 
261  int result;
262 
263  result = model->beginModel();
264  BOOST_CHECK_EQUAL(result, BVH_OK);
265 
266  result = model->addSubModel(points, tri_indices);
267  BOOST_CHECK_EQUAL(result, BVH_OK);
268 
269  result = model->endModel();
270  BOOST_CHECK_EQUAL(result, BVH_OK);
271 
272  model->computeLocalAABB();
273 
274  BOOST_CHECK_EQUAL(model->num_vertices, 8);
275  BOOST_CHECK_EQUAL(model->num_tris, 12);
276  BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
277 }
278 
279 template <typename BV>
280 void testBVHModel() {
281  testBVHModelTriangles<BV>();
282  testBVHModelPointCloud<BV>();
283  testBVHModelSubModel<BV>();
284 }
285 
286 BOOST_AUTO_TEST_CASE(building_bvh_models) {
287  testBVHModel<AABB>();
288  testBVHModel<OBB>();
289  testBVHModel<RSS>();
290  testBVHModel<kIOS>();
291  testBVHModel<OBBRSS>();
292  testBVHModel<KDOP<16> >();
293  testBVHModel<KDOP<18> >();
294  testBVHModel<KDOP<24> >();
295 }
296 
297 template <class BoundingVolume>
298 void testLoadPolyhedron() {
299  boost::filesystem::path path(TEST_RESOURCES_DIR);
300  std::string env = (path / "env.obj").string(),
301  rob = (path / "rob.obj").string();
302 
303  typedef BVHModel<BoundingVolume> Polyhedron_t;
304  typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
305  PolyhedronPtr_t P1(new Polyhedron_t), P2;
306 
307  Vec3f scale;
308  scale.setConstant(1);
309  loadPolyhedronFromResource(env, scale, P1);
310 
311  scale.setConstant(-1);
312  CachedMeshLoader loader(P1->getNodeType());
313  CollisionGeometryPtr_t geom = loader.load(env, scale);
314  P2 = dynamic_pointer_cast<Polyhedron_t>(geom);
315  BOOST_REQUIRE(P2);
316 
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());
320 
321  CollisionGeometryPtr_t geom2 = loader.load(env, scale);
322  BOOST_CHECK_EQUAL(geom, geom2);
323 }
324 
325 template <class BoundingVolume>
326 void testLoadGerardBauzil() {
327  boost::filesystem::path path(TEST_RESOURCES_DIR);
328  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  PolyhedronPtr_t P1(new Polyhedron_t), P2;
333 
334  Vec3f scale;
335  scale.setConstant(1);
336  loadPolyhedronFromResource(env, scale, P1);
337  CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27));
338  Transform3f pos(Vec3f(-1.33, 1.36, .14));
339  CollisionObject obj(cylinder, pos);
340  CollisionObject stairs(P1);
341 
342  CollisionRequest request;
343  CollisionResult result;
344 
345  collide(&stairs, &obj, request, result);
346  BOOST_CHECK(result.isCollision());
347 }
348 
349 BOOST_AUTO_TEST_CASE(load_polyhedron) {
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> >();
358 }
359 
360 BOOST_AUTO_TEST_CASE(gerard_bauzil) {
361  testLoadGerardBauzil<OBB>();
362  testLoadGerardBauzil<RSS>();
363  testLoadGerardBauzil<kIOS>();
364  testLoadGerardBauzil<OBBRSS>();
365 }
366 
367 BOOST_AUTO_TEST_CASE(load_illformated_mesh) {
368  boost::filesystem::path path(TEST_RESOURCES_DIR);
369  const std::string filename = (path / "illformated_mesh.dae").string();
370 
371  MeshLoader loader;
372  BOOST_CHECK_NO_THROW(loader.load(filename));
373 }
374 
375 BOOST_AUTO_TEST_CASE(test_convex) {
376  Box* box_ptr = new hpp::fcl::Box(1, 1, 1);
377  CollisionGeometryPtr_t b1(box_ptr);
378  BVHModel<OBBRSS> box_bvh_model = BVHModel<OBBRSS>();
379  generateBVHModel(box_bvh_model, *box_ptr, Transform3f());
380  box_bvh_model.buildConvexRepresentation(false);
381 
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());
385 }
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: shape/geometric_shapes.h:148
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::BVHModelBase::convex
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH/BVH_model.h:84
gjk.P1
tuple P1
Definition: test/scripts/gjk.py:22
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:8
hpp::fcl::Transform3f::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
octree.pos
pos
Definition: octree.py:8
hpp::fcl::Transform3f::setQuatRotation
void setQuatRotation(const Quaternion3f &q_)
set transform from rotation
Definition: transform.h:147
geometric_shapes.h
c
c
hpp::fcl::BV_AABB
Definition: collision_object.h:67
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(building_bvh_models)
Definition: bvh_models.cpp:286
BVH_utility.h
testBVHModelPointCloud
void testBVHModelPointCloud()
Definition: bvh_models.cpp:58
testLoadPolyhedron
void testLoadPolyhedron()
Definition: bvh_models.cpp:298
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
BVH_model.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::BVHModelBase::buildConvexRepresentation
void buildConvexRepresentation(bool share_memory)
Build this Convex<Triangle> representation of this model. The result is stored in attribute convex...
Definition: BVH_model.cpp:116
hpp::fcl::BV_KDOP16
Definition: collision_object.h:72
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
testBVHModelTriangles
void testBVHModelTriangles()
Definition: bvh_models.cpp:140
hpp::fcl::getNodeTypeName
std::string getNodeTypeName(NODE_TYPE node_type)
Definition: utility.cpp:325
hpp::fcl::Matrixx3f
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 3 > Matrixx3f
Definition: data_types.h:69
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
geometric_shape_to_BVH_model.h
hpp::fcl::Quaternion3f
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:46
hpp::fcl::BV_KDOP18
Definition: collision_object.h:73
hpp::fcl::BVHExtract
HPP_FCL_DLLAPI BVHModel< BV > * BVHExtract(const BVHModel< BV > &model, const Transform3f &pose, const AABB &aabb)
Extract the part of the BVHModel that is inside an AABB. A triangle in collision with the AABB is con...
Definition: BVH_utility.cpp:47
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
Definition: src/collision.cpp:63
testBVHModel
void testBVHModel()
Definition: bvh_models.cpp:280
gjk.P2
tuple P2
Definition: test/scripts/gjk.py:23
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: collision_data.h:339
a
list a
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:215
testBVHModelSubModel
void testBVHModelSubModel()
Definition: bvh_models.cpp:230
testLoadGerardBauzil
void testLoadGerardBauzil()
Definition: bvh_models.cpp:326
hpp::fcl::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:50
hpp::fcl::loadPolyhedronFromResource
void loadPolyhedronFromResource(const std::string &resource_path, const fcl::Vec3f &scale, const shared_ptr< BVHModel< BoundingVolume > > &polyhedron)
Read a mesh file and convert it to a polyhedral mesh.
Definition: assimp.h:118
hpp::fcl::MeshLoader::load
virtual BVHModelPtr_t load(const std::string &filename, const Vec3f &scale=Vec3f::Ones())
Definition: loader.cpp:66
hpp::fcl::BV_KDOP24
Definition: collision_object.h:74

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00