public void GetNeighbors(NodeAStar node, ArrayList neighbors)
    {
        Vector3 neighborPosition = node.position;
        int     neighborIndex    = GetGridIndex(neighborPosition);
        int     row    = GetRowOfIndex(neighborIndex);
        int     column = GetColumnOfIndex(neighborIndex);

        //Bottom
        int leftNodeRow    = row - 1;
        int leftNodeColumn = column;

        AssignNeighbor(leftNodeRow, leftNodeColumn, neighbors);

        //Top
        leftNodeRow    = row + 1;
        leftNodeColumn = column;
        AssignNeighbor(leftNodeRow, leftNodeColumn, neighbors);

        //Right
        leftNodeRow    = row;
        leftNodeColumn = column + 1;
        AssignNeighbor(leftNodeRow, leftNodeColumn, neighbors);

        //Left
        leftNodeRow    = row;
        leftNodeColumn = column - 1;
        AssignNeighbor(leftNodeRow, leftNodeColumn, neighbors);
    }
Example #2
0
    void CreateGrid()
    {
        grid = new NodeAStar[gridSizeX, gridSizeY];
        Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2f - Vector3.forward * gridWorldSize.y / 2f;

        for (int y = 0; y < gridSizeY; y++)
        {
            for (int x = 0; x < gridSizeX; x++)
            {
                // Determine height of terrain mesh at pos
                RaycastHit hit;
                Vector3    worldPoint = Vector3.zero;
                Ray        ray        = new Ray(new Vector3(worldBottomLeft.x + (x * nodeDiameter + nodeRadius), 150, worldBottomLeft.z + (y * nodeDiameter + nodeRadius)), Vector3.down);
                if (Physics.Raycast(ray, out hit, 200))
                {
                    worldPoint = hit.point;
                }
                else
                {
                    Debug.LogError("NO TERRAIN FOUND IN RAYCAST");
                }

                bool walkable = !Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask);
                grid[x, y] = new NodeAStar(walkable, worldPoint, x, y);
            }
        }
    }
Example #3
0
 public NodeAStar()
 {
     hCost     = 0.0f;
     gCost     = 1.0f;
     bObstacle = false;
     parent    = null;
 }
Example #4
0
 public NodeAStar(Vector3 pos)
 {
     hCost     = 0.0f;
     gCost     = 1.0f;
     bObstacle = false;
     parent    = null;
     position  = pos;
 }
Example #5
0
 public NodeAStar(GraphPoint <NaviPoint> point, NodeAStar father)
 {
     this.point  = point;
     this.father = father;
     this.G      = 0;
     this.H      = 0;
     this.F      = 0;
 }
Example #6
0
    private void FindPath()
    {
        startPosition = startCube.transform;
        endPosition   = endCube.transform;

        startNode = new NodeAStar(gridManager.GetGridCellCenter(gridManager.GetGridIndex(startPosition.position)));
        goalNode  = new NodeAStar(gridManager.GetGridCellCenter(gridManager.GetGridIndex(endPosition.position)));

        pathArray = AStar.FindPath(startNode, goalNode);
    }
Example #7
0
    private static ArrayList CalculatePath(NodeAStar node)
    {
        ArrayList list = new ArrayList();

        while (node != null)
        {
            list.Add(node);
            node = node.parent;
        }
        list.Reverse();
        return(list);
    }
    // Check the neighbor. If it's not an obstacle, assign the neighbor.
    private void AssignNeighbor(int row, int column, ArrayList neighbors)
    {
        if (row != -1 && column != -1 && row < numberOfRows && column < numberOfColumns)
        {
            NodeAStar nodeToAdd = nodes[row, column];

            if (!nodeToAdd.bObstacle)
            {
                neighbors.Add(nodeToAdd);
            }
        }
    }
Example #9
0
    int GetDistance(NodeAStar nodeA, NodeAStar nodeB)
    {
        int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
        int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);

        if (dstX > dstY)
        {
            return(14 * dstY + 10 * (dstX - dstY));
        }
        else
        {
            return(14 * dstX + 10 * (dstY - dstX));
        }
    }
