Example #1
0
 public NodeCollection(int gridSize)
 {
     nodes = new Node[gridSize, gridSize];
     for (int x = 0; x < gridSize; x++)
         for (int y = 0; y < gridSize; y++)
             nodes[x, y] = new Node();
 }
        /*
         * public functions
         */
        /// <summary>
        /// A*, modified to find a nearest path in case of failure
        /// </summary>
        /// <param name="map">The Map</param>
        /// <param name="startNode">The starting Node</param>
        /// <param name="endNode">The ending Node</param>
        /// <returns>The path as a list of waypoints</returns>
        public static List<Node> findPath(NodeMap map, Node startNode, Node endNode)
        {
            // initialize data
            PQueue open = new PQueue();
            open.enqueue(startNode);
            List<Node> adjacentNodes = new List<Node>();

            // good ol' A*
            while (open.Count > 0)
            {
                // find the open Node with the lowest Fscore and remove it from the open PQueue
                Node current = open.dequeue();

                // if this is our destination Node, we're done; otherwise, close it so we don't travel to it again
                if (current == endNode)
                    return reconstruct(endNode);
                current.close();

                // find every valid Node adjacent to the current Node
                adjacentNodes = map.getAdjacentNodes(current);

                // iterate over all of them
                for (int i = 0; i < adjacentNodes.Count; i++)
                {
                    // grab an adjacent Node and calculate a new GScore and HScore for it
                    Node adjacent = adjacentNodes[i];
                    int tempGScore = current.Gscore + map.pathDistance(current, adjacent);
                    int tempHScore = map.pathDistance(adjacent, endNode);

                    // if we have not opened this Cell, give it new stats and open it
                    if (!adjacent.isOpen)
                    {
                        setAdjacentStats(adjacent, current, tempGScore, tempHScore);
                        open.enqueue(adjacent);
                    }

                    // otherwise, if we have opened it but the new path to it is shorter than the old one, give it new stats and reset its position in the open PQueue
                    else if (tempGScore < adjacent.Gscore)
                    {
                        setAdjacentStats(adjacent, current, tempGScore, tempHScore);
                        //open.resetPosition(adjacent);
                    }
                }
            }

            // no valid path exists, so find the nearest path
            List<Node> closed = createClosed(map);
            Node nearestNode = closed[0];

            // find the closed Node with the lowest Hscore (distance from the intended end); return a path to that Node
            if (closed.Count > 0)
            {
                for (int i = 1; i < closed.Count; i++)
                {
                    if (closed[i].Hscore < nearestNode.Hscore)
                        nearestNode = closed[i];
                }
            }
            return reconstruct(nearestNode);
        }
Example #3
0
		//This method should never be called on it's own.
		//It is recursively called by FindPath()
		public bool Search(Node currentNode) {

			//Mark the current node as considered, to never check again.
			currentNode.State = Node.NodeState.Closed;
		
			//Get all of the neighbouring nodes that aren't walls and are in bounds.
			List<Node> nextNodes = GetAdjacentWalkableNodes(currentNode);

			//Sort the list by the cheapest nodes.
			nextNodes.Sort((node1, node2) => node1.F.CompareTo(node2.F));

			foreach (var nextNode in nextNodes) {
				//Has the path been reached?
				if (nextNode.X == goalX && nextNode.Y == goalY) {
					return true;
				}
					
				else {
					//Have we reached a dead end?
					if (Search (nextNode))
						return true;

					//If this isn't true, we reached a dead end.
				}
			}

			//The starting node is a wall itself.
			return false;
		}
Example #4
0
 public Node(MapCell cell, Node parent, float cost, float heur)
 {
     Cell = cell;
     Heuristic = heur;
     Parent = parent;
     Cost = cost;
 }
