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 36 /** \author Jia Pan */ 37 38 #ifndef FCL_GJK_H 39 #define FCL_GJK_H 40 41 #include "fcl/shape/geometric_shapes.h" 42 #include "fcl/math/transform.h" 43 44 namespace fcl 45 { 46 47 namespace details 48 { 49 50 /// @brief the support function for shape 51 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir); 52 53 /// @brief Minkowski difference class of two shapes 54 struct MinkowskiDiff 55 { 56 /// @brief points to two shapes 57 const ShapeBase* shapes[2]; 58 59 /// @brief rotation from shape0 to shape1 60 Matrix3f toshape1; 61 62 /// @brief transform from shape1 to shape0 63 Transform3f toshape0; 64 MinkowskiDiffMinkowskiDiff65 MinkowskiDiff() { } 66 67 /// @brief support function for shape0 support0MinkowskiDiff68 inline Vec3f support0(const Vec3f& d) const 69 { 70 return getSupport(shapes[0], d); 71 } 72 73 /// @brief support function for shape1 support1MinkowskiDiff74 inline Vec3f support1(const Vec3f& d) const 75 { 76 return toshape0.transform(getSupport(shapes[1], toshape1 * d)); 77 } 78 79 /// @brief support function for the pair of shapes supportMinkowskiDiff80 inline Vec3f support(const Vec3f& d) const 81 { 82 return support0(d) - support1(-d); 83 } 84 85 /// @brief support function for the d-th shape (d = 0 or 1) supportMinkowskiDiff86 inline Vec3f support(const Vec3f& d, size_t index) const 87 { 88 if(index) 89 return support1(d); 90 else 91 return support0(d); 92 } 93 94 /// @brief support function for translating shape0, which is translating at velocity v support0MinkowskiDiff95 inline Vec3f support0(const Vec3f& d, const Vec3f& v) const 96 { 97 if(d.dot(v) <= 0) 98 return getSupport(shapes[0], d); 99 else 100 return getSupport(shapes[0], d) + v; 101 } 102 103 /// @brief support function for the pair of shapes, where shape0 is translating at velocity v supportMinkowskiDiff104 inline Vec3f support(const Vec3f& d, const Vec3f& v) const 105 { 106 return support0(d, v) - support1(-d); 107 } 108 109 /// @brief support function for the d-th shape (d = 0 or 1), where shape0 is translating at velocity v supportMinkowskiDiff110 inline Vec3f support(const Vec3f& d, const Vec3f& v, size_t index) const 111 { 112 if(index) 113 return support1(d); 114 else 115 return support0(d, v); 116 } 117 }; 118 119 120 /// @brief class for GJK algorithm 121 struct GJK 122 { 123 struct SimplexV 124 { 125 /// @brief support direction 126 Vec3f d; 127 /// @brieg support vector (i.e., the furthest point on the shape along the support direction) 128 Vec3f w; 129 }; 130 131 struct Simplex 132 { 133 /// @brief simplex vertex 134 SimplexV* c[4]; 135 /// @brief weight 136 FCL_REAL p[4]; 137 /// @brief size of simplex (number of vertices) 138 size_t rank; 139 SimplexGJK::Simplex140 Simplex() : rank(0) {} 141 }; 142 143 enum Status {Valid, Inside, Failed}; 144 145 MinkowskiDiff shape; 146 Vec3f ray; 147 FCL_REAL distance; 148 Simplex simplices[2]; 149 150 GJKGJK151 GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_), 152 tolerance(tolerance_) 153 { 154 initialize(); 155 } 156 157 void initialize(); 158 159 /// @brief GJK algorithm, given the initial value guess 160 Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess); 161 162 /// @brief apply the support function along a direction, the result is return in sv 163 void getSupport(const Vec3f& d, SimplexV& sv) const; 164 165 /// @brief apply the support function along a direction, the result is return is sv, here shape0 is translating at velocity v 166 void getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const; 167 168 /// @brief discard one vertex from the simplex 169 void removeVertex(Simplex& simplex); 170 171 /// @brief append one vertex to the simplex 172 void appendVertex(Simplex& simplex, const Vec3f& v); 173 174 /// @brief whether the simplex enclose the origin 175 bool encloseOrigin(); 176 177 /// @brief get the underlying simplex using in GJK, can be used for cache in next iteration getSimplexGJK178 inline Simplex* getSimplex() const 179 { 180 return simplex; 181 } 182 183 /// @brief get the guess from current simplex 184 Vec3f getGuessFromSimplex() const; 185 186 private: 187 SimplexV store_v[4]; 188 SimplexV* free_v[4]; 189 size_t nfree; 190 size_t current; 191 Simplex* simplex; 192 Status status; 193 194 unsigned int max_iterations; 195 FCL_REAL tolerance; 196 197 }; 198 199 200 static const size_t EPA_MAX_FACES = 128; 201 static const size_t EPA_MAX_VERTICES = 64; 202 static const FCL_REAL EPA_EPS = 0.000001; 203 static const size_t EPA_MAX_ITERATIONS = 255; 204 205 /// @brief class for EPA algorithm 206 struct EPA 207 { 208 private: 209 typedef GJK::SimplexV SimplexV; 210 struct SimplexF 211 { 212 Vec3f n; 213 FCL_REAL d; 214 SimplexV* c[3]; // a face has three vertices 215 SimplexF* f[3]; // a face has three adjacent faces 216 SimplexF* l[2]; // the pre and post faces in the list 217 size_t e[3]; 218 size_t pass; 219 }; 220 221 struct SimplexList 222 { 223 SimplexF* root; 224 size_t count; SimplexListEPA::SimplexList225 SimplexList() : root(NULL), count(0) {} appendEPA::SimplexList226 void append(SimplexF* face) 227 { 228 face->l[0] = NULL; 229 face->l[1] = root; 230 if(root) root->l[0] = face; 231 root = face; 232 ++count; 233 } 234 removeEPA::SimplexList235 void remove(SimplexF* face) 236 { 237 if(face->l[1]) face->l[1]->l[0] = face->l[0]; 238 if(face->l[0]) face->l[0]->l[1] = face->l[1]; 239 if(face == root) root = face->l[1]; 240 --count; 241 } 242 }; 243 bindEPA244 static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb) 245 { 246 fa->e[ea] = eb; fa->f[ea] = fb; 247 fb->e[eb] = ea; fb->f[eb] = fa; 248 } 249 250 struct SimplexHorizon 251 { 252 SimplexF* cf; // current face in the horizon 253 SimplexF* ff; // first face in the horizon 254 size_t nf; // number of faces in the horizon SimplexHorizonEPA::SimplexHorizon255 SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {} 256 }; 257 258 private: 259 unsigned int max_face_num; 260 unsigned int max_vertex_num; 261 unsigned int max_iterations; 262 FCL_REAL tolerance; 263 264 public: 265 266 enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed}; 267 268 Status status; 269 GJK::Simplex result; 270 Vec3f normal; 271 FCL_REAL depth; 272 SimplexV* sv_store; 273 SimplexF* fc_store; 274 size_t nextsv; 275 SimplexList hull, stock; 276 EPAEPA277 EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_), 278 max_vertex_num(max_vertex_num_), 279 max_iterations(max_iterations_), 280 tolerance(tolerance_) 281 { 282 initialize(); 283 } 284 ~EPAEPA285 ~EPA() 286 { 287 delete [] sv_store; 288 delete [] fc_store; 289 } 290 291 void initialize(); 292 293 bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist); 294 295 SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced); 296 297 /// @brief Find the best polytope face to split 298 SimplexF* findBest(); 299 300 Status evaluate(GJK& gjk, const Vec3f& guess); 301 302 /// @brief the goal is to add a face connecting vertex w and face edge f[e] 303 bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); 304 }; 305 306 307 } // details 308 309 310 311 } 312 313 314 #endif 315