Example #10
0
    void RetracePath(NodeAStar startNode, NodeAStar endNode)
    {
        List <NodeAStar> path        = new List <NodeAStar>();
        NodeAStar        currentNode = endNode;

        while (currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parentNode;
        }
        path.Reverse();

        grid.path = path;
    }
Example #11
0
//IComparable Interface method implementation
    public int CompareTo(object obj)
    {
        NodeAStar node = (NodeAStar)obj;

        if (hCost < node.hCost)
        {
            return(-1);
        }

        if (hCost > node.hCost)
        {
            return(1);
        }

        return(0);
    }
    private void InitializeNodes()
    {
        nodes = new NodeAStar[numberOfColumns, numberOfRows];
        int index = 0;

        for (int i = 0; i < numberOfColumns; i++)
        {
            for (int j = 0; j < numberOfRows; j++)
            {
                Vector3   cellPosition = GetGridCellCenter(index);
                NodeAStar node         = new NodeAStar(cellPosition);
                nodes[i, j] = node;
                index++;
            }
        }
    }
Example #13
0
        private void SortInsert(List <NodeAStar> open, NodeAStar childNode)
        {
            int i = 0;

            while (i < open.Count && open[i].F > childNode.F)
            {
                i++;
            }
            if (i == open.Count)
            {
                open.Add(childNode);
            }
            else
            {
                open.Insert(i, childNode);
            }
        }
Example #14
0
    void FindPath(Vector3 startPos, Vector3 targetPos)
    {
        NodeAStar startNode  = grid.NodeFromWorldPoint(startPos);
        NodeAStar targetNode = grid.NodeFromWorldPoint(targetPos);

        Heap <NodeAStar>    openSet   = new Heap <NodeAStar>(grid.MaxSize);
        HashSet <NodeAStar> closedSet = new HashSet <NodeAStar>();

        openSet.Add(startNode);

        while (openSet.Count > 0)
        {
            NodeAStar currentNode = openSet.PopFirst();
            closedSet.Add(currentNode);

            if (currentNode == targetNode)
            {
                RetracePath(startNode, targetNode);
                return;
            }

            foreach (NodeAStar neighbourNode in grid.GetNeighbouringNodes(currentNode))
            {
                if (!neighbourNode.walkable || closedSet.Contains(neighbourNode))
                {
                    continue;
                }

                int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbourNode);
                if (newMovementCostToNeighbour < neighbourNode.gCost || !openSet.Contains(neighbourNode))
                {
                    neighbourNode.gCost      = newMovementCostToNeighbour;
                    neighbourNode.hCost      = GetDistance(neighbourNode, targetNode);
                    neighbourNode.parentNode = currentNode;

                    if (!openSet.Contains(neighbourNode))
                    {
                        openSet.Add(neighbourNode);
                    }
                }
            }
        }
    }
            public Int32 CompareTo(object obj)
            {
                if (!(obj is NodeAStar))
                {
                    throw new Exception("Bad NodeAStar comparison.");
                }

                NodeAStar compareWithMe = (NodeAStar)obj;
                Int32 returnValue = 0;
                float difference = this.estimatedTotalCost - compareWithMe.estimatedTotalCost;
                if (difference > 0)
                {
                    returnValue = 1;
                }
                else if (difference < 0)
                {
                    returnValue = -1;
                }

                return returnValue;
            }
Example #16
0
    private void OnDrawGizmos()
    {
        if (pathArray == null)
        {
            return;
        }

        if (pathArray.Count > 0)
        {
            int index = 1;
            foreach (NodeAStar node in pathArray)
            {
                if (index < pathArray.Count)
                {
                    NodeAStar nextNode = (NodeAStar)pathArray[index];
                    Debug.DrawLine(node.position, nextNode.position, Color.green);
                    index++;
                }
            }
            ;
        }
    }
