//--- The Gilbert-Johnson-Keerthi Algorithm

//We only have a collision if we can wrap a triangle around the Origin of a Minkowski sum
//A is always the last point added to our simplex
bool collision::gjk_test (c_vec &shape0, c_vec &shape1)
{
	c_vec verts; verts.resize(3);
	coord d(-1,-1), d_neg; //D can be initialised with any random direction
	int n = 1; //start at second point
   
   //D does not need to be normalised for GJK - support functions still need to be aware D that is not normalised
	coord max0, max1;
	
	max0 = support(shape0,d);
	max1 = support(shape1,d);
	verts[0] = max0 - max1;
   
   //-A equals a vector pointing at the Origin
   d = verts[0];
   d.negate();

   do
   {
	   max0 = support(shape0,d);
	   max1 = support(shape1,d);
	   verts[n] = max0 - max1;
	   
	   //A must be beyond the Origin in the direction D - if not we are unable to encapsulate the Origin
       //A is ZERO when we have opposite touching vertices in Shape1 and Shape2 - you MUST exit in this case
       //I choose not to explicityly test for ZERO thus giving me wrong result in this rare case
      if (dot(verts[n], d) <= 0)
         return false;

      switch (n + 1)
      {
         case 2:   n = test_line (verts, d);   break;
         case 3:   n = test_triangle (verts, d);   break;
         default: break;
      }

   } while (n < 3);

   //All three points of the simplex passed our test - we've got a collision!
   return true;
}

coord collision::support(c_vec verts, coord d)
{
	float max = 0, val;
	coord coord_;
	for(int i = 0; i < verts.size(); i++){
		val = dot(d, verts);
		if(val > max){
			max = val;
			coord_ = verts;
		}
	}

	return coord_;
}

int collision::test_line(c_vec &verts, coord &d)
{
	coord &a= verts[1];
	coord &b= verts[0],
		ao, ab;
	
	ao = a;
	ao.negate();
	ab = b - a;
	
	d = perp(ab);
	if (!(dot(ao,d) > 0))
		d.negate();
	
	return 2;
}

int collision::test_triangle (c_vec &verts, coord &d)
{
	coord a = verts[2],
		b = verts[1],
		c = verts[0],
		ao,ab, ac;
	
	ao = a;
    ao.negate();
    ab = b - a;
    ac = c - a;

    //Obtain perpendicular vector to AB in the direction opposite of C
    d = perp(ab);
    if (0.0f < cross(ab, ac))
       d.negate();

    if ((dot(ao,d) > 0))
    {
       //Rory's optimisation: line AB will always be closest to the Origin, do not test point case
	    verts[0] = a; //Remove C from the Simplex
       return 2;
    }

    //Obtain perpendicular vector to AC in the direction opposite of B
    d = perp(ac);
    if (0.0f < cross(ac, ab))
       d.negate();

    if ((dot(ao,d) > 0))
    {
       //Rory's optimisation: line AC will always be closest to the Origin, do not test point case
        verts[1] = a;   //Remove B from Simplex
       return 2;
    }

    //Origin was not found outside the triangle - I guess we've encapsulated it!
    return 3;
}

coord perp(coord a)
{
	return coord(-a.y,a.x);
}
float cross(coord a, coord b)
{
	return a.x * b.y - a.y * b.x;
}