FCL  0.6.0
Flexible Collision Library
BV_fitter-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, 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 FCL_BV_FITTER_INL_H
39 #define FCL_BV_FITTER_INL_H
40 
41 #include "fcl/geometry/bvh/detail/BV_fitter.h"
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 template <typename BV>
52 {
53  // Do nothing
54 }
55 
56 //==============================================================================
57 template <typename S, typename BV>
58 struct SetImpl;
59 
60 //==============================================================================
61 template <typename BV>
63  Vector3<typename BVFitter<BV>::S>* vertices_,
64  Triangle* tri_indices_,
65  BVHModelType type_)
66 {
67  SetImpl<typename BV::S, BV>::run(*this, vertices_, tri_indices_, type_);
68 }
69 
70 //==============================================================================
71 template <typename BV>
73  Vector3<typename BVFitter<BV>::S>* vertices_,
74  Vector3<typename BVFitter<BV>::S>* prev_vertices_,
75  Triangle* tri_indices_,
76  BVHModelType type_)
77 {
79  *this, vertices_, prev_vertices_, tri_indices_, type_);
80 }
81 
82 //==============================================================================
83 template <typename S, typename BV>
84 struct FitImpl;
85 
86 //==============================================================================
87 template <typename BV>
88 BV BVFitter<BV>::fit(unsigned int* primitive_indices, int num_primitives)
89 {
91  *this, primitive_indices, num_primitives);
92 }
93 
94 //==============================================================================
95 template <typename BV>
97 {
98  vertices = nullptr;
99  prev_vertices = nullptr;
100  tri_indices = nullptr;
101  type = BVH_MODEL_UNKNOWN;
102 }
103 
104 //==============================================================================
105 template <typename S, typename BV>
106 struct SetImpl
107 {
108  static void run(
109  BVFitter<BV>& fitter,
110  Vector3<S>* vertices_,
111  Triangle* tri_indices_,
112  BVHModelType type_)
113  {
114  fitter.vertices = vertices_;
115  fitter.prev_vertices = nullptr;
116  fitter.tri_indices = tri_indices_;
117  fitter.type = type_;
118  }
119 
120  static void run(
121  BVFitter<BV>& fitter,
122  Vector3<S>* vertices_,
123  Vector3<S>* prev_vertices_,
124  Triangle* tri_indices_,
125  BVHModelType type_)
126  {
127  fitter.vertices = vertices_;
128  fitter.prev_vertices = prev_vertices_;
129  fitter.tri_indices = tri_indices_;
130  fitter.type = type_;
131  }
132 };
133 
134 //==============================================================================
135 template <typename S>
136 struct SetImpl<S, OBB<S>>
137 {
138  static void run(
139  BVFitter<OBB<S>>& fitter,
140  Vector3<S>* vertices_,
141  Triangle* tri_indices_,
142  BVHModelType type_)
143  {
144  fitter.vertices = vertices_;
145  fitter.prev_vertices = nullptr;
146  fitter.tri_indices = tri_indices_;
147  fitter.type = type_;
148  }
149 
150  static void run(
151  BVFitter<OBB<S>>& fitter,
152  Vector3<S>* vertices_,
153  Vector3<S>* prev_vertices_,
154  Triangle* tri_indices_,
155  BVHModelType type_)
156  {
157  fitter.vertices = vertices_;
158  fitter.prev_vertices = prev_vertices_;
159  fitter.tri_indices = tri_indices_;
160  fitter.type = type_;
161  }
162 };
163 
164 //==============================================================================
165 template <typename S>
166 struct SetImpl<S, RSS<S>>
167 {
168  static void run(
169  BVFitter<RSS<S>>& fitter,
170  Vector3<S>* vertices_,
171  Triangle* tri_indices_,
172  BVHModelType type_)
173  {
174  fitter.vertices = vertices_;
175  fitter.prev_vertices = nullptr;
176  fitter.tri_indices = tri_indices_;
177  fitter.type = type_;
178  }
179 
180  static void run(
181  BVFitter<RSS<S>>& fitter,
182  Vector3<S>* vertices_,
183  Vector3<S>* prev_vertices_,
184  Triangle* tri_indices_,
185  BVHModelType type_)
186  {
187  fitter.vertices = vertices_;
188  fitter.prev_vertices = prev_vertices_;
189  fitter.tri_indices = tri_indices_;
190  fitter.type = type_;
191  }
192 };
193 
194 //==============================================================================
195 template <typename S>
196 struct SetImpl<S, kIOS<S>>
197 {
198  static void run(
199  BVFitter<kIOS<S>>& fitter,
200  Vector3<S>* vertices_,
201  Triangle* tri_indices_,
202  BVHModelType type_)
203  {
204  fitter.vertices = vertices_;
205  fitter.prev_vertices = nullptr;
206  fitter.tri_indices = tri_indices_;
207  fitter.type = type_;
208  }
209 
210  static void run(
211  BVFitter<kIOS<S>>& fitter,
212  Vector3<S>* vertices_,
213  Vector3<S>* prev_vertices_,
214  Triangle* tri_indices_,
215  BVHModelType type_)
216  {
217  fitter.vertices = vertices_;
218  fitter.prev_vertices = prev_vertices_;
219  fitter.tri_indices = tri_indices_;
220  fitter.type = type_;
221  }
222 };
223 
224 //==============================================================================
225 template <typename S>
226 struct SetImpl<S, OBBRSS<S>>
227 {
228  static void run(
229  BVFitter<OBBRSS<S>>& fitter,
230  Vector3<S>* vertices_,
231  Triangle* tri_indices_,
232  BVHModelType type_)
233  {
234  fitter.vertices = vertices_;
235  fitter.prev_vertices = nullptr;
236  fitter.tri_indices = tri_indices_;
237  fitter.type = type_;
238  }
239 
240  static void run(
241  BVFitter<OBBRSS<S>>& fitter,
242  Vector3<S>* vertices_,
243  Vector3<S>* prev_vertices_,
244  Triangle* tri_indices_,
245  BVHModelType type_)
246  {
247  fitter.vertices = vertices_;
248  fitter.prev_vertices = prev_vertices_;
249  fitter.tri_indices = tri_indices_;
250  fitter.type = type_;
251  }
252 };
253 
254 //==============================================================================
255 template <typename S, typename BV>
256 struct FitImpl
257 {
258  static BV run(
259  const BVFitter<BV>& fitter,
260  unsigned int* primitive_indices,
261  int num_primitives)
262  {
263  BV bv;
264 
265  if(fitter.type == BVH_MODEL_TRIANGLES)
266  {
267  for(int i = 0; i < num_primitives; ++i)
268  {
269  Triangle t = fitter.tri_indices[primitive_indices[i]];
270  bv += fitter.vertices[t[0]];
271  bv += fitter.vertices[t[1]];
272  bv += fitter.vertices[t[2]];
273 
274  if(fitter.prev_vertices)
275  {
276  bv += fitter.prev_vertices[t[0]];
277  bv += fitter.prev_vertices[t[1]];
278  bv += fitter.prev_vertices[t[2]];
279  }
280  }
281  }
282  else if(fitter.type == BVH_MODEL_POINTCLOUD)
283  {
284  for(int i = 0; i < num_primitives; ++i)
285  {
286  bv += fitter.vertices[primitive_indices[i]];
287 
288  if(fitter.prev_vertices)
289  {
290  bv += fitter.prev_vertices[primitive_indices[i]];
291  }
292  }
293  }
294 
295  return bv;
296  }
297 };
298 
299 //==============================================================================
300 template <typename S>
301 struct FitImpl<S, OBB<S>>
302 {
303  static OBB<S> run(
304  const BVFitter<OBB<S>>& fitter,
305  unsigned int* primitive_indices,
306  int num_primitives)
307  {
308  OBB<S> bv;
309 
310  Matrix3<S> M; // row first matrix
311  Matrix3<S> E; // row first eigen-vectors
312  Vector3<S> s; // three eigen values
313  getCovariance(
314  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
315  primitive_indices, num_primitives, M);
316  eigen_old(M, s, E);
317  axisFromEigen(E, s, bv.axis);
318 
319  // set obb centers and extensions
320  getExtentAndCenter(
321  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
322  primitive_indices, num_primitives,
323  bv.axis, bv.To, bv.extent);
324 
325  return bv;
326  }
327 };
328 
329 //==============================================================================
330 template <typename S>
331 struct FitImpl<S, RSS<S>>
332 {
333  static RSS<S> run(
334  const BVFitter<RSS<S>>& fitter,
335  unsigned int* primitive_indices,
336  int num_primitives)
337  {
338  RSS<S> bv;
339 
340  Matrix3<S> M; // row first matrix
341  Matrix3<S> E; // row first eigen-vectors
342  Vector3<S> s; // three eigen values
343  getCovariance(
344  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
345  primitive_indices, num_primitives, M);
346  eigen_old(M, s, E);
347  axisFromEigen(E, s, bv.axis);
348 
349  // set rss origin, rectangle size and radius
350  getRadiusAndOriginAndRectangleSize(
351  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
352  primitive_indices, num_primitives, bv.axis, bv.To, bv.l, bv.r);
353 
354  return bv;
355  }
356 };
357 
358 //==============================================================================
359 template <typename S>
360 struct FitImpl<S, kIOS<S>>
361 {
362  static kIOS<S> run(
363  const BVFitter<kIOS<S>>& fitter,
364  unsigned int* primitive_indices,
365  int num_primitives)
366  {
367  kIOS<S> bv;
368 
369  Matrix3<S> M; // row first matrix
370  Matrix3<S> E; // row first eigen-vectors
371  Vector3<S> s;
372  getCovariance(
373  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
374  primitive_indices, num_primitives, M);
375  eigen_old(M, s, E);
376  axisFromEigen(E, s, bv.obb.axis);
377 
378  // get centers and extensions
379  getExtentAndCenter(
380  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
381  primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent);
382 
383  const Vector3<S>& center = bv.obb.To;
384  const Vector3<S>& extent = bv.obb.extent;
385  S r0 = maximumDistance(
386  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
387  primitive_indices, num_primitives, center);
388 
389  // decide k in kIOS
390  if(extent[0] > kIOS<S>::ratio() * extent[2])
391  {
392  if(extent[0] > kIOS<S>::ratio() * extent[1]) bv.num_spheres = 5;
393  else bv.num_spheres = 3;
394  }
395  else bv.num_spheres = 1;
396 
397  bv.spheres[0].o = center;
398  bv.spheres[0].r = r0;
399 
400  if(bv.num_spheres >= 3)
401  {
402  S r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * kIOS<S>::invSinA();
403  Vector3<S> delta = bv.obb.axis.col(2) * (r10 * kIOS<S>::cosA() - extent[2]);
404  bv.spheres[1].o = center - delta;
405  bv.spheres[2].o = center + delta;
406 
407  S r11 = maximumDistance(
408  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
409  primitive_indices, num_primitives, bv.spheres[1].o);
410  S r12 = maximumDistance(
411  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
412  primitive_indices, num_primitives, bv.spheres[2].o);
413 
414  bv.spheres[1].o.noalias() += bv.obb.axis.col(2) * (-r10 + r11);
415  bv.spheres[2].o.noalias() += bv.obb.axis.col(2) * (r10 - r12);
416 
417  bv.spheres[1].r = r10;
418  bv.spheres[2].r = r10;
419  }
420 
421  if(bv.num_spheres >= 5)
422  {
423  S r10 = bv.spheres[1].r;
424  Vector3<S> delta = bv.obb.axis.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
425  bv.spheres[3].o = bv.spheres[0].o - delta;
426  bv.spheres[4].o = bv.spheres[0].o + delta;
427 
428  S r21 = 0, r22 = 0;
429  r21 = maximumDistance(
430  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
431  primitive_indices, num_primitives, bv.spheres[3].o);
432  r22 = maximumDistance(
433  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
434  primitive_indices, num_primitives, bv.spheres[4].o);
435 
436  bv.spheres[3].o.noalias() += bv.obb.axis.col(1) * (-r10 + r21);
437  bv.spheres[4].o.noalias() += bv.obb.axis.col(1) * (r10 - r22);
438 
439  bv.spheres[3].r = r10;
440  bv.spheres[4].r = r10;
441  }
442 
443  return bv;
444  }
445 };
446 
447 //==============================================================================
448 template <typename S>
449 struct FitImpl<S, OBBRSS<S>>
450 {
451  static OBBRSS<S> run(
452  const BVFitter<OBBRSS<S>>& fitter,
453  unsigned int* primitive_indices,
454  int num_primitives)
455  {
456  OBBRSS<S> bv;
457  Matrix3<S> M;
458  Matrix3<S> E;
459  Vector3<S> s;
460  getCovariance(
461  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
462  primitive_indices, num_primitives, M);
463  eigen_old(M, s, E);
464  axisFromEigen(E, s, bv.obb.axis);
465  bv.rss.axis = bv.obb.axis;
466 
467  getExtentAndCenter(
468  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
469  primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent);
470 
471  getRadiusAndOriginAndRectangleSize(
472  fitter.vertices, fitter.prev_vertices, fitter.tri_indices,
473  primitive_indices, num_primitives,
474  bv.rss.axis, bv.rss.To, bv.rss.l, bv.rss.r);
475 
476  return bv;
477  }
478 };
479 
480 } // namespace detail
481 } // namespace fcl
482 
483 #endif
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
triangle model
Definition: BVH_internal.h:79
Matrix3< S > axis
Orientation of RSS. The axes of the rotation matrix are the principle directions of the box...
Definition: RSS.h:60
Vector3< S > To
Origin of the rectangle in RSS.
Definition: RSS.h:63
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:49
BVHModelType
BVH model type.
Definition: BVH_internal.h:75
void set(Vector3< S > *vertices_, Triangle *tri_indices_, BVHModelType type_)
Prepare the geometry primitive data for fitting.
virtual ~BVFitter()
default deconstructor
Definition: BV_fitter-inl.h:51
Definition: BV_fitter-inl.h:84
Definition: BV_fitter-inl.h:58
Triangle with 3 indices for points.
Definition: triangle.h:47
unknown model type
Definition: BVH_internal.h:78
OBB< S > obb
OBB member, for rotation.
Definition: OBBRSS.h:57
S l[2]
Side lengths of rectangle.
Definition: RSS.h:66
BV fit(unsigned int *primitive_indices, int num_primitives)
Compute a bounding volume that fits a set of primitives (points or triangles). The primitive data was...
Definition: BV_fitter-inl.h:88
OBB< S > obb
OBB related with kIOS.
Definition: kIOS.h:72
RSS< S > rss
RSS member, for distance.
Definition: OBBRSS.h:60
static BV run(const BVFitter< BV > &fitter, unsigned int *primitive_indices, int num_primitives)
Definition: BV_fitter-inl.h:258
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:69
Vector3< S > extent
Half dimensions of OBB.
Definition: OBB.h:68
S r
Radius of sphere summed with rectangle to form RSS.
Definition: RSS.h:69
Matrix3< S > axis
Orientation of OBB. The axes of the rotation matrix are the principle directions of the box...
Definition: OBB.h:62
void clear()
Clear the geometry primitive data.
Definition: BV_fitter-inl.h:96
The class for the default algorithm fitting a bounding volume to a set of points. ...
Definition: BV_fitter.h:56
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
Vector3< S > To
Center of OBB.
Definition: OBB.h:65
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:66
Oriented bounding box class.
Definition: OBB.h:51