hpp-fcl: include/hpp/fcl/internal/BV_splitter.h Source File

hpp-fcl 1.8.0 HPP fork of FCL -- The Flexible Collision Library

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
41#include <hpp/fcl/BVH/BVH_internal.h>
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
53 enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER};
54
55
57template<typename BV>
58 class HPP_FCL_DLLAPI BVSplitter
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 {
82 case SPLIT_METHOD_MEAN:
83 computeRule_mean(bv, primitive_indices, num_primitives);
84 break;
85 case SPLIT_METHOD_MEDIAN:
86 computeRule_median(bv, primitive_indices, num_primitives);
87 break;
88 case SPLIT_METHOD_BV_CENTER:
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
114 int split_axis;
115 Vec3f split_vector;
116
118 FCL_REAL split_value;
119
121 Vec3f* vertices;
122
124 Triangle* tri_indices;
125
127 BVHModelType type;
128
130 SplitMethodType split_method;
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<>
223 bool BVSplitter<OBB>::apply(const Vec3f& q) const;
224
225template<>
226 bool BVSplitter<RSS>::apply(const Vec3f& q) const;
227
228template<>
229 bool BVSplitter<kIOS>::apply(const Vec3f& q) const;
230
231template<>
232 bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
233
234template<>
235 void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives);
236
237template<>
238 void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives);
239
240template<>
241 void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives);
242
243template<>
244 void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
245
246template<>
247 void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
248
249template<>
250 void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
251
252template<>
253 void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
254
255template<>
256 void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
257
258template<>
259 void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
260
261template<>
262 void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
263
264template<>
265 void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
266
267template<>
268 void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives);
269
270}
271
272} // namespace hpp
273
274#endif
OBBRSS.h
BVH_internal.h
hpp::fcl::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BV_splitter.h:59
hpp::fcl::BVSplitter::split_vector
Vec3f split_vector
Definition: BV_splitter.h:115
hpp::fcl::BVSplitter::computeRule_median
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
hpp::fcl::BVSplitter::split_method
SplitMethodType split_method
The split algorithm used.
Definition: BV_splitter.h:130
hpp::fcl::BVSplitter::~BVSplitter
virtual ~BVSplitter()
Default deconstructor.
Definition: BV_splitter.h:67
hpp::fcl::BVSplitter::computeRule_bvcenter
void computeRule_bvcenter(const BV &bv, unsigned int *, unsigned int)
Split algorithm 1: Split the node from center.
Definition: BV_splitter.h:133
hpp::fcl::BVSplitter::BVSplitter
BVSplitter(SplitMethodType method)
Definition: BV_splitter.h:62
hpp::fcl::BVSplitter::set
void set(Vec3f *vertices_, Triangle *tri_indices_, BVHModelType type_)
Set the geometry data needed by the split rule.
Definition: BV_splitter.h:70
hpp::fcl::BVSplitter::computeRule_mean
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
hpp::fcl::BVSplitter::type
BVHModelType type
Whether the geometry is mesh or point cloud.
Definition: BV_splitter.h:127
hpp::fcl::BVSplitter::computeRule
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
hpp::fcl::BVSplitter::vertices
Vec3f * vertices
The mesh vertices or points handled by the splitter.
Definition: BV_splitter.h:121
hpp::fcl::BVSplitter::split_axis
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
hpp::fcl::BVSplitter::split_value
FCL_REAL split_value
The split threshold, different primitives are splitted according whether their projection on the spli...
Definition: BV_splitter.h:118
hpp::fcl::BVSplitter::tri_indices
Triangle * tri_indices
The triangles handled by the splitter.
Definition: BV_splitter.h:124
hpp::fcl::BVSplitter::clear
void clear()
Clear the geometry data set before.
Definition: BV_splitter.h:103
hpp::fcl::BVSplitter::apply
bool apply(const Vec3f &q) const
Apply the split rule on a given point.
Definition: BV_splitter.h:97
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:77
hpp::fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:56
kIOS.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: BV_splitter.h:53
hpp::fcl::SPLIT_METHOD_MEAN
@ SPLIT_METHOD_MEAN
Definition: BV_splitter.h:53
hpp::fcl::SPLIT_METHOD_MEDIAN
@ SPLIT_METHOD_MEDIAN
Definition: BV_splitter.h:53
hpp::fcl::SPLIT_METHOD_BV_CENTER
@ SPLIT_METHOD_BV_CENTER
Definition: BV_splitter.h:53
hpp::fcl::BVHModelType
BVHModelType
BVH model type.
Definition: BVH_internal.h:78
hpp::fcl::BVH_MODEL_POINTCLOUD
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:81
hpp::fcl::BVH_MODEL_UNKNOWN
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:79
hpp::fcl::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:80
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
hpp::fcl::OBB
Oriented bounding box class.
Definition: OBB.h:55
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:57
hpp::fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:56