Example #5
0
 public Node()
 {
     cost = INITIAL_COST;
     inPath = false;
     closed = false;
     parent = null;
     position = new Coord2(-1, -1);
 }
 /// <summary>
 /// Returns a Node[,] representation of a subsection of the map.  Coordinates outside map bounds are returned as null
 /// </summary>
 /// <param name="x">The starting X-coordinate</param>
 /// <param name="y">The starting Y-coordinate</param>
 /// <param name="width">Subsection width</param>
 /// <param name="height">Subsection height</param>
 /// <returns>A Node[,] subsection of the map</returns>
 public Node[,] getNodes(int x, int y, int width, int height)
 {
     Node[,] temp = new Node[width, height];
     for (int j = 0; j < height; j++)
     {
         for (int i = 0; i < width; i++)
         {
             temp[i, j] = getNode(i + x, j + y);
         }
     }
     return temp;
 }
 /*
  * constructors
  */
 /// <summary>
 /// Default constructor
 /// </summary>
 /// <param name="m">The Gameworld.Map to copy dimensional and validity data from</param>
 public NodeMap(Map m)
 {
     this.height = m.height;
     this.width = m.width;
     this.nodes = new Node[width, height];
     for (int j = 0; j < height; j++)
     {
         for (int i = 0; i < width; i++)
         {
             nodes[i, j] = new Node(m.getCell(i, j).Xcoord, m.getCell(i,j).Ycoord, m.getCell(i,j).isValid);
         }
     }
 }
 /*
  * constructors
  */
 /// <summary>
 /// Default constructor
 /// </summary>
 /// <param name="m">The Gameworld.Map to copy dimensional and validity data from</param>
 public NodeMap(Map m)
 {
     this.height = m.height;
     this.width = m.width;
     Console.WriteLine("  Node Map Dimensions: ({0}, {1})", this.width, this.height);
     this.nodes = new Node[width, height];
     for (int j = 0; j < height; j++)
     {
         for (int i = 0; i < width; i++)
         {
             nodes[i, j] = new Node(m.getCell(i, j).Xcoord, m.getCell(i,j).Ycoord, m.getCell(i,j).isValid);
         }
     }
 }
        /*
         * public functions
         */
        /// <summary>
        /// Given an invalid Node, returns the approximate nearest valid Node.
        /// </summary>
        /// <param name="end"></param>
        /// <returns></returns>
        public static Node nearestValidEnd(NodeMap map, Node start, Node end)
        {
            int max = Math.Max(map.height, map.width);
            PQueue ring = new PQueue();

            // iterate outward from the end Node in progressively larger rings, putting all valid Nodes into a PQueue.
            // Continue until we find a ring with a valid Node.
            for (int i = 1; i < max; i++)
            {
                ring = getRing(map, end, i);
                if (ring.Count != 0)
                    break;
            }

            // if this is a completely invalid NodeMap (yeah, right, I know), return null
            if (ring.Count == 0)
                return null;

            // find the valid Node nearest the intended end
            Node newEnd = ring.peek(0);

            // if the PQueue has a lead tie (i.e. there are 2+ nodes of equal distance from the center node),
            // pick the one closest to the intended start Node.
            if (ring.hasLeadTie)
            {
                int distToStart = map.pathDistance(start, newEnd);
                int distToEnd = map.pathDistance(end, newEnd);
                for (int i = 1; i < ring.Count; i++)
                {
                    if (map.pathDistance(ring.peek(i), end) > distToEnd)
                        break;
                    int currentDist = map.pathDistance(ring.peek(i), start);
                    if (currentDist < distToStart)
                    {
                        distToStart = currentDist;
                        newEnd = ring.peek(i);
                    }
                }
            }

            // clean the FScores of all enqueued Nodes so they can be properly used by the pathfinder
            for (int i = 0; i < ring.Count; i++)
                ring.peek(i).Fscore = 0;

            return newEnd;
        }
        /*
         * helper functions
         */
        /// <summary>
        /// Enqueues all valid Nodes in a ring around a center Node, offset from the center by [offset] Nodes.
        /// For example, an offset of 2 searches a 5x5 ring (x+-2, y+-2) around the center Node.
        /// Since modifying the Node's Fscores is necessary for enqueueing, each enqueued node's Fscore must be reset after use.
        /// </summary>
        /// <param name="map">The NodeMap to search over</param>
        /// <param name="center">The center Node to search around</param>
        /// <param name="offset">The ring "radius"</param>
        /// <returns>A PQueue containing all valid Nodes found in the ring</returns>
        private static PQueue getRing(NodeMap map, Node center, int offset)
        {
            PQueue ring = new PQueue();
            int x = center.Xcoord;
            int y = center.Ycoord;

            // grab left and right columns
            for (int i = y - offset; i <= y + offset; i++)
            {
                Node Xmin = map.getNode(x - offset, i);
                Node Xmax = map.getNode(x + offset, i);
                if (Xmin.isValid)
                {
                    Xmin.Fscore = map.pathDistance(Xmin, center);
                    ring.enqueue(Xmin);
                }
                if (Xmax.isValid)
                {
                    Xmax.Fscore = map.pathDistance(Xmax, center);
                    ring.enqueue(Xmax);
                }
            }

            // grab remainder of top and bottom rows
            for (int i = x - offset + 1; i < x + offset; i++)
            {
                Node Ymin = map.getNode(i, y - offset);
                Node Ymax = map.getNode(i, y + offset);
                if (Ymin.isValid)
                {
                    Ymin.Fscore = map.pathDistance(Ymin, center);
                    ring.enqueue(Ymin);
                }
                if (Ymax.isValid)
                {
                    Ymax.Fscore = map.pathDistance(Ymax, center);
                    ring.enqueue(Ymax);
                }
            }

            return ring;
        }
 /// <summary>
 /// Inserts the given Node by its Fscore
 /// </summary>
 /// <param name="Node">The Node to insert</param>
 public void enqueue(Node node)
 {
     if (node == null)
         return;
     node.open();
     if (pq.Count == 0)
         pq.Add(node);
     else
     {
         int i = 0;
         while (i < pq.Count)
         {
             if (node.Fscore >= pq[i].Fscore)
                 i++;
             else
                 break;
         }
         pq.Insert(i, node);
     }
 }
