bool SteerLib::GJK_EPA::GJK(const std::vector<Util::Vector>& shapeA, const std::vector<Util::Vector>& shapeB, std::vector<Util::Vector>& simplex) {
Util::Vector centerShapeA(0,0,0);
Util::Vector centerShapeB(0,0,0);
Util::Vector d(0,0,0);
centerShapeA = central_origin_of_shape(shapeA);
centerShapeB = central_origin_of_shape(shapeB);
// direction
d = centerShapeB - centerShapeA;
simplex.push_back(support_function(shapeA, shapeB, d));
// negate the direction
d = -d;
while (true) {
simplex.push_back(support_function(shapeA, shapeB, d));
if (dot(simplex.back(), d) <= 0){
return false;
}
else if (containsOrigin(simplex, d)) {
if (simplex.size() < 3) {
simplex.push_back(support_function(shapeA, shapeB, d));
}
return true;
}
}
}
void SteerLib::GJK_EPA::epa(const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB, std::vector<Util::Vector>& simplex, float& penetration_depth, Util::Vector& penetration_vector)
{
Util::Vector normal (0,0,0);
Util::Vector supportPoint (0,0,0);
// has to be a point close to 0, but not actually zero.
float TOLERANCE = 0.000001;
float distance = 0;
int i = 0;
while (true)
{
findNearestEdge(simplex, distance, normal, i);
supportPoint = support_function(_shapeA, _shapeB, normal);
double d = supportPoint * normal;
if (d - distance < TOLERANCE) {
penetration_vector = normal;
penetration_depth = d;
break;
} else {
simplex.insert(simplex.begin()+i, supportPoint);
}
}
return;
}
/* The main GJK distance routine. This routine implements the routine
* of Gilbert, Johnson and Keerthi, as described in the paper (GJK88)
* listed below. It also optionally runs my speed-up extension to this
* algorithm, as described in (Cam97).
*
* The first 4 parameters are two pairs of parameters, one pair for
* each hull; each pair is an object data-structure, plus a
* transformation data-structure. These data-structures are defined
* to this code in gjk.h, and are designed to be opaque to this code;
* the data is accessed through selectors, iterators and prediciates,
* which are discussed below.
*
* The 5th and 6th parameters are point arrays, that are set to the
* coordinates of two witness points (one within each convex hull)
* that realise the minimum distance between them.
*
* The actual return value for the function is the square of the
* minimum distance between the hulls, which is equal to the (square
* of the) distance between the witness points.
*
* The 7th parameter is a pointer to a simplex_point structure. If
* this is non-NULL then a special form of the witness points is
* stored in the structure by the routine, suitable for passing to
* this routine as seed points for any further calls involving these
* two objects. The 8th parameter is a flag, which when set tells the
* routine to use the given simplex_point structure instance as as
* seed, otherwise it just uses any seed. (If the 7th parameter is
* NULL then no harm is done.)
*
* Note that with this version one field of the simplex_point structure
* can be used to pass back the confidence region for the routine: when
* the routine returns with distance squared equal to D*d, it means that
* the true distance is known to lie between D-(E/D) and D, where E is
* the positive value returned in the `error' field of the simplex_point.
* Equivalently; the true value of the distance squared is less than or equal
* to the value returned DSQ, with an error bound width of 2E - E*E/DSQ.
* (In `normal' cases E is very small, and so the error bound width 2E can
* be sensibly used.)
*
* The code will attempt to return with E<=EPSILON, which the user
* can set, but will in any event return with some value of E. In particular,
* the code should return even with EPSILON set to zero.
*
* Alternatively, either or both of the pointer values for the witness
* points can be zero, in which case those witness points are not
* returned. The caller can later extract the coordinates of the
* witness points from the simplex_point structure by using the
* function gjk_extract_point.
*
* Returning to the opaque data-structures used to describe the objects
* and their transformations. For an object then the minimum information
* required is a list of vertices. The vertices themselves are another
* opaque type, accessed through the type VertexID. The following macros
* are defined for the vertices:
*
* InvalidVertexID a VertexID which cannot point to a valid vertex
* FirstVertex( obj) an arbitrary first vertex for the object
* NumVertices( obj) the number of vertices in the object
* IncrementVertex(o,v) set vertex to the next vertex
* ValidVertex( obj, v) says whether the VertexID is valid for obj
* LastVertex( obj, v) is this the last vertex?
* SupportValue(o,v,d) returns support value for v in direction d
*
* Optionally, the object data-structures encode the wireframe of the objects;
* this is used in my extended GJK algorithm to greatly speed up the routine
* in many cases. For an object the predicate ValidRing( obj) says whether
* this information is provided, in which case the edges that surround any
* can be accessed and traversed with the following:
*
* FirstEdge( obj, vertex) Returns the first edge (type EdgeID)
* IncrementEdge( obj, edge) Sets edge to the next edge
* ValidEdge( obj, edge) Indicates whether edge is a real edge
* VertexOfEdge( obj, edge) Returns the (other) vertex of an edge
*
* With this information this routine runs in expected constant time
* for tracking operations and small relative motions. If the
* information is not supplied the the routine reverts to using the
* original GJK technique, which takes time roughly linear in the number
* of vertices. (As a rough rule of thumb, this difference becomes
* measurable at around 10 vertices per hull, and important at about
* 20 vertices per hull.)
*
* Transformations are stored in data-structures given by opaque type
* Transform, for which the following operations need to be defined:
*
* IdentityTransform( t) Might t be an identity transformation?
* ExtractTranslation( t, v) Set v to the translation component of t
* ApplyTransform( t,o,v,tgt) Apply transform to vertex v of o, result in tgt
* ApplyInverseRotation(t,d,r) Apply inverse of t to direction d, result in r
*
* Notes:
* + it is OK for IdentityTransform( t) to return false when t is
* in fact the identity (with a small time penalty)
* + ExtractTranslation equivalently sets v to where the origin is
* transformed to by t
*
* References:
* (GJK88) "A Fast Procedure for Computing the Distance between Complex
* Objects in Three-Dimensional Space" by EG Gilbert, DW Johnson and SS
* Keerthi, IEEE Trans Robotics and Automation 4(2):193--203, April 1988.
*
* (Cam97) "A Comparison of Two Fast Algorithms for Computing the Distance
* between Convex Polyhedra" by Stephen Cameron, IEEE Trans Robotics and
* Automation 13(6):915-920, December 1997.
*
*/
REAL gjk_distance(
Object obj1, Transform tr1,
Object obj2, Transform tr2,
REAL wpt1[DIM], REAL wpt2[DIM],
struct simplex_point * simplex, int use_seed
) {
VertexID v, p, maxp, minp;
REAL minus_minv, maxv, sqrd, g_val;
REAL displacementv[DIM], reverse_displacementv[DIM];
REAL local_witness1[DIM], local_witness2[DIM];
REAL local_fdisp[DIM], local_rdisp[DIM], trv[DIM];
REAL * fdisp, * rdisp;
struct simplex_point local_simplex;
int d, compute_both_witnesses, use_default, first_iteration, max_iterations;
double oldsqrd;
assert( NumVertices( obj1)>0 && NumVertices( obj2)>0 );
use_default = first_iteration = 1;
#ifdef CONSTRUCT_TABLES
initialise_simplex_distance();
/* will return immediately if already initialised */
#else
assert( PRE_DEFINED_TABLE_DIM >= DIM );
#endif /* CONSTRUCT_TABLES */
compute_both_witnesses = ( wpt1!=0 ) || ( wpt2!=0 ) ||
( tr1!=0 ) || ( tr2!=0 );
if ( wpt1==0 )
wpt1 = local_witness1;
if ( wpt2==0 )
wpt2 = local_witness2;
fdisp = IdentityTransform( tr1) ? displacementv : local_fdisp;
rdisp = IdentityTransform( tr2) ? reverse_displacementv : local_rdisp;
if ( simplex==0 ) {
use_seed = 0;
simplex = & local_simplex;
}
if ( use_seed==0 ) {
simplex->simplex1[0] = 0; simplex->simplex2[0] = 0;
simplex->npts = 1; simplex->lambdas[0] = ONE;
simplex->last_best1 = 0; simplex->last_best2 = 0;
add_simplex_vertex( simplex, 0,
obj1, FirstVertex( obj1), tr1,
obj2, FirstVertex( obj2), tr2);
}
else {
/* If we are being told to use this seed point, there
is a good chance that the near point will be on
the current simplex. Besides, if we don't confirm
that the seed point given satisfies the invariant
(that the witness points given are the closest points
on the current simplex) things can and will fall down.
*/
for ( v=0 ; v<simplex->npts ; v++ )
add_simplex_vertex( simplex, v,
obj1, simplex->simplex1[v], tr1,
obj2, simplex->simplex2[v], tr2);
}
/* Now the main loop. We first compute the distance between the
current simplicies, the check whether this gives the globally
correct answer, and if not construct new simplices and try again.
*/
max_iterations = NumVertices( obj1)*NumVertices( obj2) ;
/* in practice we never see more than about 6 iterations. */
/* Counting the iterations in this way should not be necessary;
a while( 1) should do just as well. */
while ( max_iterations-- > 0 ) {
if ( simplex->npts==1 ) { /* simple case */
simplex->lambdas[0] = ONE;
}
else { /* normal case */
compute_subterms( simplex);
if ( use_default ) {
use_default = default_distance( simplex);
}
if ( !use_default ) {
backup_distance( simplex);
}
}
/* compute at least the displacement vectors given by the
simplex_point structure. If we are to provide both witness
points, it's slightly faster to compute those first.
*/
if ( compute_both_witnesses ) {
compute_point( wpt1, simplex->npts, simplex->coords1,
simplex->lambdas);
compute_point( wpt2, simplex->npts, simplex->coords2,
simplex->lambdas);
overd( d) {
displacementv[ d] = wpt2[d] - wpt1[d];
reverse_displacementv[ d] = - displacementv[d];
}
}
else {
overd( d) {
displacementv[d] = 0;
for ( p=0 ; p<simplex->npts ; p++ )
displacementv[d] +=
DO_MULTIPLY( simplex->lambdas[p],
simplex->coords2[p][d] - simplex->coords1[p][d]);
reverse_displacementv[ d] = - displacementv[d];
}
}
sqrd = OTHER_DOT_PRODUCT( displacementv, displacementv);
/* if we are using a c-space simplex with DIM_PLUS_ONE
points, this is interior to the simplex, and indicates
that the original hulls overlap, as does the distance
between them being too small. */
if ( sqrd<EPSILON ) {
simplex->error = EPSILON;
return sqrd;
}
if ( ! IdentityTransform( tr1) )
ApplyInverseRotation( tr1, displacementv, fdisp);
if ( ! IdentityTransform( tr2) )
ApplyInverseRotation( tr2, reverse_displacementv, rdisp);
/* find the point in obj1 that is maximal in the
direction displacement, and the point in obj2 that
is minimal in direction displacement;
*/
maxp = support_function(
obj1,
( use_seed ? simplex->last_best1 : InvalidVertexID),
&maxv, fdisp
);
minp = support_function(
obj2,
( use_seed ? simplex->last_best2 : InvalidVertexID),
&minus_minv, rdisp
);
/* Now apply the G-test on this pair of points */
INCREMENT_G_TEST_COUNTER;
g_val = sqrd + maxv + minus_minv;
if ( ! IdentityTransform( tr1) ) {
ExtractTranslation( tr1, trv);
g_val += OTHER_DOT_PRODUCT( displacementv, trv);
}
if ( ! IdentityTransform( tr2) ) {
ExtractTranslation( tr2, trv);
g_val += OTHER_DOT_PRODUCT( reverse_displacementv, trv);
}
if ( g_val < 0.0 ) /* not sure how, but it happens! */
g_val = 0;
if ( g_val < EPSILON ) {
/* then no better points - finish */
simplex->error = g_val;
return sqrd;
}
/* check for good calculation above */
if ( (first_iteration || (sqrd < oldsqrd))
&& (simplex->npts <= DIM ) ) {
/* Normal case: add the new c-space points to the current
simplex, and call simplex_distance() */
simplex->simplex1[ simplex->npts] = simplex->last_best1 = maxp;
simplex->simplex2[ simplex->npts] = simplex->last_best2 = minp;
simplex->lambdas[ simplex->npts] = ZERO;
add_simplex_vertex( simplex, simplex->npts,
obj1, maxp, tr1,
obj2, minp, tr2);
simplex->npts++;
oldsqrd = sqrd;
first_iteration = 0;
use_default = 1;
continue;
}
/* Abnormal cases! */
if ( use_default ) {
use_default = 0;
}
else { /* give up trying! */
simplex->error = g_val;
return sqrd;
}
} /* end of `while ( 1 )' */
