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 "coal/collision_data.h"
39 #include "coal/BV/kDOP.h"
40 
41 #include <limits>
42 #include <iostream>
43 
44 namespace coal {
45 
47 inline void minmax(CoalScalar a, CoalScalar b, CoalScalar& minv,
48  CoalScalar& 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(CoalScalar p, CoalScalar& minv, CoalScalar& maxv) {
59  if (p > maxv) maxv = p;
60  if (p < minv) minv = p;
61 }
62 
65 template <short N>
66 void getDistances(const Vec3s& /*p*/, CoalScalar* /*d*/) {}
67 
69 template <>
70 inline void getDistances<5>(const Vec3s& p, CoalScalar* 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 Vec3s& p, CoalScalar* 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 Vec3s& p, CoalScalar* 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  CoalScalar real_max = (std::numeric_limits<CoalScalar>::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 Vec3s& v) {
110  for (short i = 0; i < 3; ++i) {
111  dist_[i] = dist_[N / 2 + i] = v[i];
112  }
113 
114  CoalScalar 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 Vec3s& a, const Vec3s& b) {
123  for (short i = 0; i < 3; ++i) {
124  minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
125  }
126 
127  CoalScalar 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  CoalScalar& sqrDistLowerBound) const {
147  const CoalScalar breakDistance(request.break_distance +
148  request.security_margin);
149 
150  CoalScalar 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  CoalScalar 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 Vec3s& 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<CoalScalar, 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 Vec3s& p) {
187  for (short i = 0; i < 3; ++i) {
188  minmax(p[i], dist_[i], dist_[N / 2 + i]);
189  }
190 
191  CoalScalar 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 CoalScalar KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3s* /*P*/,
217  Vec3s* /*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 Vec3s& 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  CoalScalar 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 Vec3s&);
245 template KDOP<18> translate<18>(const KDOP<18>&, const Vec3s&);
246 template KDOP<24> translate<24>(const KDOP<24>&, const Vec3s&);
247 
248 } // namespace coal
coal::KDOP::inside
bool inside(const Vec3s &p) const
Definition: kDOP.cpp:171
coal::translate< 16 >
template KDOP< 16 > translate< 16 >(const KDOP< 16 > &, const Vec3s &)
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
coal::minmax
void minmax(CoalScalar a, CoalScalar b, CoalScalar &minv, CoalScalar &maxv)
Find the smaller and larger one of two values.
Definition: kDOP.cpp:47
coal::KDOP::overlap
bool overlap(const KDOP< N > &other) const
Check whether two KDOPs overlap.
Definition: kDOP.cpp:136
coal::CollisionRequest::security_margin
CoalScalar security_margin
Distance below which objects are considered in collision. See Collision.
Definition: coal/collision_data.h:328
collision_data.h
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
coal::translate< 24 >
template KDOP< 24 > translate< 24 >(const KDOP< 24 > &, const Vec3s &)
coal::KDOP::dist_
Eigen::Array< CoalScalar, N, 1 > dist_
Origin's distances to N KDOP planes.
Definition: coal/BV/kDOP.h:94
a
list a
coal::getDistances< 5 >
void getDistances< 5 >(const Vec3s &p, CoalScalar *d)
Specification of getDistances.
Definition: kDOP.cpp:70
res
res
kDOP.h
d
d
coal::getDistances< 9 >
void getDistances< 9 >(const Vec3s &p, CoalScalar *d)
Definition: kDOP.cpp:89
coal::KDOP::operator+=
KDOP< N > & operator+=(const Vec3s &p)
Merge the point and the KDOP.
Definition: kDOP.cpp:186
P
P
coal::KDOP::KDOP
EIGEN_MAKE_ALIGNED_OPERATOR_NEW KDOP()
Creating kDOP containing nothing.
Definition: kDOP.cpp:102
coal::KDOP::operator+
KDOP< N > operator+(const KDOP< N > &other) const
Create a KDOP by mergin two KDOPs.
Definition: kDOP.cpp:210
coal::CollisionRequest
request to the collision algorithm
Definition: coal/collision_data.h:311
coal::translate
static AABB translate(const AABB &aabb, const Vec3s &t)
translate the center of AABB by t
Definition: coal/BV/AABB.h:233
coal::translate< 18 >
template KDOP< 18 > translate< 18 >(const KDOP< 18 > &, const Vec3s &)
coal::getDistances< 6 >
void getDistances< 6 >(const Vec3s &p, CoalScalar *d)
Definition: kDOP.cpp:79
coal::getDistances
void getDistances(const Vec3s &, CoalScalar *)
Compute the distances to planes with normals from KDOP vectors except those of AABB face planes.
Definition: kDOP.cpp:66
coal::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter,...
Definition: coal/BV/kDOP.h:91
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
coal::KDOP::distance
CoalScalar distance(const KDOP< N > &other, Vec3s *P=NULL, Vec3s *Q=NULL) const
The distance between two KDOP<N>. Not implemented.
Definition: kDOP.cpp:216
t
dictionary t
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
coal::CollisionRequest::break_distance
CoalScalar break_distance
Distance below which bounding volumes are broken down. See Collision.
Definition: coal/collision_data.h:332
obb.v
list v
Definition: obb.py:48

hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:58