public static List <Node> JoinLines(StraightLinesFromNode l1, StraightLinesFromNode l2, Node[,] nodesArray)
{
var list = new List <Node>();
list.AddRange(ToList(l1, nodesArray));
list.AddRange(ToList(l2, nodesArray));
return(list);
}
public static List <Node> ToList(StraightLinesFromNode lines, Node[,] nodesArray)
{
var list = new List <Node>();
foreach (var line in lines.Lines)
{
foreach (var point in line.Points)
{
var node = nodesArray[point.X, point.Y];
if (node.InformerNode.IsObstacle)
{
continue;
}
list.Add(node);
}
}
return(list);
}
public List <Node> JPS(Informer from, Informer to, bool debugFlag, out DebugInformationAlgorithm debugInformation)
{
if (from == null || to == null)
{
Debug.LogError("Can't run JPS+. Enter proper from and to parameters!");
debugInformation = null;
return(null);
}
if (!IsPrecomputed)
{
Debug.Log("Precomputing...");
PrecomputeMap();
Debug.Log("Done!");
}
var finish = NodesArray[(int)to.transform.position.x / 3, (int)to.transform.position.z / 3];
var linesFromFinish = new StraightLinesFromNode(finish);
var start = new Tree_Node(null, NodesArray[(int)from.transform.position.x / 3, (int)from.transform.position.z / 3]);
start.Currentnode.Distance = Extensions.Metrics(start, finish);
var current = start;
var path = new List <Tree_Node>();
var observed = new List <Tree_Node> {
current
};
if (debugFlag)
{
debugInformation = new DebugInformationAlgorithm
{
From = from,
To = to,
Observed = new List <Node>(),
FinalPath = new List <Informer>(),
LinesFromFinish = StraightLinesFromNode.ToList(linesFromFinish, NodesArray)
};
}
else
{
debugInformation = null;
}
while (current.Currentnode != finish)
{
if (!observed.Exists(arg => arg.Currentnode.Visited != NodeState.Processed))
{
Debug.Log("No path was found");
if (debugFlag)
{
debugInformation.Observed = Extensions.ToNodes(
observed.Where(arg => arg.Currentnode.Visited == NodeState.Processed).
OrderBy(arg => arg.Level).ToList());
}
return(null);
}
observed[0].Currentnode.Visited = NodeState.Processed;
//Go to finish if in Target JP
current.Currentnode = Extensions.IsTargetJP(current.Currentnode, linesFromFinish);
if (current.Currentnode.TargetJP && Extensions.Reachable(current.Currentnode, finish, NodesArray))
{
finish.DestinationFromPrevious = Extensions.DestinationInverse(current.Currentnode.DestinationToFinish);
path.Add(current);
current = new Tree_Node(current, finish);
path.Add(current);
break;
}
//Find next nodes
//Neighbours
var neighbours = Extensions.Neighbours(current, NodesArray, finish);
//Target JP
var lines = new StraightLinesFromNode(current.Currentnode, Extensions.GetDestinationsFromNeighbours(neighbours));
var minMetrics = current.Currentnode.Distance;
var tempList = new List <Node>();
if (lines.Lines != null)
{
foreach (var lineFromFinish in linesFromFinish.Lines)
{
foreach (var line in lines.Lines)
{
var coordinates = StraightLine.Crossing(line, lineFromFinish);
if (coordinates != null &&
Extensions.Reachable(current.Currentnode, NodesArray[coordinates.X, coordinates.Y], NodesArray))
{
if (coordinates != null && Extensions.Reachable(NodesArray[coordinates.X, coordinates.Y], finish, NodesArray) &&
Extensions.Reachable(current.Currentnode, NodesArray[coordinates.X, coordinates.Y], NodesArray))
{
var tempNode = new Node(NodesArray[coordinates.X, coordinates.Y]);
tempNode.Distance = Extensions.Metrics(new Tree_Node(current, tempNode), finish);
tempNode.TargetJP = true;
tempNode.DestinationToFinish = Extensions.DestinationInverse(lineFromFinish.Destination);
tempNode.Visited = NodeState.Discovered;
if (tempNode.Distance < minMetrics)
{
minMetrics = tempNode.Distance;
tempList.Clear();
tempList.Add(tempNode);
}
else if (Math.Abs(tempNode.Distance - minMetrics) < 0.00000000001)
{
tempNode.Distance = Extensions.Metrics(new Tree_Node(current, tempNode), finish);
tempList.Add(tempNode);
}
}
}
}
}
}
Tree_Node tempTargetJP = null;
if (tempList.Count != 0)
{
tempTargetJP = new Tree_Node(current, tempList[0]);
if (tempList.Count > 1)
{
var min = tempTargetJP.DistanceFromParent;
foreach (var node in tempList)
{
var tempMetrics = current.Currentnode.InformerNode.MetricsAStar(node.InformerNode);
if (tempMetrics < min)
{
tempTargetJP = new Tree_Node(current, node);
min = tempMetrics;
}
}
}
if (debugFlag)
{
debugInformation.CrossPoints.Add(tempTargetJP.Currentnode);
}
if (!observed.Exists(arg => arg.Currentnode.Position == tempTargetJP.Currentnode.Position))
{
observed.Add(tempTargetJP);
}
else
{
var index =
observed.FindIndex(arg => arg.Currentnode.Position == tempTargetJP.Currentnode.Position);
if (observed[index].Currentnode.Visited == NodeState.Discovered)
{
observed[index].Currentnode.Distance = tempTargetJP.Currentnode.Distance;
}
}
}
if (neighbours.Count != 0)
{
foreach (var neighbour in neighbours)
{
if (!observed.Exists(arg => arg.Currentnode.Position == neighbour.Currentnode.Position))
{
if (Extensions.SelectJPFromNeighbours(current, neighbour))
{
observed.Add(neighbour);
}
}
else
{
var index =
observed.FindIndex(arg => arg.Currentnode.Position == neighbour.Currentnode.Position);
if (observed[index].Currentnode.Visited == NodeState.Discovered)
{
observed[index].Currentnode.Distance = neighbour.Currentnode.Distance;
}
}
}
}
observed = observed.OrderBy(arg => arg.Currentnode.Visited).
ThenBy(arg => arg.Currentnode.Distance).ToList();
path.Add(current);
if (!observed[0].Currentnode.TargetJP && tempTargetJP != null)
{
current = observed[0].DistanceFromParent < tempTargetJP.DistanceFromParent ? observed[0] : tempTargetJP;
}
else
{
current = observed[0];
}
}
Debug.Log("Path: " + path.Count);
if (path.Count > 1)
{
var finalPath = new List <Node>();
while (current != start)
{
var middlePoints = StraightLine.FindMiddlePoints(current.Parent, current.Currentnode);
if (current.Parent != start.Currentnode)
{
middlePoints.RemoveAt(0);
}
finalPath.InsertRange(0, Extensions.ToNodes(middlePoints, NodesArray));
current = path.Find(arg => arg.Currentnode.Position == current.Parent.Position &&
arg.Level == current.Level - 1);
}
if (debugFlag)
{
debugInformation.Observed = Extensions.ToNodes(
observed.Where(arg => arg.Currentnode.Visited == NodeState.Processed).
OrderBy(arg => arg.Level).ToList());
debugInformation.FinalPath = Extensions.ToInformers(finalPath);
}
Debug.Log("Final path: " + finalPath.Count);
return(finalPath);
}
else
{
return(null);
}
}