In the previous post we looked into the Dyn4J implementation of the GJK algorithm to detect overlap between two convex shapes. That particular implementation simply returned true or false depending on whether the convex shapes overlaps or not. With a little modification, GJK can actually be used to calculate the closest distance between two convex shapes, as well as the points on each shape that determine the closest distance.
Again, we look at the Dyn4J implementation. The Dyn4J site here explains the algorithm extremely well. In my post I will add some notes on peripheral implementation details that took me a bit of time to understand.
Overview
Instead of iteratively trying to enclose the origin with the simplex, we want to find the simplex that is closest to the origin. The closets simplex will always be an "edge" of the Minkowski Difference, i.e. a 1-simplex.
If we can find the closest edge to the origin, we can find the distance by the distance of that edge to the origin (i.e. the magnitude of the point on the edge that is closest to the origin). The key difference of the previous implementation is that as we are only interested in finding an "edge" we only keep 2 points in the simplex at all times.
Finding the points that make up the closest distance
So now we know the closest poitn to the origin in the termination spliex. but how do we translate that tot he closets points on the indiviual shapes as you can image the source points we always vvertex and the closes poin will be on an edge.
The points used to create the minkowski difference points are not necessarily the closest points. But wiht a bit more work we can find the source points to find the closets points. SOMETMES CALLED WITNESS POINTS!
As always, some comments on the new classes / objects required of this implementation.
In addition to the separation distance, the GJK algorithm can provide a pair of closest points between the input objects. Upon termination with nonintersection, the set Q contains up to three points Qi, forming a simplex from the Minkowski difference C = A B. Let k denote the number of points in Q and let P be the point of minimum norm inCH(Q). Because Q contains the smallest number of vertices needed to express P as a convex combination, P can be expressed as
where (c1,c2, ... ,ck) are the barycentric coordinates of P with respect to points Qi. Given that each point Qi is the difference of points Ai and Bi from input objects A and B, respectively, P can be rewritten as
Because P realizes the shortest distance between A and B, there must exist (not necessarily unique) points on the boundaries of A and B such that their difference is P. One such pair of points is G and H. Thus, by maintaining not just points Qi but Ai and Bi a pair of closest points between A and B is directly obtained per the previous. Note that the calculations needed to determine P on the convex hull of Q amounts to computing the barycentric coordinates of P with respect to points Qi, and thus the barycentric coordinates are effectively obtained for free. In addition to the closest pair of points, it is possible to derive contact manifolds from the simplices at the termination of the GJK algorithm for two colliding (or near-colliding) polyhedra. See [Zhang95] and [Nagle02] for details.
class MinkowskiSumPoint {
constructor(supportPoint1, supportPoint2) {
this.supportPoint1 = supportPoint1;
this.supportPoint2 = supportPoint2;
this.point = supportPoint1.difference(supportPoint2);
}
getSupportPoint1() {
return this.supportPoint1;
}
getSupportPoint2() {
return this.supportPoint2;
}
/**
* Returns the Minkowski sum point given the two support points.
* @return Vector2
*/
getPoint() {
return this.point;
}
}Determine if a point is contained in a triangle
/**
* Returns true if the origin is within the triangle given by
* a, b, and c.
* <p>
* If the origin lies on the same side of all the points then we
* know that the origin is in the triangle.
* <pre> sign(location(origin, a, b)) == sign(location(origin, b, c)) == sign(location(origin, c, a))</pre>
* The {@link Segment#getLocation(Vector2, Vector2, Vector2)} method
* can be simplified because we are using the origin as the search point:
* <pre> = (b.x - a.x) * (origin.y - a.y) - (origin.x - a.x) * (b.y - a.y)
* = (b.x - a.x) * (-a.y) - (-a.x) * (b.y - a.y)
* = -a.y * b.x + a.y * a.x + a.x * b.y - a.x * a.y
* = -a.y * b.x + a.x * b.y
* = a.x * b.y - a.y * b.x
* = a.cross(b)</pre>
* @param a the first point
* @param b the second point
* @param c the third point
* @return boolean
*/
function containsOrigin( a, b, c) {
var sa = a.cross(b);
var sb = b.cross(c);
var sc = c.cross(a);
// this is sufficient (we do not need to test sb * sc)
return (sa * sb > 0 && sa * sc > 0);
}GJK to find closest distance
As before we start by
find one simplex
etc
Note how we set the pointing that is closest on the intiial edge ie d is vector pointing away from the origin to the closets point in the initial simplex. note that this point the simpe may not be an edge
function distance(convex1, convex2, /*Separation*/ separation) {
var c1 = convex1.position;
var c2 = convex2.position;
// create a Minkowski sum
var /*MinkowskiSum*/ ms = new MinkowskiSum(convex1, convex2);
// define some Minkowski points
var /*MinkowskiSumPoint*/ a = null;
var /*MinkowskiSumPoint*/ b = null;
var /*MinkowskiSumPoint*/ c = null;
// choose some search direction
var d = c1.to(c2);
// check for a zero direction vector
// a zero direction vector indicates that the center's are coincident
// which guarantees that the convex shapes are overlapping
if (d.isZero()) return false;
// add the first point
a = ms.getSupportPoints(d);
// negate the direction
d.negate();
// get a second support point
b = ms.getSupportPoints(d);
// find the point on the simplex (segment) closest to the origin
// and use that as the new search direction
d = Segment.getPointOnSegmentClosestToPoint(ORIGIN, b.point, a.point);
////////////////////////////////////////////////////////////////////
// MAIN ITERATIVE LOOP GOES HERE - SEE BELOW ///
///////////////////////////////////////////////////////////////////
// if we made it here then we know that we hit the maximum number of iterations
// this is really a catch all termination case
// get the closest points
this.findClosestPoints(a, b, separation);
// return true to indicate separation
return true;
}Main iterative loop
now we entered the main iteration loop. First we negate d based on the previous stp now we have vecotor pointin from th closes poin on edge to the origin.
for (let i = 0; i < MAX_DISTANCE_ITERATIONS; i++) {
// the vector from the point we found to the origin is the new search direction
d.negate();
// check if d is zero
if (d.getMagnitudeSquared() <= Epsilon.E) {
// if the closest point is the origin then the shapes are not separated
return false;
}
// get the farthest point along d
c = ms.getSupportPoints(d);
// test if the triangle made by a, b, and c contains the origin
if (this.containsOrigin(a.point, b.point, c.point)) {
// if it does then return false;
return false;
}
Are we making progres? is point C same as the previous point? Note itration is used because for curved shapes the differnce in projection will approach zero but may not actualy reach zero. So may not terminate unless if we have max number of iterations, defined at the begiining.
// see if the new point is far enough along d
var projection = c.point.dot(d);
if ((projection - a.point.dot(d)) < Epsilon.E) {
// then the new point we just made is not far enough
// in the direction of n so we can stop now and
// get the closest points
// get the closest points
this.findClosestPoints(a, b, separation);
// return true to indicate separation
return true;
}
// get the closest point on each segment to the origin
var p1 = Segment.getPointOnSegmentClosestToPoint(ORIGIN, a.point, c.point);
var p2 = Segment.getPointOnSegmentClosestToPoint(ORIGIN, c.point, b.point);
// get the distance to the origin
var p1Mag = p1.getMagnitudeSquared();
var p2Mag = p2.getMagnitudeSquared();
// check if the origin lies close enough to either edge
if (p1Mag <= Epsilon.E) {
// if so then we have a separation (although its
// nearly zero separation)
this.findClosestPoints(a, c, separation);
return true;
} else if (p2Mag <= Epsilon.E) {
// if so then we have a separation (although its
// nearly zero separation)
this.findClosestPoints(c, b, separation);
return true;
}
// test which point is closer and replace the one that is farthest
// with the new point c and set the new search direction
if (p1Mag < p2Mag) {
// a was closest so replace b with c
b = c;
d = p1;
} else {
// b was closest so replace a with c
a = c;
d = p2;
}
}Now Find the closest points
/**
* Finds the closest points on A and B given the termination simplex and places
* them into point1 and point2 of the given {@link Separation} object.
* <p>
* The support points used to obtain a and b are not good enough since the support
* methods of {@link Convex} {@link Shape}s only return the farthest <em>vertex</em>, not
* necessarily the farthest point.
* @param a the first simplex point
* @param b the second simplex point
* @param separation the {@link Separation} object to populate
* @see <a href="http://www.dyn4j.org/2010/04/gjk-distance-closest-points/" target="_blank">GJK - Distance & Closest Points</a>
*/
function findClosestPoints(/*MinkowskiSumPoint*/ a, /*MinkowskiSumPoint*/ b, /*Separation*/ separation) {
var p1 = new Vector2();
var p2 = new Vector2();
// find lambda1 and lambda2
var l = a.point.to(b.point);
// check if a and b are the same point
if (l.isZero()) {
// then the closest points are a or b support points
p1.set(a.supportPoint1);
p2.set(a.supportPoint2);
} else {
// otherwise compute lambda1 and lambda2
var ll = l.dot(l);
var l2 = -l.dot(a.point) / ll;
// check if either lambda1 or lambda2 is less than zero
if (l2 > 1) {
// if lambda1 is less than zero then that means that
// the support points of the Minkowski point B are
// the closest points
p1.set(b.supportPoint1);
p2.set(b.supportPoint2);
} else if (l2 < 0) {
// if lambda2 is less than zero then that means that
// the support points of the Minkowski point A are
// the closest points
p1.set(a.supportPoint1);
p2.set(a.supportPoint2);
} else {
// compute the closest points using lambda1 and lambda2
// this is the expanded version of
// p1 = a.p1.multiply(1 - l2).add(b.p1.multiply(l2));
// p2 = a.p2.multiply(1 - l2).add(b.p2.multiply(l2));
p1.x = a.supportPoint1.x + l2 * (b.supportPoint1.x - a.supportPoint1.x);
p1.y = a.supportPoint1.y + l2 * (b.supportPoint1.y - a.supportPoint1.y);
p2.x = a.supportPoint2.x + l2 * (b.supportPoint2.x - a.supportPoint2.x);
p2.y = a.supportPoint2.y + l2 * (b.supportPoint2.y - a.supportPoint2.y);
}
}
// set the new points in the separation object
separation.point1.x = p1.x;
separation.point1.y = p1.y;
separation.point2.x = p2.x;
separation.point2.y = p2.y;
// compute the normal and distance from the closest points
var n = p1.to(p2);
var d = n.normalize();
separation.normal.x = n.x;
separation.normal.y = n.y;
separation.distance = d;
}
Sample Code
Commentaires