private IntPointNode GetWayPointInside(IntPoint position, out IntPointNode waypointAtPosition)
{
Tuple <int, int, IntPoint> foundPolyPointPosition;
waypointAtPosition = null;
BoundaryPolygons.MovePointInsideBoundary(position, out foundPolyPointPosition, BoundaryEdgeQuadTrees, BoundaryPointQuadTrees);
if (foundPolyPointPosition == null)
{
// The point is already inside
var existingNode = Waypoints.FindNode(position, findNodeDist);
if (existingNode == null)
{
waypointAtPosition = AddTempWayPoint(removePointList, position);
return(waypointAtPosition);
}
waypointAtPosition = existingNode;
return(waypointAtPosition);
}
else // The point had to be moved inside the polygon
{
if (position == foundPolyPointPosition.Item3)
{
var existingNode = Waypoints.FindNode(position, findNodeDist);
if (existingNode != null)
{
waypointAtPosition = existingNode;
return(waypointAtPosition);
}
else
{
// get the way point that we need to insert
waypointAtPosition = AddTempWayPoint(removePointList, position);
HookUpToEdge(waypointAtPosition, foundPolyPointPosition.Item1, foundPolyPointPosition.Item2);
return(waypointAtPosition);
}
}
else // the point was outside and hook it up to the nearest edge
{
// fand the start node if we can
IntPointNode startNode = Waypoints.FindNode(foundPolyPointPosition.Item3, findNodeDist);
// After that create a temp way point at the current position
waypointAtPosition = AddTempWayPoint(removePointList, position);
if (startNode != null)
{
Waypoints.AddPathLink(startNode, waypointAtPosition);
}
else
{
// get the way point that we need to insert
startNode = AddTempWayPoint(removePointList, foundPolyPointPosition.Item3);
HookUpToEdge(startNode, foundPolyPointPosition.Item1, foundPolyPointPosition.Item2);
Waypoints.AddPathLink(startNode, waypointAtPosition);
}
return(startNode);
}
}
}
private void HookUpToEdge(IntPointNode crossingNode, int polyIndex, int pointIndex)
{
int count = BoundaryPolygons[polyIndex].Count;
pointIndex = (pointIndex + count) % count;
IntPointNode prevPolyPointNode = Waypoints.FindNode(BoundaryPolygons[polyIndex][pointIndex]);
Waypoints.AddPathLink(crossingNode, prevPolyPointNode);
IntPointNode nextPolyPointNode = Waypoints.FindNode(BoundaryPolygons[polyIndex][(pointIndex + 1) % count]);
Waypoints.AddPathLink(crossingNode, nextPolyPointNode);
}
public PathFinder(Polygons outlinePolygons, long avoidInset, IntRect?stayInsideBounds = null)
{
if (outlinePolygons.Count == 0)
{
return;
}
OutlinePolygons = FixWinding(outlinePolygons);
OutlinePolygons = Clipper.CleanPolygons(OutlinePolygons, avoidInset / 60);
InsetAmount = avoidInset;
if (stayInsideBounds != null)
{
var boundary = stayInsideBounds.Value;
OutlinePolygons.Add(new Polygon()
{
new IntPoint(boundary.minX, boundary.minY),
new IntPoint(boundary.maxX, boundary.minY),
new IntPoint(boundary.maxX, boundary.maxY),
new IntPoint(boundary.minX, boundary.maxY),
});
OutlinePolygons = FixWinding(OutlinePolygons);
}
BoundaryPolygons = OutlinePolygons.Offset(stayInsideBounds == null ? -avoidInset : -2 * avoidInset);
BoundaryPolygons = FixWinding(BoundaryPolygons);
OutlineEdgeQuadTrees = OutlinePolygons.GetEdgeQuadTrees();
OutlinePointQuadTrees = OutlinePolygons.GetPointQuadTrees();
BoundaryEdgeQuadTrees = BoundaryPolygons.GetEdgeQuadTrees();
BoundaryPointQuadTrees = BoundaryPolygons.GetPointQuadTrees();
foreach (var polygon in BoundaryPolygons)
{
Waypoints.AddPolygon(polygon);
}
// hook up path segments between the separate islands
if (simpleHookup) // do a simple hookup
{
for (int indexA = 0; indexA < BoundaryPolygons.Count; indexA++)
{
var polyA = BoundaryPolygons[indexA];
if (polyA.GetWindingDirection() > 0)
{
Func <int, Polygon, bool> ConsiderPolygon = (polyIndex, poly) =>
{
return(polyIndex != indexA &&
poly.GetWindingDirection() > 0);
};
// find the closest two points between A and any other polygon
IntPoint bestAPos = polyA.Center();
Func <int, IntPoint, bool> ConsiderPoint = (polyIndex, edgeEnd) =>
{
if (OutlinePolygons.PointIsInside((bestAPos + edgeEnd) / 2, OutlineEdgeQuadTrees, OutlinePointQuadTrees))
{
return(true);
}
return(false);
};
var bestBPoly = BoundaryPolygons.FindClosestPoint(bestAPos, ConsiderPolygon, ConsiderPoint);
if (bestBPoly.polyIndex == -1)
{
// find one that intersects
bestBPoly = BoundaryPolygons.FindClosestPoint(bestAPos, ConsiderPolygon);
}
if (bestBPoly.polyIndex != -1)
{
bestAPos = polyA.FindClosestPoint(bestBPoly.Item3).Item2;
var bestBResult = BoundaryPolygons[bestBPoly.Item1].FindClosestPoint(bestAPos, ConsiderPoint);
IntPoint bestBPos = new IntPoint();
if (bestBResult.index != -1)
{
bestBPos = bestBResult.Item2;
}
else
{
// find one that intersects
bestBPos = BoundaryPolygons[bestBPoly.Item1].FindClosestPoint(bestAPos).Item2;
}
bestAPos = polyA.FindClosestPoint(bestBPos).Item2;
bestBPos = BoundaryPolygons[bestBPoly.Item1].FindClosestPoint(bestAPos).Item2;
// hook the polygons up along this connection
IntPointNode nodeA = Waypoints.FindNode(bestAPos);
IntPointNode nodeB = Waypoints.FindNode(bestBPos);
Waypoints.AddPathLink(nodeA, nodeB);
}
}
}
}
else // hook up using thin lines code
{
// this is done with merge close edges and finding candidates
// then joining the ends of the merged segments with the closest points
Polygons thinLines;
if (OutlinePolygons.FindThinLines(avoidInset * 2, 0, out thinLines))
{
ThinLinePolygons = thinLines;
for (int thinIndex = 0; thinIndex < thinLines.Count; thinIndex++)
{
var thinPolygon = thinLines[thinIndex];
if (thinPolygon.Count > 1)
{
Waypoints.AddPolygon(thinPolygon, false);
}
}
Polygons allPolygons = new Polygons(thinLines);
allPolygons.AddRange(BoundaryPolygons);
for (int thinIndex = 0; thinIndex < thinLines.Count; thinIndex++)
{
var thinPolygon = thinLines[thinIndex];
if (thinPolygon.Count > 1)
{
// now hook up the start and end of this polygon to the existing way points
var closestStart = allPolygons.FindClosestPoint(thinPolygon[0], (polyIndex, poly) => { return(polyIndex == thinIndex); });
var closestEnd = allPolygons.FindClosestPoint(thinPolygon[thinPolygon.Count - 1], (polyIndex, poly) => { return(polyIndex == thinIndex); }); // last point
if (OutlinePolygons.PointIsInside((closestStart.Item3 + closestEnd.Item3) / 2, OutlineEdgeQuadTrees))
{
IntPointNode nodeA = Waypoints.FindNode(closestStart.Item3);
IntPointNode nodeB = Waypoints.FindNode(closestEnd.Item3);
if (nodeA != null && nodeB != null)
{
Waypoints.AddPathLink(nodeA, nodeB);
}
}
}
}
}
}
removePointList = new WayPointsToRemove(Waypoints);
}
public bool CreatePathInsideBoundary(IntPoint startPointIn, IntPoint endPointIn, Polygon pathThatIsInside, bool optomizePath = true)
{
double z = startPointIn.Z;
startPointIn.Z = 0;
endPointIn.Z = 0;
if (BoundaryPolygons == null || BoundaryPolygons.Count == 0)
{
return(false);
}
// neither needed to be moved
if (BoundaryPolygons.FindIntersection(startPointIn, endPointIn, BoundaryEdgeQuadTrees) == Intersection.None &&
BoundaryPolygons.PointIsInside((startPointIn + endPointIn) / 2, BoundaryEdgeQuadTrees, BoundaryPointQuadTrees))
{
return(true);
}
removePointList.Dispose();
pathThatIsInside.Clear();
//Check if we are inside the boundaries
IntPointNode startPlanNode = null;
var lastAddedNode = GetWayPointInside(startPointIn, out startPlanNode);
IntPointNode endPlanNode = null;
var lastToAddNode = GetWayPointInside(endPointIn, out endPlanNode);
long startToEndDistanceSqrd = (endPointIn - startPointIn).LengthSquared();
long moveStartInDistanceSqrd = (startPlanNode.Position - lastAddedNode.Position).LengthSquared();
long moveEndInDistanceSqrd = (endPlanNode.Position - lastToAddNode.Position).LengthSquared();
if (startToEndDistanceSqrd < moveStartInDistanceSqrd ||
startToEndDistanceSqrd < moveEndInDistanceSqrd)
{
return(true);
}
var crossings = new List <Tuple <int, int, IntPoint> >(BoundaryPolygons.FindCrossingPoints(lastAddedNode.Position, lastToAddNode.Position, BoundaryEdgeQuadTrees));
crossings.Sort(new PolygonAndPointDirectionSorter(lastAddedNode.Position, lastToAddNode.Position));
foreach (var crossing in crossings.SkipSame())
{
IntPointNode crossingNode = Waypoints.FindNode(crossing.Item3, findNodeDist);
// for every crossing try to connect it up in the waypoint data
if (crossingNode == null)
{
crossingNode = AddTempWayPoint(removePointList, crossing.Item3);
// also connect it to the next and prev points on the polygon it came from
HookUpToEdge(crossingNode, crossing.Item1, crossing.Item2);
}
if (lastAddedNode != crossingNode &&
BoundaryPolygons.PointIsInside((lastAddedNode.Position + crossingNode.Position) / 2, BoundaryEdgeQuadTrees, BoundaryPointQuadTrees))
{
Waypoints.AddPathLink(lastAddedNode, crossingNode);
}
else if (crossingNode.Links.Count == 0)
{
// link it to the edge it is on
HookUpToEdge(crossingNode, crossing.Item1, crossing.Item2);
}
lastAddedNode = crossingNode;
}
if (lastAddedNode != lastToAddNode &&
BoundaryPolygons.PointIsInside((lastAddedNode.Position + lastToAddNode.Position) / 2, BoundaryEdgeQuadTrees))
{
// connect the last crossing to the end node
Waypoints.AddPathLink(lastAddedNode, lastToAddNode);
}
Path <IntPointNode> path = Waypoints.FindPath(startPlanNode, endPlanNode, true);
foreach (var node in path.Nodes.SkipSamePosition())
{
pathThatIsInside.Add(new IntPoint(node.Position, z));
}
if (path.Nodes.Length == 0)
{
if (saveBadPathToDisk)
{
WriteErrorForTesting(startPointIn, endPointIn, 0);
}
CalculatedPath?.Invoke(this, pathThatIsInside, startPointIn, endPointIn);
return(false);
}
if (optomizePath)
{
OptomizePathPoints(pathThatIsInside);
}
if (saveBadPathToDisk)
{
AllPathSegmentsAreInsideOutlines(pathThatIsInside, startPointIn, endPointIn, true);
}
CalculatedPath?.Invoke(this, pathThatIsInside, startPointIn, endPointIn);
return(true);
}
