GJK Algorithm Collision Detection 2D in C#


GJK 
 algorithm finds the distance between two convex sets of points or finds the collision between them. The best explanation I found on how the algorithm works is on this Video Tutorial  and on Code Zealot. However I tried code zealot code and didn’t work in some cases but I strongly recommend you to read it because I’m sure that clarify some things. I watched tht guy on mollyrocket(video tutorial link from above)  for three or four times to understand it before even got into writing code. Of course, you can use other collision detection algorithms but GJK algorithm is very easy to use and understand.

I want to clarify some things:

  •  Simplex : a collection of points or a list of points
  • You don’t have to build the Minkowski difference for GJK  but however I implemented just to see the results
  • The algorithm works only for convex shapes
  • It works for 2D and of course for 3D
  • Start the algorithm with any direction, doesn’t have to be normalized
  • There is no cross product in 2D only some hacks. To find a vector perpendicular on another vector in 2D  , check here :2D perpendicular vector
  • We have collision if  Minkowski difference contain origin point (0,0)

First thing first we need to get the farthest point in a direction for both shapes. Build the Dot Product between the direction and every point in your shape.  The  point that gives the biggest dot product with the distance is what we are looking for. If you know some relationship between vertices you can do a Hill Climb algorithm for performance but if you  don’t have , just iterate through  all shape points.

 

 

All code is written in C#:

  public Vector2 getFarthestPointInDirection(Vector2 d)
        {
            int index = 0;
            double maxDot = this.worldPoints[index].Dot(d);

            for(int i=1;i<worldPoints.Count;i++)
            {
                double  dot = Vector2.Dot(worldPoints[i], d);

                if(dot>maxDot)
                {
                    maxDot = dot;
                    index = i;
                }
            }

            return worldPoints[index];
        }

 

Next is the support function to get  the point on the edge of the Minkowski Difference using the points we got from above function. In the video tutorial mentioned earlier it’s explained how this works

//Code taken and adapted from CodeZealot.org
        private Vector2 support(PolygonShape shape1, PolygonShape shape2, Vector2 d)
        {
           // get points on the edge of the shapes in opposite directions
            Vector2 p1 = shape1.getFarthestPointInDirection(d);
            Vector2 p2 = shape2.getFarthestPointInDirection(new Vector2(-d.X,-d.Y));


            // Minkowski Difference
            Vector2 p3 = new Vector2((p1 - p2).X, (p1 - p2).Y);

            // p3 is now a point in Minkowski space on the edge of the Minkowski Difference
            return p3;
        }

 

Now let’s see  the GJK Algorithm iteration code with the explanation from Code Zealot:

 //Simplex is a list of points declared globally 
        public bool IsCollide(PolygonShape shape1, PolygonShape shape2)
        {

            Vector2 d = new Vector2(1, -1);
            Simplex.Add(support(shape1, shape2, d));
            
            // negate d for the next point
            d.Negate();
            // start looping
            while (true)
            {
              
                // add a new point to the simplex because we haven't terminated yet
                Simplex.Add(support(shape1, shape2, d));

                
                // make sure that the last point we added actually passed the origin
                if (Simplex.Last().Dot(d) <= 0)
                {
                    // if the point added last was not past the origin in the direction of d
                    // then the Minkowski Sum cannot possibly contain the origin since
                    // the last point added is on the edge of the Minkowski Difference
                    return false;
                }
                else
                {
                    if (containsOrigin(ref d) )//also change direction
                    {
                      // if it does then we know there is a collision
                      return true;
                    }
                }
            }

What I changed from Code Zealot is the  containsOrigin function, read the comments for explanation, and keep in mind how perpendicular vector works in 2D

 private bool containsOrigin(ref Vector2  d)
        {
            // get the last point added to the simplex
           Vector2 a = Simplex.Last();
            // compute AO (same thing as -A)
           Vector2 ao = new Vector2(-a.X, -a.Y);

            //triangle  ABC
            if (Simplex.Count() == 3)
            {
               
                // get b and c
                Vector2 b = Simplex[1];
                Vector2 c = Simplex[0];
                
                Vector2 ab = b - a;
                Vector2 ac = c - a;
           
                //direction perpendicular to AB
                d=new Vector2(-ab.Y,ab.X);
                //away from C
                if(d.Dot(c)>0)// if same direction, make d opposite
                {
                    d.Negate();
                }
                                
                //If the new vector (d) perpenicular on AB is in the same direction with the origin (A0)
                //it means that C is the furthest from origin and remove to create a new simplex
                if(d.Dot(ao)>0)//same direction
                {
                    Simplex.Remove(c);
                    return false;
                }

                //direction to be perpendicular to AC
                d = new Vector2(-ac.Y, ac.X);

                //away form B
                if(d.Dot(b) >0)
                {
                    d.Negate();
                }
                
                //If the new vector (d) perpenicular on AC edge, is in the same direction with the origin (A0)
                //it means that B is the furthest from origin and remove to create a new simplex
                             
                if(d.Dot(ao) > 0)
                {
                    Simplex.Remove(b);
                    return false;
                }

               //origin must be inside the triangle, so this is the simplex
                return true;
            }

            //line
            else
            {
                // then its the line segment case
                var b = Simplex[0];
                // compute AB
                var ab = b - a;

                //direction perpendicular to ab, to orgin: ABXAOXAB
                d = new Vector2(-ab.Y, ab.X);
                if(d.Dot(ao)<0)
                {
                    d.Negate();
                }
                   
                              
            }
            return false;
        }

 

Here is the full implementation:

  • Black Shape is static
  • Pink Shape is dynamic , if press step button it will change its points
  • Green Shape is the Minkowski Difference Convex Hull to see if we have the origin point, when the black shape and pink shape overlap
  • Vector2 class is not completed, only some function needed for this example

GJK Algorithm

 

Full source code in WPF with VS2013, you can take the algorithm classes and create a C++, Java/etc program. WPF is used only for drawing.

GJK Algorithm WPF source code

Stay tuned for 3D gjk algorithm verison. Soon !


blog comments powered by Disqus