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 {
 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