kDOP.cpp
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 #include <hpp/fcl/BV/kDOP.h>
39 #include <limits>
40 #include <iostream>
41 
42 #include <hpp/fcl/collision_data.h>
43 
44 namespace hpp {
45 namespace fcl {
46 
48 inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) {
49  if (a > b) {
50  minv = b;
51  maxv = a;
52  } else {
53  minv = a;
54  maxv = b;
55  }
56 }
58 inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) {
59  if (p > maxv) maxv = p;
60  if (p < minv) minv = p;
61 }
62 
65 template <short N>
66 void getDistances(const Vec3f& /*p*/, FCL_REAL* /*d*/) {}
67 
69 template <>
70 inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) {
71  d[0] = p[0] + p[1];
72  d[1] = p[0] + p[2];
73  d[2] = p[1] + p[2];
74  d[3] = p[0] - p[1];
75  d[4] = p[0] - p[2];
76 }
77 
78 template <>
79 inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) {
80  d[0] = p[0] + p[1];
81  d[1] = p[0] + p[2];
82  d[2] = p[1] + p[2];
83  d[3] = p[0] - p[1];
84  d[4] = p[0] - p[2];
85  d[5] = p[1] - p[2];
86 }
87 
88 template <>
89 inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) {
90  d[0] = p[0] + p[1];
91  d[1] = p[0] + p[2];
92  d[2] = p[1] + p[2];
93  d[3] = p[0] - p[1];
94  d[4] = p[0] - p[2];
95  d[5] = p[1] - p[2];
96  d[6] = p[0] + p[1] - p[2];
97  d[7] = p[0] + p[2] - p[1];
98  d[8] = p[1] + p[2] - p[0];
99 }
100 
101 template <short N>
102 KDOP<N>::KDOP() {
103  FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
104  dist_.template head<N / 2>().setConstant(real_max);
105  dist_.template tail<N / 2>().setConstant(-real_max);
106 }
107 
108 template <short N>
109 KDOP<N>::KDOP(const Vec3f& v) {
110  for (short i = 0; i < 3; ++i) {
111  dist_[i] = dist_[N / 2 + i] = v[i];
112  }
113 
114  FCL_REAL d[(N - 6) / 2];
115  getDistances<(N - 6) / 2>(v, d);
116  for (short i = 0; i < (N - 6) / 2; ++i) {
117  dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
118  }
119 }
120 
121 template <short N>
122 KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b) {
123  for (short i = 0; i < 3; ++i) {
124  minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
125  }
126 
127  FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
128  getDistances<(N - 6) / 2>(a, ad);
129  getDistances<(N - 6) / 2>(b, bd);
130  for (short i = 0; i < (N - 6) / 2; ++i) {
131  minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
132  }
133 }
134 
135 template <short N>
136 bool KDOP<N>::overlap(const KDOP<N>& other) const {
137  if ((dist_.template head<N / 2>() > other.dist_.template tail<N / 2>()).any())
138  return false;
139  if ((dist_.template tail<N / 2>() < other.dist_.template head<N / 2>()).any())
140  return false;
141  return true;
142 }
143 
144 template <short N>
145 bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
146  FCL_REAL& sqrDistLowerBound) const {
147  const FCL_REAL breakDistance(request.break_distance +
148  request.security_margin);
149 
150  FCL_REAL a =
151  (dist_.template head<N / 2>() - other.dist_.template tail<N / 2>())
152  .minCoeff();
153  if (a > breakDistance) {
154  sqrDistLowerBound = a * a;
155  return false;
156  }
157 
158  FCL_REAL b =
159  (other.dist_.template head<N / 2>() - dist_.template tail<N / 2>())
160  .minCoeff();
161  if (b > breakDistance) {
162  sqrDistLowerBound = b * b;
163  return false;
164  }
165 
166  sqrDistLowerBound = std::min(a, b);
167  return true;
168 }
169 
170 template <short N>
171 bool KDOP<N>::inside(const Vec3f& p) const {
172  if ((p.array() < dist_.template head<3>()).any()) return false;
173  if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false;
174 
175  enum { P = ((N - 6) / 2) };
176  Eigen::Array<FCL_REAL, P, 1> d;
177  getDistances<P>(p, d.data());
178 
179  if ((d < dist_.template segment<P>(3)).any()) return false;
180  if ((d > dist_.template segment<P>(3 + N / 2)).any()) return false;
181 
182  return true;
183 }
184 
185 template <short N>
186 KDOP<N>& KDOP<N>::operator+=(const Vec3f& p) {
187  for (short i = 0; i < 3; ++i) {
188  minmax(p[i], dist_[i], dist_[N / 2 + i]);
189  }
190 
191  FCL_REAL pd[(N - 6) / 2];
192  getDistances<(N - 6) / 2>(p, pd);
193  for (short i = 0; i < (N - 6) / 2; ++i) {
194  minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
195  }
196 
197  return *this;
198 }
199 
200 template <short N>
201 KDOP<N>& KDOP<N>::operator+=(const KDOP<N>& other) {
202  for (short i = 0; i < N / 2; ++i) {
203  dist_[i] = std::min(other.dist_[i], dist_[i]);
204  dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
205  }
206  return *this;
207 }
208 
209 template <short N>
210 KDOP<N> KDOP<N>::operator+(const KDOP<N>& other) const {
211  KDOP<N> res(*this);
212  return res += other;
213 }
214 
215 template <short N>
216 FCL_REAL KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/,
217  Vec3f* /*Q*/) const {
218  std::cerr << "KDOP distance not implemented!" << std::endl;
219  return 0.0;
220 }
221 
222 template <short N>
223 KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t) {
224  KDOP<N> res(bv);
225  for (short i = 0; i < 3; ++i) {
226  res.dist(i) += t[i];
227  res.dist(short(N / 2 + i)) += t[i];
228  }
229 
230  FCL_REAL d[(N - 6) / 2];
231  getDistances<(N - 6) / 2>(t, d);
232  for (short i = 0; i < (N - 6) / 2; ++i) {
233  res.dist(short(3 + i)) += d[i];
234  res.dist(short(3 + i + N / 2)) += d[i];
235  }
236 
237  return res;
238 }
239 
240 template class KDOP<16>;
241 template class KDOP<18>;
242 template class KDOP<24>;
243 
244 template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&);
245 template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&);
246 template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&);
247 
248 } // namespace fcl
249 
250 } // namespace hpp
hpp::fcl::translate< 18 >
template KDOP< 18 > translate< 18 >(const KDOP< 18 > &, const Vec3f &)
hpp::fcl::KDOP::inside
bool inside(const Vec3f &p) const
Definition: kDOP.cpp:171
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:8
hpp::fcl::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 The KDOP structure is defined by some pairs of parallel planes defined by some axes. For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 8 (0,-1,0) and (0,1,0) -> indices 1 and 9 (0,0,-1) and (0,0,1) -> indices 2 and 10 (-1,-1,0) and (1,1,0) -> indices 3 and 11 (-1,0,-1) and (1,0,1) -> indices 4 and 12 (0,-1,-1) and (0,1,1) -> indices 5 and 13 (-1,1,0) and (1,-1,0) -> indices 6 and 14 (-1,0,1) and (1,0,-1) -> indices 7 and 15 For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 9 (0,-1,0) and (0,1,0) -> indices 1 and 10 (0,0,-1) and (0,0,1) -> indices 2 and 11 (-1,-1,0) and (1,1,0) -> indices 3 and 12 (-1,0,-1) and (1,0,1) -> indices 4 and 13 (0,-1,-1) and (0,1,1) -> indices 5 and 14 (-1,1,0) and (1,-1,0) -> indices 6 and 15 (-1,0,1) and (1,0,-1) -> indices 7 and 16 (0,-1,1) and (0,1,-1) -> indices 8 and 17 For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 12 (0,-1,0) and (0,1,0) -> indices 1 and 13 (0,0,-1) and (0,0,1) -> indices 2 and 14 (-1,-1,0) and (1,1,0) -> indices 3 and 15 (-1,0,-1) and (1,0,1) -> indices 4 and 16 (0,-1,-1) and (0,1,1) -> indices 5 and 17 (-1,1,0) and (1,-1,0) -> indices 6 and 18 (-1,0,1) and (1,0,-1) -> indices 7 and 19 (0,-1,1) and (0,1,-1) -> indices 8 and 20 (-1, -1, 1) and (1, 1, -1) –> indices 9 and 21 (-1, 1, -1) and (1, -1, 1) –> indices 10 and 22 (1, -1, -1) and (-1, 1, 1) –> indices 11 and 23.
Definition: kDOP.h:92
hpp::fcl::getDistances< 6 >
void getDistances< 6 >(const Vec3f &p, FCL_REAL *d)
Definition: kDOP.cpp:79
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::translate< 24 >
template KDOP< 24 > translate< 24 >(const KDOP< 24 > &, const Vec3f &)
hpp::fcl::translate< 16 >
template KDOP< 16 > translate< 16 >(const KDOP< 16 > &, const Vec3f &)
hpp::fcl::KDOP::operator+=
KDOP< N > & operator+=(const Vec3f &p)
Merge the point and the KDOP.
Definition: kDOP.cpp:186
hpp::fcl::getDistances
void getDistances(const Vec3f &, FCL_REAL *)
Compute the distances to planes with normals from KDOP vectors except those of AABB face planes...
Definition: kDOP.cpp:66
P
P
obb.v
list v
Definition: obb.py:45
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::getDistances< 5 >
void getDistances< 5 >(const Vec3f &p, FCL_REAL *d)
Specification of getDistances.
Definition: kDOP.cpp:70
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::minmax
void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL &minv, FCL_REAL &maxv)
Find the smaller and larger one of two values.
Definition: kDOP.cpp:48
hpp::fcl::KDOP::KDOP
KDOP()
Creating kDOP containing nothing.
Definition: kDOP.cpp:102
hpp::fcl::KDOP::operator+
KDOP< N > operator+(const KDOP< N > &other) const
Create a KDOP by mergin two KDOPs.
Definition: kDOP.cpp:210
hpp::fcl::KDOP::dist_
Eigen::Array< FCL_REAL, N, 1 > dist_
Origin&#39;s distances to N KDOP planes.
Definition: kDOP.h:95
hpp::fcl::CollisionRequest::break_distance
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision.
Definition: collision_data.h:255
hpp::fcl::translate
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
Definition: BV/AABB.h:226
collision_data.h
hpp::fcl::KDOP::dist
FCL_REAL dist(short i) const
Definition: kDOP.h:162
hpp::fcl::getDistances< 9 >
void getDistances< 9 >(const Vec3f &p, FCL_REAL *d)
Definition: kDOP.cpp:89
res
res
hpp::fcl::KDOP::overlap
bool overlap(const KDOP< N > &other) const
Check whether two KDOPs overlap.
Definition: kDOP.cpp:136
a
list a
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::CollisionRequest::security_margin
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.
Definition: collision_data.h:251
hpp::fcl::KDOP::distance
FCL_REAL distance(const KDOP< N > &other, Vec3f *P=NULL, Vec3f *Q=NULL) const
The distance between two KDOP<N>. Not implemented.
Definition: kDOP.cpp:216

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01