Example #17
0
    private void OnDrawGizmos()
    {
        Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 100, gridWorldSize.y));

        if (onlyDisplayPathGizmos)
        {
            if (path != null)
            {
                foreach (NodeAStar node in path)
                {
                    Gizmos.color = Color.black;
                    Gizmos.DrawSphere(node.worldPosition, nodeRadius - .5f);
                }
            }
        }
        else
        {
            if (grid != null)
            {
                NodeAStar agentNode = NodeFromWorldPoint(testAgent.position);

                foreach (NodeAStar node in grid)
                {
                    Gizmos.color = (node.walkable) ? Color.white : Color.red;
                    if (path != null)
                    {
                        if (path.Contains(node))
                        {
                            Gizmos.color = Color.black;
                        }
                    }
                    Gizmos.DrawSphere(node.worldPosition, (nodeRadius - .5f));
                }
            }
        }
    }
Example #18
0
    public List <NodeAStar> GetNeighbouringNodes(NodeAStar node)
    {
        List <NodeAStar> neighbours = new List <NodeAStar>();

        for (int x = -1; x <= 1; x++)
        {
            for (int y = -1; y <= 1; y++)
            {
                if (x == 0 && y == 0)
                {
                    continue;
                }
                int checkX = node.gridX + x;
                int checkY = node.gridY + y;

                if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
                {
                    neighbours.Add(grid[checkX, checkY]);
                }
            }
        }

        return(neighbours);
    }
Example #19
0
 public void Push(NodeAStar node)
 {
     nodes.Add(node);
     nodes.Sort();
 }
Example #20
0
 private bool Contains(List <NodeAStar> list, GraphPoint <NaviPoint> graphPoint, out NodeAStar findNode)
 {
     if ((findNode = list.Find(new Predicate <NodeAStar>(
                                   delegate(NodeAStar node)
     {
         if (node.point == graphPoint)
         {
             return(true);
         }
         else
         {
             return(false);
         }
     }))) == null)
     {
         return(false);
     }
     else
     {
         return(true);
     }
 }
Example #21
0
 public void Remove(NodeAStar node)
 {
     nodes.Remove(node);
     nodes.Sort();
 }
