Program Listing for File gjk.h
↰ Return to documentation for file (/tmp/ws/src/hpp-fcl/include/hpp/fcl/narrowphase/gjk.h)
/*
* 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