hpp-fcl 1.8.1
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
48{
49namespace fcl
50{
51
54
55
57template<typename BV>
59{
60public:
61
62 BVSplitter(SplitMethodType method) : split_vector(0,0,0), split_method(method)
63 {
64 }
65
67 virtual ~BVSplitter() {}
68
70 void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
71 {
72 vertices = vertices_;
73 tri_indices = tri_indices_;
74 type = type_;
75 }
76
78 void computeRule(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives)
79 {
80 switch(split_method)
81 {
83 computeRule_mean(bv, primitive_indices, num_primitives);
84 break;
86 computeRule_median(bv, primitive_indices, num_primitives);
87 break;
89 computeRule_bvcenter(bv, primitive_indices, num_primitives);
90 break;
91 default:
92 std::cerr << "Split method not supported" << std::endl;
93 }
94 }
95
97 bool apply(const Vec3f& q) const
98 {
99 return q[split_axis] > split_value;
100 }
101
103 void clear()
104 {
105 vertices = NULL;
106 tri_indices = NULL;
107 type = BVH_MODEL_UNKNOWN;
108 }
109
110protected:
111
116
119
122
125
128
131
133 void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int)
134 {
135 Vec3f center = bv.center();
136 int axis = 2;
137
138 if(bv.width() >= bv.height() && bv.width() >= bv.depth())
139 axis = 0;
140 else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
141 axis = 1;
142
143 split_axis = axis;
144 split_value = center[axis];
145 }
146
148 void computeRule_mean(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives)
149 {
150 int axis = 2;
151
152 if(bv.width() >= bv.height() && bv.width() >= bv.depth())
153 axis = 0;
154 else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
155 axis = 1;
156
157 split_axis = axis;
158 FCL_REAL sum = 0;
159
160 if(type == BVH_MODEL_TRIANGLES)
161 {
162 for(unsigned int i = 0; i < num_primitives; ++i)
163 {
164 const Triangle& t = tri_indices[primitive_indices[i]];
165 sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]);
166 }
167
168 sum /= 3;
169 }
170 else if(type == BVH_MODEL_POINTCLOUD)
171 {
172 for(unsigned int i = 0; i < num_primitives; ++i)
173 {
174 sum += vertices[primitive_indices[i]][split_axis];
175 }
176 }
177
178 split_value = sum / num_primitives;
179 }
180
182 void computeRule_median(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives)
183 {
184 int axis = 2;
185
186 if(bv.width() >= bv.height() && bv.width() >= bv.depth())
187 axis = 0;
188 else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
189 axis = 1;
190
191 split_axis = axis;
192 std::vector<FCL_REAL> proj((size_t)num_primitives);
193
194 if(type == BVH_MODEL_TRIANGLES)
195 {
196 for(unsigned int i = 0; i < num_primitives; ++i)
197 {
198 const Triangle& t = tri_indices[primitive_indices[i]];
199 proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3;
200 }
201 }
202 else if(type == BVH_MODEL_POINTCLOUD)
203 {
204 for(unsigned int i = 0; i < num_primitives; ++i)
205 proj[i] = vertices[primitive_indices[i]][split_axis];
206 }
207
208 std::sort(proj.begin(), proj.end());
209
210 if(num_primitives % 2 == 1)
211 {
212 split_value = proj[(num_primitives - 1) / 2];
213 }
214 else
215 {
216 split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
217 }
218 }
219};
220
221
222template<>
223bool BVSplitter<OBB>::apply(const Vec3f& q) const;
224
225template<>
226bool BVSplitter<RSS>::apply(const Vec3f& q) const;
227
228template<>
229bool BVSplitter<kIOS>::apply(const Vec3f& q) const;
230
231template<>
232bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
233
234template<>
235void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives);
236
237template<>
238void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives);
239
240template<>
241void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives);
242
243template<>
244void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
245
246template<>
247void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
248
249template<>
250void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
251
252template<>
253void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
254
255template<>
256void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
257
258template<>
259void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
260
261template<>
262void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
263
264template<>
265void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
266
267template<>
268void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
269
270}
271
272} // namespace hpp
273
274#endif
A class describing the split rule that splits each BV node.
Definition: BV_splitter.h:59
Vec3f split_vector
Definition: BV_splitter.h:115
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:182
SplitMethodType split_method
The split algorithm used.
Definition: BV_splitter.h:130
virtual ~BVSplitter()
Default deconstructor.
Definition: BV_splitter.h:67
void computeRule_bvcenter(const BV &bv, unsigned int *, unsigned int)
Split algorithm 1: Split the node from center.
Definition: BV_splitter.h:133
BVSplitter(SplitMethodType method)
Definition: BV_splitter.h:62
void set(Vec3f *vertices_, Triangle *tri_indices_, BVHModelType type_)
Set the geometry data needed by the split rule.
Definition: BV_splitter.h:70
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:148
BVHModelType type
Whether the geometry is mesh or point cloud.
Definition: BV_splitter.h:127
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:78
Vec3f * vertices
The mesh vertices or points handled by the splitter.
Definition: BV_splitter.h:121
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:114
FCL_REAL split_value
The split threshold, different primitives are splitted according whether their projection on the spli...
Definition: BV_splitter.h:118
Triangle * tri_indices
The triangles handled by the splitter.
Definition: BV_splitter.h:124
void clear()
Clear the geometry data set before.
Definition: BV_splitter.h:103
bool apply(const Vec3f &q) const
Apply the split rule on a given point.
Definition: BV_splitter.h:97
Triangle with 3 indices for points.
Definition: data_types.h:77
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:56
#define HPP_FCL_DLLAPI
Definition: config.hh:64
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: BV_splitter.h:53
@ SPLIT_METHOD_MEAN
Definition: BV_splitter.h:53
@ SPLIT_METHOD_MEDIAN
Definition: BV_splitter.h:53
@ SPLIT_METHOD_BV_CENTER
Definition: BV_splitter.h:53
BVHModelType
BVH model type.
Definition: BVH_internal.h:78
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:81
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:79
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:80
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:57
Oriented bounding box class.
Definition: OBB.h:55
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:56