.. _program_listing_file_include_coal_narrowphase_gjk.h:

Program Listing for File gjk.h
==============================

|exhale_lsh| :ref:`Return to documentation for file <file_include_coal_narrowphase_gjk.h>` (``include/coal/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
    *  Copyright (c) 2021-2024, INRIA
    *  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 COAL_GJK_H
   #define COAL_GJK_H
   
   #include <vector>
   
   #include "coal/narrowphase/minkowski_difference.h"
   
   namespace coal {
   
   namespace details {
   
   struct COAL_DLLAPI GJK {
     struct COAL_DLLAPI SimplexV {
       Vec3ps w0, w1;
       Vec3ps w;
     };
   
     typedef unsigned char vertex_id_t;
   
     struct COAL_DLLAPI Simplex {
       SimplexV* vertex[4];
       vertex_id_t rank;
   
       Simplex() {}
   
       void reset() {
         rank = 0;
         for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr;
       }
     };
   
     enum Status {
       DidNotRun,
       Failed,
       NoCollisionEarlyStopped,
       NoCollision,
       CollisionWithPenetrationInformation,
       Collision
     };
   
    public:
     SolverScalar distance_upper_bound;
     Status status;
     GJKVariant gjk_variant;
     GJKConvergenceCriterion convergence_criterion;
     GJKConvergenceCriterionType convergence_criterion_type;
   
     MinkowskiDiff const* shape;
     Vec3ps ray;
     support_func_guess_t support_hint;
     SolverScalar distance;
     Simplex* simplex;  // Pointer to the result of the last run of GJK.
   
    private:
     // max_iteration and tolerance are made private
     // because they are meant to be set by the `reset` function.
     size_t max_iterations;
     SolverScalar tolerance;
   
     SimplexV store_v[4];
     SimplexV* free_v[4];
     vertex_id_t nfree;
     vertex_id_t current;
     Simplex simplices[2];
     size_t iterations;
     size_t iterations_momentum_stop;
   
    public:
     GJK(size_t max_iterations_, SolverScalar tolerance_)
         : max_iterations(max_iterations_), tolerance(tolerance_) {
       COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.",
                   std::invalid_argument);
       initialize();
     }
   
     void reset(size_t max_iterations_, SolverScalar tolerance_);
   
     Status evaluate(
         const MinkowskiDiff& shape, const Vec3ps& guess,
         const support_func_guess_t& supportHint = support_func_guess_t::Zero());
   
     inline void getSupport(const Vec3ps& d, SimplexV& sv,
                            support_func_guess_t& hint) const {
       Vec3s w0, w1;
       shape->support(d.cast<Scalar>(), w0, w1, hint);
       sv.w0 = w0.cast<SolverScalar>();
       sv.w1 = w1.cast<SolverScalar>();
       sv.w = sv.w0 - sv.w1;
     }
   
     bool encloseOrigin();
   
     inline Simplex* getSimplex() const { return simplex; }
   
     bool hasClosestPoints() const { return distance < distance_upper_bound; }
   
     void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0,
                                    Vec3ps& w1, Vec3ps& normal) const;
   
     Vec3ps getGuessFromSimplex() const;
   
     void setDistanceEarlyBreak(const SolverScalar& dup) {
       distance_upper_bound = dup;
     }
   
     bool checkConvergence(const Vec3ps& w, const SolverScalar& rl,
                           SolverScalar& alpha, const SolverScalar& omega) const;
   
     size_t getNumMaxIterations() const { return max_iterations; }
   
     SolverScalar getTolerance() const { return tolerance; }
   
     size_t getNumIterations() const { return iterations; }
   
     size_t getNumIterationsMomentumStopped() const {
       return iterations_momentum_stop;
     }
   
    private:
     void initialize();
   
     inline void removeVertex(Simplex& simplex);
   
     inline void appendVertex(Simplex& simplex, const Vec3ps& v,
                              support_func_guess_t& hint);
   
     bool projectLineOrigin(const Simplex& current, Simplex& next);
   
     bool projectTriangleOrigin(const Simplex& current, Simplex& next);
   
     bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
   };
   
   struct COAL_DLLAPI EPA {
     typedef GJK::SimplexV SimplexVertex;
     struct COAL_DLLAPI SimplexFace {
       Vec3ps n;
       SolverScalar d;
       bool ignore;          // If the origin does not project inside the face, we
                             // ignore this face.
       size_t vertex_id[3];  // Index of vertex in sv_store.
       SimplexFace* adjacent_faces[3];  // A face has three adjacent faces.
       SimplexFace* prev_face;          // The previous face in the list.
       SimplexFace* next_face;          // The next face in the list.
       size_t adjacent_edge[3];         // Each face has 3 edges: `0`, `1` and `2`.
                                 // The i-th adjacent face is bound (to this face)
                                 // along its `adjacent_edge[i]`-th edge
                                 // (with 0 <= i <= 2).
       size_t pass;
   
       SimplexFace() : n(Vec3ps::Zero()), ignore(false) {};
     };
   
     struct COAL_DLLAPI SimplexFaceList {
       SimplexFace* root;
       size_t count;
       SimplexFaceList() : root(nullptr), count(0) {}
   
       void reset() {
         root = nullptr;
         count = 0;
       }
   
       void append(SimplexFace* face) {
         face->prev_face = nullptr;
         face->next_face = root;
         if (root != nullptr) root->prev_face = face;
         root = face;
         ++count;
       }
   
       void remove(SimplexFace* face) {
         if (face->next_face != nullptr)
           face->next_face->prev_face = face->prev_face;
         if (face->prev_face != nullptr)
           face->prev_face->next_face = face->next_face;
         if (face == root) root = face->next_face;
         --count;
       }
     };
   
     static inline void bind(SimplexFace* fa, size_t ea, SimplexFace* fb,
                             size_t eb) {
       assert(ea == 0 || ea == 1 || ea == 2);
       assert(eb == 0 || eb == 1 || eb == 2);
       fa->adjacent_edge[ea] = eb;
       fa->adjacent_faces[ea] = fb;
       fb->adjacent_edge[eb] = ea;
       fb->adjacent_faces[eb] = fa;
     }
   
     struct COAL_DLLAPI SimplexHorizon {
       SimplexFace* current_face;  // current face in the horizon
       SimplexFace* first_face;    // first face in the horizon
       size_t num_faces;           // number of faces in the horizon
       SimplexHorizon()
           : current_face(nullptr), first_face(nullptr), num_faces(0) {}
     };
   
     enum Status {
       DidNotRun = -1,
       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
     };
   
    public:
     Status status;
     GJK::Simplex result;
     Vec3ps normal;
     support_func_guess_t support_hint;
     SolverScalar depth;
     SimplexFace* closest_face;
   
    private:
     // max_iteration and tolerance are made private
     // because they are meant to be set by the `reset` function.
     size_t max_iterations;
     SolverScalar tolerance;
   
     std::vector<SimplexVertex> sv_store;
     std::vector<SimplexFace> fc_store;
     SimplexFaceList hull, stock;
     size_t num_vertices;  // number of vertices in polytpoe constructed by EPA
     size_t iterations;
   
    public:
     EPA(size_t max_iterations_, SolverScalar tolerance_)
         : max_iterations(max_iterations_), tolerance(tolerance_) {
       initialize();
     }
   
     EPA(const EPA& other)
         : max_iterations(other.max_iterations),
           tolerance(other.tolerance),
           sv_store(other.sv_store),
           fc_store(other.fc_store) {
       initialize();
     }
   
     size_t getNumMaxIterations() const { return max_iterations; }
   
     size_t getNumMaxVertices() const { return sv_store.size(); }
   
     size_t getNumMaxFaces() const { return fc_store.size(); }
   
     SolverScalar getTolerance() const { return tolerance; }
   
     size_t getNumIterations() const { return iterations; }
   
     size_t getNumVertices() const { return num_vertices; }
   
     size_t getNumFaces() const { return hull.count; }
   
     void reset(size_t max_iterations, SolverScalar tolerance);
   
     Status evaluate(GJK& gjk, const Vec3ps& guess);
   
     void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0,
                                    Vec3ps& w1, Vec3ps& normal) const;
   
    private:
     void initialize();
   
     bool getEdgeDist(SimplexFace* face, const SimplexVertex& a,
                      const SimplexVertex& b, SolverScalar& dist);
   
     SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex,
                          bool force = false);
   
     SimplexFace* findClosestFace();
   
     bool expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
                 SimplexHorizon& horizon);
   
     // @brief Use this function to debug expand if needed.
     // void PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w);
   };
   
   }  // namespace details
   
   }  // namespace coal
   
   #endif