.. _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 <file__tmp_ws_src_hpp-fcl_include_hpp_fcl_narrowphase_gjk.h>` (``/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 <vector>
   
   #include <hpp/fcl/shape/geometric_shapes.h>
   #include <hpp/fcl/math/transform.h>
   
   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<FCL_REAL, 1, 2> Array2d;
   
     const ShapeBase* shapes[2];
   
     struct ShapeData {
       std::vector<int8_t> 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<ShapeData*>(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