| 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 |
7/14✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 43 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 43 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 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/8✓ 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.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
1 | managers.push_back(new NaiveCollisionManager()); |
| 210 |
3/8✓ 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.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
1 | managers.push_back(new SSaPCollisionManager()); |
| 211 |
3/8✓ 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.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
1 | managers.push_back(new SaPCollisionManager()); |
| 212 |
3/8✓ 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.
✗ Branch 9 not taken.
✗ Branch 10 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/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 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/8✓ 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.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
1 | managers.push_back(new DynamicAABBTreeCollisionManager()); |
| 233 |
3/8✓ 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.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
1 | managers.push_back(new DynamicAABBTreeArrayCollisionManager()); |
| 234 | |||
| 235 | { | ||
| 236 |
2/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 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/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 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 2 taken 1 times.
✗ Branch 3 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/8✓ 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 9 not taken.
✗ Branch 10 not taken.
|
12 | managers.push_back(new NaiveCollisionManager()); |
| 379 |
3/8✓ 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 9 not taken.
✗ Branch 10 not taken.
|
12 | managers.push_back(new SSaPCollisionManager()); |
| 380 | |||
| 381 |
3/8✓ 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 9 not taken.
✗ Branch 10 not taken.
|
12 | managers.push_back(new SaPCollisionManager()); |
| 382 |
3/8✓ 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 9 not taken.
✗ Branch 10 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/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 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/8✓ 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 9 not taken.
✗ Branch 10 not taken.
|
12 | managers.push_back(new DynamicAABBTreeCollisionManager()); |
| 406 |
3/8✓ 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 9 not taken.
✗ Branch 10 not taken.
|
12 | managers.push_back(new DynamicAABBTreeArrayCollisionManager()); |
| 407 | |||
| 408 | { | ||
| 409 |
2/6✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 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/6✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 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 2 taken 12 times.
✗ Branch 3 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 2 taken 8 times.
✗ Branch 3 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 2 taken 1872 times.
✗ Branch 3 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 2 taken 1248 times.
✗ Branch 3 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 |
2/4✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 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 |
2/4✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 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 |
2/4✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 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 |
2/4✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 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 |
2/4✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 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 |
2/4✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 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 |