GCC Code Coverage Report

Directory: ./
File: test/broadphase_collision_1.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 258 289 89.3%
Branches: 590 1198 49.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-2016, Open Source Robotics Foundation
6 * Copyright (c) 2022, INRIA
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Open Source Robotics Foundation nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 */
36 /** @author Jia Pan */
37
38 #define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_1
39 #include <boost/test/included/unit_test.hpp>
40
41 #include "coal/broadphase/broadphase_bruteforce.h"
42 #include "coal/broadphase/broadphase_spatialhash.h"
43 #include "coal/broadphase/broadphase_SaP.h"
44 #include "coal/broadphase/broadphase_SSaP.h"
45 #include "coal/broadphase/broadphase_interval_tree.h"
46 #include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
47 #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
48 #include "coal/broadphase/default_broadphase_callbacks.h"
49 #include "coal/broadphase/detail/sparse_hash_table.h"
50 #include "coal/broadphase/detail/spatial_hash.h"
51 #include "utility.h"
52
53 #include <boost/math/constants/constants.hpp>
54
55 #if USE_GOOGLEHASH
56 #include <sparsehash/sparse_hash_map>
57 #include <sparsehash/dense_hash_map>
58 #include <hash_map>
59 #endif
60
61 #include <iostream>
62 #include <iomanip>
63
64 using namespace coal;
65
66 /// @brief make sure if broadphase algorithms doesn't check twice for the same
67 /// collision object pair
68 void broad_phase_duplicate_check_test(Scalar env_scale, std::size_t env_size,
69 bool verbose = false);
70
71 /// @brief test for broad phase update
72 void broad_phase_update_collision_test(Scalar env_scale, std::size_t env_size,
73 std::size_t query_size,
74 std::size_t num_max_contacts = 1,
75 bool exhaustive = false,
76 bool use_mesh = false);
77
78 #if USE_GOOGLEHASH
79 template <typename U, typename V>
80 struct GoogleSparseHashTable
81 : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>,
82 std::equal_to<size_t>> {};
83
84 template <typename U, typename V>
85 struct GoogleDenseHashTable
86 : public google::dense_hash_map<U, V, std::tr1::hash<size_t>,
87 std::equal_to<size_t>> {
88 GoogleDenseHashTable()
89 : google::dense_hash_map<U, V, std::tr1::hash<size_t>,
90 std::equal_to<size_t>>() {
91 this->set_empty_key(nullptr);
92 }
93 };
94 #endif
95
96 /// make sure if broadphase algorithms doesn't check twice for the same
97 /// collision object pair
98
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_broad_phase_dont_duplicate_check) {
99 #ifdef NDEBUG
100 broad_phase_duplicate_check_test(2000, 1000);
101 #else
102 2 broad_phase_duplicate_check_test(2000, 100);
103 #endif
104 2 }
105
106 /// check the update, only return collision or not
107
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_update_collision_binary) {
108 #ifdef NDEBUG
109 broad_phase_update_collision_test(2000, 100, 1000, 1, false);
110 broad_phase_update_collision_test(2000, 1000, 1000, 1, false);
111 #else
112 2 broad_phase_update_collision_test(2000, 10, 100, 1, false);
113 2 broad_phase_update_collision_test(2000, 100, 100, 1, false);
114 #endif
115 2 }
116
117 /// check the update, return 10 contacts
118
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_update_collision) {
119 #ifdef NDEBUG
120 broad_phase_update_collision_test(2000, 100, 1000, 10, false);
121 broad_phase_update_collision_test(2000, 1000, 1000, 10, false);
122 #else
123 2 broad_phase_update_collision_test(2000, 10, 100, 10, false);
124 2 broad_phase_update_collision_test(2000, 100, 100, 10, false);
125 #endif
126 2 }
127
128 /// check the update, exhaustive
129
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_update_collision_exhaustive) {
130 #ifdef NDEBUG
131 broad_phase_update_collision_test(2000, 100, 1000, 1, true);
132 broad_phase_update_collision_test(2000, 1000, 1000, 1, true);
133 #else
134 2 broad_phase_update_collision_test(2000, 10, 100, 1, true);
135 2 broad_phase_update_collision_test(2000, 100, 100, 1, true);
136 #endif
137 2 }
138
139 /// check broad phase update, in mesh, only return collision or not
140
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(
141 test_core_mesh_bf_broad_phase_update_collision_mesh_binary) {
142 #ifdef NDEBUG
143 broad_phase_update_collision_test(2000, 100, 1000, 1, false, true);
144 broad_phase_update_collision_test(2000, 1000, 1000, 1, false, true);
145 #else
146 2 broad_phase_update_collision_test(2000, 2, 4, 1, false, true);
147 2 broad_phase_update_collision_test(2000, 4, 4, 1, false, true);
148 #endif
149 2 }
150
151 /// check broad phase update, in mesh, return 10 contacts
152
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_update_collision_mesh) {
153 #ifdef NDEBUG
154 broad_phase_update_collision_test(2000, 100, 1000, 10, false, true);
155 broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true);
156 #else
157 2 broad_phase_update_collision_test(200, 2, 4, 10, false, true);
158 2 broad_phase_update_collision_test(200, 4, 4, 10, false, true);
159 #endif
160 2 }
161
162 /// check broad phase update, in mesh, exhaustive
163
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(
164 test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) {
165 #ifdef NDEBUG
166 broad_phase_update_collision_test(2000, 100, 1000, 1, true, true);
167 broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true);
168 #else
169 2 broad_phase_update_collision_test(2000, 2, 4, 1, true, true);
170 2 broad_phase_update_collision_test(2000, 4, 4, 1, true, true);
171 #endif
172 2 }
173
174 //==============================================================================
175 struct CollisionDataForUniquenessChecking {
176 std::set<std::pair<CollisionObject*, CollisionObject*>> checkedPairs;
177
178 43 bool checkUniquenessAndAddPair(CollisionObject* o1, CollisionObject* o2) {
179
2/4
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
 43 auto search = checkedPairs.find(std::make_pair(o1, o2));
180
181
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 43 times.
 43 if (search != checkedPairs.end()) return false;
182
183
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 checkedPairs.emplace(o1, o2);
184
185 43 return true;
186 }
187 };
188
189 //==============================================================================
190 struct CollisionFunctionForUniquenessChecking : CollisionCallBackBase {
191 43 bool collide(CollisionObject* o1, CollisionObject* o2) {
192
6/12
✓ Branch 3 taken 43 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 43 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 43 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 43 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 43 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 43 times.
 43 BOOST_CHECK(data.checkUniquenessAndAddPair(o1, o2));
193 43 return false;
194 }
195
196 CollisionDataForUniquenessChecking data;
197 };
198
199 //==============================================================================
200 1 void broad_phase_duplicate_check_test(Scalar env_scale, std::size_t env_size,
201 bool verbose) {
202 1 std::vector<TStruct> ts;
203 1 std::vector<BenchTimer> timers;
204
205 1 std::vector<CollisionObject*> env;
206
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 generateEnvironments(env, env_scale, env_size);
207
208 1 std::vector<BroadPhaseCollisionManager*> managers;
209
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 managers.push_back(new NaiveCollisionManager());
210
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 managers.push_back(new SSaPCollisionManager());
211
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 managers.push_back(new SaPCollisionManager());
212
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 managers.push_back(new IntervalTreeCollisionManager());
213
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Vec3s lower_limit, upper_limit;
214
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
215
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
216
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 (upper_limit[1] - lower_limit[1]) / 20),
217
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 (upper_limit[2] - lower_limit[2]) / 20);
218 1 managers.push_back(
219
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 new SpatialHashingCollisionManager<
220 detail::SparseHashTable<AABB, CollisionObject*, detail::SpatialHash>>(
221
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 cell_size, lower_limit, upper_limit));
222 #if USE_GOOGLEHASH
223 managers.push_back(
224 new SpatialHashingCollisionManager<detail::SparseHashTable<
225 AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable>>(
226 cell_size, lower_limit, upper_limit));
227 managers.push_back(
228 new SpatialHashingCollisionManager<detail::SparseHashTable<
229 AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable>>(
230 cell_size, lower_limit, upper_limit));
231 #endif
232
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 managers.push_back(new DynamicAABBTreeCollisionManager());
233
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 managers.push_back(new DynamicAABBTreeArrayCollisionManager());
234
235 {
236
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
237 1 m->tree_init_level = 2;
238
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 managers.push_back(m);
239 }
240
241 {
242 DynamicAABBTreeArrayCollisionManager* m =
243
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 new DynamicAABBTreeArrayCollisionManager();
244 1 m->tree_init_level = 2;
245
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 managers.push_back(m);
246 }
247
248
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ts.resize(managers.size());
249
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 timers.resize(managers.size());
250
251
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
 10 for (size_t i = 0; i < managers.size(); ++i) {
252
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 timers[i].start();
253
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 managers[i]->registerObjects(env);
254
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 timers[i].stop();
255
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 ts[i].push_back(timers[i].getElapsedTime());
256 }
257
258
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
 10 for (size_t i = 0; i < managers.size(); ++i) {
259
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 timers[i].start();
260
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 managers[i]->setup();
261
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 timers[i].stop();
262
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 ts[i].push_back(timers[i].getElapsedTime());
263 }
264
265 // update the environment
266 1 Scalar delta_angle_max =
267 Scalar(10) / Scalar(360 * 2) * boost::math::constants::pi<Scalar>();
268 1 Scalar delta_trans_max = Scalar(0.01) * env_scale;
269 1 const Scalar half = Scalar(0.5);
270
2/2
✓ Branch 1 taken 300 times.
✓ Branch 2 taken 1 times.
 301 for (size_t i = 0; i < env.size(); ++i) {
271 Scalar rand_angle_x =
272 300 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max;
273 Scalar rand_trans_x =
274 300 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max;
275 Scalar rand_angle_y =
276 300 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max;
277 Scalar rand_trans_y =
278 300 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max;
279 Scalar rand_angle_z =
280 300 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max;
281 Scalar rand_trans_z =
282 300 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max;
283
284 using AngleAxis = Eigen::AngleAxis<Scalar>;
285
3/6
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
 300 Matrix3s dR(AngleAxis(rand_angle_x, Vec3s::UnitX()) *
286
3/6
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
 600 AngleAxis(rand_angle_y, Vec3s::UnitY()) *
287
3/6
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
 600 AngleAxis(rand_angle_z, Vec3s::UnitZ()));
288
1/2
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
 300 Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z);