Example #12
0
		//Populate the map with nodes
		public PathAlgorithm(int width, int height) {
			nodes = new Node[width, height];

			//Instantiate a new node for every tile in the map
			for (int i = 0; i < nodes.GetLength (0); i++)
				for (int j = 0; j < nodes.GetLength (1); j++) {
					nodes [i, j] = new Node (i, j);
				
					//Gets the path cost of the starting node to avoid null reference exceptions
					if (nodes [i, j].ParentNode == null)
						nodes [i, j].G = 0;
					//The path cost of the node is the parent's cost + the cost to get to this tile. (Running total)
					else
						nodes [i, j].G = nodes [i, j].ParentNode.G + 1;

					//The remaining cost is estimated with the Manhattan Distance heuristic. (Faster than Euclidean Distance)
					nodes [i, j].H = Math.Abs (goalX - nodes [i, j].X) + Math.Abs (goalY - nodes [i, j].Y);

					//The total cost is the G cost + the H cost
					nodes [i, j].F = nodes [i, j].G + nodes [i, j].H;
				}

		}
Example #13
0
        /// <summary>
        /// Find the node with the lowest cost.
        /// </summary>
        protected virtual void FindLowestCost()
        {
            currentLowest = target;

            for (int x = 0; x < GridSize; x++)
            {
                for (int y = 0; y < GridSize; y++)
                {
                    // If cost is lower than current, position not closed, and position is valid within level, new lowest is found
                    if (nodes.Get(currentLowest.position).cost >= nodes.Get(x, y).cost && nodes.Get(x, y).closed == false &&
                        map.ValidPosition(new Coord2(x, y)))
                            currentLowest = nodes.Get(x, y);
                }
            }
        }
Example #14
0
        /// <summary>
        /// Get the valid neighbour locations for the given node.
        /// </summary>
        protected virtual List<Node> GetNeighours(Node node)
        {
            List<Node> list = new List<Node>();

            // Horizontal and vertical
            if (map.ValidPosition(node.position + new Coord2(1, 0)))
                list.Add(nodes.Get(node.position + new Coord2(1, 0)));

            if (map.ValidPosition(node.position + new Coord2(-1, 0)))
                list.Add(nodes.Get(node.position + new Coord2(-1, 0)));

            if (map.ValidPosition(node.position + new Coord2(0, 1)))
                list.Add(nodes.Get(node.position + new Coord2(0, 1)));

            if (map.ValidPosition(node.position + new Coord2(0, -1)))
                list.Add(nodes.Get(node.position + new Coord2(0, -1)));

            // Diagonal
            if (map.ValidPosition(node.position + new Coord2(1, 1)))
                list.Add(nodes.Get(node.position + new Coord2(1, 1)));

            if (map.ValidPosition(node.position + new Coord2(-1, -1)))
                list.Add(nodes.Get(node.position + new Coord2(-1, -1)));

            if (map.ValidPosition(node.position + new Coord2(1, -1)))
                list.Add(nodes.Get(node.position + new Coord2(1, -1)));

            if (map.ValidPosition(node.position + new Coord2(-1, 1)))
                list.Add(nodes.Get(node.position + new Coord2(-1, 1)));

            return list;
        }
