GJK algorithm in C#(WinForms) for 2D collision - Issue with the direction for simplexes

Question

As the title implies I have been trying to implement the GJK algorithm in 2D using C# mainly based off this blog: https://blog.hamaluik.ca/posts/building-a-collision-engine-part-1-2d-gjk-collision-detection/

We have two shapes, a square(ShapeA) and a triangle(ShapeB). The triangle can move using WASD. We also have a point in green representing the origin (offset), and the points of the minkowski difference in red. The minkowski difference points will move when the triangle moves, as it should. If the origin is inside the shape those points form then there is a collision between our square and triangle.

Image 1

Then comes the GJK algorithm, and my issue. A 2D implementation of the GJK algorithm should look for triangles within the minkowski difference. In this case it should start with a triangle that has for vertices: The furthest point in shapeB in the direction it is moving. The furthest point in shapeA in the opposite direction. And the furthest point in the minkowski difference in the direction perpendicular to the line formed by the first two vertices and towards the origin.

The first two points returned by the SupportPoint function seem to be correct and form the line in pink on the minkowski difference. The issue is with obtaining the third vertex. If we move the triangle upwards into the square we can see the third point i'm currently obtaining (that forms the line in yellow) is not in the direction of the origin. Yet when moving in most other directions it seems ok.

Image 2

Here is the code I'm using. To copy and run simply create a new Windows Forms project in C#, replace the default Form1 Class by the following:

public partial class Form1 : Form
{
    public Form1()
    {
        InitializeComponent();
    }

    bool up = false;
    bool down = false;
    bool left = false;
    bool right = false;

    V2 ShapeDirection = new V2();

    List<V2> ShapeA = new List<V2>();
    List<V2> ShapeB = new List<V2>();

    private void Form1_Paint(object sender, PaintEventArgs e)
    {
        Graphics g = e.Graphics;
        V2 Direction = ShapeDirection;

        //Draw Shape A
        for (int i = 0; i < ShapeA.Count - 1; i++)
        {
            g.DrawLine(new Pen(Brushes.Black, 2),
                new Point((int)ShapeA[i].X, (int)ShapeA[i].Y),
                new Point((int)ShapeA[i + 1].X, (int)ShapeA[i + 1].Y));
        }
        g.DrawLine(new Pen(Brushes.Black, 2),
            new Point((int)ShapeA[ShapeA.Count - 1].X, (int)ShapeA[ShapeA.Count - 1].Y),
            new Point((int)ShapeA[0].X, (int)ShapeA[0].Y));

        //Move Shape B
        for (int i = 0; i < ShapeB.Count; i++)
        {
            ShapeB[i] += Direction;
        }

        //Draw Shape B
        for (int i = 0; i < ShapeB.Count - 1; i++)
        {
            g.DrawLine(new Pen(Brushes.Blue, 2),
                new Point((int)ShapeB[i].X, (int)ShapeB[i].Y),
                new Point((int)ShapeB[i + 1].X, (int)ShapeB[i + 1].Y));
        }
        g.DrawLine(new Pen(Brushes.Blue, 2),
            new Point((int)ShapeB[ShapeB.Count - 1].X, (int)ShapeB[ShapeB.Count - 1].Y),
            new Point((int)ShapeB[0].X, (int)ShapeB[0].Y));

        //Offset origin to see minkowski difference points
        g.TranslateTransform(900, 600);
        g.FillEllipse(Brushes.Green, new Rectangle(-3, -3, 6, 6));

        List<V2> MDifference = Minkowski(ShapeA, ShapeB);

        //Draw minkowski difference points
        for (int i = 0; i < MDifference.Count - 1; i++)
        {
            g.FillEllipse(Brushes.Red,
                new Rectangle((int)(MDifference[i].X - 3), (int)(MDifference[i].Y - 3), 6, 6));
        }

        //Start of GJK
        V2 C = SupportPoint(ShapeA, Direction) - SupportPoint(ShapeB, Direction.Inverse());
        Direction = Direction.Inverse();
        V2 B = SupportPoint(ShapeA, Direction) - SupportPoint(ShapeB, Direction.Inverse());

        V2 Cline = B - C;
        V2 C0 = C.Inverse();

        Direction = V2.Cross(Cline, V2.Cross(C0, Cline)); //Issue

        V2 A = SupportPoint(ShapeA, Direction) - SupportPoint(ShapeB, Direction.Inverse());

        g.DrawLine(new Pen(Brushes.Yellow, 2), new Point((int)A.X, (int)A.Y), new Point((int)B.X, (int)B.Y));
        g.DrawLine(new Pen(Brushes.Pink, 2), new Point((int)B.X, (int)B.Y), new Point((int)C.X, (int)C.Y));
        g.DrawLine(new Pen(Brushes.Yellow, 2), new Point((int)A.X, (int)A.Y), new Point((int)C.X, (int)C.Y));
    }

    private List<V2> Minkowski(List<V2> shape1, List<V2> shape2)
    {
        List<V2> MinkowskiDifferencePoints = new List<V2>();

        foreach (V2 VertexA in shape1)
        {
            foreach (V2 VertexB in shape2)
            {
                MinkowskiDifferencePoints.Add(VertexA - VertexB);
            }
        }

        return MinkowskiDifferencePoints;
    }

    public V2 SupportPoint(List<V2> shape, V2 direction)
    {
        float max = float.NegativeInfinity;
        int index = 0;

        for (int i = 0; i < shape.Count; i++)
        {
            float dot = V2.Dot(shape[i], direction);
            if (dot > max)
            {
                max = dot;
                index = i;
            }
        }

        return shape[index];
    }