289
290
1/2
✓ Branch 3 taken 300 times.
✗ Branch 4 not taken.
 300 Matrix3s R = env[i]->getRotation();
291
1/2
✓ Branch 3 taken 300 times.
✗ Branch 4 not taken.
 300 Vec3s T = env[i]->getTranslation();
292
6/12
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 300 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 300 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 300 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 300 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 300 times.
✗ Branch 18 not taken.
 300 env[i]->setTransform(dR * R, dR * T + dT);
293
1/2
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
 300 env[i]->computeAABB();
294 }
295
296
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
 10 for (size_t i = 0; i < managers.size(); ++i) {
297
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 timers[i].start();
298
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 managers[i]->update();
299
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 timers[i].stop();
300
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 ts[i].push_back(timers[i].getElapsedTime());
301 }
302
303
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::vector<CollisionDataForUniquenessChecking> self_data(managers.size());
304
305
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
 10 for (size_t i = 0; i < managers.size(); ++i) {
306 9 CollisionFunctionForUniquenessChecking callback;
307
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 timers[i].start();
308
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 managers[i]->collide(&callback);
309
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 timers[i].stop();
310
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 ts[i].push_back(timers[i].getElapsedTime());
311 9 }
312
313
3/4
✓ Branch 3 taken 300 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 300 times.
✓ Branch 10 taken 1 times.
 301 for (auto obj : env) delete obj;