Example #22
0
        /// <summary>
        /// 使用A*算法依据当前信息计算一条最短的路径。
        /// 注意,如果目标点在警戒线以内,会返回一条并非如你期望的路径。
        /// 所以请自行实现目标点无法到达时的处理逻辑。
        /// </summary>
        /// <param name="curPos"></param>
        /// <param name="aimPos"></param>
        /// <returns></returns>
        public NaviPoint[] CalPathUseAStar(Vector2 curPos, Vector2 aimPos)
        {
            #region  制导航图
            GraphPoint <NaviPoint>[] map  = new GraphPoint <NaviPoint> [Map.Length + 2];
            GraphPoint <NaviPoint>[] temp = GraphPoint <NaviPoint> .DepthCopy(Map);

            for (int i = 0; i < temp.Length; i++)
            {
                map[i] = temp[i];
            }
            #endregion

            #region 将当前点和目标点加入到导航图中
            int prePointSum = temp.Length;
            GraphPoint <NaviPoint> curNaviPoint = new GraphPoint <NaviPoint>(new NaviPoint(null, -1, curPos), new List <GraphPath <NaviPoint> >());
            GraphPoint <NaviPoint> aimNaviPoint = new GraphPoint <NaviPoint>(new NaviPoint(null, -1, aimPos), new List <GraphPath <NaviPoint> >());
            AddCurPosToNaviMap(map, curNaviPoint, prePointSum, GuardLines, BorderLines);
            AddAimPosToNaviMap(map, aimNaviPoint, curNaviPoint, prePointSum, GuardLines, BorderLines);

            #endregion

            #region 计算最短路径,使用A*算法

            List <NodeAStar> open  = new List <NodeAStar>();
            List <NodeAStar> close = new List <NodeAStar>();
            open.Add(new NodeAStar(curNaviPoint, null));

            NodeAStar cur = null;
            while (open.Count != 0)
            {
                cur = open[open.Count - 1];

                if (cur.point == aimNaviPoint)
                {
                    break;
                }

                open.RemoveAt(open.Count - 1);
                close.Add(cur);

                foreach (GraphPath <NaviPoint> path in cur.point.neighbors)
                {
                    if (Contains(close, path.neighbor))
                    {
                        continue;
                    }
                    else
                    {
                        NodeAStar inOpenNode;
                        if (Contains(open, path.neighbor, out inOpenNode))
                        {
                            float G = cur.G + path.weight;
                            if (inOpenNode.G > G)
                            {
                                inOpenNode.G = G;
                                inOpenNode.F = G + inOpenNode.H;
                            }
                        }
                        else
                        {
                            NodeAStar childNode = new NodeAStar(path.neighbor, cur);
                            childNode.G = cur.G + path.weight;
                            childNode.H = Vector2.Distance(aimPos, childNode.point.value.Pos);
                            childNode.F = childNode.G + childNode.H;
                            SortInsert(open, childNode);
                        }
                    }
                }
            }

            //if (cur == null)
            //    return null;

            Stack <NodeAStar> cahe = new Stack <NodeAStar>();
            while (cur.father != null)
            {
                cahe.Push(cur);
                cur = cur.father;
            }

            NaviPoint[] result = new NaviPoint[cahe.Count];

            int j = 0;
            foreach (NodeAStar node in cahe)
            {
                result[j] = node.point.value;
                j++;
            }

            return(result);

            #endregion
        }
        private List<Direction> _PathfinderAStar(Coords start, Coords endTopLeft, Coords endBottomRight, BitArray[] _passabilityMap, hFunction h)
        {
            // NOTE: Should later implemented a collision predictor mechanic to work in tandem
            // with the path-finder to provide better agent behavior.
            // NOTE: Consider returning the number of tiles scanned in case no path is found.
            // This will alert a boxed-in creature of its predicament.
            // NOTE: Introduce a flag for a straight-line initial check(for outdoors environmens and
            // for when the goal is near).

            Int32 rangeX = _passabilityMap.Length;
            Int32 rangeY = _passabilityMap[0].Count;

            NodeAStar?[,] nodeArray = new NodeAStar?[rangeX, rangeY];

            NodeAStar startNode = new NodeAStar();
            startNode.costSoFar = 0;
            startNode.estimatedTotalCost = h(start);

            nodeArray[start.X, start.Y] = startNode;

            List<Coords> ListOpen = new List<Coords>();
            ListOpen.Add(start);
            while (ListOpen.Count > 0)
            {
                // I have to use this bool the way I've implemented the algo. Consider rewriting.
                bool resortList = false;

                Coords currentCoords = ListOpen.First();
                // Check to see if goal is reached.
                //if (currentCoords.Equals(endTopLeft))
                if (StaticMathFunctions.CoordinateIsInBox(currentCoords, endTopLeft, endBottomRight))
                {
                    break;
                }

                NodeAStar currentNode = nodeArray[currentCoords.X, currentCoords.Y].Value;
                for (byte i = 0; i <= 3; ++i)
                {
                    Direction currentDir = (Direction)(2 * i + 1);
                    Coords dirCoords = StaticMathFunctions.DirectionToCoords(currentDir);
                    Coords potential = currentCoords + dirCoords;
                    // check if move in dir is allowed
                    if (potential.X >= 0 && potential.X < rangeX && potential.Y >= 0 && potential.Y < rangeY // bounds check
                        && _passabilityMap[potential.X][potential.Y]) // passability check
                    {
                        // Using the simplest cost function possible. Can be easily updated
                        // once tile walkability coefficients are added.
                        Coords newNodePosition = new Coords(CoordsType.General, currentCoords.X + dirCoords.X, currentCoords.Y + dirCoords.Y);
                        float accruedCost = currentNode.costSoFar + Constants.MovementCost[(byte)currentDir];

                        // Straight line correction
                        if (currentDir == nodeArray[currentCoords.X, currentCoords.Y].Value.connection)
                        {
                            accruedCost -= Constants.PathfinderStraightPathCorrection;
                        }

                        // Check to see if the node under examination is in the closed list.
                        //NodeAStar? oldNode = nodeArray[newNodePosition.X, newNodePosition.Y];
                        if (nodeArray[newNodePosition.X, newNodePosition.Y] != null)
                        {
                            // If node is in closed list, see if it needs updating.
                            if (nodeArray[newNodePosition.X, newNodePosition.Y].Value.costSoFar > accruedCost)
                            {
                                float expectedAdditionalCost =
                                    nodeArray[newNodePosition.X, newNodePosition.Y].Value.estimatedTotalCost -
                                    nodeArray[newNodePosition.X, newNodePosition.Y].Value.costSoFar;
                                NodeAStar nodeToAdd =
                                    new NodeAStar(currentDir, accruedCost, accruedCost + expectedAdditionalCost);
                                nodeArray[newNodePosition.X, newNodePosition.Y] = nodeToAdd;
                                ListOpen.Add(newNodePosition);
                                resortList = true;
                            }
                        }
                        // Node is in open list. Process it.
                        else
                        {
                            float expectedAdditionalCost = h(newNodePosition);
                            NodeAStar nodeToAdd =
                                new NodeAStar(currentDir, accruedCost, accruedCost + expectedAdditionalCost);
                            nodeArray[newNodePosition.X, newNodePosition.Y] = nodeToAdd;
                            ListOpen.Add(newNodePosition);
                            resortList = true;
                        }
                    }
                }

                ListOpen.RemoveAt(0);
                if (resortList)
                {
                    ListOpen.Sort(
                        delegate(Coords c1, Coords c2)
                        {
                            float difference = nodeArray[c1.X, c1.Y].Value.estimatedTotalCost -
                                nodeArray[c2.X, c2.Y].Value.estimatedTotalCost;

                            Int32 returnValue = 0;
                            if (difference > 0)
                            {
                                returnValue = 1;
                            }
                            else if (difference < 0)
                            {
                                returnValue = -1;
                            }
                            return returnValue;
                        }
                    );
                }
            }

            List<Direction> ListRoute = new List<Direction>();

            // Return empty route if the open list is empty, i.e. there is no path to the target
            // Ideally, the game logic should be fixed so that the search isn't even attempted
            // if there is no path between the two points.
            if (ListOpen.Count == 0)
            {
                return ListRoute;
            }

            Coords trackbackCoords = endTopLeft;
            while (trackbackCoords != start)
            {
                Direction newDirection = nodeArray[trackbackCoords.X, trackbackCoords.Y].Value.connection;
                ListRoute.Add(newDirection);
                trackbackCoords = StaticMathFunctions.CoordsNeighboringInDirection(new Coords(CoordsType.Tile, trackbackCoords),
                    StaticMathFunctions.OppositeDirection(newDirection));
            }

            // Might be faster without reversing
            //ListRoute.Reverse();

            // We skip the reversal, so pick directions from the END of the list.
            return ListRoute;
        }
