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 COAL_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 "coal/collision.h"
45 #include "coal/BVH/BVH_model.h"
46 #include "coal/BVH/BVH_utility.h"
47 #include "coal/math/transform.h"
48 #include "coal/shape/geometric_shapes.h"
49 #include "coal/shape/geometric_shape_to_BVH_model.h"
50 #include "coal/mesh_loader/assimp.h"
51 #include "coal/mesh_loader/loader.h"
52 #include "utility.h"
53 #include <iostream>
54 
55 using namespace coal;
56 
57 template <typename BV>
58 void testBVHModelPointCloud() {
59  Box box(Vec3s::Ones());
60  double a = box.halfSide[0];
61  double b = box.halfSide[1];
62  double c = box.halfSide[2];
63  std::vector<Vec3s> 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  MatrixX3s 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(Vec3s::Ones());
143  AABB aabb(Vec3s(-1, 0, -1), Vec3s(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<Vec3s> 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  Transform3s 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(Vec3s(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(Vec3s(0, 0, 0));
208  CoalScalar sqrt2_2 = std::sqrt(2) / 2;
209  pose.setQuatRotation(Quatf(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(-Vec3s(1, 1, 1));
217  pose.setQuatRotation(Quatf::Identity());
218  cropped.reset(BVHExtract(*model, pose, aabb));
219  BOOST_CHECK(!cropped);
220 
221  aabb = AABB(Vec3s(-0.1, -0.1, -0.1), Vec3s(0.1, 0.1, 0.1));
222  pose.setTranslation(Vec3s(-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(Vec3s::Ones());
233 
234  double a = box.halfSide[0];
235  double b = box.halfSide[1];
236  double c = box.halfSide[2];
237  std::vector<Vec3s> 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  Vec3s 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  Vec3s scale;
335  scale.setConstant(1);
336  loadPolyhedronFromResource(env, scale, P1);
337  CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27));
338  Transform3s pos(Vec3s(-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 coal::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, Transform3s());
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 }
collision.h
coal::BV_AABB
@ BV_AABB
Definition: coal/collision_object.h:66
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
coal::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:128
coal::MeshLoader
Definition: coal/mesh_loader/loader.h:52
coal::BV_KDOP24
@ BV_KDOP24
Definition: coal/collision_object.h:73
collision_manager.box
box
Definition: collision_manager.py:11
coal::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: coal/collision_data.h:443
coal::loadPolyhedronFromResource
void loadPolyhedronFromResource(const std::string &resource_path, const coal::Vec3s &scale, const shared_ptr< BVHModel< BoundingVolume > > &polyhedron)
Read a mesh file and convert it to a polyhedral mesh.
Definition: coal/mesh_loader/assimp.h:116
coal::MeshLoader::load
virtual BVHModelPtr_t load(const std::string &filename, const Vec3s &scale=Vec3s::Ones())
Definition: loader.cpp:67
loader.h
coal::getNodeTypeName
std::string getNodeTypeName(NODE_TYPE node_type)
Definition: utility.cpp:327
gjk.P2
tuple P2
Definition: test/scripts/gjk.py:22
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
coal::Quatf
Eigen::Quaternion< CoalScalar > Quatf
Definition: coal/math/transform.h:47
utility.h
coal::CachedMeshLoader::load
virtual BVHModelPtr_t load(const std::string &filename, const Vec3s &scale)
Definition: loader.cpp:102
coal::BV_KDOP18
@ BV_KDOP18
Definition: coal/collision_object.h:72
testBVHModelPointCloud
void testBVHModelPointCloud()
Definition: bvh_models.cpp:58
coal::BVH_BUILD_STATE_PROCESSED
@ BVH_BUILD_STATE_PROCESSED
Definition: coal/BVH/BVH_internal.h:53
coal::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: coal/BV/AABB.h:55
a
list a
coal::BVHExtract
COAL_DLLAPI BVHModel< BV > * BVHExtract(const BVHModel< BV > &model, const Transform3s &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
coal::Box
Center at zero point, axis aligned box.
Definition: coal/shape/geometric_shapes.h:166
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(building_bvh_models)
Definition: bvh_models.cpp:286
coal::Transform3s
Simple transform class used locally by InterpMotion.
Definition: coal/math/transform.h:55
coal::Transform3s::setQuatRotation
void setQuatRotation(const Quatf &q_)
set transform from rotation
Definition: coal/math/transform.h:148
coal::Transform3s::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: coal/math/transform.h:143
transform.h
testLoadPolyhedron
void testLoadPolyhedron()
Definition: bvh_models.cpp:298
BVH_model.h
testBVHModelTriangles
void testBVHModelTriangles()
Definition: bvh_models.cpp:140
gjk.P1
tuple P1
Definition: test/scripts/gjk.py:21
c
c
collision-bench.filename
filename
Definition: collision-bench.py:6
coal::BVHModelBase::convex
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: coal/BVH/BVH_model.h:86
coal::CollisionResult
collision result
Definition: coal/collision_data.h:390
coal::CollisionRequest
request to the collision algorithm
Definition: coal/collision_data.h:311
coal::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: coal/collision_object.h:214
octree.pos
pos
Definition: octree.py:7
coal::BVH_OK
@ BVH_OK
Definition: coal/BVH/BVH_internal.h:64
coal::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: coal/shape/geometric_shapes.h:560
coal::MatrixX3s
Eigen::Matrix< CoalScalar, Eigen::Dynamic, 3, Eigen::RowMajor > MatrixX3s
Definition: coal/data_types.h:82
collision.path
path
Definition: scripts/collision.py:6
assimp.h
testBVHModel
void testBVHModel()
Definition: bvh_models.cpp:280
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
coal::CachedMeshLoader
Definition: coal/mesh_loader/loader.h:73
coal::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: coal/BVH/BVH_model.h:314
geometric_shapes.h
BVH_utility.h
testBVHModelSubModel
void testBVHModelSubModel()
Definition: bvh_models.cpp:230
geometric_shape_to_BVH_model.h
coal::collide
COAL_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,...
Definition: src/collision.cpp:61
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
testLoadGerardBauzil
void testLoadGerardBauzil()
Definition: bvh_models.cpp:326
coal::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3s &pose)
Generate BVH model from box.
Definition: coal/shape/geometric_shape_to_BVH_model.h:49
coal::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/coal/fwd.hh:134
coal::BV_KDOP16
@ BV_KDOP16
Definition: coal/collision_object.h:71

hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:57