• Categories

    Archives

  • 3D Convex Collision Detection, Part One: GJK

    6 Comments

    hulls_funnel

    This is the first of a three article series that will teach you how to efficiently detect collisions, generate contact information, and generate convex hulls for 3D objects. This article focuses on detecting the collisions via the Gilbert–Johnson–Keerthi (GJK) algorithm.

     

    GJK:

    The GJK algorithm takes advantage of a very special property of the Minkowski Difference, which is that if the Minkowski Difference of two volumes contains the origin then they are intersecting. The Minkowski Difference of volumes A and B contains every single point in A minus every single point in B. This is easiest to visualize with spheres/circles as you don’t have to worry about the shape, for example:

    gjk_minkowskidifference2gjk_minkowskidifference

    Notice how in the second image A and B were intersecting and their Difference contained the origin?

    GJK exploits this by trying to build a simplex (tetrahedron) that encloses the origin using only points on the hull of the Minkowski Difference.

     

    Calculating the Minkowski Difference:

    We don’t need to calculate the entire Minkowski Difference, that would take a huge amount of processor time, we really only need to be able to get a single point on the exterior surface of the Difference. We do this by using a support mapping, which is nothing more than a function that takes in a direction and returns the point on the shape that is the greatest distance in that direction. We take the support point of shape A in direction d and subtract the support point of shape B in direction -d:

    auto lam_support = [&](HDXVector3 d)->SupportPoint {
         d.setNormal(); // to quell numerical instability
         return bv0->getFurthestPointInDirection(d) - bv1->getFurthestPointInDirection(-d);
    };
    

     

    Support Mapping (furthestPointInDirection()):

    Generating the support point of a shape is quite easy, especially if the shape is quadric, like a sphere or a box. For a sphere you need only multiply the radius times the direction and add the position. Where it gets slightly more complicated is with arbitrary convex hulls. The naïve method would be to just loop over every point and choose the one with the highest dot-product with the desired direction. This would work fine, but it would do a lot of unnecessary work. The better way requires you to have the adjacency information of all of the points of your hull (something that the third article in this series will cover). It consists of using hill climbing:

    1: choose an arbitrary point

    2: choose the neighbor with the highest dot-product with the direction, if all neighbors are lower than the current point move to 4

    3: set the current point to the one chosen in 2, return to 2;

    4: apply the local-to-world transform to this point and return it

    // support mapping for a convex hull w/ adjacency data
    
    //hill-climbing algorithm using the hull's adjacency data:
    //pick an arbetrary vert to start with, then move to the best adjacent
    //one, continue until no better adjacent vert exists
    
    unsigned cur_vid = 0;
    float cur_dst = mhull.mverts[cur_vid] * d; // d is the direction
    while(true) {
         unsigned new_vid = cur_vid;
         float new_dst = cur_dst;
         ; {
              for(unsigned i = 0; i < mhull.madjacency[cur_vid].size(); i++) {
                   unsigned vid = mhull.madjacency[cur_vid][i];
                   float dst = mhull.mverts[vid] * d;
                   if(dst > new_dst) {
                        new_vid = vid;
                        new_dst = dst;
                   }
              }
         }
         if(new_vid == cur_vid) break;
         cur_vid = new_vid;
         cur_dst = new_dst;
    }
    
    return mhull.mverts[cur_vid] * getTransform();
    

    Dealing With Rotation:

    One snag you will run into when generating support points is how to deal with a shape that has a rotation associated with it, the solution is to rotate the direction by the inverse of the objects rotation before generating the point, then you can just transform the point by the object’s transform matrix to rotate and translate it where it’s supposed to be.

    // Support mapping for an OBB
    const HDXQuaternion &trans_rot = body->getComp()->getTrans()->getOrientation();
    HDXVector3 rd = HDXVector3::gen::transformQuaternion(d,trans_rot.getInverse()).getNormal();
    
    HDXVector3 ret = HDXVector3(SIGN(rd.x)*halfsize.x,
                                SIGN(rd.y)*halfsize.y,
                                SIGN(rd.z)*halfsize.z) * getTransform();
    return ret;
    

    Now that that is out of way, back to the GJK algorithm. Remember that GJK tries to enclose the origin by building a simplex around it. The algorithm only needs to store the current simplex(1-4 points) and the current search direction. Here is the flow of the algorithm:

    1: generate a support point (lam_support) in an arbitrary direction and push it to the simplex, which is now referred to as sim.a

    2: set the search direction to –sim.a

    One thing to note, when a point is pushed to the simplex it is referred to as A, and the old points are pushed up, A->B, B->C, C->D. This allows us to use the order of the points in the simplex to our advantage later on.

    At this point the iterative portion begins:

    3: generate a new point in the current search direction

    4: test if this new point passed the origin by dotting it with the direction, it should be positive, if not return false, no intersection

    5: push this point into the simplex

    At this point the algorithm splits into one of three areas, depending on how many points are currently in the simplex.

    2 Points:

    gjk_simplex2

    When there are only two points in the simplex we don’t have to test anything, just set the search direction to AB%AO%AB, which calculates the vector perpendicular to edge AB and parallel to AO. This is because we already know the origin cannot be behind point B, as we already calculated that A was in the direction of the origin. It cannot be in front of A because the early out (#4) would have failed.

    // simplex is a line, being here means that the early out was passed, and thus
    // the origin must be between point a and b
    // search direction is perpendicular to ab and coplaner with ao
    const HDXVector3 ab = (sim.b.v-sim.a.v);
    dir = ab % ao % ab;
    continue;
    

    3 Points:

    gjk_simplex3

    When there are three points in the simplex we have to a bit of work, as the origin can either be near the edge AB, near the edge AC or be inside the triangular prism inside the triangle. It cannot be behind BC because we already determined that it was on the other side in the last test. Same goes for in it being in front of A, the early out would have triggered. First test the two edges like this:

    // note: for clarity I am using the macro gjk_simtest()
    // it just dots v with ao (which is just -a) and tests if that is greater than zero
    #define gjk_simtest(v) ((v)*(ao) > 0)
    
    const HDXVector3 ab = (sim.b-sim.a);
    const HDXVector3 ac = (sim.c-sim.a);
    const HDXVector3 ad = (sim.d-sim.a);
    const HDXVector3 abc = ab % ac;
    
    if(gjk_simtest(ab % abc)) {
         // origin is outside the triangle, near the edge ab
         // reset the simplex to the line ab and continue
         // search direction is perpendicular to ab and parallel to ao
         sim.set(sim.a,sim.b);
         dir = ab % ao % ab;
         continue;
    }
    
    if(gjk_simtest(abc % ac)) {
         // origin is outside the triangle, near the edge ac
         // reset the simplex to the line ac and continue
         // search direction is perpendicular to ac and parallel to ao
         sim.set(sim.a,sim.c);
         dir = ac % ao % ac;
         continue;
    }
    

    If we both of those fail we know the origin must lie inside the triangle. We must now determine whether it is above it or below it. To do this just check whether the dot product of the triangle’s normal and AO is positive. If it is the simplex remains unchanged, and the search direction is set to the triangle’s normal. Otherwise the origin is below the triangle, and we must rewind the simplex to reflect this. Just swap B and C. The search direction is in the face direction of the new triangle.

    // origin is within the triangular prism defined by the triangle
    // determine if it is above or below
    if(gjk_simtest(abc)) {
         // origin is above the triangle, so the simplex is not modified,
         // the search direction is the triangle's face normal
         dir = abc;
         continue;
    }
    
    // origin is below the triangle, so the simplex is rewound the oposite direction
    // the search direction is the new triangle's face normal
    sim.set(sim.a,sim.c,sim.b);
    dir = -abc;
    continue;
    

    4 Points:

    gjk_simplex4 donegjk_simplex4 face

    (A is the tip)

    This stage may seem like it should be more complicated, but in fact it is simpler than the triangle case. All we need to do is determine which face (ABC, ACD, ADB) if any the origin is in front of. If it is in front of any of the three faces then the simplex is set to that triangle and is sent to a simplified version of the 3 point case. The simplification is that we know that the point is not below the triangle. If the origin is not in front of any of the three faces then we know we have successfully enclosed the origin and that we have an intersection!

    // 4 point simplex case
    HDXVector3 ab = (sim.b-sim.a);
    HDXVector3 ac = (sim.c-sim.a);
    
    if(gjk_simtest(ab % ac)) {
         // origin is in front of triangle abc, simplex is already what it needs to be
         // go to jmp_face
         goto jmp_face;
    }
    
    const HDXVector3 ad = (sim.d-sim.a);
    
    if(gjk_simtest(ac % ad)) {
         // origin is in front of triangle acd, simplex is set to this triangle
         // go to jmp_face
         sim.set(sim.a,sim.c,sim.d);
         goto jmp_face;
    }
    
    if(gjk_simtest(ad % ab)) {
         // origin is in front of triangle adb, simplex is set to this triangle
         // go to jmp_face
         sim.set(sim.a,sim.d,sim.b);
         goto jmp_face;
    }
    
    // intersction confirmed, break from the loop
    break;
    
    jmp_face:
    // the simplex is equal to the triangle that the origin is infront of
    // this is exactly the same as the triangular simplex test except that we know
    // that the origin is not behind the triangle
    ab = (sim.b-sim.a);
    ac = (sim.c-sim.a);
    const HDXVector3 abc = ab % ac;
    
    if(gjk_simtest(ab % abc)) {
         sim.set(sim.a,sim.b);
         d = ab % ao % ab;
         continue;
    }
    
    if(gjk_simtest(abc % ac)) {
         sim.set(sim.a,sim.c);
         d = ac % ao % ac;
         continue;
    }
    
    sim.set(sim.a,sim.b,sim.c);
    d = abc;
    continue;
    

    Keeping Track of The Simplex:

    Clarity is important, so in my implementation I created a Simplex class that has methods for pushing a single point as well as overloaded methods for setting the whole simplex, set up such that it automatically keeps track of the simplex size.

    struct Simplex {
    public:
         HDXVector3 _simplex[4];
         int num;
         HDXVector3 &a;
         HDXVector3 &b;
         HDXVector3 &c;
         HDXVector3 &d;
    
         Simplex() : a(_simplex[0]),b(_simplex[1]),c(_simplex[2]),d(_simplex[3]) { clear(); }
    
         void clear() { num = 0; }
    
         void set(HDXVector3 a,HDXVector3 b,HDXVector3 c,HDXVector3 d) { num = 4; this->a = a; this->b = b; this->c = c; this->d = d; }
         void set(HDXVector3 a,HDXVector3 b,HDXVector3 c) { num = 3; this->a = a; this->b = b; this->c = c; }
         void set(HDXVector3 a,HDXVector3 b) { num = 2; this->a = a; this->b = b; }
         void set(HDXVector3 a) { num = 1; this->a = a; }
    
         void push(HDXVector3 p) { num = (std::min)(num+1,4); for(int i = num-1; i > 0; i--) _simplex[i] = _simplex[i-1]; _simplex[0] = p; }
    };
    

     

    What Now?:

    Now that we have a simplex that encloses the origin we can use it to calculate the contact data (contact point & normal and penetration depth) using the Expanding Polytope Algorithm (EPA), which I will be covering in my next article.

     

    Tips:

    The support map is very powerful, it can allow you to do awesome things like give your object a “collision skin” or sweep volumes over eachother (rod + sphere = capsule) (sphere + X = X w/ rounded corners :)) by just adding their support points togeter.

    You will likely need to add an iteration cap to your implementation to avoid nasty infinite loops.

    FULL SOURCE CODE (integrated into an engine) IS AVAILABLE HEREE: Lattice3D Engine BitBucket

     

    Comment RSS

    6 Responses to “3D Convex Collision Detection, Part One: GJK”

    1. saadne says:

      Hi. Thanks for a great GJK explanation. But I have a question. I really hope you can help me out. I want to know the distance between the two objects. I cant undertand how to terminate the loop? Because I want to stop searching for a new tetrahedron (4 points) when I have a triangle on the closest face. How can the algorithm stop with this triangle. This triangle (3 point) will be used as the final simplex. I guess I will have to do the same (3 point) check as above to check where on the triangle the origin lies. E.g. if it is closest to an edge I will compute the closest point on this edge to the origin. Then I will have the distance. If origin is closest to the triangle face I will do a “Closest point – plane” test to find distance between origin and the triangle face.

    2. Hacktank says:

      Hello Saadne. If I understand your question, you are only concerned with the distance between two convex objects? The GJK algorithm only works if the objects are intersecting, however, I have given this some thought, and I believe that you could skip the GJK and jump straight into EPA to get the information you are after.

      The main problem we are left with is the lack of a 4-point polytope. Since GJK cannot reliably generate one unless the objects are intersecting you need to come up with a new method of generating it. Perhaps by taking four support points, from four directions, say [0,1,0], [0,-0.707,0.707], [-0.577,-0.577,-0.577], and [0.577,-0.577,-0.577]. These should be the four most unique directions possible in 3 dimensions. The EPA implementation explained in the second article in this series should work, however it was designed with the assumption that the polytope it builds will always contain the origin. You will have to modify a couple of the checks to ensure that it is actually still finding the closest face to the origin.

    3. saadne says:

      Thanks for trying to help me out here. I have another question. I would like to find the point (on the closest face of Minkowski Sum) closest to origin. The problem is that the point found might not be inside the face. It will be on the plane, but outside its face. So I will need a check whether the point found is inside or outside the face. This should be easy to figure out if I only knew the fourth (4) minkowski sum point on this face. (I can get a triangle on closest face from GJK). Since I am dealing with OBBs (find minimum distance between 2 OBBs in 3D) my minkowski sum shape will also be an OBB. So how can I find this last point for the current closest face? I get a triangle from GJK (3 points in minkowski sum) but I actually need all 4 points in order to know if the closest point found is actually inside the rectagle based face. I hope you understand my question and could give some help. Thanks!

    4. Subrahmanyam says:

      Hello, I have a question in your code GJKEPAGenerator.cpp. When determining the initial support point using any random direction, I did not understand on Lines 190 and 191, the if condition: if(fabs(dir|s.v)>=s.v.Length()*0.8f) { ……
      “the chosen initial direction is invalid, will produce (0,0,0) for a subsequent direction later”. Why is this ? Can you explain ?

    5. […] Code Cave – Second only to Allen Chou for understanding GJK and EPA. Bonus points for representing 3D concepts using sticks and putty. I may very well rip that off. […]

    6. […] you are a bit unclear, why not have a look at another article explaining the same thing. I have to give the author credit for using sticks and clay to create a 3D representation of the […]

       

    Leave a Reply

  • Recent Posts

home login
content