private void TraceContainer(PolygonsContainer container)
{
var currentContainer = container;
var segment = new Segment(currentPoint, goal);
if (currentContainer.IsEmpty())
{
return;
}
if (!currentContainer.LayInSegmentBounds(segment))
{
return;
}
do
{
if (currentContainer.HasChildren)
{
if (currentContainer.Contains(currentPoint))
{
var child = currentContainer.GetChildContainedPoint(currentPoint);
if (child != null && !excludedPolygons.Contains(child.GetId()))
{
currentContainer = child;
continue;
}
}
var intersection = FindNearestPolygonIntersection(currentContainer.ChildContainers);
if (intersection == null)
{
excludedPolygons.Add(currentContainer.GetId());
currentContainer = currentContainer.Parent;
if (currentContainer == null)
{
break;
}
continue;
}
currentContainer = intersection.Polygon as PolygonsContainer;
}
else
{
excludedPolygons.Add(currentContainer.GetId());
if (currentContainer.HasPolygons())
{
TracePolygons(currentContainer.Polygons);
}
if (currentContainer.HasParent)
{
currentContainer = currentContainer.Parent;
}
else
{
return;
}
}
} while (true);
}
public void Init(Vector2[][] obstacles)
{
stopwatch = new Stopwatch();
stopwatch.Start();
map = MapBuilder.Build(obstacles);
stopwatch.Stop();
// Console.WriteLine("total init time is "+ stopwatch.Elapsed.TotalMilliseconds);
pathFinder = new Finder(map);
}
public PolygonsContainer(Vector2 bottomLeft, Vector2 topRight, PolygonsContainer parent)
{
var vertices = new Vector2 [4];
vertices[0] = bottomLeft;
vertices[1] = new Vector2(bottomLeft.x, topRight.y);
vertices[2] = topRight;
vertices[3] = new Vector2(topRight.x, bottomLeft.y);
rectBounds = new Polygon(vertices);
Parent = parent;
}
public void CreateChildren()
{
ChildContainers = new PolygonsContainer[4];
var minX = rectBounds.MinX;
var minY = rectBounds.MinY;
var maxX = rectBounds.MaxX;
var maxY = rectBounds.MaxY;
var halfX = minX + (maxX - minX) / 2;
var halfY = minY + (maxY - minY) / 2;
ChildContainers[0] = new PolygonsContainer(new Vector2(minX, minY), new Vector2(halfX, halfY), this);
ChildContainers[1] = new PolygonsContainer(new Vector2(minX, halfY), new Vector2(halfX, maxY), this);
ChildContainers[2] = new PolygonsContainer(new Vector2(halfX, minY), new Vector2(maxX, halfY), this);
ChildContainers[3] = new PolygonsContainer(new Vector2(halfX, halfY), new Vector2(maxX, maxY), this);
}
public static PolygonsContainer Build(Vector2[][] obstacles)
{
Vector2 bottomLeft = new Vector2(float.MaxValue, float.MaxValue);
Vector2 topRight = new Vector2(float.MinValue, float.MinValue);
Polygon[] polygons = new Polygon[obstacles.Length];
for (var i = 0; i < obstacles.Length; i++)
{
var obstacle = obstacles[i];
var polygon = new Polygon(obstacle);
if (polygon.MinX < bottomLeft.x)
{
bottomLeft.x = polygon.MinX;
}
if (polygon.MinY < bottomLeft.y)
{
bottomLeft.y = polygon.MinY;
}
if (polygon.MaxX > topRight.x)
{
topRight.x = polygon.MaxX;
}
if (polygon.MaxY > topRight.y)
{
topRight.y = polygon.MaxY;
}
polygons[i] = polygon;
}
var result = new PolygonsContainer(bottomLeft, topRight, null);
result.InitializeChildren(polygons);
return(result);
}
public Finder(PolygonsContainer map)
{
this.map = map;
}