FCL  0.6.0
Flexible Collision Library
minkowski_diff-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_NARROWPHASE_DETAIL_MINKOWSKIDIFF_INL_H
39 #define FCL_NARROWPHASE_DETAIL_MINKOWSKIDIFF_INL_H
40 
41 #include "fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h"
42 
43 #include "fcl/geometry/shape/box.h"
44 #include "fcl/geometry/shape/capsule.h"
45 #include "fcl/geometry/shape/cone.h"
46 #include "fcl/geometry/shape/convex.h"
47 #include "fcl/geometry/shape/cylinder.h"
48 #include "fcl/geometry/shape/ellipsoid.h"
49 #include "fcl/geometry/shape/halfspace.h"
50 #include "fcl/geometry/shape/plane.h"
51 #include "fcl/geometry/shape/sphere.h"
52 #include "fcl/geometry/shape/triangle_p.h"
53 
54 namespace fcl
55 {
56 
57 namespace detail
58 {
59 
60 //==============================================================================
61 extern template
62 struct MinkowskiDiff<double>;
63 
64 //==============================================================================
65 template <typename S, typename Derived>
66 Vector3<S> getSupport(
67  const ShapeBase<S>* shape,
68  const Eigen::MatrixBase<Derived>& dir)
69 {
70  // Check the number of rows is 6 at compile time
71  EIGEN_STATIC_ASSERT(
72  Derived::RowsAtCompileTime == 3
73  && Derived::ColsAtCompileTime == 1,
74  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
75 
76  switch(shape->getNodeType())
77  {
78  case GEOM_TRIANGLE:
79  {
80  const auto* triangle = static_cast<const TriangleP<S>*>(shape);
81  S dota = dir.dot(triangle->a);
82  S dotb = dir.dot(triangle->b);
83  S dotc = dir.dot(triangle->c);
84  if(dota > dotb)
85  {
86  if(dotc > dota)
87  return triangle->c;
88  else
89  return triangle->a;
90  }
91  else
92  {
93  if(dotc > dotb)
94  return triangle->c;
95  else
96  return triangle->b;
97  }
98  }
99  break;
100  case GEOM_BOX:
101  {
102  const Box<S>* box = static_cast<const Box<S>*>(shape);
103  return Vector3<S>((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2),
104  (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2),
105  (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2));
106  }
107  break;
108  case GEOM_SPHERE:
109  {
110  const Sphere<S>* sphere = static_cast<const Sphere<S>*>(shape);
111  return dir * sphere->radius;
112  }
113  break;
114  case GEOM_ELLIPSOID:
115  {
116  const Ellipsoid<S>* ellipsoid = static_cast<const Ellipsoid<S>*>(shape);
117 
118  const S a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
119  const S b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
120  const S c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
121 
122  const Vector3<S> v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
123  const S d = std::sqrt(v.dot(dir));
124 
125  return v / d;
126  }
127  break;
128  case GEOM_CAPSULE:
129  {
130  const Capsule<S>* capsule = static_cast<const Capsule<S>*>(shape);
131  S half_h = capsule->lz * 0.5;
132  Vector3<S> pos1(0, 0, half_h);
133  Vector3<S> pos2(0, 0, -half_h);
134  Vector3<S> v = dir * capsule->radius;
135  pos1 += v;
136  pos2 += v;
137  if(dir.dot(pos1) > dir.dot(pos2))
138  return pos1;
139  else return pos2;
140  }
141  break;
142  case GEOM_CONE:
143  {
144  const Cone<S>* cone = static_cast<const Cone<S>*>(shape);
145  S zdist = dir[0] * dir[0] + dir[1] * dir[1];
146  S len = zdist + dir[2] * dir[2];
147  zdist = std::sqrt(zdist);
148  len = std::sqrt(len);
149  S half_h = cone->lz * 0.5;
150  S radius = cone->radius;
151 
152  S sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);
153 
154  if(dir[2] > len * sin_a)
155  return Vector3<S>(0, 0, half_h);
156  else if(zdist > 0)
157  {
158  S rad = radius / zdist;
159  return Vector3<S>(rad * dir[0], rad * dir[1], -half_h);
160  }
161  else
162  return Vector3<S>(0, 0, -half_h);
163  }
164  break;
165  case GEOM_CYLINDER:
166  {
167  const Cylinder<S>* cylinder = static_cast<const Cylinder<S>*>(shape);
168  S zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
169  S half_h = cylinder->lz * 0.5;
170  if(zdist == 0.0)
171  {
172  return Vector3<S>(0, 0, (dir[2]>0)? half_h:-half_h);
173  }
174  else
175  {
176  S d = cylinder->radius / zdist;
177  return Vector3<S>(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h);
178  }
179  }
180  break;
181  case GEOM_CONVEX:
182  {
183  const Convex<S>* convex = static_cast<const Convex<S>*>(shape);
184  S maxdot = - std::numeric_limits<S>::max();
185  Vector3<S>* curp = convex->points;
186  Vector3<S> bestv = Vector3<S>::Zero();
187  for(int i = 0; i < convex->num_points; ++i, curp+=1)
188  {
189  S dot = dir.dot(*curp);
190  if(dot > maxdot)
191  {
192  bestv = *curp;
193  maxdot = dot;
194  }
195  }
196  return bestv;
197  }
198  break;
199  case GEOM_PLANE:
200  break;
201  default:
202  ; // nothing
203  }
204 
205  return Vector3<S>::Zero();
206 }
207 
208 //==============================================================================
209 template <typename S>
210 MinkowskiDiff<S>::MinkowskiDiff()
211 {
212  // Do nothing
213 }
214 
215 //==============================================================================
216 template <typename S>
217 Vector3<S> MinkowskiDiff<S>::support0(const Vector3<S>& d) const
218 {
219  return getSupport(shapes[0], d);
220 }
221 
222 //==============================================================================
223 template <typename S>
224 Vector3<S> MinkowskiDiff<S>::support1(const Vector3<S>& d) const
225 {
226  return toshape0 * getSupport(shapes[1], toshape1 * d);
227 }
228 
229 //==============================================================================
230 template <typename S>
231 Vector3<S> MinkowskiDiff<S>::support(const Vector3<S>& d) const
232 {
233  return support0(d) - support1(-d);
234 }
235 
236 //==============================================================================
237 template <typename S>
238 Vector3<S> MinkowskiDiff<S>::support(const Vector3<S>& d, size_t index) const
239 {
240  if(index)
241  return support1(d);
242  else
243  return support0(d);
244 }
245 
246 //==============================================================================
247 template <typename S>
248 Vector3<S> MinkowskiDiff<S>::support0(const Vector3<S>& d, const Vector3<S>& v) const
249 {
250  if(d.dot(v) <= 0)
251  return getSupport(shapes[0], d);
252  else
253  return getSupport(shapes[0], d) + v;
254 }
255 
256 //==============================================================================
257 template <typename S>
258 Vector3<S> MinkowskiDiff<S>::support(const Vector3<S>& d, const Vector3<S>& v) const
259 {
260  return support0(d, v) - support1(-d);
261 }
262 
263 //==============================================================================
264 template <typename S>
265 Vector3<S> MinkowskiDiff<S>::support(const Vector3<S>& d, const Vector3<S>& v, size_t index) const
266 {
267  if(index)
268  return support1(d);
269  else
270  return support0(d, v);
271 }
272 
273 } // namespace detail
274 } // namespace fcl
275 
276 #endif
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
Vector3< S > support(const Vector3< S > &d) const
support function for the pair of shapes
Definition: minkowski_diff-inl.h:231
Vector3< S > support0(const Vector3< S > &d) const
support function for shape0
Definition: minkowski_diff-inl.h:217
Vector3< S > support1(const Vector3< S > &d) const
support function for shape1
Definition: minkowski_diff-inl.h:224