Directory: | ./ |
---|---|
File: | include/coal/internal/BV_splitter.h |
Date: | 2025-04-01 09:23:31 |
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 |