hpp-fcl  1.4.4
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 
42 #include <hpp/fcl/BV/kIOS.h>
43 #include <hpp/fcl/BV/OBBRSS.h>
44 #include <vector>
45 #include <iostream>
46 
47 namespace hpp
48 {
49 namespace fcl
50 {
51 
54 
55 
57 template<typename BV>
58 class BVSplitter
59 {
60 public:
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, int num_primitives)
79  {
80  switch(split_method)
81  {
82  case SPLIT_METHOD_MEAN:
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 
110 private:
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*, 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, 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(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(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, 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(num_primitives);
193 
194  if(type == BVH_MODEL_TRIANGLES)
195  {
196  for(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(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 
222 template<>
223 bool BVSplitter<OBB>::apply(const Vec3f& q) const;
224 
225 template<>
226 bool BVSplitter<RSS>::apply(const Vec3f& q) const;
227 
228 template<>
229 bool BVSplitter<kIOS>::apply(const Vec3f& q) const;
230 
231 template<>
232 bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
233 
234 template<>
235 void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
236 
237 template<>
238 void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
239 
240 template<>
241 void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
242 
243 template<>
244 void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
245 
246 template<>
247 void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
248 
249 template<>
250 void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
251 
252 template<>
253 void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
254 
255 template<>
256 void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
257 
258 template<>
259 void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
260 
261 template<>
262 void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
263 
264 template<>
265 void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
266 
267 template<>
268 void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
269 
270 }
271 
272 } // namespace hpp
273 
274 #endif
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:55
Main namespace.
Definition: AABB.h:43
virtual ~BVSplitter()
Default deconstructor.
Definition: BV_splitter.h:67
Definition: BVH_internal.h:79
Oriented bounding box class.
Definition: OBB.h:54
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:55
unknown model type
Definition: BVH_internal.h:80
void computeRule(const BV &bv, unsigned int *primitive_indices, int num_primitives)
Compute the split rule according to a subset of geometry and the corresponding BV node...
Definition: BV_splitter.h:78
double FCL_REAL
Definition: data_types.h:68
Definition: BV_splitter.h:53
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:56
BVHModelType
BVH model type.
Definition: BVH_internal.h:77
BVSplitter(SplitMethodType method)
Definition: BV_splitter.h:62
Definition: BV_splitter.h:53
Definition: BV_splitter.h:53
Triangle with 3 indices for points.
Definition: data_types.h:77
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: BV_splitter.h:53
bool apply(const Vec3f &q) const
Apply the split rule on a given point.
Definition: BV_splitter.h:97
A class describing the split rule that splits each BV node.
Definition: BVH_model.h:59
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
void clear()
Clear the geometry data set before.
Definition: BV_splitter.h:103
triangle model
Definition: BVH_internal.h:81