Example #24
0
    public int Act(ref WindjermanGameState gs, NativeList <int> availableActions)
    {
        //déterminer la distance entre le joueur et le frisbee au moment T
        currentDistanceFromFrisbee = CalculerCurrentDistanceFromFrisbee(ref gs);
        FindHighGround(ref gs);

        //création de la liste des nodes
        var listeNodes = new NativeList <NodeAStar>(10, Allocator.Temp);

        //pour chaque action disponible
        for (int i = 0; i < availableActions.Length; i++)
        {
            //on crée un node
            NodeAStar n = new NodeAStar();
            n.distanceFromFrisbee = 0;
            n.playerID            = this.playerID;
            n.gsNode = Rules.Clone(ref gs);

            //on execute ladite action pour avoir le gamestate T+1 associé
            if (playerID == 0)
            {
                Rules.Step(ref n.gsNode, availableActions[i], 0);
            }
            else
            {
                Rules.Step(ref n.gsNode, 0, availableActions[i]);
            }

            //déterminer si le joueur a le frisbee à T+1
            if (playerID == 0)
            {
                if (n.gsNode.isFreeze1)
                {
                    n.hasFrisbee = true;
                }
                else
                {
                    n.hasFrisbee = false;
                }
            }
            else
            {
                if (n.gsNode.isFreeze2)
                {
                    n.hasFrisbee = true;
                }
                else
                {
                    n.hasFrisbee = false;
                }
            }

            //déterminer la distance entre le joueur et le frisbee si celui-ci ne l'a pas en main à T+1
            if (!n.hasFrisbee)
            {
                n.CalculerDistanceFromFrisbee();
            }

            //si le joueur a le frisbee, déterminer sa situation par rapport à l'autre joueur
            else
            {
                n.FindHighGround();
            }

            //ajouter le node à la liste
            listeNodes.Add(n);
        }

        //une fois les nodes créés, on détermine l'action à réaliser
        int action = FindBestAction(ref listeNodes, ref gs, ref availableActions);

        //listeNodes.Dispose();
        return(action);
    }
