GCC Code Coverage Report


Directory: ./
File: test/octree.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 93 130 71.5%
Branches: 207 518 40.0%

Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, CNRS-LAAS.
5 * Copyright (c) 2023, 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 Willow Garage, Inc. 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 Florent Lamiraux <florent@laas.fr> */
37
38 #define BOOST_TEST_MODULE COAL_OCTREE
39 #include <fstream>
40 #include <boost/test/included/unit_test.hpp>
41 #include <boost/filesystem.hpp>
42
43 #include "coal/BVH/BVH_model.h"
44 #include "coal/collision.h"
45 #include "coal/distance.h"
46 #include "coal/hfield.h"
47 #include "coal/shape/geometric_shapes.h"
48 #include "coal/shape/geometric_shapes_utility.h"
49 #include "coal/internal/BV_splitter.h"
50
51 #include "utility.h"
52 #include "fcl_resources/config.h"
53
54 namespace utf = boost::unit_test::framework;
55
56 using namespace coal;
57
58 3 void makeMesh(const std::vector<Vec3s>& vertices,
59 const std::vector<Triangle>& triangles, BVHModel<OBBRSS>& model) {
60 3 coal::SplitMethodType split_method(coal::SPLIT_METHOD_MEAN);
61
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
3 model.bv_splitter.reset(new BVSplitter<OBBRSS>(split_method));
62
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
3 model.bv_splitter.reset(new BVSplitter<OBBRSS>(split_method));
63
64 3 model.beginModel();
65 3 model.addSubModel(vertices, triangles);
66 3 model.endModel();
67 3 }
68
69 coal::OcTree makeOctree(const BVHModel<OBBRSS>& mesh,
70 const Scalar& resolution) {
71 Vec3s m(mesh.aabb_local.min_);
72 Vec3s M(mesh.aabb_local.max_);
73 coal::Box box(resolution, resolution, resolution);
74 CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1);
75 CollisionResult result;
76 Transform3s tfBox;
77 octomap::OcTreePtr_t octree(new octomap::OcTree(resolution));
78
79 for (Scalar x = resolution * floor(m[0] / resolution); x <= M[0];
80 x += resolution) {
81 for (Scalar y = resolution * floor(m[1] / resolution); y <= M[1];
82 y += resolution) {
83 for (Scalar z = resolution * floor(m[2] / resolution); z <= M[2];
84 z += resolution) {
85 const Scalar half = Scalar(0.5);
86 Vec3s center(x + half * resolution, y + half * resolution,
87 z + half * resolution);
88 tfBox.setTranslation(center);
89 coal::collide(&box, tfBox, &mesh, Transform3s(), request, result);
90 if (result.isCollision()) {
91 octomap::point3d p((float)center[0], (float)center[1],
92 (float)center[2]);
93 octree->updateNode(p, true);
94 result.clear();
95 }
96 }
97 }
98 }
99
100 octree->updateInnerOccupancy();
101 octree->writeBinary("./env.octree");
102 return OcTree(octree);
103 }
104
105
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(octree_mesh) {
106 Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
107
7/14
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
4 "", "", "(", ")");
108 2 Scalar resolution(10.);
109 2 std::vector<Vec3s> pRob, pEnv;
110 2 std::vector<Triangle> tRob, tEnv;
111
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 boost::filesystem::path path(TEST_RESOURCES_DIR);
112
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
2 loadOBJFile((path / "rob.obj").string().c_str(), pRob, tRob);
113
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
2 loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv);
114
115
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 BVHModel<OBBRSS> robMesh, envMesh;
116 // Build meshes with robot and environment
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 makeMesh(pRob, tRob, robMesh);
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 makeMesh(pEnv, tEnv, envMesh);
119 // Build octomap with environment
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 envMesh.computeLocalAABB();
121 // Load octree built from envMesh by makeOctree(envMesh, resolution)
122 OcTree envOctree(
123
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
4 coal::loadOctreeFile((path / "env.octree").string(), resolution));
124
125
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 std::cout << "Finished loading octree." << std::endl;
126
127 // Test operator==
128 {
129
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK(envOctree == envOctree);
130
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK(envOctree == OcTree(envOctree));
131
132
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
4 const OcTree envOctree_from_tree(envOctree.getTree());
133
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK(envOctree == envOctree_from_tree);
134 2 }
135
136 // Test tobytes()
137 {
138
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 const std::vector<uint8_t> bytes = envOctree.tobytes();
139
10/22
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
2 BOOST_CHECK(bytes.size() > 0 && bytes.size() <= envOctree.toBoxes().size() *
140 3 * sizeof(Scalar));
141 2 }
142
143 2 std::vector<Transform3s> transforms;
144 2 Scalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
145 #ifndef NDEBUG // if debug mode
146 2 std::size_t N = 100;
147 #else
148 std::size_t N = 10000;
149 #endif
150
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 N = coal::getNbRun(utf::master_test_suite().argc,
151
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 utf::master_test_suite().argv, N);
152
153
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 generateRandomTransforms(extents, transforms, 2 * N);
154
155
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1);
156
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
202 for (std::size_t i = 0; i < N; ++i) {
157
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
200 CollisionResult resultMesh;
158
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
200 CollisionResult resultOctree;
159
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
200 Transform3s tf1(transforms[2 * i]);
160
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
200 Transform3s tf2(transforms[2 * i + 1]);
161 ;
162 // Test collision between meshes with random transform for robot.
163
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
200 coal::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh);
164 // Test collision between mesh and octree for the same transform.
165
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
200 coal::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree);
166 200 bool resMesh(resultMesh.isCollision());
167 200 bool resOctree(resultOctree.isCollision());
168
9/16
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 53 times.
✓ Branch 14 taken 47 times.
✓ Branch 15 taken 53 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 100 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 100 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 100 times.
200 BOOST_CHECK(!resMesh || resOctree);
169
3/4
✓ Branch 0 taken 47 times.
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 47 times.
200 if (!resMesh && resOctree) {
170 coal::DistanceRequest dreq;
171 coal::DistanceResult dres;
172 coal::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres);
173 std::cout << "distance mesh mesh: " << dres.min_distance << std::endl;
174 BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution);
175 }
176 200 }
177 2 }
178
179
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(octree_height_field) {
180 Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
181
7/14
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
4 "", "", "(", ")");
182 2 Scalar resolution(10.);
183 2 std::vector<Vec3s> pEnv;
184 2 std::vector<Triangle> tEnv;
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 boost::filesystem::path path(TEST_RESOURCES_DIR);
186
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
2 loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv);
187
188
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 BVHModel<OBBRSS> envMesh;
189 // Build meshes with robot and environment
190
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 makeMesh(pEnv, tEnv, envMesh);
191 // Build octomap with environment
192
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 envMesh.computeLocalAABB();
193 // Load octree built from envMesh by makeOctree(envMesh, resolution)
194 OcTree envOctree(
195
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
4 coal::loadOctreeFile((path / "env.octree").string(), resolution));
196
197
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 std::cout << "Finished loading octree." << std::endl;
198
199 // Building hfield
200 2 const Scalar x_dim = 10, y_dim = 20;
201 2 const int nx = 100, ny = 100;
202 2 const Scalar max_altitude = 1., min_altitude = 0.;
203
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude);
204
205
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 HeightField<AABB> hfield(x_dim, y_dim, heights, min_altitude);
206
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 hfield.computeLocalAABB();
207
208 2 std::vector<Transform3s> transforms;
209 2 Scalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
210 #ifndef NDEBUG // if debug mode
211 2 std::size_t N = 1000;
212 #else
213 std::size_t N = 100000;
214 #endif
215
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 N = coal::getNbRun(utf::master_test_suite().argc,
216
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 utf::master_test_suite().argv, N);
217
218
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 generateRandomTransforms(extents, transforms, 2 * N);
219
220
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1);
221
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 1 times.
2002 for (std::size_t i = 0; i < N; ++i) {
222
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
2000 CollisionResult resultBox;
223
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
2000 CollisionResult resultHfield1, resultHfield2;
224
1/2
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
2000 Transform3s tf1(transforms[2 * i]);
225
1/2
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
2000 Transform3s tf2(transforms[2 * i + 1]);
226
227
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
2000 Box box;
228
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
2000 Transform3s box_tf;
229
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
2000 constructBox(hfield.aabb_local, tf2, box, box_tf);
230
231 // Test collision between octree and equivalent box.
232
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
2000 coal::collide(&envOctree, tf1, &box, box_tf, request, resultBox);
233 // Test collision between octree and hfield.
234
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
2000 coal::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1);
235
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
2000 coal::collide(&hfield, tf2, &envOctree, tf1, request, resultHfield2);
236
237 2000 bool resBox(resultBox.isCollision());
238 2000 bool resHfield(resultHfield1.isCollision());
239
6/12
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1000 times.
2000 BOOST_CHECK(resBox == resHfield);
240
6/12
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1000 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1000 times.
2000 BOOST_CHECK(resultHfield1.isCollision() == resultHfield2.isCollision());
241
3/4
✓ Branch 0 taken 984 times.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 984 times.
2000 if (!resBox && resHfield) {
242 coal::DistanceRequest dreq;
243 coal::DistanceResult dres;
244 coal::distance(&envMesh, tf1, &box, box_tf, dreq, dres);
245 std::cout << "distance mesh box: " << dres.min_distance << std::endl;
246 BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution);
247 }
248 2000 }
249 2 }
250