314
315
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (!verbose) return;
316
317 ✗ std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
318 ✗ int w = 7;
319
320 ✗ std::cout << "collision timing summary" << std::endl;
321 ✗ std::cout << env_size << " objs" << std::endl;
322 ✗ std::cout << "register time" << std::endl;
323 ✗ for (size_t i = 0; i < ts.size(); ++i)
324 ✗ std::cout << std::setw(w) << ts[i].records[0] << " ";
325 ✗ std::cout << std::endl;
326
327 ✗ std::cout << "setup time" << std::endl;
328 ✗ for (size_t i = 0; i < ts.size(); ++i)
329 ✗ std::cout << std::setw(w) << ts[i].records[1] << " ";
330 ✗ std::cout << std::endl;
331
332 ✗ std::cout << "update time" << std::endl;
333 ✗ for (size_t i = 0; i < ts.size(); ++i)
334 ✗ std::cout << std::setw(w) << ts[i].records[2] << " ";
335 ✗ std::cout << std::endl;
336
337 ✗ std::cout << "self collision time" << std::endl;
338 ✗ for (size_t i = 0; i < ts.size(); ++i)
339 ✗ std::cout << std::setw(w) << ts[i].records[3] << " ";
340 ✗ std::cout << std::endl;
341
342 ✗ std::cout << "collision time" << std::endl;
343 ✗ for (size_t i = 0; i < ts.size(); ++i) {
344 ✗ double tmp = 0;
345 ✗ for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
346 ✗ std::cout << std::setw(w) << tmp << " ";
347 }
348 ✗ std::cout << std::endl;
349
350 ✗ std::cout << "overall time" << std::endl;
351 ✗ for (size_t i = 0; i < ts.size(); ++i)
352 ✗ std::cout << std::setw(w) << ts[i].overall_time << " ";
353 ✗ std::cout << std::endl;
354 ✗ std::cout << std::endl;
355
5/10
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 5 }
356
357 12 void broad_phase_update_collision_test(Scalar env_scale, std::size_t env_size,
358 std::size_t query_size,
359 std::size_t num_max_contacts,
360 bool exhaustive, bool use_mesh) {
361 12 std::vector<TStruct> ts;
362 12 std::vector<BenchTimer> timers;
363
364 12 std::vector<CollisionObject*> env;
365
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 12 if (use_mesh)
366
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 generateEnvironmentsMesh(env, env_scale, env_size);
367 else
368
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 generateEnvironments(env, env_scale, env_size);
369
370 12 std::vector<CollisionObject*> query;
371
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 12 if (use_mesh)
372
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 generateEnvironmentsMesh(query, env_scale, query_size);
373 else
374
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 generateEnvironments(query, env_scale, query_size);
375
376 12 std::vector<BroadPhaseCollisionManager*> managers;
377
378
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 managers.push_back(new NaiveCollisionManager());
379
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 managers.push_back(new SSaPCollisionManager());
380
381
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 managers.push_back(new SaPCollisionManager());
382
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 managers.push_back(new IntervalTreeCollisionManager());
383
384
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 Vec3s lower_limit, upper_limit;
385
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
386
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
387
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 (upper_limit[1] - lower_limit[1]) / 20),
388
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 (upper_limit[2] - lower_limit[2]) / 20);
389 // managers.push_back(new SpatialHashingCollisionManager(cell_size,
390 // lower_limit, upper_limit));
391 12 managers.push_back(
392
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 new SpatialHashingCollisionManager<
393 detail::SparseHashTable<AABB, CollisionObject*, detail::SpatialHash>>(
394
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 cell_size, lower_limit, upper_limit));
395 #if USE_GOOGLEHASH
396 managers.push_back(
397 new SpatialHashingCollisionManager<detail::SparseHashTable<
398 AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable>>(
399 cell_size, lower_limit, upper_limit));
400 managers.push_back(
401 new SpatialHashingCollisionManager<detail::SparseHashTable<
402 AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable>>(
403 cell_size, lower_limit, upper_limit));
404 #endif
405
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 managers.push_back(new DynamicAABBTreeCollisionManager());
406
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 managers.push_back(new DynamicAABBTreeArrayCollisionManager());
407
408 {
409
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
410 12 m->tree_init_level = 2;
411
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 managers.push_back(m);
412 }
413
414 {
415 DynamicAABBTreeArrayCollisionManager* m =
416
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 new DynamicAABBTreeArrayCollisionManager();
417 12 m->tree_init_level = 2;
418
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 managers.push_back(m);
419 }
420
421
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 ts.resize(managers.size());
422
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 timers.resize(managers.size());
423
424
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < managers.size(); ++i) {
425
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 timers[i].start();
426
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 managers[i]->registerObjects(env);
427
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 timers[i].stop();
428
2/4
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
 108 ts[i].push_back(timers[i].getElapsedTime());
429 }
430
431
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < managers.size(); ++i) {
432
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 timers[i].start();
433
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 managers[i]->setup();
434
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 timers[i].stop();
435
2/4
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
 108 ts[i].push_back(timers[i].getElapsedTime());
436 }
437
438 // update the environment
439 12 Scalar delta_angle_max =
440 Scalar(10) / Scalar(360 * 2) * boost::math::constants::pi<Scalar>();
441 12 Scalar delta_trans_max = Scalar(0.01) * env_scale;
442 12 const Scalar half = Scalar(0.5);
443
2/2
✓ Branch 1 taken 1044 times.
✓ Branch 2 taken 12 times.
 1056 for (size_t i = 0; i < env.size(); ++i) {
444 Scalar rand_angle_x =
445 1044 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max;
446 Scalar rand_trans_x =
447 1044 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max;
448 Scalar rand_angle_y =
449 1044 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max;
450 Scalar rand_trans_y =
451 1044 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max;
452 Scalar rand_angle_z =
453 1044 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max;
454 Scalar rand_trans_z =
455 1044 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max;
456
457 using AngleAxis = Eigen::AngleAxis<Scalar>;
458
3/6
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1044 times.
✗ Branch 8 not taken.
 1044 Matrix3s dR(AngleAxis(rand_angle_x, Vec3s::UnitX()) *
459
3/6
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1044 times.
✗ Branch 8 not taken.
 2088 AngleAxis(rand_angle_y, Vec3s::UnitY()) *
460
3/6
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1044 times.
✗ Branch 8 not taken.
 2088 AngleAxis(rand_angle_z, Vec3s::UnitZ()));
461
1/2
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
 1044 Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z);
