public static void NeighboursAndTJP(Informer start, Informer finish, Node[,] nodesArray, out DebugInformationAlgorithm debugInfo) { var startNode = new Node(start, NodeState.Processed); startNode.NormMatrix = nodesArray[startNode.X(), startNode.Y()].NormMatrix; startNode.Distance = MetricsAStar(start, finish); var startTreeNode = new Tree_Node(null, startNode); var finishNode = new Node(finish, NodeState.Processed); var sraightLinesFromStart = new StraightLinesFromNode(startNode); var sraightLinesFromFinish = new StraightLinesFromNode(finishNode); var neighbours = Neighbours(startTreeNode, nodesArray, finishNode); var minMetrics = startNode.Distance; var tempList = new List <Node>(); if (sraightLinesFromStart.Lines != null) { foreach (var lineFromFinish in sraightLinesFromFinish.Lines) { foreach (var line in sraightLinesFromStart.Lines) { var coordinates = StraightLine.Crossing(line, lineFromFinish); if (coordinates != null && Reachable(nodesArray[coordinates.X, coordinates.Y], finishNode, nodesArray) && Reachable(startNode, nodesArray[coordinates.X, coordinates.Y], nodesArray)) { var tempNode = new Node(nodesArray[coordinates.X, coordinates.Y]); tempNode.Distance = Metrics(new Tree_Node(startTreeNode, tempNode), finishNode); tempNode.TargetJP = true; tempNode.DestinationToFinish = 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) { tempList.Add(tempNode); } } } } } var straightLines = StraightLinesFromNode.JoinLines(sraightLinesFromStart, sraightLinesFromFinish, nodesArray); debugInfo = new DebugInformationAlgorithm { From = startNode.InformerNode, To = finishNode.InformerNode, Observed = ToNodes(neighbours), LinesFromFinish = straightLines, CrossPoints = tempList, Destroy = false }; }
public static Node IsTargetJP(Node node, StraightLinesFromNode lines) { var isTargetJP = node; foreach (var line in lines.Lines) { var point = new Point(node.X(), node.Y()); if (point.Belongs(line)) { /*Debug.Log("Point "+point.X+" "+point.Y); * Debug.Log("line "+line.Start.X+" "+line.Start.Y+" "+line.Finish.X+" "+line.Finish.Y);*/ isTargetJP.TargetJP = true; isTargetJP.DestinationToFinish = DestinationInverse(line.Destination); break; } } return(isTargetJP); }