Tuesday, January 17, 2012

The GJK Algorithm

This code sample uses the GJK Algorithm to determine if two convex regions in 3-space are intersecting.
There's a good tutorial on how this algorithm actually works here and another one here. This is my implementation in c#.
One can extend this to handle any convex shape at all as long as it has a function to determine the furthest point in the shape along a give direction, ie. if one were to walk along a line determined by a direction, starting "behind" the shape, determine the last point of the shape that you'll pass. This basically boils down to finding the shape's position that has the largest dot product with the direction.
While this can be tricky, GJK can handle any convex shape that implements this function making it very flexible. It's also faster than many other methods and uses minimal resources.
Now on to the code.
PhysicsExtensionMethods static class:

static class PhysicsExtensionMethods
{
    public static bool IsInSameDirection(this Vector3 vector, Vector3 otherVector)
    {
        return Vector3.Dot(vector, otherVector) > 0;
    }

    public static bool IsInOppositeDirection(this Vector3 vector, Vector3 otherVector)
    {
        return Vector3.Dot(vector, otherVector) < 0;
    }
}

IConvexRegion interface:

public interface IConvexRegion
{
    /// <summary>
    /// Calculates the furthest point on the region 
    /// along a given direction.
    /// </summary>
    Vector3 GetFurthestPoint(Vector3 direction);
}

Simplex class:

/// <summary>
/// Represents a generalized Tetrehedron
/// </summary>
class Simplex
{
    List<Vector3> _vertices =
        new List<Vector3>();

    public int Count
    {
        get { return _vertices.Count; }
    }

    public Vector3 this[int i]
    {
        get { return _vertices[i]; }
    }

    public Simplex(params Vector3[] vertices)
    {
        for (int i = 0; i < vertices.Length; i++)
        {
            _vertices.Add(vertices[i]);
        }
    }
      
    public void Add(Vector3 vertex)
    {
        _vertices.Add(vertex);
    }

    public void Remove(Vector3 vertex)
    {
        _vertices.Remove(vertex);
    }
}

GJKAlgorithm static class:

public static class GJKAlgorithm
{
    public static bool Intersects(IConvexRegion regioneOne, IConvexRegion regionTwo)
    {
        //Get an initial point on the Minkowski difference.
        Vector3 s = Support(regioneOne, regionTwo, Vector3.One);
        
        //Create our initial simplex.
        Simplex simplex = new Simplex(s);

        //Choose an initial direction toward the origin.
        Vector3 d = -s;

        //Choose a maximim number of iterations to avoid an 
        //infinite loop during a non-convergent search.
        int maxIterations = 50;

        for (int i = 0; i < maxIterations; i++)
        {
            //Get our next simplex point toward the origin.
            Vector3 a = Support(regioneOne, regionTwo, d);

            //If we move toward the origin and didn't pass it 
            //then we never will and there's no intersection.
            if (a.IsInOppositeDirection(d))
            {
                return false;
            }
            //otherwise we add the new
            //point to the simplex and
            //process it.
            simplex.Add(a);
            //Here we either find a collision or we find the closest feature of
            //the simplex to the origin, make that the new simplex and update the direction
            //to move toward the origin from that feature.
            if (ProcessSimplex(ref simplex, ref d))
            {
                return true;
            }
        }
        //If we still couldn't find a simplex 
        //that contains the origin then we
        //"probably" have an intersection.
        return true;
    }

    /// <summary>
    ///Either finds a collision or the closest feature of the simplex to the origin, 
    ///and updates the simplex and direction.
    /// </summary>
    static bool ProcessSimplex(ref Simplex simplex, ref Vector3 direction)
    {
        if (simplex.Count == 2)
        {
            return ProcessLine(ref simplex, ref direction);
        }
        else if (simplex.Count == 3)
        {
            return ProcessTriangle(ref simplex, ref direction);
        }
        else
        {
            return ProcessTetrehedron(ref simplex, ref direction);
        }
    }