462
463
1/2
✓ Branch 3 taken 1044 times.
✗ Branch 4 not taken.
 1044 Matrix3s R = env[i]->getRotation();
464
1/2
✓ Branch 3 taken 1044 times.
✗ Branch 4 not taken.
 1044 Vec3s T = env[i]->getTranslation();
465
6/12
✓ Branch 2 taken 1044 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1044 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1044 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1044 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1044 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1044 times.
✗ Branch 18 not taken.
 1044 env[i]->setTransform(dR * R, dR * T + dT);
466
1/2
✓ Branch 2 taken 1044 times.
✗ Branch 3 not taken.
 1044 env[i]->computeAABB();
467 }
468
469
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < managers.size(); ++i) {
470
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 timers[i].start();
471
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 managers[i]->update();
472
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 timers[i].stop();
473
2/4
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
 108 ts[i].push_back(timers[i].getElapsedTime());
474 }
475
476
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 std::vector<CollisionData> self_data(managers.size());
477
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < managers.size(); ++i) {
478
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 72 times.
 108 if (exhaustive)
479 36 self_data[i].request.num_max_contacts = 100000;
480 else
481 72 self_data[i].request.num_max_contacts = num_max_contacts;
482 }
483
484
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < managers.size(); ++i) {
485
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 108 CollisionCallBackDefault callback;
486
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 timers[i].start();
487
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 managers[i]->collide(&callback);
488
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 108 timers[i].stop();
489
2/4
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
 108 ts[i].push_back(timers[i].getElapsedTime());
490 108 }
491
492
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < managers.size(); ++i)
493
2/4
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
 108 std::cout << self_data[i].result.numContacts() << " ";