Example #15
0
 /// <summary>
 /// Clears the current node collection.
 /// </summary>
 public void Clear()
 {
     for (int x = 0; x < nodes.GetLength(0); x++)
         for (int y = 0; y < nodes.GetLength(1); y++)
             nodes[x, y] = new Node();
 }
Example #16
0
 /// <summary>
 /// Create a new collection of nodes.
 /// </summary>
 /// <param name="gridSize">The size of the node grid.</param>
 public NodeCollection(int gridSize)
 {
     nodes = new Node[gridSize, gridSize];
     this.gridSize = gridSize;
     for (int x = 0; x < gridSize; x++)
         for (int y = 0; y < gridSize; y++)
             nodes[x, y] = new Node(new Coord2(x, y));
 }
Example #17
0
        /// <summary>
        /// Recalcualtes the costs in a set of neighbours based upon the given parent node.
        /// </summary>
        /// <param name="neighbours">The neighbours to recalculate costs for.</param>
        /// <param name="node">The parent node to use.</param>
        protected virtual void RecalculateCosts(List<Node> neighbours, Node node)
        {
            for (int i = 0; i < neighbours.Count; i++)
            {
                if(map.ValidPosition(neighbours[i].position) && neighbours[i].closed == false)
                {
                    float costToAdd = 0.0f;

                    if (neighbours[i].position.X != 0 && neighbours[i].position.Y != 0)
                        costToAdd = D_COST;
                    else
                        costToAdd = HV_COST;

                   float newCost = nodes.Get(node.position).cost + costToAdd;

                   if (newCost < neighbours[i].cost)
                   {
                       neighbours[i].cost = newCost;
                       neighbours[i].parent = node;
                   }
                }
            }
        }
Example #18
0
        public Point[] FindPath(Point start, Point end)
        {
            //Ищем путь в кэше, если находим, то тут же завершаемся
            Point[] PrecompPath = ExaminePathCache(start, end);
            if (PrecompPath != null)
                return PrecompPath;

            List<Point> p = new List<Point>(); //конечный список

            current = new Node(start, 0, 0, 0);
            current.H = calcH(start, end);
            current.F = current.H;
            openList.Add(current);
            existsArray[current.P.X, current.P.Y] = 1;

            bool ex = false;
            bool can = true;
            while (!ex)
            {
                int i = findMinF(openList);
                current = openList[i];
                openList.RemoveAt(i);
                closeList.Add(current);
                existsArray[current.P.X, current.P.Y] = 2;

                //проверка соседних клеток
                Point nb = current.P;
                nbPoint(new Point(nb.X + 1, nb.Y), current, end, x, y);
                nbPoint(new Point(nb.X, nb.Y - 1), current, end, x, y);
                nbPoint(new Point(nb.X - 1, nb.Y), current, end, x, y);
                nbPoint(new Point(nb.X, nb.Y + 1), current, end, x, y);

                if (openList.Count == 0) //открытый списко пустой - путь невозможен
                {
                    ex = true;
                    can = false;
                }
                if (existsArray[end.X, end.Y] == 1) //дошли до финала
                    ex = true;
            }

            //составление искомого пути
            if (can)
            {
                p.Add(end);
                Point pp = end;
                int i = 0;

                while (pp != start)
                {
                    if (existsArray[pp.X, pp.Y] == 1)
                    {
                        i = inList(pp, openList);
                        if (i < 0)
                            break;
                        pp = openList[i].Parent;
                    }
                    else if (existsArray[pp.X, pp.Y] == 2)
                    {
                        i = inList(pp, closeList);
                        if (i < 0)
                            break;
                        pp = closeList[i].Parent;
                    }
                    p.Add(pp);
                }
            }

            Point[] ret = new Point[p.Count];
            for (int i = 0; i < p.Count; i++)
                ret[i] = p[p.Count - i - 1];

            clear();

            pathCache.Add(new PrecomputedPath(start, end, ret)); //запись пути в кеш
            return ret;
        }
