GCC Code Coverage Report


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