Example #25
0
    // Find the path between start node and goal node using A* Algorithm
    public static ArrayList FindPath(NodeAStar start, NodeAStar goal)
    {
        openList = new PriorityQueue();
        openList.Push(start);
        start.gCost = 0.0f;
        start.hCost = EstimateHeuristicCost(start, goal);

        closedList = new PriorityQueue();
        NodeAStar node = null;

        GridManager gridManager = GameObject.FindObjectOfType <GridManager>();

        if (gridManager == null)
        {
            return(null);
        }

        while (openList.Length != 0)
        {
            node = openList.GetFirstNode();

            if (node.position == goal.position)
            {
                return(CalculatePath(node));
            }

            ArrayList neighbors = new ArrayList();
            gridManager.GetNeighbors(node, neighbors);

            //Update the costs of each neighbor node.
            for (int i = 0; i < neighbors.Count; i++)
            {
                NodeAStar neighborNode = (NodeAStar)neighbors[i];

                if (!closedList.Contains(neighborNode))
                {
                    //Cost from current node to this neighbor node
                    float cost = EstimateHeuristicCost(node, neighborNode);

                    //Total Cost So Far from start to this neighbor node
                    float totalCost = node.gCost + cost;

                    //Estimated cost for neighbor node to the goal
                    float neighborNodeEstCost = EstimateHeuristicCost(neighborNode, goal);

                    //Assign neighbor node properties
                    neighborNode.gCost  = totalCost;
                    neighborNode.parent = node;
                    neighborNode.hCost  = totalCost + neighborNodeEstCost;

                    //Add the neighbor node to the open list if we haven't already done so.
                    if (!openList.Contains(neighborNode))
                    {
                        openList.Push(neighborNode);
                    }
                }
            }
            closedList.Push(node);
            openList.Remove(node);
        }

        //We handle the scenario where no goal was found after looping thorugh the open list
        if (node.position != goal.position)
        {
            Debug.LogError("Goal Not Found");
            return(null);
        }

        //Calculate the path based on the final node
        return(CalculatePath(node));
    }
