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 |