GCC Code Coverage Report


Directory: ./
File: test/broadphase.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 250 338 74.0%
Branches: 447 928 48.2%

Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
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 Jia Pan */
37
38 #define BOOST_TEST_MODULE COAL_BROADPHASE
39 #include <boost/test/included/unit_test.hpp>
40
41 #include "coal/config.hh"
42 #include "coal/broadphase/broadphase.h"
43 #include "coal/shape/geometric_shape_to_BVH_model.h"
44 #include "coal/math/transform.h"
45 #include "utility.h"
46
47 #if USE_GOOGLEHASH
48 #include <sparsehash/sparse_hash_map>
49 #include <sparsehash/dense_hash_map>
50 #include <hash_map>
51 #endif
52
53 #include <boost/math/constants/constants.hpp>
54 #include <iostream>
55 #include <iomanip>
56
57 using namespace coal;
58 using namespace coal::detail;
59
60 /// @brief Generate environment with 3 * n objects for self distance, so we try
61 /// to make sure none of them collide with each other.
62 void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
63 Scalar env_scale, std::size_t n);
64
65 /// @brief Generate environment with 3 * n objects for self distance, but all in
66 /// meshes.
67 void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
68 Scalar env_scale, std::size_t n);
69
70 /// @brief test for broad phase distance
71 void broad_phase_distance_test(Scalar env_scale, std::size_t env_size,
72 std::size_t query_size, bool use_mesh = false);
73
74 /// @brief test for broad phase self distance
75 void broad_phase_self_distance_test(Scalar env_scale, std::size_t env_size,
76 bool use_mesh = false);
77
78 Scalar DELTA = Scalar(0.01);
79
80 #if USE_GOOGLEHASH
81 template <typename U, typename V>
82 struct GoogleSparseHashTable
83 : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>,
84 std::equal_to<size_t> > {};
85
86 template <typename U, typename V>
87 struct GoogleDenseHashTable
88 : public google::dense_hash_map<U, V, std::tr1::hash<size_t>,
89 std::equal_to<size_t> > {
90 GoogleDenseHashTable()
91 : google::dense_hash_map<U, V, std::tr1::hash<size_t>,
92 std::equal_to<size_t> >() {
93 this->set_empty_key(NULL);
94 }
95 };
96 #endif
97
98 // TODO(jcarpent): fix this test
99 // - test_core_bf_broad_phase_distance
100
101 /// check broad phase distance
102
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(test_core_bf_broad_phase_distance) {
103 #ifndef NDEBUG
104 2 broad_phase_distance_test(200, 100, 10);
105 2 broad_phase_distance_test(200, 1000, 10);
106 2 broad_phase_distance_test(2000, 100, 10);
107 2 broad_phase_distance_test(2000, 1000, 10);
108 #else
109 broad_phase_distance_test(200, 100, 100);
110 broad_phase_distance_test(200, 1000, 100);
111 broad_phase_distance_test(2000, 100, 100);
112 broad_phase_distance_test(2000, 1000, 100);
113 #endif
114 2 }
115
116 /// check broad phase self distance
117
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(test_core_bf_broad_phase_self_distance) {
118 2 broad_phase_self_distance_test(200, 512);
119 2 broad_phase_self_distance_test(200, 1000);
120 2 broad_phase_self_distance_test(200, 5000);
121 2 }
122
123 /// check broad phase distance
124
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(test_core_mesh_bf_broad_phase_distance_mesh) {
125 #ifndef NDEBUG
126 2 broad_phase_distance_test(200, 10, 10, true);
127 2 broad_phase_distance_test(200, 100, 10, true);
128 2 broad_phase_distance_test(2000, 10, 10, true);
129 2 broad_phase_distance_test(2000, 100, 10, true);
130 #else
131 broad_phase_distance_test(200, 100, 100, true);
132 broad_phase_distance_test(200, 1000, 100, true);
133 broad_phase_distance_test(2000, 100, 100, true);
134 broad_phase_distance_test(2000, 1000, 100, true);
135 #endif
136 2 }
137
138 /// check broad phase self distance
139
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(test_core_mesh_bf_broad_phase_self_distance_mesh) {
140 2 broad_phase_self_distance_test(200, 512, true);
141 2 broad_phase_self_distance_test(200, 1000, true);
142 2 broad_phase_self_distance_test(200, 5000, true);
143 2 }
144
145 3 void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
146 Scalar env_scale, std::size_t n) {
147 3 int n_edge = static_cast<int>(std::floor(std::pow(n, 1 / 3.0)));
148
149 3 Scalar step_size = env_scale * 2 / Scalar(n_edge);
150 3 Scalar delta_size = step_size * Scalar(0.05);
151 3 Scalar single_size = step_size - 2 * delta_size;
152
153 3 int i = 0;
154
2/2
✓ Branch 0 taken 1495 times.
✓ Branch 1 taken 3 times.
1498 for (; i < n_edge * n_edge * n_edge / 4; ++i) {
155 1495 int x = i % (n_edge * n_edge);
156 1495 int y = (i - n_edge * n_edge * x) % n_edge;
157 1495 int z = i - n_edge * n_edge * x - n_edge * y;
158
159
1/2
✓ Branch 2 taken 1495 times.
✗ Branch 3 not taken.
1495 Box* box = new Box(single_size, single_size, single_size);
160 1495 const Scalar half = Scalar(0.5);
161
1/2
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
1495 env.push_back(new CollisionObject(
162 1495 shared_ptr<CollisionGeometry>(box),
163
1/2
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
1495 Transform3s(Vec3s(
164 Scalar(x) * step_size + delta_size + half * single_size - env_scale,
165 Scalar(y) * step_size + delta_size + half * single_size - env_scale,
166
1/2
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
2990 Scalar(z) * step_size + delta_size + half * single_size -
167
2/4
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1495 times.
✗ Branch 5 not taken.
1495 env_scale))));
168 1495 env.back()->collisionGeometry()->computeLocalAABB();
169 }
170
171
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
3 for (; i < n_edge * n_edge * n_edge / 4; ++i) {
172 int x = i % (n_edge * n_edge);
173 int y = (i - n_edge * n_edge * x) % n_edge;
174 int z = i - n_edge * n_edge * x - n_edge * y;
175
176 Sphere* sphere = new Sphere(single_size / 2);
177 const Scalar half = Scalar(0.5);
178 env.push_back(new CollisionObject(
179 shared_ptr<CollisionGeometry>(sphere),
180 Transform3s(Vec3s(
181 Scalar(x) * step_size + delta_size + half * single_size - env_scale,
182 Scalar(y) * step_size + delta_size + half * single_size - env_scale,
183 Scalar(z) * step_size + delta_size + half * single_size -
184 env_scale))));
185 env.back()->collisionGeometry()->computeLocalAABB();
186 }
187
188
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
3 for (; i < n_edge * n_edge * n_edge / 4; ++i) {
189 int x = i % (n_edge * n_edge);
190 int y = (i - n_edge * n_edge * x) % n_edge;
191 int z = i - n_edge * n_edge * x - n_edge * y;
192
193 Cylinder* cylinder = new Cylinder(single_size / 2, single_size);
194 const Scalar half = Scalar(0.5);
195 env.push_back(new CollisionObject(
196 shared_ptr<CollisionGeometry>(cylinder),
197 Transform3s(Vec3s(
198 Scalar(x) * step_size + delta_size + half * single_size - env_scale,
199 Scalar(y) * step_size + delta_size + half * single_size - env_scale,
200 Scalar(z) * step_size + delta_size + half * single_size -
201 env_scale))));
202 env.back()->collisionGeometry()->computeLocalAABB();
203 }
204
205
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
3 for (; i < n_edge * n_edge * n_edge / 4; ++i) {
206 int x = i % (n_edge * n_edge);
207 int y = (i - n_edge * n_edge * x) % n_edge;
208 int z = i - n_edge * n_edge * x - n_edge * y;
209
210 Cone* cone = new Cone(single_size / 2, single_size);
211 const Scalar half = Scalar(0.5);
212 env.push_back(new CollisionObject(
213 shared_ptr<CollisionGeometry>(cone),
214 Transform3s(Vec3s(
215 Scalar(x) * step_size + delta_size + half * single_size - env_scale,
216 Scalar(y) * step_size + delta_size + half * single_size - env_scale,
217 Scalar(z) * step_size + delta_size + half * single_size -
218 env_scale))));
219 env.back()->collisionGeometry()->computeLocalAABB();
220 }
221 3 }
222
223 3 void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
224 Scalar env_scale, std::size_t n) {
225 3 int n_edge = static_cast<int>(std::floor(std::pow(n, 1 / 3.0)));
226
227 3 Scalar step_size = env_scale * 2 / Scalar(n_edge);
228 3 Scalar delta_size = step_size * Scalar(0.05);
229 3 Scalar single_size = step_size - 2 * delta_size;
230
231 3 int i = 0;
232
2/2
✓ Branch 0 taken 1495 times.
✓ Branch 1 taken 3 times.
1498 for (; i < n_edge * n_edge * n_edge / 4; ++i) {
233 1495 int x = i % (n_edge * n_edge);
234 1495 int y = (i - n_edge * n_edge * x) % n_edge;
235 1495 int z = i - n_edge * n_edge * x - n_edge * y;
236
237
1/2
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
1495 Box box(single_size, single_size, single_size);
238
2/4
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1495 times.
✗ Branch 5 not taken.
1495 BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
239
2/4
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1495 times.
✗ Branch 5 not taken.
1495 generateBVHModel(*model, box, Transform3s());
240 1495 const Scalar half = Scalar(0.5);
241
1/2
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
1495 env.push_back(new CollisionObject(
242
1/2
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
1495 shared_ptr<CollisionGeometry>(model),
243
1/2
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
1495 Transform3s(Vec3s(
244 Scalar(x) * step_size + delta_size + half * single_size - env_scale,
245 Scalar(y) * step_size + delta_size + half * single_size - env_scale,
246
1/2
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
2990 Scalar(z) * step_size + delta_size + half * single_size -
247
2/4
✓ Branch 1 taken 1495 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1495 times.
✗ Branch 5 not taken.
1495 env_scale))));
248
1/2
✓ Branch 4 taken 1495 times.
✗ Branch 5 not taken.
1495 env.back()->collisionGeometry()->computeLocalAABB();
249 1495 }
250
251
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
3 for (; i < n_edge * n_edge * n_edge / 4; ++i) {
252 int x = i % (n_edge * n_edge);
253 int y = (i - n_edge * n_edge * x) % n_edge;
254 int z = i - n_edge * n_edge * x - n_edge * y;
255
256 Sphere sphere(single_size / 2);
257 BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
258 generateBVHModel(*model, sphere, Transform3s(), 16, 16);
259 const Scalar half = Scalar(0.5);
260 env.push_back(new CollisionObject(
261 shared_ptr<CollisionGeometry>(model),
262 Transform3s(Vec3s(
263 Scalar(x) * step_size + delta_size + half * single_size - env_scale,
264 Scalar(y) * step_size + delta_size + half * single_size - env_scale,
265 Scalar(z) * step_size + delta_size + half * single_size -
266 env_scale))));
267 env.back()->collisionGeometry()->computeLocalAABB();
268 }
269
270
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
3 for (; i < n_edge * n_edge * n_edge / 4; ++i) {
271 int x = i % (n_edge * n_edge);
272 int y = (i - n_edge * n_edge * x) % n_edge;
273 int z = i - n_edge * n_edge * x - n_edge * y;
274
275 Cylinder cylinder(single_size / 2, single_size);
276 BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
277 generateBVHModel(*model, cylinder, Transform3s(), 16, 16);
278 const Scalar half = Scalar(0.5);
279 env.push_back(new CollisionObject(
280 shared_ptr<CollisionGeometry>(model),
281 Transform3s(Vec3s(
282 Scalar(x) * step_size + delta_size + half * single_size - env_scale,
283 Scalar(y) * step_size + delta_size + half * single_size - env_scale,
284 Scalar(z) * step_size + delta_size + half * single_size -
285 env_scale))));
286 env.back()->collisionGeometry()->computeLocalAABB();
287 }
288
289
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
3 for (; i < n_edge * n_edge * n_edge / 4; ++i) {
290 int x = i % (n_edge * n_edge);
291 int y = (i - n_edge * n_edge * x) % n_edge;
292 int z = i - n_edge * n_edge * x - n_edge * y;
293
294 Cone cone(single_size / 2, single_size);
295 BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
296 generateBVHModel(*model, cone, Transform3s(), 16, 16);
297 const Scalar half = Scalar(0.5);
298 env.push_back(new CollisionObject(
299 shared_ptr<CollisionGeometry>(model),
300 Transform3s(Vec3s(
301 Scalar(x) * step_size + delta_size + half * single_size - env_scale,
302 Scalar(y) * step_size + delta_size + half * single_size - env_scale,
303 Scalar(z) * step_size + delta_size + half * single_size -
304 env_scale))));
305 env.back()->collisionGeometry()->computeLocalAABB();
306 }
307 3 }
308
309 6 void broad_phase_self_distance_test(Scalar env_scale, std::size_t env_size,
310 bool use_mesh) {
311 6 std::vector<TStruct> ts;
312 6 std::vector<BenchTimer> timers;
313
314 6 std::vector<CollisionObject*> env;
315
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
6 if (use_mesh)
316
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
3 generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size);
317 else
318
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
3 generateSelfDistanceEnvironments(env, env_scale, env_size);
319
320 6 std::vector<BroadPhaseCollisionManager*> managers;
321
322
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 managers.push_back(new NaiveCollisionManager());
323
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 managers.push_back(new SSaPCollisionManager());
324
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 managers.push_back(new SaPCollisionManager());
325
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 managers.push_back(new IntervalTreeCollisionManager());
326
327
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 Vec3s lower_limit, upper_limit;
328
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
329
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5,
330
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 (upper_limit[1] - lower_limit[1]) / 5),
331
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 (upper_limit[2] - lower_limit[2]) / 5);
332 // managers.push_back(new SpatialHashingCollisionManager<>(cell_size,
333 // lower_limit, upper_limit));
334
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 managers.push_back(new SpatialHashingCollisionManager<
335 SparseHashTable<AABB, CollisionObject*, SpatialHash> >(
336
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 cell_size, lower_limit, upper_limit));
337 #if USE_GOOGLEHASH
338 managers.push_back(
339 new SpatialHashingCollisionManager<SparseHashTable<
340 AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(
341 cell_size, lower_limit, upper_limit));
342 managers.push_back(
343 new SpatialHashingCollisionManager<SparseHashTable<
344 AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(
345 cell_size, lower_limit, upper_limit));
346 #endif
347
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 managers.push_back(new DynamicAABBTreeCollisionManager());
348
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 managers.push_back(new DynamicAABBTreeArrayCollisionManager());
349
350 {
351
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
352 6 m->tree_init_level = 2;
353
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 managers.push_back(m);
354 }
355
356 {
357 DynamicAABBTreeArrayCollisionManager* m =
358
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 new DynamicAABBTreeArrayCollisionManager();
359 6 m->tree_init_level = 2;
360
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 managers.push_back(m);
361 }
362
363
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
6 ts.resize(managers.size());
364
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
6 timers.resize(managers.size());
365
366
2/2
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 6 times.
60 for (size_t i = 0; i < managers.size(); ++i) {
367
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
54 timers[i].start();
368
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
54 managers[i]->registerObjects(env);
369
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
54 timers[i].stop();
370
2/4
✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
54 ts[i].push_back(timers[i].getElapsedTime());
371 }
372
373
2/2
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 6 times.
60 for (size_t i = 0; i < managers.size(); ++i) {
374
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
54 timers[i].start();
375
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
54 managers[i]->setup();
376
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
54 timers[i].stop();
377
2/4
✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
54 ts[i].push_back(timers[i].getElapsedTime());
378 }
379
380
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
6 std::vector<DistanceCallBackDefault> self_callbacks(managers.size());
381
382
2/2
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 6 times.
60 for (size_t i = 0; i < self_callbacks.size(); ++i) {
383
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
54 timers[i].start();
384
1/2
✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
54 managers[i]->distance(&self_callbacks[i]);
385
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
54 timers[i].stop();
386
2/4
✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
54 ts[i].push_back(timers[i].getElapsedTime());
387 // std::cout << self_data[i].result.min_distance << " ";
388 }
389 // std::cout << std::endl;
390
391
2/2
✓ Branch 1 taken 48 times.
✓ Branch 2 taken 6 times.
54 for (size_t i = 1; i < managers.size(); ++i)
392
7/16
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 48 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 48 times.
✗ Branch 12 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 48 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✓ Branch 23 taken 48 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 48 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 48 times.
48 BOOST_CHECK(fabs(self_callbacks[0].data.result.min_distance -
393 self_callbacks[i].data.result.min_distance) < DELTA ||
394 fabs(self_callbacks[0].data.result.min_distance -
395 self_callbacks[i].data.result.min_distance) /
396 fabs(self_callbacks[0].data.result.min_distance) <
397 DELTA);
398
399
3/4
✓ Branch 1 taken 2990 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 2990 times.
✓ Branch 7 taken 6 times.
2996 for (size_t i = 0; i < env.size(); ++i) delete env[i];
400
401
3/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 54 times.
✓ Branch 6 taken 6 times.
60 for (size_t i = 0; i < managers.size(); ++i) delete managers[i];
402
403 6 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
404 6 int w = 7;
405
406
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 std::cout << "self distance timing summary" << std::endl;
407
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
6 std::cout << env.size() << " objs" << std::endl;
408
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 std::cout << "register time" << std::endl;
409
2/2
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 6 times.
60 for (size_t i = 0; i < ts.size(); ++i)
410
3/6
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
54 std::cout << std::setw(w) << ts[i].records[0] << " ";
411
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 std::cout << std::endl;
412
413
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 std::cout << "setup time" << std::endl;
414
2/2
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 6 times.
60 for (size_t i = 0; i < ts.size(); ++i)
415
3/6
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
54 std::cout << std::setw(w) << ts[i].records[1] << " ";
416
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 std::cout << std::endl;
417
418
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 std::cout << "self distance time" << std::endl;
419
2/2
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 6 times.
60 for (size_t i = 0; i < ts.size(); ++i)
420
3/6
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
54 std::cout << std::setw(w) << ts[i].records[2] << " ";
421
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 std::cout << std::endl;
422
423
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
6 std::cout << "overall time" << std::endl;
424
2/2
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 6 times.
60 for (size_t i = 0; i < ts.size(); ++i)
425
3/6
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 54 times.
✗ Branch 10 not taken.
54 std::cout << std::setw(w) << ts[i].overall_time << " ";
426
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 std::cout << std::endl;
427
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 std::cout << std::endl;
428 6 }
429
430 8 void broad_phase_distance_test(Scalar env_scale, std::size_t env_size,
431 std::size_t query_size, bool use_mesh) {
432 8 std::vector<TStruct> ts;
433 8 std::vector<BenchTimer> timers;
434
435 8 std::vector<CollisionObject*> env;
436
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
8 if (use_mesh)
437
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
4 generateEnvironmentsMesh(env, env_scale, env_size);
438 else
439
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
4 generateEnvironments(env, env_scale, env_size);
440
441 8 std::vector<CollisionObject*> query;
442
443
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 BroadPhaseCollisionManager* manager = new NaiveCollisionManager();
444
3/4
✓ Branch 2 taken 7260 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7260 times.
✓ Branch 6 taken 8 times.
7268 for (std::size_t i = 0; i < env.size(); ++i) manager->registerObject(env[i]);
445
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 manager->setup();
446
447 while (1) {
448 32 std::vector<CollisionObject*> candidates;
449
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 28 times.
32 if (use_mesh)
450
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
4 generateEnvironmentsMesh(candidates, env_scale, query_size);
451 else
452
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
28 generateEnvironments(candidates, env_scale, query_size);
453
454
2/2
✓ Branch 1 taken 824 times.
✓ Branch 2 taken 24 times.
848 for (std::size_t i = 0; i < candidates.size(); ++i) {
455
1/2
✓ Branch 1 taken 824 times.
✗ Branch 2 not taken.
824 CollisionCallBackDefault callback;
456
1/2
✓ Branch 2 taken 824 times.
✗ Branch 3 not taken.
824 manager->collide(candidates[i], &callback);
457
2/2
✓ Branch 1 taken 80 times.
✓ Branch 2 taken 744 times.
824 if (callback.data.result.numContacts() == 0)
458
1/2
✓ Branch 2 taken 80 times.
✗ Branch 3 not taken.
80 query.push_back(candidates[i]);
459 else
460
1/2
✓ Branch 1 taken 744 times.
✗ Branch 2 not taken.
744 delete candidates[i];
461
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 816 times.
824 if (query.size() == query_size) break;
462
2/2
✓ Branch 1 taken 816 times.
✓ Branch 2 taken 8 times.
824 }
463
464
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 24 times.
32 if (query.size() == query_size) break;
465
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 8 times.
56 }
466
467
1/2
✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
8 delete manager;
468
469 8 std::vector<BroadPhaseCollisionManager*> managers;
470
471
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
8 managers.push_back(new NaiveCollisionManager());
472
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
8 managers.push_back(new SSaPCollisionManager());
473
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
8 managers.push_back(new SaPCollisionManager());
474
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
8 managers.push_back(new IntervalTreeCollisionManager());
475
476
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 Vec3s lower_limit, upper_limit;
477
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
478
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
479
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 (upper_limit[1] - lower_limit[1]) / 20),
480
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
8 (upper_limit[2] - lower_limit[2]) / 20);
481 // managers.push_back(new SpatialHashingCollisionManager<>(cell_size,
482 // lower_limit, upper_limit));
483
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 managers.push_back(new SpatialHashingCollisionManager<
484 SparseHashTable<AABB, CollisionObject*, SpatialHash> >(
485
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 cell_size, lower_limit, upper_limit));
486 #if USE_GOOGLEHASH
487 managers.push_back(
488 new SpatialHashingCollisionManager<SparseHashTable<
489 AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(
490 cell_size, lower_limit, upper_limit));
491 managers.push_back(
492 new SpatialHashingCollisionManager<SparseHashTable<
493 AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(
494 cell_size, lower_limit, upper_limit));
495 #endif
496
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
8 managers.push_back(new DynamicAABBTreeCollisionManager());
497
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
8 managers.push_back(new DynamicAABBTreeArrayCollisionManager());
498
499 {
500
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
501 8 m->tree_init_level = 2;
502
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 managers.push_back(m);
503 }
504
505 {
506 DynamicAABBTreeArrayCollisionManager* m =
507
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 new DynamicAABBTreeArrayCollisionManager();
508 8 m->tree_init_level = 2;
509
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 managers.push_back(m);
510 }
511
512
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
8 ts.resize(managers.size());
513
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
8 timers.resize(managers.size());
514
515
2/2
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 8 times.
80 for (size_t i = 0; i < managers.size(); ++i) {
516
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
72 timers[i].start();
517
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
72 managers[i]->registerObjects(env);
518
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
72 timers[i].stop();
519
2/4
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 72 times.
✗ Branch 7 not taken.
72 ts[i].push_back(timers[i].getElapsedTime());
520 }
521
522
2/2
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 8 times.
80 for (size_t i = 0; i < managers.size(); ++i) {
523
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
72 timers[i].start();
524
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
72 managers[i]->setup();
525
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
72 timers[i].stop();
526
2/4
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 72 times.
✗ Branch 7 not taken.
72 ts[i].push_back(timers[i].getElapsedTime());
527 }
528
529
2/2
✓ Branch 1 taken 80 times.
✓ Branch 2 taken 8 times.
88 for (size_t i = 0; i < query.size(); ++i) {
530
1/2
✓ Branch 3 taken 80 times.
✗ Branch 4 not taken.
80 std::vector<DistanceCallBackDefault> query_callbacks(managers.size());
531
2/2
✓ Branch 1 taken 720 times.
✓ Branch 2 taken 80 times.
800 for (size_t j = 0; j < managers.size(); ++j) {
532
1/2
✓ Branch 2 taken 720 times.
✗ Branch 3 not taken.
720 timers[j].start();
533
1/2
✓ Branch 4 taken 720 times.
✗ Branch 5 not taken.
720 managers[j]->distance(query[i], &query_callbacks[j]);
534
1/2
✓ Branch 2 taken 720 times.
✗ Branch 3 not taken.
720 timers[j].stop();
535
2/4
✓ Branch 3 taken 720 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 720 times.
✗ Branch 7 not taken.
720 ts[j].push_back(timers[j].getElapsedTime());
536
2/4
✓ Branch 2 taken 720 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 720 times.
✗ Branch 6 not taken.
720 std::cout << query_callbacks[j].data.result.min_distance << " ";
537 }
538
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
80 std::cout << std::endl;
539
540
2/2
✓ Branch 1 taken 640 times.
✓ Branch 2 taken 80 times.
720 for (size_t j = 1; j < managers.size(); ++j) {
541 640 bool test = fabs(query_callbacks[0].data.result.min_distance -
542
2/2
✓ Branch 1 taken 152 times.
✓ Branch 2 taken 488 times.
792 query_callbacks[j].data.result.min_distance) < DELTA ||
543 152 fabs(query_callbacks[0].data.result.min_distance -
544 152 query_callbacks[j].data.result.min_distance) /
545
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 151 times.
152 fabs(query_callbacks[0].data.result.min_distance) <
546 640 DELTA;
547
6/12
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 640 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 640 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 640 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 640 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 640 times.
640 BOOST_CHECK(test);
548
549
2/2
✓ Branch 0 taken 151 times.
✓ Branch 1 taken 489 times.
640 if (!test) {
550 151 const BroadPhaseCollisionManager& self = *managers[j];
551
3/6
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 151 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 151 times.
✗ Branch 9 not taken.
151 std::cout << "j: " << typeid(self).name() << std::endl;
552
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
151 std::cout << "query_callbacks[0].data.result.min_distance: "
553
2/4
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 151 times.
✗ Branch 6 not taken.
151 << query_callbacks[0].data.result.min_distance << std::endl;
554
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
151 std::cout << "query_callbacks[j].data.result.min_distance: "
555
2/4
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 151 times.
✗ Branch 6 not taken.
151 << query_callbacks[j].data.result.min_distance << std::endl;
556 }
557 }
558 80 }
559
560
3/4
✓ Branch 1 taken 7260 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 7260 times.
✓ Branch 7 taken 8 times.
7268 for (std::size_t i = 0; i < env.size(); ++i) delete env[i];
561
3/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 80 times.
✓ Branch 7 taken 8 times.
88 for (std::size_t i = 0; i < query.size(); ++i) delete query[i];
562
563
3/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 72 times.
✓ Branch 6 taken 8 times.
80 for (size_t i = 0; i < managers.size(); ++i) delete managers[i];
564
565 8 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
566 8 int w = 7;
567
568
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 std::cout << "distance timing summary" << std::endl;
569
5/10
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
8 std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
570
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 std::cout << "register time" << std::endl;
571
2/2
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 8 times.
80 for (size_t i = 0; i < ts.size(); ++i)
572
3/6
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
72 std::cout << std::setw(w) << ts[i].records[0] << " ";
573
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 std::cout << std::endl;
574
575
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 std::cout << "setup time" << std::endl;
576
2/2
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 8 times.
80 for (size_t i = 0; i < ts.size(); ++i)
577
3/6
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
72 std::cout << std::setw(w) << ts[i].records[1] << " ";
578
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 std::cout << std::endl;
579
580
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 std::cout << "distance time" << std::endl;
581
2/2
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 8 times.
80 for (size_t i = 0; i < ts.size(); ++i) {
582 72 double tmp = 0;
583
2/2
✓ Branch 4 taken 720 times.
✓ Branch 5 taken 72 times.
792 for (size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
584
3/6
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 72 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 72 times.
✗ Branch 9 not taken.
72 std::cout << std::setw(w) << tmp << " ";
585 }
586
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 std::cout << std::endl;
587
588
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
8 std::cout << "overall time" << std::endl;
589
2/2
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 8 times.
80 for (size_t i = 0; i < ts.size(); ++i)
590
3/6
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 72 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 72 times.
✗ Branch 10 not taken.
72 std::cout << std::setw(w) << ts[i].overall_time << " ";
591
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 std::cout << std::endl;
592
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 std::cout << std::endl;
593 8 }
594