| Directory: | ./ |
|---|---|
| File: | include/coal/internal/BV_splitter.h |
| Date: | 2025-04-13 14:25:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 74 | 79 | 93.7% |
| Branches: | 75 | 108 | 69.4% |
| 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 | #ifndef COAL_BV_SPLITTER_H | ||
| 39 | #define COAL_BV_SPLITTER_H | ||
| 40 | |||
| 41 | #include "coal/BVH/BVH_internal.h" | ||
| 42 | #include "coal/BV/kIOS.h" | ||
| 43 | #include "coal/BV/OBBRSS.h" | ||
| 44 | #include <vector> | ||
| 45 | #include <iostream> | ||
| 46 | |||
| 47 | namespace coal { | ||
| 48 | |||
| 49 | /// @brief Three types of split algorithms are provided in FCL as default | ||
| 50 | enum SplitMethodType { | ||
| 51 | SPLIT_METHOD_MEAN, | ||
| 52 | SPLIT_METHOD_MEDIAN, | ||
| 53 | SPLIT_METHOD_BV_CENTER | ||
| 54 | }; | ||
| 55 | |||
| 56 | /// @brief A class describing the split rule that splits each BV node | ||
| 57 | template <typename BV> | ||
| 58 | class BVSplitter { | ||
| 59 | public: | ||
| 60 | 7047 | BVSplitter(SplitMethodType method) | |
| 61 |
1/2✓ Branch 1 taken 3528 times.
✗ Branch 2 not taken.
|
7047 | : split_vector(0, 0, 0), split_method(method) {} |
| 62 | |||
| 63 | /// @brief Default deconstructor | ||
| 64 | 13832 | virtual ~BVSplitter() {} | |
| 65 | |||
| 66 | /// @brief Set the geometry data needed by the split rule | ||
| 67 | 6350 | void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) { | |
| 68 | 6350 | vertices = vertices_; | |
| 69 | 6350 | tri_indices = tri_indices_; | |
| 70 | 6350 | type = type_; | |
| 71 | 6350 | } | |
| 72 | |||
| 73 | /// @brief Compute the split rule according to a subset of geometry and the | ||
| 74 | /// corresponding BV node | ||
| 75 | 4511846 | void computeRule(const BV& bv, unsigned int* primitive_indices, | |
| 76 | unsigned int num_primitives) { | ||
| 77 |
3/4✓ Branch 0 taken 1413549 times.
✓ Branch 1 taken 407248 times.
✓ Branch 2 taken 435126 times.
✗ Branch 3 not taken.
|
4511846 | switch (split_method) { |
| 78 | 2827098 | case SPLIT_METHOD_MEAN: | |
| 79 | 2827098 | computeRule_mean(bv, primitive_indices, num_primitives); | |
| 80 | 2827098 | break; | |
| 81 | 814496 | case SPLIT_METHOD_MEDIAN: | |
| 82 | 814496 | computeRule_median(bv, primitive_indices, num_primitives); | |
| 83 | 814496 | break; | |
| 84 | 870252 | case SPLIT_METHOD_BV_CENTER: | |
| 85 | 870252 | computeRule_bvcenter(bv, primitive_indices, num_primitives); | |
| 86 | 870252 | break; | |
| 87 | ✗ | default: | |
| 88 | ✗ | std::cerr << "Split method not supported" << std::endl; | |
| 89 | } | ||
| 90 | 4511846 | } | |
| 91 | |||
| 92 | /// @brief Apply the split rule on a given point | ||
| 93 | 6132606 | bool apply(const Vec3s& q) const { return q[split_axis] > split_value; } | |
| 94 | |||
| 95 | /// @brief Clear the geometry data set before | ||
| 96 | 6350 | void clear() { | |
| 97 | 6350 | vertices = NULL; | |
| 98 | 6350 | tri_indices = NULL; | |
| 99 | 6350 | type = BVH_MODEL_UNKNOWN; | |
| 100 | 6350 | } | |
| 101 | |||
| 102 | protected: | ||
| 103 | /// @brief The axis based on which the split decision is made. For most BV, | ||
| 104 | /// the axis is aligned with one of the world coordinate, so only split_axis | ||
| 105 | /// is needed. For oriented node, we can use a vector to make a better split | ||
| 106 | /// decision. | ||
| 107 | int split_axis; | ||
| 108 | Vec3s split_vector; | ||
| 109 | |||
| 110 | /// @brief The split threshold, different primitives are splitted according | ||
| 111 | /// whether their projection on the split_axis is larger or smaller than the | ||
| 112 | /// threshold | ||
| 113 | Scalar split_value; | ||
| 114 | |||
| 115 | /// @brief The mesh vertices or points handled by the splitter | ||
| 116 | Vec3s* vertices; | ||
| 117 | |||
| 118 | /// @brief The triangles handled by the splitter | ||
| 119 | Triangle* tri_indices; | ||
| 120 | |||
| 121 | /// @brief Whether the geometry is mesh or point cloud | ||
| 122 | BVHModelType type; | ||
| 123 | |||
| 124 | /// @brief The split algorithm used | ||
| 125 | SplitMethodType split_method; | ||
| 126 | |||
| 127 | /// @brief Split algorithm 1: Split the node from center | ||
| 128 | 328404 | void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) { | |
| 129 |
1/2✓ Branch 1 taken 164202 times.
✗ Branch 2 not taken.
|
328404 | Vec3s center = bv.center(); |
| 130 | 328404 | int axis = 2; | |
| 131 | |||
| 132 |
10/14✓ Branch 1 taken 164202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 164202 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 87940 times.
✓ Branch 7 taken 76262 times.
✓ Branch 9 taken 87940 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 87940 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 45643 times.
✓ Branch 15 taken 42297 times.
✓ Branch 16 taken 45643 times.
✓ Branch 17 taken 118559 times.
|
328404 | if (bv.width() >= bv.height() && bv.width() >= bv.depth()) |
| 133 | 91286 | axis = 0; | |
| 134 |
10/14✓ Branch 1 taken 118559 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 118559 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 78172 times.
✓ Branch 7 taken 40387 times.
✓ Branch 9 taken 78172 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 78172 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 36575 times.
✓ Branch 15 taken 41597 times.
✓ Branch 16 taken 36575 times.
✓ Branch 17 taken 81984 times.
|
237118 | else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) |
| 135 | 73150 | axis = 1; | |
| 136 | |||
| 137 | 328404 | split_axis = axis; | |
| 138 |
1/2✓ Branch 1 taken 164202 times.
✗ Branch 2 not taken.
|
328404 | split_value = center[axis]; |
| 139 | 328404 | } | |
| 140 | |||
| 141 | /// @brief Split algorithm 2: Split the node according to the mean of the data | ||
| 142 | /// contained | ||
| 143 | 399236 | void computeRule_mean(const BV& bv, unsigned int* primitive_indices, | |
| 144 | unsigned int num_primitives) { | ||
| 145 | 399236 | int axis = 2; | |
| 146 | |||
| 147 |
6/6✓ Branch 2 taken 102400 times.
✓ Branch 3 taken 97218 times.
✓ Branch 6 taken 47666 times.
✓ Branch 7 taken 54734 times.
✓ Branch 8 taken 47666 times.
✓ Branch 9 taken 151952 times.
|
399236 | if (bv.width() >= bv.height() && bv.width() >= bv.depth()) |
| 148 | 95332 | axis = 0; | |
| 149 |
6/6✓ Branch 2 taken 99811 times.
✓ Branch 3 taken 52141 times.
✓ Branch 6 taken 39185 times.
✓ Branch 7 taken 60626 times.
✓ Branch 8 taken 39185 times.
✓ Branch 9 taken 112767 times.
|
303904 | else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) |
| 150 | 78370 | axis = 1; | |
| 151 | |||
| 152 | 399236 | split_axis = axis; | |
| 153 | 399236 | Scalar sum = 0; | |
| 154 | |||
| 155 |
2/2✓ Branch 0 taken 199498 times.
✓ Branch 1 taken 120 times.
|
399236 | if (type == BVH_MODEL_TRIANGLES) { |
| 156 |
2/2✓ Branch 0 taken 1247831 times.
✓ Branch 1 taken 199498 times.
|
2894658 | for (unsigned int i = 0; i < num_primitives; ++i) { |
| 157 | 2495662 | const Triangle& t = tri_indices[primitive_indices[i]]; | |
| 158 | 4991324 | sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + | |
| 159 | 2495662 | vertices[t[2]][split_axis]); | |
| 160 | } | ||
| 161 | |||
| 162 | 398996 | sum /= 3; | |
| 163 |
1/2✓ Branch 0 taken 120 times.
✗ Branch 1 not taken.
|
240 | } else if (type == BVH_MODEL_POINTCLOUD) { |
| 164 |
2/2✓ Branch 0 taken 256 times.
✓ Branch 1 taken 120 times.
|
752 | for (unsigned int i = 0; i < num_primitives; ++i) { |
| 165 | 512 | sum += vertices[primitive_indices[i]][split_axis]; | |
| 166 | } | ||
| 167 | } | ||
| 168 | |||
| 169 | 399236 | split_value = sum / Scalar(num_primitives); | |
| 170 | 399236 | } | |
| 171 | |||
| 172 | /// @brief Split algorithm 3: Split the node according to the median of the | ||
| 173 | /// data contained | ||
| 174 | 328404 | void computeRule_median(const BV& bv, unsigned int* primitive_indices, | |
| 175 | unsigned int num_primitives) { | ||
| 176 | 328404 | int axis = 2; | |
| 177 | |||
| 178 |
10/14✓ Branch 1 taken 164202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 164202 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 83904 times.
✓ Branch 7 taken 80298 times.
✓ Branch 9 taken 83904 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 83904 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 43723 times.
✓ Branch 15 taken 40181 times.
✓ Branch 16 taken 43723 times.
✓ Branch 17 taken 120479 times.
|
328404 | if (bv.width() >= bv.height() && bv.width() >= bv.depth()) |
| 179 | 87446 | axis = 0; | |
| 180 |
10/14✓ Branch 1 taken 120479 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 120479 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 81385 times.
✓ Branch 7 taken 39094 times.
✓ Branch 9 taken 81385 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 81385 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 38466 times.
✓ Branch 15 taken 42919 times.
✓ Branch 16 taken 38466 times.
✓ Branch 17 taken 82013 times.
|
240958 | else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) |
| 181 | 76932 | axis = 1; | |
| 182 | |||
| 183 | 328404 | split_axis = axis; | |
| 184 |
1/2✓ Branch 2 taken 164202 times.
✗ Branch 3 not taken.
|
328404 | std::vector<Scalar> proj((size_t)num_primitives); |
| 185 | |||
| 186 |
1/2✓ Branch 0 taken 164202 times.
✗ Branch 1 not taken.
|
328404 | if (type == BVH_MODEL_TRIANGLES) { |
| 187 |
2/2✓ Branch 0 taken 986450 times.
✓ Branch 1 taken 164202 times.
|
2301304 | for (unsigned int i = 0; i < num_primitives; ++i) { |
| 188 | 1972900 | const Triangle& t = tri_indices[primitive_indices[i]]; | |
| 189 |
2/4✓ Branch 2 taken 986450 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 986450 times.
✗ Branch 7 not taken.
|
3945800 | proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + |
| 190 |
1/2✓ Branch 2 taken 986450 times.
✗ Branch 3 not taken.
|
1972900 | vertices[t[2]][split_axis]) / |
| 191 | 3; | ||
| 192 | } | ||
| 193 | ✗ | } else if (type == BVH_MODEL_POINTCLOUD) { | |
| 194 | ✗ | for (unsigned int i = 0; i < num_primitives; ++i) | |
| 195 | ✗ | proj[i] = vertices[primitive_indices[i]][split_axis]; | |
| 196 | } | ||
| 197 | |||
| 198 |
1/2✓ Branch 3 taken 164202 times.
✗ Branch 4 not taken.
|
328404 | std::sort(proj.begin(), proj.end()); |
| 199 | |||
| 200 |
2/2✓ Branch 0 taken 103858 times.
✓ Branch 1 taken 60344 times.
|
328404 | if (num_primitives % 2 == 1) { |
| 201 | 207716 | split_value = proj[(num_primitives - 1) / 2]; | |
| 202 | } else { | ||
| 203 | 120688 | split_value = | |
| 204 | 120688 | (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; | |
| 205 | } | ||
| 206 | 328404 | } | |
| 207 | }; | ||
| 208 | |||
| 209 | template <> | ||
| 210 | bool COAL_DLLAPI BVSplitter<OBB>::apply(const Vec3s& q) const; | ||
| 211 | |||
| 212 | template <> | ||
| 213 | bool COAL_DLLAPI BVSplitter<RSS>::apply(const Vec3s& q) const; | ||
| 214 | |||
| 215 | template <> | ||
| 216 | bool COAL_DLLAPI BVSplitter<kIOS>::apply(const Vec3s& q) const; | ||
| 217 | |||
| 218 | template <> | ||
| 219 | bool COAL_DLLAPI BVSplitter<OBBRSS>::apply(const Vec3s& q) const; | ||
| 220 | |||
| 221 | template <> | ||
| 222 | void COAL_DLLAPI BVSplitter<OBB>::computeRule_bvcenter( | ||
| 223 | const OBB& bv, unsigned int* primitive_indices, | ||
| 224 | unsigned int num_primitives); | ||
| 225 | |||
| 226 | template <> | ||
| 227 | void COAL_DLLAPI BVSplitter<OBB>::computeRule_mean( | ||
| 228 | const OBB& bv, unsigned int* primitive_indices, | ||
| 229 | unsigned int num_primitives); | ||
| 230 | |||
| 231 | template <> | ||
| 232 | void COAL_DLLAPI BVSplitter<OBB>::computeRule_median( | ||
| 233 | const OBB& bv, unsigned int* primitive_indices, | ||
| 234 | unsigned int num_primitives); | ||
| 235 | |||
| 236 | template <> | ||
| 237 | void COAL_DLLAPI BVSplitter<RSS>::computeRule_bvcenter( | ||
| 238 | const RSS& bv, unsigned int* primitive_indices, | ||
| 239 | unsigned int num_primitives); | ||
| 240 | |||
| 241 | template <> | ||
| 242 | void COAL_DLLAPI BVSplitter<RSS>::computeRule_mean( | ||
| 243 | const RSS& bv, unsigned int* primitive_indices, | ||
| 244 | unsigned int num_primitives); | ||
| 245 | |||
| 246 | template <> | ||
| 247 | void COAL_DLLAPI BVSplitter<RSS>::computeRule_median( | ||
| 248 | const RSS& bv, unsigned int* primitive_indices, | ||
| 249 | unsigned int num_primitives); | ||
| 250 | |||
| 251 | template <> | ||
| 252 | void COAL_DLLAPI BVSplitter<kIOS>::computeRule_bvcenter( | ||
| 253 | const kIOS& bv, unsigned int* primitive_indices, | ||
| 254 | unsigned int num_primitives); | ||
| 255 | |||
| 256 | template <> | ||
| 257 | void COAL_DLLAPI BVSplitter<kIOS>::computeRule_mean( | ||
| 258 | const kIOS& bv, unsigned int* primitive_indices, | ||
| 259 | unsigned int num_primitives); | ||
| 260 | |||
| 261 | template <> | ||
| 262 | void COAL_DLLAPI BVSplitter<kIOS>::computeRule_median( | ||
| 263 | const kIOS& bv, unsigned int* primitive_indices, | ||
| 264 | unsigned int num_primitives); | ||
| 265 | |||
| 266 | template <> | ||
| 267 | void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_bvcenter( | ||
| 268 | const OBBRSS& bv, unsigned int* primitive_indices, | ||
| 269 | unsigned int num_primitives); | ||
| 270 | |||
| 271 | template <> | ||
| 272 | void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_mean( | ||
| 273 | const OBBRSS& bv, unsigned int* primitive_indices, | ||
| 274 | unsigned int num_primitives); | ||
| 275 | |||
| 276 | template <> | ||
| 277 | void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_median( | ||
| 278 | const OBBRSS& bv, unsigned int* primitive_indices, | ||
| 279 | unsigned int num_primitives); | ||
| 280 | |||
| 281 | } // namespace coal | ||
| 282 | |||
| 283 | #endif | ||
| 284 |