private void QueueCollidables(Unvisited from, List <T> cols, FastPriorityQueue <Unvisited> open, HashSet <Tuple <int, int, int> > closed)
{
float aabbW = Bounds.AABB.Width;
float aabbH = Bounds.AABB.Height;
HashSet <Tuple <int, int, int> > myClosed = null;
for (int colsInd = 0, colsLen = cols.Count; colsInd < colsLen; colsInd++)
{
AACollidable collidable = cols[colsInd];
Vector2 cent = collidable.Bounds.Center;
for (int vertsInd = 0, vertsLen = collidable.Bounds.Vertices.Length; vertsInd < vertsLen; vertsInd++)
{
Vector2 vert = collidable.Bounds.Vertices[vertsInd];
for (int myVertInd = 0, myVertsLen = Bounds.Vertices.Length; myVertInd < myVertsLen; myVertInd++)
{
var tup = Tuple.Create(collidable.ID, vertsInd, myVertInd);
if (closed.Contains(tup))
{
continue;
}
if (myClosed != null && myClosed.Contains(tup))
{
continue;
}
Vector2 myVert = Bounds.Vertices[myVertInd];
Vector2 point = new Vector2(vert.X - myVert.X, vert.Y - myVert.Y);
point += collidable.Position;
if (Math2.Approximately(point, from.Location))
{
closed.Add(tup);
continue;
}
if (!Map.Contains(Bounds, point))
{
closed.Add(tup);
continue;
}
var inters = Map.TraceExhaust(ToList(Bounds), point, ExcludeIDs, ExcludeFlags);
if (inters.Count != 0)
{
if (myClosed == null)
{
myClosed = new HashSet <Tuple <int, int, int> >();
}
myClosed.Add(tup);
for (int i = 0; i < inters.Count; i++)
{
if (inters[i] != collidable)
{
cols.Add(inters[i]);
colsLen++;
}
}
continue;
}
List <T> tmp = Map.TraceExhaust(Bounds, from.Location, point, ExcludeIDs, ExcludeFlags);
if (tmp.Count == 0)
{
closed.Add(tup);
var unv = new Unvisited()
{
Parent = from,
Location = point,
DistanceStartToHere = from.DistanceStartToHere + (point - from.Location).Length(),
HeurDistanceHereToDest = (End - point).Length()
};
open.Enqueue(unv, unv.CorrectPrio);
}
else
{
if (myClosed == null)
{
myClosed = new HashSet <Tuple <int, int, int> >();
}
myClosed.Add(tup);
for (int i = 0; i < tmp.Count; i++)
{
if (tmp[i] != collidable)
{
cols.Add(tmp[i]);
colsLen++;
}
}
}
}
}
}
}