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