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