private IntPointNode GetWayPointInside(IntPoint position, out IntPointNode waypointAtPosition)
{
waypointAtPosition = null;
OutlineData.Polygons.MovePointInsideBoundary(position, out (int polyIndex, int pointIndex, IntPoint position)foundPolyPointPosition, OutlineData.EdgeQuadTrees, OutlineData.PointKDTrees, OutlineData.PointIsInside);
if (foundPolyPointPosition.polyIndex == -1)
{
// The point is already inside
var existingNode = OutlineData.Waypoints.FindNode(position, FindNodeDist);
if (existingNode == null)
{
waypointAtPosition = AddTempWayPoint(OutlineData.RemovePointList, position);
return(waypointAtPosition);
}
waypointAtPosition = existingNode;
return(waypointAtPosition);
}
else // The point had to be moved inside the polygon
{
if (position == foundPolyPointPosition.position)
{
var existingNode = OutlineData.Waypoints.FindNode(position, FindNodeDist);
if (existingNode != null)
{
waypointAtPosition = existingNode;
return(waypointAtPosition);
}
else
{
// get the way point that we need to insert
waypointAtPosition = AddTempWayPoint(OutlineData.RemovePointList, position);
HookUpToEdge(waypointAtPosition, foundPolyPointPosition.polyIndex, foundPolyPointPosition.pointIndex);
return(waypointAtPosition);
}
}
else // the point was outside, hook it up to the nearest edge
{
// find the start node if we can
IntPointNode startNode = OutlineData.Waypoints.FindNode(foundPolyPointPosition.position, FindNodeDist);
// After that create a temp way point at the current position
waypointAtPosition = AddTempWayPoint(OutlineData.RemovePointList, position);
if (startNode != null)
{
OutlineData.Waypoints.AddPathLink(startNode, waypointAtPosition);
}
else
{
// get the way point that we need to insert
startNode = AddTempWayPoint(OutlineData.RemovePointList, foundPolyPointPosition.position);
HookUpToEdge(startNode, foundPolyPointPosition.polyIndex, foundPolyPointPosition.pointIndex);
OutlineData.Waypoints.AddPathLink(startNode, waypointAtPosition);
}
return(startNode);
}
}
}
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);
}
}
}
public virtual float DistanceTo(Pathfinding.IPoint pPoint)
{
if (pPoint is IntPointNode)
{
IntPointNode otherNode = pPoint as IntPointNode;
return((Position - otherNode.Position).Length());
}
else
{
throw new NotImplementedException();
}
}
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);
}
private void HookUpToEdge(IntPointNode crossingNode, int polyIndex, int pointIndex)
{
int count = OutlineData.Polygons[polyIndex].Count;
if (count > 0)
{
pointIndex = (pointIndex + count) % count;
IntPointNode prevPolyPointNode = OutlineData.Waypoints.FindNode(OutlineData.Polygons[polyIndex][pointIndex]);
OutlineData.Waypoints.AddPathLink(crossingNode, prevPolyPointNode);
IntPointNode nextPolyPointNode = OutlineData.Waypoints.FindNode(OutlineData.Polygons[polyIndex][(pointIndex + 1) % count]);
OutlineData.Waypoints.AddPathLink(crossingNode, nextPolyPointNode);
}
}
private bool SegmentIsAllInside(IntPointNode lastAddedNode, IntPointNode crossingNode)
{
if (OutlineData.DistanceFromOutside == null)
{
// check just the center point
return(OutlineData.PointIsInside((lastAddedNode.Position + crossingNode.Position) / 2) == QTPolygonsExtensions.InsideState.Inside);
}
else
{
// check many points along the line
return(OutlineData.PointIsInside((lastAddedNode.Position + crossingNode.Position) / 2) == QTPolygonsExtensions.InsideState.Inside &&
OutlineData.PointIsInside(lastAddedNode.Position + (crossingNode.Position - lastAddedNode.Position) / 4) == QTPolygonsExtensions.InsideState.Inside &&
OutlineData.PointIsInside(lastAddedNode.Position + (crossingNode.Position - lastAddedNode.Position) * 3 / 4) == QTPolygonsExtensions.InsideState.Inside);
}
}
public int CompareTo(object obj)
{
IntPointNode target = obj as IntPointNode;
float targetValue = target.PathCostHere + target.DistanceToGoal;
float thisValue = PathCostHere + DistanceToGoal;
if (targetValue > thisValue)
{
return(1);
}
else if (targetValue == thisValue)
{
return(0);
}
else
{
return(-1);
}
}
private bool CalculatePath(IntPoint startPointIn, IntPoint endPointIn, Polygon pathThatIsInside, int layerIndex)
{
double z = startPointIn.Z;
startPointIn.Z = 0;
endPointIn.Z = 0;
if (OutlineData?.Polygons == null ||
OutlineData?.Polygons.Count == 0)
{
return(false);
}
// neither needed to be moved
if (OutlineData.Polygons.FindIntersection(startPointIn, endPointIn, OutlineData.EdgeQuadTrees) == Intersection.None &&
OutlineData.PointIsInside((startPointIn + endPointIn) / 2) == QTPolygonsExtensions.InsideState.Inside)
{
return(true);
}
OutlineData.RemovePointList.Dispose();
pathThatIsInside.Clear();
// Check if we are inside the boundaries
var lastAddedNode = GetWayPointInside(startPointIn, out IntPointNode startPlanNode);
var lastToAddNode = GetWayPointInside(endPointIn, out IntPointNode endPlanNode);
long startToEndDistanceSqrd = (endPointIn - startPointIn).LengthSquared();
long moveStartInDistanceSqrd = (startPlanNode.Position - lastAddedNode.Position).LengthSquared();
long moveEndInDistanceSqrd = (endPlanNode.Position - lastToAddNode.Position).LengthSquared();
// if we move both points less than the distance of this segment
if (startToEndDistanceSqrd < moveStartInDistanceSqrd &&
startToEndDistanceSqrd < moveEndInDistanceSqrd)
{
// then go ahead and say it is a good path
return(true);
}
var crossings = new List <(int polyIndex, int pointIndex, IntPoint position)>(OutlineData.Polygons.FindCrossingPoints(lastAddedNode.Position, lastToAddNode.Position, OutlineData.EdgeQuadTrees));
if (crossings.Count == 0)
{
return(true);
}
crossings.Sort(new PolygonAndPointDirectionSorter(lastAddedNode.Position, lastToAddNode.Position));
foreach (var(polyIndex, pointIndex, position) in crossings.SkipSame())
{
IntPointNode crossingNode = OutlineData.Waypoints.FindNode(position, FindNodeDist);
// for every crossing try to connect it up in the waypoint data
if (crossingNode == null)
{
crossingNode = AddTempWayPoint(OutlineData.RemovePointList, position);
// also connect it to the next and prev points on the polygon it came from
HookUpToEdge(crossingNode, polyIndex, pointIndex);
}
if (lastAddedNode != crossingNode &&
(SegmentIsAllInside(lastAddedNode, crossingNode) || lastAddedNode.Links.Count == 0))
{
OutlineData.Waypoints.AddPathLink(lastAddedNode, crossingNode);
}
else if (crossingNode.Links.Count == 0)
{
// link it to the edge it is on
HookUpToEdge(crossingNode, polyIndex, pointIndex);
}
lastAddedNode = crossingNode;
}
if (lastAddedNode != lastToAddNode &&
(OutlineData.PointIsInside((lastAddedNode.Position + lastToAddNode.Position) / 2) == QTPolygonsExtensions.InsideState.Inside ||
lastToAddNode.Links.Count == 0))
{
// connect the last crossing to the end node
OutlineData.Waypoints.AddPathLink(lastAddedNode, lastToAddNode);
}
Path <IntPointNode> path = OutlineData.Waypoints.FindPath(startPlanNode, endPlanNode, true);
foreach (var node in path.Nodes.SkipSamePosition())
{
pathThatIsInside.Add(new IntPoint(node.Position, z));
}
if (path.Nodes.Length == 0 && saveBadPathToDisk)
{
WriteErrorForTesting(layerIndex, startPointIn, endPointIn, 0);
return(false);
}
return(true);
}
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);
}