Example #26
0
    /// Calculate the estimated Heuristic cost to the goal
    private static float EstimateHeuristicCost(NodeAStar currentNode, NodeAStar goalNode)
    {
        Vector3 cost = currentNode.position - goalNode.position;

        return(cost.magnitude);
    }
        private List<Direction> _PathfinderAStar(Coords start, Coords endTopLeft, Coords endBottomRight, BitArray[] _passabilityMap, hFunction h)
        {
            // NOTE: Should later implemented a collision predictor mechanic to work in tandem
            // with the path-finder to provide better agent behavior.
            // NOTE: Consider returning the number of tiles scanned in case no path is found.
            // This will alert a boxed-in creature of its predicament.
            // NOTE: Introduce a flag for a straight-line initial check(for outdoors environmens and
            // for when the goal is near).

            Int32 rangeX = _passabilityMap.Length;
            Int32 rangeY = _passabilityMap[0].Count;

            NodeAStar?[,] nodeArray = new NodeAStar?[rangeX, rangeY];

            NodeAStar startNode = new NodeAStar();
            startNode.costSoFar = 0;
            startNode.estimatedTotalCost = h(start);

            nodeArray[start.X, start.Y] = startNode;

            List<Coords> ListOpen = new List<Coords>();
            ListOpen.Add(start);
            while (ListOpen.Count > 0)
            {
                // I have to use this bool the way I've implemented the algo. Consider rewriting.
                bool resortList = false;

                Coords currentCoords = ListOpen.First();
                // Check to see if goal is reached.
                //if (currentCoords.Equals(endTopLeft))
                if (StaticMathFunctions.CoordinateIsInBox(currentCoords, endTopLeft, endBottomRight))
                {
                    break;
                }

                NodeAStar currentNode = nodeArray[currentCoords.X, currentCoords.Y].Value;
                for (byte i = 0; i <= 5; ++i)
                {
                    Direction currentDir = (Direction)(i);
                    //Coords dirCoords = StaticMathFunctions.DirectionToCoords(currentDir);
                    Coords potential = StaticMathFunctions.CoordsNeighboringInDirection(currentCoords, currentDir);
                    // check if move in dir is allowed
                    if (potential.X >= 0 && potential.X < rangeX && potential.Y >= 0 && potential.Y < rangeY // bounds check
                        && _passabilityMap[potential.X][potential.Y]) // passability check
                    {
                        // Using the simplest cost function possible. Can be easily updated
                        // once tile walkability coefficients are added.
                        //Coords newNodePosition = new Coords(CoordsType.General, currentCoords.X + dirCoords.X, currentCoords.Y + dirCoords.Y);
                        Coords newNodePosition = potential;
                        float accruedCost = currentNode.costSoFar + 1;

                        // Straight line correction
                        if (currentDir == nodeArray[currentCoords.X, currentCoords.Y].Value.connection)
                        {
                            accruedCost -= Constants.PathfinderStraightPathCorrection;
                        }

                        // Check to see if the node under examination is in the closed list.
                        //NodeAStar? oldNode = nodeArray[newNodePosition.X, newNodePosition.Y];
                        if (nodeArray[newNodePosition.X, newNodePosition.Y] != null)
                        {
                            // If node is in closed list, see if it needs updating.
                            if (nodeArray[newNodePosition.X, newNodePosition.Y].Value.costSoFar > accruedCost)
                            {
                                float expectedAdditionalCost =
                                    nodeArray[newNodePosition.X, newNodePosition.Y].Value.estimatedTotalCost -
                                    nodeArray[newNodePosition.X, newNodePosition.Y].Value.costSoFar;
                                NodeAStar nodeToAdd =
                                    new NodeAStar(currentDir, accruedCost, accruedCost + expectedAdditionalCost);
                                nodeArray[newNodePosition.X, newNodePosition.Y] = nodeToAdd;
                                ListOpen.Add(newNodePosition);
                                resortList = true;
                            }
                        }
                        // Node is in open list. Process it.
                        else
                        {
                            float expectedAdditionalCost = h(newNodePosition);
                            NodeAStar nodeToAdd =
                                new NodeAStar(currentDir, accruedCost, accruedCost + expectedAdditionalCost);
                            nodeArray[newNodePosition.X, newNodePosition.Y] = nodeToAdd;
                            ListOpen.Add(newNodePosition);
                            resortList = true;
                        }
                    }
                }

                ListOpen.RemoveAt(0);
                if (resortList)
                {
                    ListOpen.Sort(
                        delegate(Coords c1, Coords c2)
                        {
                            float difference = nodeArray[c1.X, c1.Y].Value.estimatedTotalCost -
                                nodeArray[c2.X, c2.Y].Value.estimatedTotalCost;

                            Int32 returnValue = 0;
                            if (difference > 0)
                            {
                                returnValue = 1;
                            }
                            else if (difference < 0)
                            {
                                returnValue = -1;
                            }
                            return returnValue;
                        }
                    );
                }
            }

            List<Direction> ListRoute = new List<Direction>();

            // Return empty route if the open list is empty, i.e. there is no path to the target
            // Ideally, the game logic should be fixed so that the search isn't even attempted
            // if there is no path between the two points.
            if (ListOpen.Count == 0)
            {
                return ListRoute;
            }

            Coords trackbackCoords = endTopLeft;
            while (trackbackCoords != start)
            {
                Direction newDirection = nodeArray[trackbackCoords.X, trackbackCoords.Y].Value.connection;
                ListRoute.Add(newDirection);
                trackbackCoords = StaticMathFunctions.CoordsNeighboringInDirection(new Coords(CoordsType.Tile, trackbackCoords),
                    StaticMathFunctions.OppositeDirection(newDirection));
            }

            // Might be faster without reversing
            //ListRoute.Reverse();

            // We skip the reversal, so pick directions from the END of the list.
            return ListRoute;
        }
Example #28
0
 public NodeAStar ( GraphPoint<NaviPoint> point, NodeAStar father )
 {
     this.point = point;
     this.father = father;
     this.G = 0;
     this.H = 0;
     this.F = 0;
 }