    /// <summary>
    /// Determines which Veronoi region of a line segment 
    /// the origin is in, utilizing the preserved winding
    /// of the simplex to eliminate certain regions.
    /// </summary>
    static bool ProcessLine(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[1];
        Vector3 b = simplex[0];
        Vector3 ab = b - a;
        Vector3 aO = -a;

        if (ab.IsInSameDirection(aO))
        {
            float dot = Vector3.Dot(ab, aO);
            float angle = (float)Math.Acos(dot / (ab.Length() * aO.Length()));
            direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
        }
        else
        {
            simplex.Remove(b);
            direction = aO;
        }
        return false;
    }

    /// <summary>
    /// Determines which Veronoi region of a triangle 
    /// the origin is in, utilizing the preserved winding
    /// of the simplex to eliminate certain regions.
    /// </summary>
    static bool ProcessTriangle(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[2];
        Vector3 b = simplex[1];
        Vector3 c = simplex[0];
        Vector3 ab = b - a;
        Vector3 ac = c - a;
        Vector3 abc = Vector3.Cross(ab, ac);
        Vector3 aO = -a;
        Vector3 acNormal = Vector3.Cross(abc, ac);
        Vector3 abNormal = Vector3.Cross(ab, abc);

        if (acNormal.IsInSameDirection(aO))
        {
            if (ac.IsInSameDirection(aO))
            {
                simplex.Remove(b);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else
            {
                if (ab.IsInSameDirection(aO))
                {
                    simplex.Remove(c);
                    direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
                }
                else
                {
                    simplex.Remove(b);
                    simplex.Remove(c);
                    direction = aO;
                }
            }
        }
        else
        {
            if (abNormal.IsInSameDirection(aO))
            {
                if (ab.IsInSameDirection(aO))
                {
                    simplex.Remove(c);
                    direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
                }
                else
                {
                    simplex.Remove(b);
                    simplex.Remove(c);
                    direction = aO;
                }
            }
            else
            {
                if (abc.IsInSameDirection(aO))
                {
                    direction = Vector3.Cross(Vector3.Cross(abc, aO), abc);
                }
                else
                {
                    direction = Vector3.Cross(Vector3.Cross(-abc, aO), -abc);
                }
            }
        }
        return false;
    }

    /// <summary>
    /// Determines which Veronoi region of a tetrahedron
    /// the origin is in, utilizing the preserved winding
    /// of the simplex to eliminate certain regions.
    /// </summary>
    static bool ProcessTetrehedron(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[3];
        Vector3 b = simplex[2];
        Vector3 c = simplex[1];
        Vector3 d = simplex[0];
        Vector3 ac = c - a;
        Vector3 ad = d - a;
        Vector3 ab = b - a;
        Vector3 bc = c - b;
        Vector3 bd = d - b;
            
        Vector3 acd = Vector3.Cross(ad, ac);
        Vector3 abd = Vector3.Cross(ab, ad);
        Vector3 abc = Vector3.Cross(ac, ab);
            
        Vector3 aO = -a;

        if (abc.IsInSameDirection(aO))
        {
            if (Vector3.Cross(abc, ac).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else if (Vector3.Cross(ab, abc).IsInSameDirection(aO))
            {
                simplex.Remove(c);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
            }
            else
            {
                simplex.Remove(d);
                direction = abc;
            }
        }
        else if (acd.IsInSameDirection(aO))
        {
            if (Vector3.Cross(acd, ad).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(c);
                direction = Vector3.Cross(Vector3.Cross(ad, aO), ad);
            }
            else if (Vector3.Cross(ac, acd).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else
            {
                simplex.Remove(b);
                direction = acd;
            }
        }
        else if (abd.IsInSameDirection(aO))
        {
            if (Vector3.Cross(abd, ab).IsInSameDirection(aO))
            {
                simplex.Remove(c);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
            }
            else if (Vector3.Cross(ad, abd).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(c);
                direction = Vector3.Cross(Vector3.Cross(ad, aO), ad);
            }
            else
            {
                simplex.Remove(c);
                direction = abd;
            }
        }
        else
        {
            return true;
        }

        return false;
    }

    /// <summary>
    /// Calculates the furthest point on the Minkowski 
    /// difference along a given direction.
    /// </summary>
    static Vector3 Support(
        IConvexRegion regionOne, 
        IConvexRegion regionTwo,
        Vector3 direction)
    {
        return regionOne.GetFurthestPoint(direction) -
            regionTwo.GetFurthestPoint(-direction);
    }
}

Sphere class:

public class Sphere : IConvexRegion
{
    public Vector3 Center;
    public float Radius;

    public Sphere(Vector3 center, float radius)
    {
        Center = center;
        Radius = radius;
    }

    public Vector3 GetFurthestPoint(Vector3 direction)
    {
        if (direction != Vector3.Zero)
        {
            direction.Normalize();
        }
        return Center + Radius * direction;
    }
}

Box class:

public class Box : IConvexRegion
{
    public Vector3 Center;
    Vector3 _halfDimensions = Vector3.One;
    Quaternion _orientation = Quaternion.Identity;

    public Vector3 Dimensions
    {
        get { return 2f * _halfDimensions; }
    }

    public Box(Vector3 center)
        : this(center, 1f, 1f, 1f) { }

    public Box(Vector3 center,
        float width,
        float height,
        float depth)
        : this(center, width, height, depth, Matrix.Identity) { }

    public Box(Vector3 center,
        float width,
        float height,
        float depth,
        Matrix rotationMatrix)
    {
        Center = center;
        _halfDimensions = new Vector3(
            width / 2f,
            height / 2f,
            depth / 2f);
        _orientation = Quaternion.CreateFromRotationMatrix(rotationMatrix);
    }

    public Vector3 GetFurthestPoint(Vector3 direction)
    {
        Vector3 halfHeight = _halfDimensions.Y * Vector3.Up;
        Vector3 halfWidth = _halfDimensions.X * Vector3.Right;
        Vector3 halfDepth = _halfDimensions.Z * Vector3.Backward;

        Vector3[] vertices = new Vector3[8];
        vertices[0] = halfWidth + halfHeight + halfDepth;
        vertices[1] = -halfWidth + halfHeight + halfDepth;
        vertices[2] = halfWidth - halfHeight + halfDepth;
        vertices[3] = halfWidth + halfHeight - halfDepth;
        vertices[4] = -halfWidth - halfHeight + halfDepth;
        vertices[5] = halfWidth - halfHeight - halfDepth;
        vertices[6] = -halfWidth + halfHeight - halfDepth;
        vertices[7] = -halfWidth - halfHeight - halfDepth;

        Matrix rotationTransform = Matrix.CreateFromQuaternion(_orientation);
        Matrix translation = Matrix.CreateTranslation(Center);
        Matrix world = rotationTransform *
            translation;

        Vector3 furthestPoint = Vector3.Transform(vertices[0], world);
        float maxDot = Vector3.Dot(furthestPoint, direction);
        for (int i = 1; i < 8; i++)
        {
            Vector3 vertex = Vector3.Transform(vertices[i], world);
            float dot = Vector3.Dot(vertex, direction);
            if (dot > maxDot)
            {
                maxDot = dot;
                furthestPoint = vertex;
            }               
        }
        return furthestPoint;
    }

    public Matrix CalculateWorld()
    {
        return Matrix.CreateScale(Dimensions) *
            Matrix.CreateFromQuaternion(_orientation) *
            Matrix.CreateTranslation(Center);
    }
}

I hope this is helpful to someone :) Enjoy!

7 comments :

  1. I think there is an error in your box class, it has a method Support which I think is supposed to be GetFurthestPoint.

    Thanks for the code though

    ReplyDelete
  2. is there archive with whole source code?

    ReplyDelete
    Replies
    1. Not at the moment but I'll put one up when I find a little time.

      Delete
  3. It doesnt work. Contacts are randomly generated. Is it my fault or the simplex you posted is wrong?

    ReplyDelete
  4. Hi. Maybe you can help me with this one. I would like to find the point (on the closest face of Minkowski Sum) closest to origin. I have GJK up and running and I get a triangle simplex on the closest face of minkowski sum. I try to find the closest point on this plane (closest face) 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!

    ReplyDelete
    Replies
    1. given your triangle ABC cross any two vectors that make up the sides (AB with AC for example) then dot that with the negative of one of it's position vectors (a vector pointing toward the origin. If that dot product is greater than zero then the cross product is normal to your triangle face and pointing toward the origin (which is what you want) otherwise just negate the cross product. Then use the support function on it giving you the fourth point in your simplex.

      Delete