hpp-fcl 2.2.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
BV_splitter.h
Go to the documentation of this file.
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
38#ifndef HPP_FCL_BV_SPLITTER_H
39#define HPP_FCL_BV_SPLITTER_H
40
42#include <hpp/fcl/BV/kIOS.h>
43#include <hpp/fcl/BV/OBBRSS.h>
44#include <vector>
45#include <iostream>
46
47namespace hpp {
48namespace fcl {
49
55};
56
58template <typename BV>
60 public:
62 : split_vector(0, 0, 0), split_method(method) {}
63
65 virtual ~BVSplitter() {}
66
68 void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) {
69 vertices = vertices_;
70 tri_indices = tri_indices_;
71 type = type_;
72 }
73
76 void computeRule(const BV& bv, unsigned int* primitive_indices,
77 unsigned int num_primitives) {
78 switch (split_method) {
80 computeRule_mean(bv, primitive_indices, num_primitives);
81 break;
83 computeRule_median(bv, primitive_indices, num_primitives);
84 break;
86 computeRule_bvcenter(bv, primitive_indices, num_primitives);
87 break;
88 default:
89 std::cerr << "Split method not supported" << std::endl;
90 }
91 }
92
94 bool apply(const Vec3f& q) const { return q[split_axis] > split_value; }
95
97 void clear() {
98 vertices = NULL;
99 tri_indices = NULL;
101 }
102
103 protected:
110
115
118
121
124
127
129 void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) {
130 Vec3f center = bv.center();
131 int axis = 2;
132
133 if (bv.width() >= bv.height() && bv.width() >= bv.depth())
134 axis = 0;
135 else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
136 axis = 1;
137
138 split_axis = axis;
139 split_value = center[axis];
140 }
141
144 void computeRule_mean(const BV& bv, unsigned int* primitive_indices,
145 unsigned int num_primitives) {
146 int axis = 2;
147
148 if (bv.width() >= bv.height() && bv.width() >= bv.depth())
149 axis = 0;
150 else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
151 axis = 1;
152
153 split_axis = axis;
154 FCL_REAL sum = 0;
155
156 if (type == BVH_MODEL_TRIANGLES) {
157 for (unsigned int i = 0; i < num_primitives; ++i) {
158 const Triangle& t = tri_indices[primitive_indices[i]];
159 sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] +
160 vertices[t[2]][split_axis]);
161 }
162
163 sum /= 3;
164 } else if (type == BVH_MODEL_POINTCLOUD) {
165 for (unsigned int i = 0; i < num_primitives; ++i) {
166 sum += vertices[primitive_indices[i]][split_axis];
167 }
168 }
169
170 split_value = sum / num_primitives;
171 }
172
175 void computeRule_median(const BV& bv, unsigned int* primitive_indices,
176 unsigned int num_primitives) {
177 int axis = 2;
178
179 if (bv.width() >= bv.height() && bv.width() >= bv.depth())
180 axis = 0;
181 else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
182 axis = 1;
183
184 split_axis = axis;
185 std::vector<FCL_REAL> proj((size_t)num_primitives);
186
187 if (type == BVH_MODEL_TRIANGLES) {
188 for (unsigned int i = 0; i < num_primitives; ++i) {
189 const Triangle& t = tri_indices[primitive_indices[i]];
190 proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] +
191 vertices[t[2]][split_axis]) /
192 3;
193 }
194 } else if (type == BVH_MODEL_POINTCLOUD) {
195 for (unsigned int i = 0; i < num_primitives; ++i)
196 proj[i] = vertices[primitive_indices[i]][split_axis];
197 }
198
199 std::sort(proj.begin(), proj.end());
200
201 if (num_primitives % 2 == 1) {
202 split_value = proj[(num_primitives - 1) / 2];
203 } else {
205 (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
206 }
207 }
208};
209
210template <>
212
213template <>
215
216template <>
218
219template <>
221
222template <>
224 const OBB& bv, unsigned int* primitive_indices,
225 unsigned int num_primitives);
226
227template <>
229 const OBB& bv, unsigned int* primitive_indices,
230 unsigned int num_primitives);
231
232template <>
234 const OBB& bv, unsigned int* primitive_indices,
235 unsigned int num_primitives);
236
237template <>
239 const RSS& bv, unsigned int* primitive_indices,
240 unsigned int num_primitives);
241
242template <>
244 const RSS& bv, unsigned int* primitive_indices,
245 unsigned int num_primitives);
246
247template <>
249 const RSS& bv, unsigned int* primitive_indices,
250 unsigned int num_primitives);
251
252template <>
254 const kIOS& bv, unsigned int* primitive_indices,
255 unsigned int num_primitives);
256
257template <>
259 const kIOS& bv, unsigned int* primitive_indices,
260 unsigned int num_primitives);
261
262template <>
264 const kIOS& bv, unsigned int* primitive_indices,
265 unsigned int num_primitives);
266
267template <>
269 const OBBRSS& bv, unsigned int* primitive_indices,
270 unsigned int num_primitives);
271
272template <>
274 const OBBRSS& bv, unsigned int* primitive_indices,
275 unsigned int num_primitives);
276
277template <>
279 const OBBRSS& bv, unsigned int* primitive_indices,
280 unsigned int num_primitives);
281
282} // namespace fcl
283
284} // namespace hpp
285
286#endif
A class describing the split rule that splits each BV node.
Definition: BV_splitter.h:59
Vec3f split_vector
Definition: BV_splitter.h:109
void computeRule_median(const BV &bv, unsigned int *primitive_indices, unsigned int num_primitives)
Split algorithm 3: Split the node according to the median of the data contained.
Definition: BV_splitter.h:175
SplitMethodType split_method
The split algorithm used.
Definition: BV_splitter.h:126
virtual ~BVSplitter()
Default deconstructor.
Definition: BV_splitter.h:65
void computeRule_bvcenter(const BV &bv, unsigned int *, unsigned int)
Split algorithm 1: Split the node from center.
Definition: BV_splitter.h:129
BVSplitter(SplitMethodType method)
Definition: BV_splitter.h:61
void set(Vec3f *vertices_, Triangle *tri_indices_, BVHModelType type_)
Set the geometry data needed by the split rule.
Definition: BV_splitter.h:68
void computeRule_mean(const BV &bv, unsigned int *primitive_indices, unsigned int num_primitives)
Split algorithm 2: Split the node according to the mean of the data contained.
Definition: BV_splitter.h:144
BVHModelType type
Whether the geometry is mesh or point cloud.
Definition: BV_splitter.h:123
void computeRule(const BV &bv, unsigned int *primitive_indices, unsigned int num_primitives)
Compute the split rule according to a subset of geometry and the corresponding BV node.
Definition: BV_splitter.h:76
Vec3f * vertices
The mesh vertices or points handled by the splitter.
Definition: BV_splitter.h:117
int split_axis
The axis based on which the split decision is made. For most BV, the axis is aligned with one of the ...
Definition: BV_splitter.h:108
FCL_REAL split_value
The split threshold, different primitives are splitted according whether their projection on the spli...
Definition: BV_splitter.h:114
Triangle * tri_indices
The triangles handled by the splitter.
Definition: BV_splitter.h:120
void clear()
Clear the geometry data set before.
Definition: BV_splitter.h:97
bool apply(const Vec3f &q) const
Apply the split rule on a given point.
Definition: BV_splitter.h:94
Triangle with 3 indices for points.
Definition: data_types.h:96
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
#define HPP_FCL_DLLAPI
Definition: config.hh:64
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: BV_splitter.h:51
@ SPLIT_METHOD_MEAN
Definition: BV_splitter.h:52
@ SPLIT_METHOD_MEDIAN
Definition: BV_splitter.h:53
@ SPLIT_METHOD_BV_CENTER
Definition: BV_splitter.h:54
BVHModelType
BVH model type.
Definition: BVH_internal.h:80
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:83
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:81
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:82
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:54
Oriented bounding box class.
Definition: OBB.h:52
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:53