Example #29
0
 private bool Contains ( List<NodeAStar> list, GraphPoint<NaviPoint> graphPoint, out NodeAStar findNode )
 {
     if ((findNode = list.Find( new Predicate<NodeAStar>(
         delegate( NodeAStar node )
         {
             if (node.point == graphPoint)
                 return true;
             else
                 return false;
         } ) )) == null)
         return false;
     else
         return true;
 }
Example #30
0
 private void SortInsert ( List<NodeAStar> open, NodeAStar childNode )
 {
     int i = 0;
     while (i < open.Count && open[i].F > childNode.F)
     {
         i++;
     }
     if (i == open.Count)
         open.Add( childNode );
     else
         open.Insert( i, childNode );
 }
Example #31
0
        /// <summary>
        /// 使用A*算法依据当前信息计算一条最短的路径。
        /// 注意,如果目标点在警戒线以内,会返回一条并非如你期望的路径。
        /// 所以请自行实现目标点无法到达时的处理逻辑。
        /// </summary>
        /// <param name="curPos"></param>
        /// <param name="aimPos"></param>
        /// <returns></returns>
        public NaviPoint[] CalPathUseAStar ( Vector2 curPos, Vector2 aimPos )
        {

            #region 复制导航图
            GraphPoint<NaviPoint>[] map = new GraphPoint<NaviPoint>[Map.Length + 2];
            GraphPoint<NaviPoint>[] temp = GraphPoint<NaviPoint>.DepthCopy( Map );
            for (int i = 0; i < temp.Length; i++)
            {
                map[i] = temp[i];
            }
            #endregion

            #region 将当前点和目标点加入到导航图中
            int prePointSum = temp.Length;
            GraphPoint<NaviPoint> curNaviPoint = new GraphPoint<NaviPoint>( new NaviPoint( null, -1, curPos ), new List<GraphPath<NaviPoint>>() );
            GraphPoint<NaviPoint> aimNaviPoint = new GraphPoint<NaviPoint>( new NaviPoint( null, -1, aimPos ), new List<GraphPath<NaviPoint>>() );
            AddCurPosToNaviMap( map, curNaviPoint, prePointSum, GuardLines, BorderLines );
            AddAimPosToNaviMap( map, aimNaviPoint, curNaviPoint, prePointSum, GuardLines, BorderLines );

            #endregion

            #region 计算最短路径,使用A*算法

            List<NodeAStar> open = new List<NodeAStar>();
            List<NodeAStar> close = new List<NodeAStar>();
            open.Add( new NodeAStar( curNaviPoint, null ) );

            NodeAStar cur = null;
            while (open.Count != 0)
            {
                cur = open[open.Count - 1];

                if (cur.point == aimNaviPoint)
                    break;

                open.RemoveAt( open.Count - 1 );
                close.Add( cur );

                foreach (GraphPath<NaviPoint> path in cur.point.neighbors)
                {
                    if (Contains( close, path.neighbor ))
                    {
                        continue;
                    }
                    else
                    {
                        NodeAStar inOpenNode;
                        if (Contains( open, path.neighbor, out inOpenNode ))
                        {
                            float G = cur.G + path.weight;
                            if (inOpenNode.G > G)
                            {
                                inOpenNode.G = G;
                                inOpenNode.F = G + inOpenNode.H;
                            }
                        }
                        else
                        {
                            NodeAStar childNode = new NodeAStar( path.neighbor, cur );
                            childNode.G = cur.G + path.weight;
                            childNode.H = Vector2.Distance( aimPos, childNode.point.value.Pos );
                            childNode.F = childNode.G + childNode.H;
                            SortInsert( open, childNode );
                        }
                    }
                }
            }

            //if (cur == null)
            //    return null;

            Stack<NodeAStar> cahe = new Stack<NodeAStar>();
            while (cur.father != null)
            {
                cahe.Push( cur );
                cur = cur.father;
            }

            NaviPoint[] result = new NaviPoint[cahe.Count];

            int j = 0;
            foreach (NodeAStar node in cahe)
            {
                result[j] = node.point.value;
                j++;
            }

            return result;

            #endregion
        }