public static Contact FindFirstContact_ReallyCheap(PolyBody spirit, PolyBody body, Vector2 axis, float disp)
{
foreach (var v in spirit.GetTransformedVertices())
{
if (TestPointInPoly(body, v))
return new Contact(spirit, body, v, axis, disp);
}
return new Contact(null, null, Vector2.Zero, Vector2.Zero, 0f);
}
//axis must be from a->b!
/// <summary>
/// finds vertices of each polygon which are within the other polygon
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="axis"></param>
/// <param name="disp"></param>
/// <returns></returns>
public static List<Contact> FindSubContactsPoly_CheapVerts(PolyBody a, PolyBody b, Vector2 axis, float disp)
{
var cons = new List<Contact>();
foreach (var v in a.GetTransformedVertices())
{
if(TestPointInPoly(b, v))
cons.Add(new Contact(a, b, v, axis, disp));
}
foreach (var v in b.GetTransformedVertices())
{
if(TestPointInPoly(a, v))
cons.Add(new Contact(a, b, v, axis, disp));
}
return cons;
}
public static bool TestCollisionPartiPoly(Particle p, PolyBody bod, out Vector2 axis, out float disp)
{
axis = new Vector2();
disp = float.MinValue;
var norms = bod.GetNormals().ToList();
Vector2 closeV = Vector2.Zero;
var verts = bod.GetTransformedVertices();
float min = float.MaxValue;
foreach (var v in verts)
{
var d = (p.Position - v).LengthSquared();
if (d < min)
{
d = min;
closeV = v;
}
}
if (closeV != Vector2.Zero && p.Position - closeV != Vector2.Zero)
{
var pNorm = Vector2.Normalize(p.Position - closeV);
norms.Add(pNorm);
}
//
//Col test
//
foreach (Vector2 ax in norms)
{
float d = Projection.IntervalDist(p.Project(ax), bod.Project(ax));
if (d > 0) //no collision if even 1 separating axis found
{
disp = 0;
return false;
}
else if (d > disp) //return disp w/ lowset overlap (ie: highest separation)
{
disp = d;
axis = ax;
}
}
if (Vector2.Dot(p.Position - bod.Position, axis) < 0) //make sure the normal always points from p->bod. (should this use .GetCentroid() instead of position?)
axis = -axis;
return true;
}
