.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_narrowphase_gjk.h: Program Listing for File gjk.h ============================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/hpp-fcl/include/hpp/fcl/narrowphase/gjk.h``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef HPP_FCL_GJK_H #define HPP_FCL_GJK_H #include #include #include namespace hpp { namespace fcl { namespace details { Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint); struct HPP_FCL_DLLAPI MinkowskiDiff { typedef Eigen::Array Array2d; const ShapeBase* shapes[2]; struct ShapeData { std::vector visited; }; ShapeData data[2]; Matrix3f oR1; Vec3f ot1; Array2d inflation; int linear_log_convex_threshold; bool normalize_support_direction; typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff, const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1, support_func_guess_t& hint, ShapeData data[2]); GetSupportFunction getSupportFunc; MinkowskiDiff() : linear_log_convex_threshold(32), normalize_support_direction(false), getSupportFunc(NULL) {} void set(const ShapeBase* shape0, const ShapeBase* shape1); void set(const ShapeBase* shape0, const ShapeBase* shape1, const Transform3f& tf0, const Transform3f& tf1); inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const { return getSupport(shapes[0], d, dIsNormalized, hint); } inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const { return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + ot1; } inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, support_func_guess_t& hint) const { assert(getSupportFunc != NULL); getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint, const_cast(data)); } }; struct HPP_FCL_DLLAPI GJK { struct HPP_FCL_DLLAPI SimplexV { Vec3f w0, w1; Vec3f w; }; typedef unsigned char vertex_id_t; struct HPP_FCL_DLLAPI Simplex { SimplexV* vertex[4]; vertex_id_t rank; Simplex() {} }; enum Status { Valid, Inside, Failed, EarlyStopped }; MinkowskiDiff const* shape; Vec3f ray; GJKVariant gjk_variant; GJKConvergenceCriterion convergence_criterion; GJKConvergenceCriterionType convergence_criterion_type; support_func_guess_t support_hint; FCL_REAL distance; Simplex simplices[2]; GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_), tolerance(tolerance_) { initialize(); } void initialize(); Status evaluate( const MinkowskiDiff& shape, const Vec3f& guess, const support_func_guess_t& supportHint = support_func_guess_t::Zero()); inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv, support_func_guess_t& hint) const { shape->support(d, dIsNormalized, sv.w0, sv.w1, hint); sv.w = sv.w0 - sv.w1; } bool encloseOrigin(); inline Simplex* getSimplex() const { return simplex; } bool hasClosestPoints() { return distance < distance_upper_bound; } bool hasPenetrationInformation(const MinkowskiDiff& shape) { return distance > -shape.inflation.sum(); } bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); Vec3f getGuessFromSimplex() const; void setDistanceEarlyBreak(const FCL_REAL& dup) { distance_upper_bound = dup; } bool checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, const FCL_REAL& omega); inline size_t getIterations() { return iterations; } inline FCL_REAL getTolerance() { return tolerance; } private: SimplexV store_v[4]; SimplexV* free_v[4]; vertex_id_t nfree; vertex_id_t current; Simplex* simplex; Status status; unsigned int max_iterations; FCL_REAL tolerance; FCL_REAL distance_upper_bound; size_t iterations; inline void removeVertex(Simplex& simplex); inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, support_func_guess_t& hint); // of the simplex are added. To know more about these, visit // https://caseymuratori.com/blog_0003. bool projectLineOrigin(const Simplex& current, Simplex& next); bool projectTriangleOrigin(const Simplex& current, Simplex& next); bool projectTetrahedraOrigin(const Simplex& current, Simplex& next); }; static const size_t EPA_MAX_FACES = 128; static const size_t EPA_MAX_VERTICES = 64; static const FCL_REAL EPA_EPS = 0.000001; static const size_t EPA_MAX_ITERATIONS = 255; struct HPP_FCL_DLLAPI EPA { typedef GJK::SimplexV SimplexV; struct HPP_FCL_DLLAPI SimplexF { Vec3f n; FCL_REAL d; SimplexV* vertex[3]; // a face has three vertices SimplexF* f[3]; // a face has three adjacent faces SimplexF* l[2]; // the pre and post faces in the list size_t e[3]; size_t pass; SimplexF() : n(Vec3f::Zero()){}; }; struct HPP_FCL_DLLAPI SimplexList { SimplexF* root; size_t count; SimplexList() : root(NULL), count(0) {} void append(SimplexF* face) { face->l[0] = NULL; face->l[1] = root; if (root) root->l[0] = face; root = face; ++count; } void remove(SimplexF* face) { if (face->l[1]) face->l[1]->l[0] = face->l[0]; if (face->l[0]) face->l[0]->l[1] = face->l[1]; if (face == root) root = face->l[1]; --count; } }; static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb) { fa->e[ea] = eb; fa->f[ea] = fb; fb->e[eb] = ea; fb->f[eb] = fa; } struct HPP_FCL_DLLAPI SimplexHorizon { SimplexF* cf; // current face in the horizon SimplexF* ff; // first face in the horizon size_t nf; // number of faces in the horizon SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {} }; private: unsigned int max_face_num; unsigned int max_vertex_num; unsigned int max_iterations; FCL_REAL tolerance; public: enum Status { Failed = 0, Valid = 1, AccuracyReached = 1 << 1 | Valid, Degenerated = 1 << 1 | Failed, NonConvex = 2 << 1 | Failed, InvalidHull = 3 << 1 | Failed, OutOfFaces = 4 << 1 | Failed, OutOfVertices = 5 << 1 | Failed, FallBack = 6 << 1 | Failed }; Status status; GJK::Simplex result; Vec3f normal; FCL_REAL depth; SimplexV* sv_store; SimplexF* fc_store; size_t nextsv; SimplexList hull, stock; EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_), max_vertex_num(max_vertex_num_), max_iterations(max_iterations_), tolerance(tolerance_) { initialize(); } ~EPA() { delete[] sv_store; delete[] fc_store; } void initialize(); Status evaluate(GJK& gjk, const Vec3f& guess); bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); private: bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist); SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex, bool forced); SimplexF* findBest(); bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); }; } // namespace details } // namespace fcl } // namespace hpp #endif