BVH_utility-inl.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-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_BVH_UTILITY_INL_H
39 #define FCL_BVH_UTILITY_INL_H
40 
42 
43 #include "fcl/math/bv/utility.h"
44 
45 namespace fcl
46 {
47 
48 //==============================================================================
49 extern template
50 void BVHExpand(
51  BVHModel<OBB<double>>& model, const Variance3<double>* ucs, double r);
52 
53 //==============================================================================
54 extern template
55 void BVHExpand(
56  BVHModel<RSS<double>>& model, const Variance3<double>* ucs, double r);
57 
58 //==============================================================================
59 template <typename S, typename BV>
60 FCL_EXPORT
61 void BVHExpand(BVHModel<BV>& model, const Variance3<S>* ucs, S r)
62 {
63  for(int i = 0; i < model.num_bvs; ++i)
64  {
65  BVNode<BV>& bvnode = model.getBV(i);
66 
67  BV bv;
68  for(int j = 0; j < bvnode.num_primitives; ++j)
69  {
70  int v_id = bvnode.first_primitive + j;
71  const Variance3<S>& uc = ucs[v_id];
72 
73  Vector3<S>& v = model.vertices[bvnode.first_primitive + j];
74 
75  for(int k = 0; k < 3; ++k)
76  {
77  bv += (v + uc.axis.col(k) * (r * uc.sigma[k]));
78  bv += (v - uc.axis.col(k) * (r * uc.sigma[k]));
79  }
80  }
81 
82  bvnode.bv = bv;
83  }
84 }
85 
86 //==============================================================================
87 template <typename S>
88 FCL_EXPORT
89 void BVHExpand(
90  BVHModel<OBB<S>>& model,
91  const Variance3<S>* ucs,
92  S r)
93 {
94  for(int i = 0; i < model.getNumBVs(); ++i)
95  {
96  BVNode<OBB<S>>& bvnode = model.getBV(i);
97 
98  Vector3<S>* vs = new Vector3<S>[bvnode.num_primitives * 6];
99  // TODO(JS): We could use one std::vector outside of the outter for-loop,
100  // and reuse it rather than creating and destructing the array every
101  // iteration.
102 
103  for(int j = 0; j < bvnode.num_primitives; ++j)
104  {
105  int v_id = bvnode.first_primitive + j;
106  const Variance3<S>& uc = ucs[v_id];
107 
108  Vector3<S>&v = model.vertices[bvnode.first_primitive + j];
109 
110  for(int k = 0; k < 3; ++k)
111  {
112  const auto index1 = 6 * j + 2 * k;
113  const auto index2 = index1 + 1;
114  vs[index1] = v;
115  vs[index1].noalias() += uc.axis.col(k) * (r * uc.sigma[k]);
116  vs[index2] = v;
117  vs[index2].noalias() -= uc.axis.col(k) * (r * uc.sigma[k]);
118  }
119  }
120 
121  OBB<S> bv;
122  fit(vs, bvnode.num_primitives * 6, bv);
123 
124  delete [] vs;
125 
126  bvnode.bv = bv;
127  }
128 }
129 
130 //==============================================================================
131 template <typename S>
132 FCL_EXPORT
134  BVHModel<RSS<S>>& model,
135  const Variance3<S>* ucs,
136  S r)
137 {
138  for(int i = 0; i < model.getNumBVs(); ++i)
139  {
140  BVNode<RSS<S>>& bvnode = model.getBV(i);
141 
142  Vector3<S>* vs = new Vector3<S>[bvnode.num_primitives * 6];
143  // TODO(JS): We could use one std::vector outside of the outter for-loop,
144  // and reuse it rather than creating and destructing the array every
145  // iteration.
146 
147  for(int j = 0; j < bvnode.num_primitives; ++j)
148  {
149  int v_id = bvnode.first_primitive + j;
150  const Variance3<S>& uc = ucs[v_id];
151 
152  Vector3<S>&v = model.vertices[bvnode.first_primitive + j];
153 
154  for(int k = 0; k < 3; ++k)
155  {
156  vs[6 * j + 2 * k] = v + uc.axis.col(k) * (r * uc.sigma[k]);
157  vs[6 * j + 2 * k + 1] = v - uc.axis.col(k) * (r * uc.sigma[k]);
158  }
159  }
160 
161  RSS<S> bv;
162  fit(vs, bvnode.num_primitives * 6, bv);
163 
164  delete [] vs;
165 
166  bvnode.bv = bv;
167  }
168 }
169 
170 } // namespace fcl
171 
172 #endif
fcl::RSS< double >
template class FCL_EXPORT RSS< double >
fcl::BVHModel::getBV
const BVNode< BV > & getBV(int id) const
We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some...
Definition: BVH_model-inl.h:162
fcl::BVHModel::vertices
Vector3< S > * vertices
Geometry point data.
Definition: BVH_model.h:162
fcl::BVNode::bv
BV bv
bounding volume storing the geometry
Definition: BV_node.h:57
fcl::Variance3::axis
Matrix3< S > axis
Matrix whose columns are eigenvectors of Sigma.
Definition: variance3.h:61
fcl::Variance3< double >
template class FCL_EXPORT Variance3< double >
fcl::RSS< S >
fcl::BVNodeBase::num_primitives
int num_primitives
The number of primitives belonging to the current node.
Definition: BV_node_base.h:63
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Variance3
Class for variance matrix in 3d.
Definition: variance3.h:51
fcl::OBB< S >
fcl::BVHModel::num_bvs
int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH_model.h:197
fcl::BVHExpand
template void BVHExpand(BVHModel< OBB< double >> &model, const Variance3< double > *ucs, double r)
fcl::BVNode
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:52
fcl::Variance3::sigma
Vector3< S > sigma
Variations along the eign axes.
Definition: variance3.h:58
r
S r
Definition: test_sphere_box.cpp:171
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::OBB< double >
template class FCL_EXPORT OBB< double >
utility.h
BVH_utility.h
fcl::fit
FCL_EXPORT void fit(const Vector3< typename BV::S > *const ps, int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: math/bv/utility-inl.h:671
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::BVNodeBase::first_primitive
int first_primitive
The start id the primitive belonging to the current node. The index is referred to the primitive_indi...
Definition: BV_node_base.h:60


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48