FCL  0.6.0
Flexible Collision Library
kIOS-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_KIOS_INL_H
39 #define FCL_BV_KIOS_INL_H
40 
41 #include "fcl/math/bv/kIOS.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class kIOS<double>;
49 
50 //==============================================================================
51 template <typename S>
52 typename kIOS<S>::kIOS_Sphere kIOS<S>::encloseSphere(
53  const typename kIOS<S>::kIOS_Sphere& s0, const typename kIOS<S>::kIOS_Sphere& s1)
54 {
55  Vector3<S> d = s1.o - s0.o;
56  S dist2 = d.squaredNorm();
57  S diff_r = s1.r - s0.r;
58 
60  if(diff_r * diff_r >= dist2)
61  {
62  if(s1.r > s0.r) return s1;
63  else return s0;
64  }
65  else
66  {
67  float dist = std::sqrt(dist2);
68  kIOS_Sphere s;
69  s.r = dist + s0.r + s1.r;
70  if(dist > 0)
71  s.o = s0.o + d * ((s.r - s0.r) / dist);
72  else
73  s.o = s0.o;
74  return s;
75  }
76 }
77 
78 //==============================================================================
79 template <typename S>
80 bool kIOS<S>::overlap(const kIOS<S>& other) const
81 {
82  for(unsigned int i = 0; i < num_spheres; ++i)
83  {
84  for(unsigned int j = 0; j < other.num_spheres; ++j)
85  {
86  S o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
87  S sum_r = spheres[i].r + other.spheres[j].r;
88  if(o_dist > sum_r * sum_r)
89  return false;
90  }
91  }
92 
93  return obb.overlap(other.obb);
94 
95  return true;
96 }
97 
98 //==============================================================================
99 template <typename S>
101  const kIOS<S>& other, kIOS<S>& /*overlap_part*/) const
102 {
103  return overlap(other);
104 }
105 
106 //==============================================================================
107 template <typename S>
108 bool kIOS<S>::contain(const Vector3<S>& p) const
109 {
110  for(unsigned int i = 0; i < num_spheres; ++i)
111  {
112  S r = spheres[i].r;
113  if((spheres[i].o - p).squaredNorm() > r * r)
114  return false;
115  }
116 
117  return true;
118 }
119 
120 //==============================================================================
121 template <typename S>
122 kIOS<S>& kIOS<S>::operator += (const Vector3<S>& p)
123 {
124  for(unsigned int i = 0; i < num_spheres; ++i)
125  {
126  S r = spheres[i].r;
127  S new_r_sqr = (p - spheres[i].o).squaredNorm();
128  if(new_r_sqr > r * r)
129  {
130  spheres[i].r = sqrt(new_r_sqr);
131  }
132  }
133 
134  obb += p;
135  return *this;
136 }
137 
138 //==============================================================================
139 template <typename S>
141 {
142  *this = *this + other;
143  return *this;
144 }
145 
146 //==============================================================================
147 template <typename S>
149 {
150  kIOS<S> result;
151  unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres);
152  for(unsigned int i = 0; i < new_num_spheres; ++i)
153  {
154  result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]);
155  }
156 
157  result.num_spheres = new_num_spheres;
158 
159  result.obb = obb + other.obb;
160 
161  return result;
162 }
163 
164 //==============================================================================
165 template <typename S>
166 const Vector3<S>& kIOS<S>::center() const
167 {
168  return spheres[0].o;
169 }
170 
171 //==============================================================================
172 template <typename S>
173 S kIOS<S>::width() const
174 {
175  return obb.width();
176 }
177 
178 //==============================================================================
179 template <typename S>
181 {
182  return obb.height();
183 }
184 
185 //==============================================================================
186 template <typename S>
187 S kIOS<S>::depth() const
188 {
189  return obb.depth();
190 }
191 
192 //==============================================================================
193 template <typename S>
195 {
196  return obb.volume();
197 }
198 
199 //==============================================================================
200 template <typename S>
201 S kIOS<S>::size() const
202 {
203  return volume();
204 }
205 
206 //==============================================================================
207 template <typename S>
209  const kIOS<S>& other,
210  Vector3<S>* P,
211  Vector3<S>* Q) const
212 {
213  S d_max = 0;
214  int id_a = -1, id_b = -1;
215  for(unsigned int i = 0; i < num_spheres; ++i)
216  {
217  for(unsigned int j = 0; j < other.num_spheres; ++j)
218  {
219  S d = (spheres[i].o - other.spheres[j].o).norm() - (spheres[i].r + other.spheres[j].r);
220  if(d_max < d)
221  {
222  d_max = d;
223  if(P && Q)
224  {
225  id_a = i; id_b = j;
226  }
227  }
228  }
229  }
230 
231  if(P && Q)
232  {
233  if(id_a != -1 && id_b != -1)
234  {
235  Vector3<S> v = spheres[id_a].o - spheres[id_b].o;
236  S len_v = v.norm();
237  *P = spheres[id_a].o;
238  (*P).noalias() -= v * (spheres[id_a].r / len_v);
239  *Q = spheres[id_b].o;
240  (*Q).noalias() += v * (spheres[id_b].r / len_v);
241  }
242  }
243 
244  return d_max;
245 }
246 
247 //==============================================================================
248 template <typename S, typename DerivedA, typename DerivedB>
249 bool overlap(
250  const Eigen::MatrixBase<DerivedA>& R0,
251  const Eigen::MatrixBase<DerivedB>& T0,
252  const kIOS<S>& b1, const kIOS<S>& b2)
253 {
254  kIOS<S> b2_temp = b2;
255  for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
256  b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
257 
258  b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
259  b2_temp.obb.axis = R0 * b2_temp.obb.axis;
260 
261  return b1.overlap(b2_temp);
262 }
263 
264 //==============================================================================
265 template <typename S, typename DerivedA, typename DerivedB>
267  const Eigen::MatrixBase<DerivedA>& R0,
268  const Eigen::MatrixBase<DerivedB>& T0,
269  const kIOS<S>& b1, const kIOS<S>& b2,
270  Vector3<S>* P, Vector3<S>* Q)
271 {
272  kIOS<S> b2_temp = b2;
273  for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
274  b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
275 
276  return b1.distance(b2_temp, P, Q);
277 }
278 
279 //==============================================================================
280 template <typename S>
282  const Transform3<S>& tf,
283  const kIOS<S>& b1,
284  const kIOS<S>& b2,
285  Vector3<S>* P,
286  Vector3<S>* Q)
287 {
288  kIOS<S> b2_temp = b2;
289 
290  for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
291  b2_temp.spheres[i].o = tf * b2_temp.spheres[i].o;
292 
293  return b1.distance(b2_temp, P, Q);
294 }
295 
296 //==============================================================================
297 template <typename S, typename Derived>
299  const kIOS<S>& bv, const Eigen::MatrixBase<Derived>& t)
300 {
301  EIGEN_STATIC_ASSERT(
302  Derived::RowsAtCompileTime == 3
303  && Derived::ColsAtCompileTime == 1,
304  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
305 
306  kIOS<S> res(bv);
307  for(size_t i = 0; i < res.num_spheres; ++i)
308  {
309  res.spheres[i].o += t;
310  }
311 
312  translate(res.obb, t);
313  return res;
314 }
315 
316 } // namespace fcl
317 
318 #endif
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
bool overlap(const kIOS< S > &other) const
Check collision between two kIOS.
Definition: kIOS-inl.h:80
S width() const
Width of the kIOS.
Definition: kIOS-inl.h:173
S depth() const
Depth of the kIOS.
Definition: kIOS-inl.h:187
OBB< S > obb
OBB related with kIOS.
Definition: kIOS.h:72
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.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
S distance(const kIOS< S > &other, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr) const
The distance between two kIOS.
Definition: kIOS-inl.h:208
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
S height() const
Height of the kIOS.
Definition: kIOS-inl.h:180
S volume() const
Volume of the kIOS.
Definition: kIOS-inl.h:194
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