Example #19
0
		public List<Node> GetAdjacentWalkableNodes(Node fromNode) {

			//All of the nodes that are in the bounds
			List<Node> validNodes = new List<Node> ();

			//All of the nodes that are not walls
			List<Node> walkableNodes = new List<Node> ();

			//The nodes that aren't closed, and are untested or have a shorter path.
			List<Node> consideredNodes = new List<Node> ();

			//If the nodes are in the map boundaries.
			if (fromNode.X + 1 < Game1.mapWidth)
				validNodes.Add(nodes[fromNode.X + 1, fromNode.Y]);
			if (fromNode.X - 1 > -1)
				validNodes.Add (nodes [fromNode.X - 1, fromNode.Y]);
			if (fromNode.Y + 1 < Game1.mapHeight)
				validNodes.Add (nodes [fromNode.X, fromNode.Y + 1]);
			if (fromNode.Y - 1 > -1)
				validNodes.Add (nodes [fromNode.X, fromNode.Y - 1]);

			//If the nodes are walkable add them to the walkable nodes
			foreach (var node in validNodes) {
				if (node.IsWalkable ())
					walkableNodes.Add (node);
			}

			//ToArray() is necessary to prevent a collections modified enumeration error
			foreach (var node in walkableNodes.ToArray()) {

				//If the node is closed already, don't consider it.
				if (node.State == Node.NodeState.Closed)
					continue;

				else if (node.State == Node.NodeState.Open) {
					//float traversalCost = Node.GetTraversalCost (node.X, node.Y, node.ParentNode.X, node.ParentNode.Y);
					float traversalCost = node.G + node.ParentNode.G;
					float gTemp = fromNode.G + traversalCost;

					//If you have found a better path to an already considered node, update it and consider it.
					if (gTemp < node.G) {
						node.ParentNode = fromNode;
						consideredNodes.Add (node);
					}

					//If it hasn't been tested already consider it.
				} else {
					node.ParentNode = fromNode;
					node.State = Node.NodeState.Open;
					consideredNodes.Add (node);
				}
			}

			return consideredNodes;
		}
Example #20
0
        private int calcG(Node node)
        {
            int g = current.G;

            if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.LowDensity)
                g += 2;
            else if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.MediumDensity)
                g += 3;
            else if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.HighDensity)
                g += 4;

            VG.Map.BloodStream bs = tissue.IsInStream(node.P.X, node.P.Y);
            if (bs != null)
            {
                int dy = node.P.Y - current.P.Y;
                int dx = node.P.X - current.P.X;

                #region Direction
                bool plus = false;
                if (bs.Direction == VG.Map.BloodStreamDirection.NorthSouth)
                {
                    if (dy >= 0)
                        plus = false;
                    else
                        plus = true;
                }
                else if (bs.Direction == VG.Map.BloodStreamDirection.SouthNorth)
                {
                    if (dy <= 0)
                        plus = false;
                    else
                        plus = true;
                }
                else if (bs.Direction == VG.Map.BloodStreamDirection.EstWest)
                {
                    if (dx <= 0)
                        plus = false;
                    else
                        plus = true;
                }
                else if (bs.Direction == VG.Map.BloodStreamDirection.WestEst)
                {
                    if (dx >= 0)
                        plus = false;
                    else
                        plus = true;
                }

                if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.LowDensity)
                {
                    if (plus)
                        g += 2;
                    else
                        g -= 1;
                }
                else
                {
                    if (plus)
                        g += 2;
                    else
                        g -= 2;
                }

                #endregion
            }

            return g;
        }