494
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 std::cout << std::endl;
495
496
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 8 times.
 12 if (exhaustive) {
497
2/2
✓ Branch 1 taken 32 times.
✓ Branch 2 taken 4 times.
 36 for (size_t i = 1; i < managers.size(); ++i)
498
6/12
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 32 times.
 32 BOOST_CHECK(self_data[i].result.numContacts() ==
499 self_data[0].result.numContacts());
500 } else {
501
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 std::vector<bool> self_res(managers.size());
502
2/2
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 8 times.
 80 for (size_t i = 0; i < self_res.size(); ++i)
503
1/2
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
 72 self_res[i] = (self_data[i].result.numContacts() > 0);
504
505
2/2
✓ Branch 1 taken 64 times.
✓ Branch 2 taken 8 times.
 72 for (size_t i = 1; i < self_res.size(); ++i)
506
8/16
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 64 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 64 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 64 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 64 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 64 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 64 times.
 64 BOOST_CHECK(self_res[0] == self_res[i]);
507
508
2/2
✓ Branch 1 taken 64 times.
✓ Branch 2 taken 8 times.
 72 for (size_t i = 1; i < managers.size(); ++i)
509
6/12
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 64 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 64 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 64 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 64 times.
 64 BOOST_CHECK(self_data[i].result.numContacts() ==
510 self_data[0].result.numContacts());
511 8 }
512
513
2/2
✓ Branch 1 taken 1872 times.
✓ Branch 2 taken 12 times.
 1884 for (size_t i = 0; i < query.size(); ++i) {
514
1/2
✓ Branch 3 taken 1872 times.
✗ Branch 4 not taken.
 1872 std::vector<CollisionCallBackDefault> query_callbacks(managers.size());
515
516
2/2
✓ Branch 1 taken 16848 times.
✓ Branch 2 taken 1872 times.
 18720 for (size_t j = 0; j < query_callbacks.size(); ++j) {
517
2/2
✓ Branch 0 taken 5616 times.
✓ Branch 1 taken 11232 times.
 16848 if (exhaustive)
518 5616 query_callbacks[j].data.request.num_max_contacts = 100000;
519 else
520 11232 query_callbacks[j].data.request.num_max_contacts = num_max_contacts;
521 }
522
523
2/2
✓ Branch 1 taken 16848 times.
✓ Branch 2 taken 1872 times.
 18720 for (size_t j = 0; j < query_callbacks.size(); ++j) {
524
1/2
✓ Branch 2 taken 16848 times.
✗ Branch 3 not taken.
 16848 timers[j].start();
525
1/2
✓ Branch 4 taken 16848 times.
✗ Branch 5 not taken.
 16848 managers[j]->collide(query[i], &query_callbacks[j]);
526
1/2
✓ Branch 2 taken 16848 times.
✗ Branch 3 not taken.
 16848 timers[j].stop();
527
2/4
✓ Branch 3 taken 16848 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16848 times.
✗ Branch 7 not taken.
 16848 ts[j].push_back(timers[j].getElapsedTime());
528 }
529
530 // for(size_t j = 0; j < managers.size(); ++j)
531 // std::cout << query_callbacks[j].result.numContacts() << " ";
532 // std::cout << std::endl;
533
534
2/2
✓ Branch 0 taken 624 times.
✓ Branch 1 taken 1248 times.
 1872 if (exhaustive) {
535
2/2
✓ Branch 1 taken 4992 times.
✓ Branch 2 taken 624 times.
 5616 for (size_t j = 1; j < managers.size(); ++j)
536
6/12
✓ Branch 1 taken 4992 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4992 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4992 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 4992 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 4992 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 4992 times.
 4992 BOOST_CHECK(query_callbacks[j].data.result.numContacts() ==
537 query_callbacks[0].data.result.numContacts());
538 } else {
539
1/2
✓ Branch 3 taken 1248 times.
✗ Branch 4 not taken.
 1248 std::vector<bool> query_res(managers.size());
540
2/2
✓ Branch 1 taken 11232 times.
✓ Branch 2 taken 1248 times.
 12480 for (size_t j = 0; j < query_res.size(); ++j)
541
1/2
✓ Branch 3 taken 11232 times.
✗ Branch 4 not taken.
 11232 query_res[j] = (query_callbacks[j].data.result.numContacts() > 0);
542
2/2
✓ Branch 1 taken 9984 times.
✓ Branch 2 taken 1248 times.
 11232 for (size_t j = 1; j < query_res.size(); ++j)
543
8/16
✓ Branch 1 taken 9984 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9984 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9984 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9984 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9984 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 9984 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 9984 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 9984 times.
 9984 BOOST_CHECK(query_res[0] == query_res[j]);
544
545
2/2
✓ Branch 1 taken 9984 times.
✓ Branch 2 taken 1248 times.
 11232 for (size_t j = 1; j < managers.size(); ++j)
546
6/12
✓ Branch 1 taken 9984 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9984 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9984 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 9984 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 9984 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 9984 times.
 9984 BOOST_CHECK(query_callbacks[j].data.result.numContacts() ==
547 query_callbacks[0].data.result.numContacts());
548 1248 }
549 1872 }
550
551
3/4
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1044 times.
✓ Branch 7 taken 12 times.
 1056 for (size_t i = 0; i < env.size(); ++i) delete env[i];
