Program Listing for File gjk.h

↰ Return to documentation for file (include/coal/narrowphase/gjk.h)

/*
 * 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 {
    Vec3s w0, w1;
    Vec3s 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:
  CoalScalar distance_upper_bound;
  Status status;
  GJKVariant gjk_variant;
  GJKConvergenceCriterion convergence_criterion;
  GJKConvergenceCriterionType convergence_criterion_type;

  MinkowskiDiff const* shape;
  Vec3s ray;
  support_func_guess_t support_hint;
  CoalScalar 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;
  CoalScalar 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_, CoalScalar 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_, CoalScalar tolerance_);

  Status evaluate(
      const MinkowskiDiff& shape, const Vec3s& guess,
      const support_func_guess_t& supportHint = support_func_guess_t::Zero());

  inline void getSupport(const Vec3s& d, SimplexV& sv,
                         support_func_guess_t& hint) const {
    shape->support(d, sv.w0, sv.w1, hint);
    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, Vec3s& w0,
                                 Vec3s& w1, Vec3s& normal) const;

  Vec3s getGuessFromSimplex() const;

  void setDistanceEarlyBreak(const CoalScalar& dup) {
    distance_upper_bound = dup;
  }

  bool checkConvergence(const Vec3s& w, const CoalScalar& rl, CoalScalar& alpha,
                        const CoalScalar& omega) const;

  size_t getNumMaxIterations() const { return max_iterations; }

  CoalScalar 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 Vec3s& 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 {
    Vec3s n;
    CoalScalar 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(Vec3s::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;
  Vec3s normal;
  support_func_guess_t support_hint;
  CoalScalar 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;
  CoalScalar 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_, CoalScalar 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(); }

  CoalScalar 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, CoalScalar tolerance);

  Status evaluate(GJK& gjk, const Vec3s& guess);

  void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
                                 Vec3s& w1, Vec3s& normal) const;

 private:
  void initialize();

  bool getEdgeDist(SimplexFace* face, const SimplexVertex& a,
                   const SimplexVertex& b, CoalScalar& 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