Example #21
0
        /// <summary>
        /// Recalcualtes the costs in a set of neighbours based upon the given parent node.
        /// </summary>
        /// <param name="neighbours">The neighbours to recalculate costs for.</param>
        /// <param name="node">The parent node to use.</param>
        protected override void RecalculateCosts(List<Node> neighbours, Node node)
        {
            for (int i = 0; i < neighbours.Count; i++)
            {
                if(map.ValidPosition(neighbours[i].position) && neighbours[i].closed == false)
                {
                    float costToAdd = 0.0f;

                    Vector2 dirToNode = new Vector2(MathHelper.Clamp(neighbours[i].position.X - node.position.X, -1, 1),
                        MathHelper.Clamp(neighbours[i].position.Y - node.position.Y, -1, 1));

                    if (dirToNode.X != 0 && dirToNode.Y != 0)
                        costToAdd = D_COST;
                    else
                        costToAdd = HV_COST;

                   float newCost = nodes.Get(node.position.X, node.position.Y).cost + costToAdd;

                   if (newCost < neighbours[i].cost)
                   {
                       neighbours[i].cost = newCost;
                       neighbours[i].parent = node;
                   }
                }
            }
        }
        /*
         * public functions
         */
        /// <summary>
        /// A*, modified to find a nearest path in case of failure
        /// </summary>
        /// <param name="map">The Map</param>
        /// <param name="startNode">The starting Node</param>
        /// <param name="endNode">The ending Node</param>
        /// <returns>The path as a list of waypoints</returns>
        public static List<Node> findPath(NodeMap map, Node startNode, Node endNode)
        {
            /*
             * Terminology
             *		Gscore			cumulative calculated distance from the start Node to the given Node
             *		Hscore			estimated distance from the given Node to the end Node.
             *							Overestimating this can result in the calculation of an incorrect (inefficient) path,
             *							but the more it is underestimated, the longer correct path calculation will take
             *		Fscore			Gscore + Hscore; estimated total distance from start to end on a path through the given Node.
             *							Priority queues (PQueues) are ordered by ascending Fscore, so shortest estimated paths are examined first
             *		open list		A PQueue of Nodes to be examined.  Initially contains the start Node
             *		closed list		A List<Node> of Nodes that have been examined
             *		adjacent list	A PQueue of Nodes adjacent to the current Node
             */

            // initialize the lists
            PQueue open = new PQueue();
            open.enqueue(startNode);
            List<Node> closed = new List<Node>();
            PQueue adjacentNodes = new PQueue();
            iterations = 0;

            // good ol' A*
            while (open.Count > 0)											    // iterate until we have examined every appropriate Node
            {
                //open.print("Open", true);
                Node currentNode = open.dequeue();								    // look at the Node with the lowest Fscore and remove it from the open list
                if (currentNode == endNode)										    // if this is our destination Node, we're done!
                    return reconstruct(endNode);									    // so return the path
                closed.Add(currentNode);										    // otherwise, close this Node so we don't travel to it again
                adjacentNodes = getAdjacentNodes(map, open, closed, currentNode);	// now find every valid Node adjacent to the current Node
                //adjacentNodes.print("Adjacent", false);
                while (adjacentNodes.Count != 0)                                  // iterate over all of them, from lowest to highest Fscore
                {
                    Node adjacentNode = adjacentNodes.dequeue();									    // grab the current adjacent Node
                    int tempGScore = currentNode.Gscore + map.pathDistance(currentNode, adjacentNode);	// calculate a temporary Gscore as if we were traveling to this Node from the current Node
                    if (!open.contains(adjacentNode) || tempGScore < adjacentNode.Gscore)			    // if this Node has not been added to the open list, or if tempGscore is less than the Node's current Gscore
                    {
                        int h = map.pathDistance(adjacentNode, endNode);									// estimate the Node's Hscore
                        adjacentNode.prev = currentNode;												    // set the Node's prev pointer to the current Node
                        adjacentNode.Hscore = h;														    // set the Node's Hscore
                        adjacentNode.Gscore = tempGScore;												    // set the Node's Gscore
                        adjacentNode.Fscore = tempGScore + h;											    // set the Node's Fscore
                    }
                    if (!open.contains(adjacentNode))												    // if the adjacent Node we just examined is not yet on the open list, add it
                        open.enqueue(adjacentNode);
                }
                iterations++;
            }

            // no valid path exists, so find the nearest path
            closed.RemoveAt(0);										// remove the start Node from the closed List
            if (closed.Count > 0)									// if there are still Nodes on the closed list
            {
                Node nearestNode = closed[0];							// find the closed Node with the lowest Hscore;
                for (int i = 1; i < closed.Count; i++)					// this should be the Node closest to the desired destination,
                {														// so return a path ending with that Node.
                    if (closed[i].Hscore < nearestNode.Hscore)
                        nearestNode = closed[i];
                }
                return reconstruct(nearestNode);
            }
            else
            {
                List<Node> path = new List<Node>();					// otherwise, our only path was the start Node (i.e. we are completely trapped);
                path.Add(endNode);									// so return a path with just that Node.
                return path;
            }
        }