552
3/4
✓ Branch 1 taken 1872 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1872 times.
✓ Branch 7 taken 12 times.
 1884 for (size_t i = 0; i < query.size(); ++i) delete query[i];
553
554
3/4
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 108 times.
✓ Branch 6 taken 12 times.
 120 for (size_t i = 0; i < managers.size(); ++i) delete managers[i];
555
556 12 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
557 12 int w = 7;
558
559
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 std::cout << "collision timing summary" << std::endl;
560
5/10
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
 12 std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
561
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 std::cout << "register time" << std::endl;
562
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < ts.size(); ++i)
563
3/6
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 108 times.
✗ Branch 11 not taken.
 108 std::cout << std::setw(w) << ts[i].records[0] << " ";
564
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 std::cout << std::endl;
565
566
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 std::cout << "setup time" << std::endl;
567
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < ts.size(); ++i)
568
3/6
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 108 times.
✗ Branch 11 not taken.
 108 std::cout << std::setw(w) << ts[i].records[1] << " ";
569
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 std::cout << std::endl;
570
571
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 std::cout << "update time" << std::endl;
572
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < ts.size(); ++i)
573
3/6
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 108 times.
✗ Branch 11 not taken.
 108 std::cout << std::setw(w) << ts[i].records[2] << " ";
574
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 std::cout << std::endl;
575
576
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 std::cout << "self collision time" << std::endl;
577
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < ts.size(); ++i)
578
3/6
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 108 times.
✗ Branch 11 not taken.
 108 std::cout << std::setw(w) << ts[i].records[3] << " ";
579
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 std::cout << std::endl;
580
581
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 std::cout << "collision time" << std::endl;
582
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < ts.size(); ++i) {
583 108 double tmp = 0;
584
2/2
✓ Branch 4 taken 16848 times.
✓ Branch 5 taken 108 times.
 16956 for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
585
3/6
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
 108 std::cout << std::setw(w) << tmp << " ";
586 }
587
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 std::cout << std::endl;
588
589
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 std::cout << "overall time" << std::endl;
590
2/2
✓ Branch 1 taken 108 times.
✓ Branch 2 taken 12 times.
 120 for (size_t i = 0; i < ts.size(); ++i)
591
3/6
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 108 times.
✗ Branch 10 not taken.
 108 std::cout << std::setw(w) << ts[i].overall_time << " ";
592
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 std::cout << std::endl;
593
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 std::cout << std::endl;
594 12 }
595