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);
}