Example #23
0
        /// <summary>
        /// Builds a new path between two given locations.
        /// </summary>
        /// <param name="startPos">The starting position.</param>
        /// <param name="targetPos">The target position.</param>
        /// <param name="testMode">Whether or not the algorithm is being run in testing mode, or if it is live within a map visualization.</param>
        public override void Build(Coord2 startPos, Coord2 targetPos, bool testMode = false)
        {
            path = new List<Coord2>();
            nodes = new NodeCollection(GridSize);

            searchedNodes = 0;

            this.start = nodes.Get(startPos);
            this.target = nodes.Get(targetPos);

            // Initialize bot position
            nodes.Get(startPos).cost = 0;
            bool firstLoop = true;

            while (!target.closed)
            {
                if (firstLoop)
                {
                    currentLowest = start;
                    firstLoop = false;
                }
                else
                    FindLowestCost(); // Find lowest cost

                // Mark lowest cost as closed
                currentLowest.closed = true;
                map.SetRenderColor(currentLowest.position, CLOSED_COLOR);

                // Find the neigbour positions
                List<Node> neighbours = GetNeighours(currentLowest);

                // Update visualization
                UpdateVisualization(neighbours);

                // Recalculate Costs
                RecalculateCosts(neighbours, currentLowest);

                // Update number of searched nodes
                searchedNodes++;
            }

            // Trace the completed path, if target has been found
            if(target.parent != null)
                TracePath();
        }
        /*
         * helper functions
         */
        /// <summary>
        /// Puts all valid Nodes adjacent to the given Node in a PQueue and returns it
        /// </summary>
        /// <param name="map">The Map</param>
        /// <param name="closed">The closed list</param>
        /// <param name="currentNode">The current (center) Node</param>
        /// <returns>A PQueue of all traversable adjacent Nodes</returns>
        private static PQueue getAdjacentNodes(NodeMap map, PQueue open, List<Node> closed, Node currentNode)
        {
            int x = currentNode.Xcoord;
            int y = currentNode.Ycoord;
            List<Node> immediate = new List<Node>();
            List<Node> diagonal = new List<Node>();
            PQueue adjacentNodes = new PQueue();

            // grab all adjacent Nodes (or null values) and store them here
            Node[,] temp = map.getNodes(x - 1, y - 1, 3, 3);

            // iterate over all adjacent Nodes; add the ones that are open and in bounds to the appropriate List<Node>
            for (int j = 0; j < 3; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    if (temp[i, j] != null && !closed.Contains(temp[i, j]))
                    {
                        // if the Node is horizontally or vertically adjacent,
                        // add the Node to the list of immediately adjacent Nodes
                        if (Math.Abs(2 - i - j) == 1)
                            immediate.Add(temp[i, j]);

                        // otherwise, if the Node is valid, add it to the list of diagonally adjacent Nodes
                        else if (temp[i, j].isValid)
                            diagonal.Add(temp[i, j]);
                    }
                }
            }

            // iterate over all immediately adjacent Nodes.  If they are valid, enqueue them;
            // otherwise, remove the neighboring diagonally adjacent Nodes from the diagonal List
            for (int i = 0; i < immediate.Count(); i++)
            {
                if (!immediate[i].isValid)
                {
                    Node one, two = null;
                    if (immediate[i].Xcoord == x)   // the Node is vertically adjacent
                    {
                        one = map.getNode(x + 1, immediate[i].Ycoord);
                        two = map.getNode(x - 1, immediate[i].Ycoord);
                    }
                    else                            // the Node is horizontally adjacent
                    {
                        one = map.getNode(immediate[i].Xcoord, y - 1);
                        two = map.getNode(immediate[i].Xcoord, y + 1);
                    }
                    if (one != null)
                        diagonal.Remove(one);
                    if (two != null)
                        diagonal.Remove(two);
                }
                else {
                    adjacentNodes.enqueue(immediate[i]);
                }
            }

            // enqueue all remaining diagonally adjacent Nodes
            for (int i = 0; i < diagonal.Count(); i++)
                adjacentNodes.enqueue(diagonal[i]);

            // return the finished PQueue
            return adjacentNodes;
        }
 /// <summary>
 /// Returns true if the PQueue contains the given Node, false otherwise
 /// </summary>
 /// <param name="Node">The Node to check for</param>
 /// <returns>True if the PQueue contains the given Node, false otherwise</returns>
 public bool contains(Node node)
 {
     return pq.Contains(node);
 }
        /// <summary>
        /// Calculates the minimum path-based distance between two Nodes
        /// </summary>
        /// <param name="one">The first Node</param>
        /// <param name="two">The second Node</param>
        /// <returns>A rough integer distance between the two; 10 per vertical/horizontal move, 14 per vertical move</returns>
        public int pathDistance(Node one, Node two)
        {
            /*
             * Theory
             *		This function is used to calculate the path-based distance between two Nodes;
             *		i.e., the shortest possible path you can take from point A to point B
             *		when you can only move at 45-degree and 90-degree angles.
             *		The diagonal distance is multiplied by 14 instead of the square root of 2 (~1.41) to avoid using floating-point numbers;
             *		the straight distance is multiplied by 10 to compensate.
             */

            int x = Math.Abs(one.Xcoord - two.Xcoord);
            int y = Math.Abs(one.Ycoord - two.Ycoord);
            int diagonal = Math.Min(x, y);
            int straight = Math.Max(x, y) - diagonal;
            return (10 * straight) + (14 * diagonal);
        }
 /// <summary>
 /// A wrapper function for path reconstruction
 /// </summary>
 /// <param name="endNode">The destination Node</param>
 /// <returns>The path as a list of waypoints</returns>
 private static List<Node> reconstruct(Node endNode)
 {
     List<Node> path = new List<Node>();
     return reconstruct(endNode, path);
 }
