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