gjk.h

Go to the documentation of this file.

/*
 * 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