Example #28
0
        //функция обрабатывющая "соседнюю" клетку
        private void nbPoint(Point nb, Node current, Point end, int X, int Y)
        {
            //различные проверки клетки - границы массива,закрытый список итд.
            if (nb.X >= X || nb.X < 0)
                return;
            if (nb.Y >= Y || nb.Y < 0)
                return;
            if (existsArray[nb.X, nb.Y] == 2)
                return;
            if (!tissue.IsInMap(nb.X, nb.Y))
                return;
            if( tissue[nb.X,nb.Y].AreaType == VG.Map.AreaEnum.Bone )
                return;

            //если нет в открытом списке - добавляем
            if(existsArray[nb.X,nb.Y] !=1 )
            {
                Node n = new Node(nb, 0, 0, 0);
                n.H = calcH(nb, end);
                n.G = calcG(n);

                n.F = n.G + n.H;
                n.Parent = current.P;
                openList.Add(n);
                existsArray[n.P.X, n.P.Y] = 1;
            }
            else //если есть - страдаем некоторой фигней =)
            {
                int j = inList(nb, openList);
                Node n = openList[j];
                if (n.G > calcG(n))
                {
                    n.Parent = current.P;
                    n.G = calcG(n);
                    n.F = n.G + n.H;
                    openList.RemoveAt(j);
                    openList.Add(n);
                }
            }
        }
        /// <summary>
        /// Actual path reconstruction
        /// </summary>
        /// <param name="endNode">The ending Node</param>
        /// <param name="path">The (initially blank) path</param>
        /// <returns>The path as a list of waypoints</returns>
        private static List<Node> reconstruct(Node endNode, List<Node> path)
        {
            // recurse if necessary
            if (endNode.prev != null)
                path = reconstruct(endNode.prev, path);

            // if we are moving in the same direction we moved in last time,
            // there is no need to keep the previous Node in the path, so remove it
            if (path.Count >= 2)
            {
                int i = path.Count - 1;
                int prevX = path[i].Xcoord - path[i - 1].Xcoord;
                int prevY = path[i].Ycoord - path[i - 1].Ycoord;
                int curX = endNode.Xcoord - path[i].Xcoord;
                int curY = endNode.Ycoord - path[i].Ycoord;
                if (theta(prevX, prevY) == theta(curX, curY))
                    path.RemoveAt(i);
            }

            // add the current Node and return
            path.Add(endNode);
            return path;
        }
 /// <summary>
 /// Sets a Cell's "adjacent stats."  Called whenever an adjacent Cell is added to the Open structure or otherwise updated.
 /// </summary>
 /// <param name="adjacent">The adjacent Cell to set/reset</param>
 /// <param name="current">The current Cell we are adjacent to</param>
 /// <param name="gScore">The adjacent Cell's calculated gScore</param>
 /// <param name="hScore">The adjacent Cell's calculated hScore</param>
 private static void setAdjacentStats(Node adjacent, Node current, int gScore, int hScore)
 {
     adjacent.prev = current;					// set the Cell's prev pointer to the current Cell
     adjacent.Hscore = hScore;					// set the Cell's Hscore
     adjacent.Gscore = gScore;					// set the Cell's Gscore
     adjacent.Fscore = gScore + hScore;			// set the Cell's Fscore
 }