kDOP-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_BV_KDOP_INL_H
39 #define FCL_BV_KDOP_INL_H
40 
41 #include "fcl/math/bv/kDOP.h"
42 
43 #include "fcl/common/unused.h"
44 
45 namespace fcl
46 {
47 
48 //==============================================================================
49 extern template
50 class FCL_EXPORT KDOP<double, 16>;
51 
52 //==============================================================================
53 extern template
54 class FCL_EXPORT KDOP<double, 18>;
55 
56 //==============================================================================
57 extern template
58 class FCL_EXPORT KDOP<double, 24>;
59 
60 //==============================================================================
61 extern template
62 void minmax(double a, double b, double& minv, double& maxv);
63 
64 //==============================================================================
65 extern template
66 void minmax(double p, double& minv, double& maxv);
67 
68 //==============================================================================
69 extern template
70 void getDistances<double, 5>(const Vector3<double>& p, double* d);
71 
72 //==============================================================================
73 extern template
74 void getDistances<double, 6>(const Vector3<double>& p, double* d);
75 
76 //==============================================================================
77 extern template
78 void getDistances<double, 9>(const Vector3<double>& p, double* d);
79 
80 //==============================================================================
81 template <typename S, std::size_t N>
82 FCL_EXPORT
84 {
85  static_assert(N == 16 || N == 18 || N == 24, "N should be 16, 18, or 24");
86 
87  S real_max = std::numeric_limits<S>::max();
88  for(std::size_t i = 0; i < N / 2; ++i)
89  {
90  dist_[i] = real_max;
91  dist_[i + N / 2] = -real_max;
92  }
93 }
94 
95 //==============================================================================
96 template <typename S, std::size_t N>
97 FCL_EXPORT
99 {
100  for(std::size_t i = 0; i < 3; ++i)
101  {
102  dist_[i] = dist_[N / 2 + i] = v[i];
103  }
104 
105  S d[(N - 6) / 2];
106  getDistances<S, (N - 6) / 2>(v, d);
107  for(std::size_t i = 0; i < (N - 6) / 2; ++i)
108  {
109  dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
110  }
111 }
112 
113 //==============================================================================
114 template <typename S, std::size_t N>
115 FCL_EXPORT
117 {
118  for(std::size_t i = 0; i < 3; ++i)
119  {
120  minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
121  }
122 
123  S ad[(N - 6) / 2], bd[(N - 6) / 2];
124  getDistances<S, (N - 6) / 2>(a, ad);
125  getDistances<S, (N - 6) / 2>(b, bd);
126  for(std::size_t i = 0; i < (N - 6) / 2; ++i)
127  {
128  minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
129  }
130 }
131 
132 //==============================================================================
133 template <typename S, std::size_t N>
134 FCL_EXPORT
135 bool KDOP<S, N>::overlap(const KDOP<S, N>& other) const
136 {
137  for(std::size_t i = 0; i < N / 2; ++i)
138  {
139  if(dist_[i] > other.dist_[i + N / 2]) return false;
140  if(dist_[i + N / 2] < other.dist_[i]) return false;
141  }
142 
143  return true;
144 }
145 
146 //==============================================================================
147 template <typename S, std::size_t N>
148 FCL_EXPORT
149 bool KDOP<S, N>::inside(const Vector3<S>& p) const
150 {
151  for(std::size_t i = 0; i < 3; ++i)
152  {
153  if(p[i] < dist_[i] || p[i] > dist_[i + N / 2])
154  return false;
155  }
156 
157  S d[(N - 6) / 2];
158  getDistances<S, (N - 6) / 2>(p, d);
159  for(std::size_t i = 0; i < (N - 6) / 2; ++i)
160  {
161  if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2])
162  return false;
163  }
164 
165  return true;
166 }
167 
168 //==============================================================================
169 template <typename S, std::size_t N>
170 FCL_EXPORT
172 {
173  for(std::size_t i = 0; i < 3; ++i)
174  {
175  minmax(p[i], dist_[i], dist_[N / 2 + i]);
176  }
177 
178  S pd[(N - 6) / 2];
179  getDistances<S, (N - 6) / 2>(p, pd);
180  for(std::size_t i = 0; i < (N - 6) / 2; ++i)
181  {
182  minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
183  }
184 
185  return *this;
186 }
187 
188 //==============================================================================
189 template <typename S, std::size_t N>
190 FCL_EXPORT
192 {
193  for(std::size_t i = 0; i < N / 2; ++i)
194  {
195  dist_[i] = std::min(other.dist_[i], dist_[i]);
196  dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
197  }
198  return *this;
199 }
200 
201 //==============================================================================
202 template <typename S, std::size_t N>
203 FCL_EXPORT
205 {
206  KDOP<S, N> res(*this);
207  return res += other;
208 }
209 
210 //==============================================================================
211 template <typename S, std::size_t N>
212 FCL_EXPORT
214 {
215  return dist_[N / 2] - dist_[0];
216 }
217 
218 //==============================================================================
219 template <typename S, std::size_t N>
220 FCL_EXPORT
222 {
223  return dist_[N / 2 + 1] - dist_[1];
224 }
225 
226 //==============================================================================
227 template <typename S, std::size_t N>
228 FCL_EXPORT
230 {
231  return dist_[N / 2 + 2] - dist_[2];
232 }
233 
234 //==============================================================================
235 template <typename S, std::size_t N>
236 FCL_EXPORT
238 {
239  return width() * height() * depth();
240 }
241 
242 //==============================================================================
243 template <typename S, std::size_t N>
244 FCL_EXPORT
246 {
247  return width() * width() + height() * height() + depth() * depth();
248 }
249 
250 //==============================================================================
251 template <typename S, std::size_t N>
252 FCL_EXPORT
254 {
255  return Vector3<S>(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
256 }
257 
258 //==============================================================================
259 template <typename S, std::size_t N>
260 FCL_EXPORT
262 {
263  FCL_UNUSED(other);
264  FCL_UNUSED(P);
265  FCL_UNUSED(Q);
266 
267  std::cerr << "KDOP distance not implemented!\n";
268  return 0.0;
269 }
270 
271 //==============================================================================
272 template <typename S, std::size_t N>
273 FCL_EXPORT
274 S KDOP<S, N>::dist(std::size_t i) const
275 {
276  return dist_[i];
277 }
278 
279 //==============================================================================
280 template <typename S, std::size_t N>
281 FCL_EXPORT
282 S& KDOP<S, N>::dist(std::size_t i)
283 {
284  return dist_[i];
285 }
286 
287 //==============================================================================
288 template <typename S, std::size_t N, typename Derived>
289 FCL_EXPORT
291  const KDOP<S, N>& bv, const Eigen::MatrixBase<Derived>& t)
292 {
293  KDOP<S, N> res(bv);
294  for(std::size_t i = 0; i < 3; ++i)
295  {
296  res.dist(i) += t[i];
297  res.dist(N / 2 + i) += t[i];
298  }
299 
300  S d[(N - 6) / 2];
301  getDistances<S, (N - 6) / 2>(t, d);
302  for(std::size_t i = 0; i < (N - 6) / 2; ++i)
303  {
304  res.dist(3 + i) += d[i];
305  res.dist(3 + i + N / 2) += d[i];
306  }
307 
308  return res;
309 }
310 
311 //==============================================================================
312 template <typename S>
313 FCL_EXPORT
314 void minmax(S a, S b, S& minv, S& maxv)
315 {
316  if(a > b)
317  {
318  minv = b;
319  maxv = a;
320  }
321  else
322  {
323  minv = a;
324  maxv = b;
325  }
326 }
327 
328 //==============================================================================
329 template <typename S>
330 FCL_EXPORT
331 void minmax(S p, S& minv, S& maxv)
332 {
333  if(p > maxv) maxv = p;
334  if(p < minv) minv = p;
335 }
336 
337 //==============================================================================
338 template <typename S, std::size_t N>
340 {
341  static void run(const Vector3<S>& /*p*/, S* /*d*/)
342  {
343  // Do nothing
344  }
345 };
346 
347 //==============================================================================
348 template <typename S, std::size_t N>
349 FCL_EXPORT
350 void getDistances(const Vector3<S>& p, S* d)
351 {
353 }
354 
355 //==============================================================================
356 template <typename S>
357 struct FCL_EXPORT GetDistancesImpl<S, 5>
358 {
359  static void run(const Vector3<S>& p, S* d)
360  {
361  d[0] = p[0] + p[1];
362  d[1] = p[0] + p[2];
363  d[2] = p[1] + p[2];
364  d[3] = p[0] - p[1];
365  d[4] = p[0] - p[2];
366  }
367 };
368 
369 //==============================================================================
370 template <typename S>
371 struct FCL_EXPORT GetDistancesImpl<S, 6>
372 {
373  static void run(const Vector3<S>& p, S* d)
374  {
375  d[0] = p[0] + p[1];
376  d[1] = p[0] + p[2];
377  d[2] = p[1] + p[2];
378  d[3] = p[0] - p[1];
379  d[4] = p[0] - p[2];
380  d[5] = p[1] - p[2];
381  }
382 };
383 
384 //==============================================================================
385 template <typename S>
386 struct FCL_EXPORT GetDistancesImpl<S, 9>
387 {
388  static void run(const Vector3<S>& p, S* d)
389  {
390  d[0] = p[0] + p[1];
391  d[1] = p[0] + p[2];
392  d[2] = p[1] + p[2];
393  d[3] = p[0] - p[1];
394  d[4] = p[0] - p[2];
395  d[5] = p[1] - p[2];
396  d[6] = p[0] + p[1] - p[2];
397  d[7] = p[0] + p[2] - p[1];
398  d[8] = p[1] + p[2] - p[0];
399  }
400 };
401 
402 } // namespace fcl
403 
404 #endif
fcl::KDOP::width
S width() const
The (AABB) width.
Definition: kDOP-inl.h:213
fcl::KDOP::height
S height() const
The (AABB) height.
Definition: kDOP-inl.h:221
fcl::GetDistancesImpl< S, 9 >::run
static void run(const Vector3< S > &p, S *d)
Definition: kDOP-inl.h:388
fcl::getDistances
FCL_EXPORT void getDistances(const Vector3< S > &p, S *d)
Compute the distances to planes with normals from KDOP vectors except those of AABB face planes.
Definition: kDOP-inl.h:350
fcl::KDOP::size
S size() const
Size of the kDOP (used in BV_Splitter to order two kDOPs)
Definition: kDOP-inl.h:245
fcl::minmax
template void minmax(double a, double b, double &minv, double &maxv)
fcl::KDOP::S
S_ S
Definition: kDOP.h:88
fcl::KDOP::dist_
S dist_[N]
Origin's distances to N KDOP planes.
Definition: kDOP.h:139
fcl::KDOP::distance
S distance(const KDOP< S, N > &other, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr) const
The distance between two KDOP<S, N>. Not implemented.
Definition: kDOP-inl.h:261
fcl::KDOP< double, 16 >
template class FCL_EXPORT KDOP< double, 16 >
fcl::KDOP::KDOP
KDOP()
Creating kDOP containing nothing.
Definition: kDOP-inl.h:83
fcl::KDOP::operator+=
KDOP< S, N > & operator+=(const Vector3< S > &p)
Merge the point and the KDOP.
Definition: kDOP-inl.h:171
fcl::translate
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
fcl::getDistances< double, 5 >
template void getDistances< double, 5 >(const Vector3< double > &p, double *d)
unused.h
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::KDOP::inside
bool inside(const Vector3< S > &p) const
Definition: kDOP-inl.h:149
fcl::GetDistancesImpl< S, 6 >::run
static void run(const Vector3< S > &p, S *d)
Definition: kDOP-inl.h:373
fcl::GetDistancesImpl
Definition: kDOP-inl.h:339
fcl::GetDistancesImpl::run
static void run(const Vector3< S > &, S *)
Definition: kDOP-inl.h:341
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::KDOP< double, 24 >
template class FCL_EXPORT KDOP< double, 24 >
fcl::GetDistancesImpl< S, 5 >
Definition: kDOP-inl.h:357
fcl::GetDistancesImpl< S, 6 >
Definition: kDOP-inl.h:371
fcl::KDOP::dist
S dist(std::size_t i) const
Definition: kDOP-inl.h:274
fcl::KDOP< double, 18 >
template class FCL_EXPORT KDOP< double, 18 >
fcl::GetDistancesImpl< S, 5 >::run
static void run(const Vector3< S > &p, S *d)
Definition: kDOP-inl.h:359
fcl::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter,...
Definition: kDOP.h:84
fcl::GetDistancesImpl< S, 9 >
Definition: kDOP-inl.h:386
kDOP.h
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::getDistances< double, 9 >
template void getDistances< double, 9 >(const Vector3< double > &p, double *d)
fcl::KDOP::center
Vector3< S > center() const
The (AABB) center.
Definition: kDOP-inl.h:253
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl::getDistances< double, 6 >
template void getDistances< double, 6 >(const Vector3< double > &p, double *d)
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::KDOP::volume
S volume() const
The (AABB) volume.
Definition: kDOP-inl.h:237
fcl::KDOP::operator+
KDOP< S, N > operator+(const KDOP< S, N > &other) const
Create a KDOP by mergin two KDOPs.
Definition: kDOP-inl.h:204
fcl::KDOP::overlap
bool overlap(const KDOP< S, N > &other) const
Check whether two KDOPs are overlapped.
Definition: kDOP-inl.h:135
fcl::KDOP::depth
S depth() const
The (AABB) depth.
Definition: kDOP-inl.h:229


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