Ejemplo n.º 1
0
        public static bool OnOneLine(Node sNode, Node fNode, NodeArray nodesArray)
        {
            var destination = FindDestination(sNode, fNode);
            var finishPoint = new Vector2(Convert.ToInt32(fNode.Position.X), Convert.ToInt32(fNode.Position.Y));
            var line        = new StraightLine(Convert.ToInt32(sNode.Position.X), Convert.ToInt32(sNode.Position.Y),
                                               destination, nodesArray);

            if (line.Points.Exists(arg => arg == finishPoint))
            {
                for (var i = 1; i < line.Points.Count; ++i)
                {
                    var pointX = Convert.ToInt32(line.Points[i].X);
                    var pointY = Convert.ToInt32(line.Points[i].Y);
                    if (nodesArray.Array[pointX, pointY].IsObstacle)
                    {
                        return(false);
                    }
                    if (line.Points[i] == finishPoint)
                    {
                        return(true);
                    }
                    if ((destination == Destinations.UpRight || destination == Destinations.DownRight) &&
                        !nodesArray.Array[pointX, pointY + 1].IsObstacle)
                    {
                        return(false);
                    }
                    if ((destination == Destinations.UpLeft || destination == Destinations.DownLeft) &&
                        !nodesArray.Array[pointX, pointY - 1].IsObstacle)
                    {
                        return(false);
                    }
                }
            }
            return(false);
        }
Ejemplo n.º 2
0
        public static List <Node> AStar(Node from, Node to)
        {
            var finalPath = new List <Node>();

            if (from.IsObstacle || to.IsObstacle)
            {
                Console.WriteLine("Can't run A*! Enter proper from and to, one of them is an obsttacle!");
                return(finalPath);
            }
            var current = new Node(from)
            {
                DistanceToGoal = Node.MetricsAStar(@from, to), Visited = NodeState.Processed
            };
            var observed = new List <Node> {
                current
            };

            while (current.Position != to.Position)
            {
                var query = NodesArray.Neighbours(current);
                query = query.Where(node => !node.IsObstacle).ToList();
                foreach (var node in query)
                {
                    if (!observed.Exists(arg => arg.Position == node.Position))
                    {
                        node.DistanceToGoal = Node.MetricsAStar(node, to);
                        node.Visited        = NodeState.Discovered;
                        observed.Add(node);
                    }
                }
                observed = observed.OrderBy(arg => arg.Visited).ThenBy(arg => arg.DistanceToGoal).ToList();
                if (observed[0].Visited != NodeState.Processed)
                {
                    current             = observed[0];
                    observed[0].Visited = NodeState.Processed;
                }
                else
                {
                    Console.WriteLine("No path was found");
                    return(finalPath);
                }
            }
            observed = observed.Where(informer => informer.Visited == NodeState.Processed).ToList();

            finalPath.Add(observed[0]);
            for (var i = 0; i < observed.Count; ++i)
            {
                var maxIndex = i;
                for (var j = i + 1; j < observed.Count; ++j)
                {
                    if (StraightLine.OnOneLine(observed[i], observed[j], NodesArray))
                    {
                        maxIndex = j;
                    }
                }
                if (maxIndex != i)
                {
                    var points = StraightLine.FindMiddlePoints(observed[i], observed[maxIndex]);
                    points.RemoveAt(0);
                    foreach (var point in points)
                    {
                        var coordinatesX = Convert.ToInt32(point.X);
                        var coordinatesY = Convert.ToInt32(point.Y);
                        finalPath.Add(NodesArray.Array[coordinatesX, coordinatesY]);
                    }
                    i = maxIndex - 1;
                }
            }
            finalPath.Reverse();
            return(finalPath);
        }