    private void Form1_KeyDown(object sender, KeyEventArgs e)
    {
        if (e.KeyCode == Keys.A) { left = true; }
        if (e.KeyCode == Keys.D) { right = true; }
        if (e.KeyCode == Keys.W) { up = true; }
        if (e.KeyCode == Keys.S) { down = true; }
    }

    private void Form1_KeyUp(object sender, KeyEventArgs e)
    {
        if (e.KeyCode == Keys.A) { left = false; }
        if (e.KeyCode == Keys.D) { right = false; }
        if (e.KeyCode == Keys.W) { up = false; }
        if (e.KeyCode == Keys.S) { down = false; }
    }

    private void Form1_Load(object sender, EventArgs e)
    {
        System.Windows.Forms.Timer timer1 = new System.Windows.Forms.Timer();
        timer1.Interval = 1;
        timer1.Enabled = true;
        timer1.Tick += timer1_Tick;
        this.DoubleBuffered = true;
        this.Size = new Size(1200, 1000);
        this.StartPosition = FormStartPosition.CenterScreen;

        ShapeA.Add(new V2(300, 300));
        ShapeA.Add(new V2(350, 300));
        ShapeA.Add(new V2(350, 350));
        ShapeA.Add(new V2(300, 350));

        ShapeB.Add(new V2(600, 600));
        ShapeB.Add(new V2(650, 650));
        ShapeB.Add(new V2(550, 650));
    }

    private void timer1_Tick(object sender, EventArgs e)
    {
        ShapeDirection = new V2();

        if (left) { ShapeDirection.X = -1; }
        else if (right) { ShapeDirection.X = 1; }
        if (up) { ShapeDirection.Y = -1; }
        else if (down) { ShapeDirection.Y = 1; }

        this.Invalidate();
    }
}

Then add a new class to the project called "V2", and paste this:

public class V2
{
    public float X { get; set; }
    public float Y { get; set; }
    public float Z { get; set; }

    public V2()
    { X = 0; Y = 0; Z = 0; }

    public V2(float x, float y)
    { X = x; Y = y; Z = 0; }

    public static V2 operator +(V2 VectorA, V2 VectorB)
    { return new V2(VectorA.X + VectorB.X, VectorA.Y + VectorB.Y); }
    public static V2 operator +(V2 Vector, float Scalar)
    { return new V2(Vector.X + Scalar, Vector.Y + Scalar); }

    public static V2 operator -(V2 VectorA, V2 VectorB)
    { return new V2(VectorA.X - VectorB.X, VectorA.Y - VectorB.Y); }
    public static V2 operator -(V2 Vector, float Scalar)
    { return new V2(Vector.X - Scalar, Vector.Y - Scalar); }

    public static V2 operator *(V2 VectorA, V2 VectorB)
    { return new V2(VectorA.X * VectorB.X, VectorA.Y * VectorB.Y); }
    public static V2 operator *(V2 Vector, float Scalar)
    { return new V2(Vector.X * Scalar, Vector.Y * Scalar); }

    public static V2 operator /(V2 VectorA, V2 VectorB)
    { return new V2(VectorA.X / VectorB.X, VectorA.Y / VectorB.Y); }
    public static V2 operator /(V2 Vector, float Scalar)
    { return new V2(Vector.X / Scalar, Vector.Y / Scalar); }

    public static float Dot(V2 VectorA, V2 VectorB)
    {
        return VectorA.X * VectorB.X + VectorA.Y * VectorB.Y;
    }

    public static float Length(V2 VectorA, V2 VectorB)
    {
        return (float)Math.Sqrt(Math.Pow((VectorB.X - VectorA.X), 2) + Math.Pow((VectorB.Y - VectorA.Y), 2));
    }

    public static V2 Cross(V2 VectorA, V2 VectorB)
    {
        V2 VtempRes = new V2();

        VtempRes.X = (VectorA.Y * VectorB.Z) - (VectorA.Z * VectorB.Y);
        VtempRes.Y = (VectorA.Z * VectorB.X) - (VectorA.X * VectorB.Z);
        VtempRes.Z = (VectorA.Z * VectorB.Y) - (VectorA.Y * VectorB.X);

        return VtempRes;
    }

    public V2 Inverse()
    { return new V2(-1 * X, -1 * Y); }
}

That's it you can now move the triangle around, observe the minkowski difference, and hopefully tell me what i'm doing wrong (somewhere at the end of the Form1.Paint event when implementing GJK).

A big thanks in advance :)

Answer 1

I tried your code, to me nothing seems wrong, you're just not finished yet. My first observation was, that the "tripple-cross-product" is getting very-large numbers, but since you only need this as an direction it should be no problem. and as in the original algorythm the first simplex (triangle) is found. (in case you have a direction in the first place)

My advice would be that you read the article again, and follow it to the end. Maybe separate the moving-direction from the building-direction since these are two separate things.

Instead of the "tripple-cross-product" you could use the direction rotated by 90°, this is done by switching the koordinates and negating one of them. From the two resulting new directions, choose the one that fits the best.

"The third vertex should be chosen as the support in the direction perpendicular to the line formed by the first two vertices, in the direction of the origin, again maximizing the size of the simplex so that we can either complete or fail as early as possible."

e.G:

  Direction = new V2(Cline.Y, -Cline.X); //(No) Issue 
  if (V2.Dot(Direction,C) >0)
      Direction = Direction.Inverse();

I hope this gives you some hints to go on.