Ejemplo n.º 1
0
        /// <summary>
        /// Creates a temp node connected to the local
        /// </summary>
        /// <returns></returns>
        private WaypointNode MakeTempNode(bool stillOnCurrentPath)
        {
            WaypointNode tempNode = new WaypointNode(Position);

            if (!stillOnCurrentPath)
            {
                tempNode.ConnectedNodes = currentPlatform.ConnectedWaypoints;
            }
            else
            {
                tempNode.ConnectedNodes.Add(currentPath.Peek());
            }

            return(tempNode);
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Select the next TravellingToNode State based on the node positions
        /// </summary>
        /// <param name="currentNode">The Node the character is at</param>
        /// <param name="nextNode">The next Node the character is going to</param>
        /// <returns>The TravellingToNode State</returns>
        private TravellingToNodeState SelectTravellingState(WaypointNode currentNode, WaypointNode nextNode)
        {
            //Reset jumped flag
            hasJumped = false;

            if (currentNode.ConnectedPlatform == nextNode.ConnectedPlatform)
            {
                return(TravellingToNodeState.SamePlatform);
            }
            else if (Math.Abs(nextNode.Position.Y - currentNode.Position.Y) > MinDistance)
            {
                return(TravellingToNodeState.DifferentPlatformVertical);
            }
            else
            {
                return(TravellingToNodeState.DifferentPlatformHorizontal);
            }
        }
Ejemplo n.º 3
0
        /// <summary>
        /// Draw whats needed this frame
        /// </summary>
        /// <param name="gameTime">Current Game Time</param>
        /// <param name="spriteBatch">Sprite Batch</param>
        public override void Draw(GameTime gameTime, SpriteBatch spriteBatch, GraphicsDevice graphicsDevice)
        {
            //Create waypoint texture
            if (waypointTex == null)
            {
                waypointTex = new Texture2D(graphicsDevice, 1, 1);
                waypointTex.SetData(new Color[] { Color.White });
            }

            if (CameraRef.IsInViewport(this))
            {
                spriteBatch.Draw(enemyTexture, Position, null, Color.White, Rotation.Z, Vector2.Zero, Scale, SpriteEffects.None, 0.0f);
            }
            else
            {
                //Draw an arrow pointing to our location
                Vector2 midpoint  = CameraRef.GetViewportMid();
                Vector2 direction = midpoint - Position;

                Vector2?offscreenAnchor = null;
                if (//Top of Viewport
                    (offscreenAnchor = PhysicsManager.FindLineIntersection(midpoint, Position, CameraRef.Position, CameraRef.Position + new Vector2(CameraRef.Viewport.X, 0))).HasValue ||
                    //Bottom of Viewport
                    (offscreenAnchor = PhysicsManager.FindLineIntersection(midpoint, Position, CameraRef.Position + new Vector2(0, CameraRef.Viewport.Y), CameraRef.Position + CameraRef.Viewport)).HasValue ||
                    //Right of Viewport
                    (offscreenAnchor = PhysicsManager.FindLineIntersection(midpoint, Position, CameraRef.Position + new Vector2(CameraRef.Viewport.X, 0), CameraRef.Position + CameraRef.Viewport)).HasValue ||
                    //Left of Viewport
                    (offscreenAnchor = PhysicsManager.FindLineIntersection(midpoint, Position, CameraRef.Position, CameraRef.Position + new Vector2(0, CameraRef.Viewport.Y))).HasValue)
                {
                    //Draw a line at the direction angle
                    Vector2 offscreenAngle = offscreenAnchor.Value - Position;
                    float   offscreenRot   = (float)Math.Atan2(offscreenAngle.Y, offscreenAngle.X);

                    offscreenAngle.Normalize();

                    //Draw indicator
                    spriteBatch.Draw(offscreenTex, offscreenAnchor.Value + (offscreenAngle * OffscreenIndicatorOffset), null, Color.White, offscreenRot, new Vector2(0, offscreenTex.Height / 2), new Vector2(1), Math.Abs(offscreenRot) > (Math.PI / 2) ? SpriteEffects.FlipVertically : SpriteEffects.None, 0.006f);
                }
            }

            //Debug Draw
            if (GameManager.DebugMode && currentPath != null && currentPath.Count > 0)
            {
                spriteBatch.DrawString(debugFont, CurrentMovingState.ToString(), new Vector2(Position.X, Position.Y - 25), Color.Red);

                //Draw his planned path in red
                WaypointNode prevNode = NextNode;
                spriteBatch.Draw(waypointTex, new Rectangle((int)prevNode.Position.X - 3, (int)prevNode.Position.Y - 3, 6, 6), Color.Yellow);
                foreach (WaypointNode node in currentPath)
                {
                    //Draw Waypoints
                    spriteBatch.Draw(waypointTex, new Rectangle((int)node.Position.X - 3, (int)node.Position.Y - 3, 6, 6), Color.Red);

                    Vector2 waypointLine = node.Position - prevNode.Position;
                    float   lineAngle    = (float)Math.Atan2(waypointLine.Y, waypointLine.X);

                    spriteBatch.Draw(waypointTex,
                                     new Rectangle((int)prevNode.Position.X, (int)prevNode.Position.Y, (int)waypointLine.Length(), 2),
                                     null,
                                     prevNode == NextNode ? Color.Yellow : Color.Red,
                                     lineAngle,
                                     new Vector2(0, 0),
                                     SpriteEffects.None,
                                     0);

                    prevNode = node;
                }
            }

            base.Draw(gameTime, spriteBatch, graphicsDevice);
        }
Ejemplo n.º 4
0
        /// <summary>
        /// Move Towards the next node based on the TravellingToNode State
        /// </summary>
        /// <param name="targetNode">The character's target node</param>
        private void MoveTowards(WaypointNode targetNode)
        {
            //Get distance data
            Vector2 deltaXY     = targetNode.Position - (new Vector2(Position.X + (BoxCollider.X / 2), Position.Y));
            Vector2 nodeDeltaXY = targetNode.Position - PreviousNode.Position;

            switch (CurrentMovingState)
            {
            case TravellingToNodeState.SamePlatform:
            {
                //Higher leniency means more velocity at the target node
                //Peek ahead to see whether we can afford to keep some momentum
                //Or whether we need to slow down completely
                float leniency = 1.5f;
                try
                {
                    WaypointNode peekNode = currentPath.Peek();
                    if (peekNode != null)
                    {
                        Vector2 peekDeltaXY = peekNode.Position - targetNode.Position;
                        if ((peekDeltaXY.X > 0 && deltaXY.X > 0) ||
                            (peekDeltaXY.X < 0 && deltaXY.X < 0))
                        {
                            leniency = 4.0f;
                        }
                    }
                }
                catch
                {
                    //Do Nothing
                }

                //Move in a straight line towards the target node, slowing down when near it
                double oppAcc           = CalculateAcceleration(new Vector2(-Force.X, Force.Y)).X;
                double timeToSlowdown   = -Velocity.X / (oppAcc * leniency);
                double distanceRequired = Math.Abs(Velocity.X * timeToSlowdown);

                //If we're within that distance
                if (Math.Abs(deltaXY.X) < distanceRequired)
                {
                    //Console.WriteLine("Slowing Down");
                    //Slowdown to a 0 vel
                    SlowdownHorizontal(deltaXY.X);
                }
                else
                {
                    //Move in direction
                    MoveHorizontal(deltaXY.X);
                }

                //Check if we're lost
                if (currentPlatform != targetNode.ConnectedPlatform)
                {
                    CurrentMovingState = TravellingToNodeState.Lost;
                }
                break;
            }

            case TravellingToNodeState.DifferentPlatformHorizontal:
            {
                //If we're below the target node, then we've fallen and are lost
                if ((currentPlatform.Position.Y < targetNode.ConnectedPlatform.Position.Y) ||
                    (hasJumped && OnGround && currentPlatform != targetNode.ConnectedPlatform))
                {
                    CurrentMovingState = TravellingToNodeState.Lost;
                    break;
                }
                else if (hasJumped && OnGround && currentPlatform == targetNode.ConnectedPlatform)
                {
                    //Change to Same-Platform traversal
                    WaypointNode tempNode = new WaypointNode(Position);
                    PreviousNode       = tempNode;
                    CurrentMovingState = TravellingToNodeState.SamePlatform;
                    break;
                }

                //Jump so we're in the air
                Jump();

                //Move in direction
                MoveHorizontal(deltaXY.X);
                break;
            }

            case TravellingToNodeState.DifferentPlatformVertical:
            {
                //If we've landed on the same platform as the node, switch to same platform
                if (currentPlatform == targetNode.ConnectedPlatform)
                {
                    CurrentState       = EnemyState.SelectingNode;
                    CurrentMovingState = TravellingToNodeState.SamePlatform;
                    break;
                }
                else if ((currentPlatform != targetNode.ConnectedPlatform &&
                          currentPlatform != PreviousNode.ConnectedPlatform) ||
                         (hasJumped && OnGround && currentPlatform != targetNode.ConnectedPlatform))
                {
                    //Lost
                    CurrentMovingState = TravellingToNodeState.Lost;
                    break;
                }

                //Calculate the wanted jump location
                float jumpXPos = targetNode.Position.X +
                                 ((Math.Abs(targetNode.Position.X - targetNode.ConnectedPlatform.Position.X) < Math.Abs(targetNode.Position.X - (targetNode.ConnectedPlatform.Position.X + targetNode.ConnectedPlatform.BoxCollider.X))) ? -MinNodeJumpX : MinNodeJumpX);
                float jumpPosDeltaX  = jumpXPos - (Position.X + (BoxCollider.X / 2));
                float prevNodeDeltaX = PreviousNode.Position.X - (Position.X + (BoxCollider.X / 2));

                //Are we on a moving platform?
                if (currentPlatform.PlatformType == Platform.PlatformTypes.DynamicMoving)
                {
                    //Move with the platform until we're at the jump
                    Vector2 platformVel = currentPlatform.Velocity;
                    //Navigate to the jumpPos
                    if (((nodeDeltaXY.X > MinNodeJumpX && nodeDeltaXY.X < MaxNodeJumpX && currentPlatform.Velocity.X < 0) ||
                         (nodeDeltaXY.X < MinNodeJumpX && nodeDeltaXY.X > MaxNodeJumpX && currentPlatform.Velocity.X > 0)) && !hasJumped)
                    {
                        //Needs to jump
                        Console.WriteLine($"Jump Delta: {jumpPosDeltaX} Node Delta: {nodeDeltaXY}");
                        Jump();
                    }
                    else if (!hasJumped)
                    {
                        //Hasnt jumped
                        Console.WriteLine($"Move with platform Delta: {prevNodeDeltaX} Node Delta: {nodeDeltaXY}");
                        MoveHorizontal(prevNodeDeltaX);
                    }
                    else
                    {
                        //Has jumped
                        Console.WriteLine($"In Air Delta: {deltaXY}");
                        Console.WriteLine(deltaXY);
                        MoveHorizontal(deltaXY.X);
                    }
                }
                //Are we jumping to a moving platform?
                else if (targetNode.ConnectedPlatform.PlatformType == Platform.PlatformTypes.DynamicMoving)
                {
                    //Wait until the platform is in range
                    if (((nodeDeltaXY.X > MinNodeJumpX && nodeDeltaXY.X < MaxNodeJumpX && targetNode.ConnectedPlatform.Velocity.X < 0) ||
                         (nodeDeltaXY.X < MinNodeJumpX && nodeDeltaXY.X > MaxNodeJumpX && targetNode.ConnectedPlatform.Velocity.X > 0)) && !hasJumped)
                    {
                        Jump();
                    }
                    else if (hasJumped)
                    {
                        MoveHorizontal(deltaXY.X);
                    }
                    else
                    {
                        MoveHorizontal(prevNodeDeltaX);
                    }
                }
                //Otherwise
                else
                {
                    //Navigate to the jumpPos
                    if (((jumpPosDeltaX > MinDistance && jumpPosDeltaX > 0) ||
                         (jumpPosDeltaX < -MinDistance && jumpPosDeltaX < 0)) && !hasJumped)
                    {
                        //Needs to jump
                        Jump();
                    }
                    else if (!hasJumped)
                    {
                        //Hasnt jumped
                        MoveHorizontal(jumpPosDeltaX);
                    }
                    else
                    {
                        //Has jumped
                        MoveHorizontal(deltaXY.X);
                    }
                }

                break;
            }

            case TravellingToNodeState.Lost:
            {
                //We're not where we should be...
                //Find a new path
                NextNode     = MakeTempNode(false);
                CurrentState = EnemyState.AtGoal;
                break;
            }
            }
        }
Ejemplo n.º 5
0
        /// <summary>
        /// Update Method for Enemy, Called once a frame
        /// </summary>
        /// <param name="gameTime">Current Game Time</param>
        public override void Update(GameTime gameTime)
        {
            //React based on current state
            switch (CurrentState)
            {
            case EnemyState.AtGoal:
            {
                //Select a new goal and populate the path
                WaypointNode goalNode = AI.GetNewGoalNode();
                if (goalNode != null)
                {
                    currentPath = AI.AStarSearch(NextNode, goalNode);
                }
                CurrentState = EnemyState.SelectingNode;

                break;
            }

            case EnemyState.SelectingNode:
            {
                //Select the next node in the list then begin travelling to it
                PreviousNode = NextNode;
                if (currentPath != null && currentPath.Count > 0)
                {
                    NextNode           = currentPath.Dequeue();
                    CurrentMovingState = SelectTravellingState(PreviousNode, NextNode);
                    CurrentState       = EnemyState.TravellingToNode;
                }
                else
                {
                    CurrentState = EnemyState.AtGoal;
                }

                break;
            }

            case EnemyState.TravellingToNode:
            {
                //Travel to the next node
                Vector2 dxy = NextNode.Position - Position;
                //Console.WriteLine($"Next Node: ({NextNode.Position.X}, {NextNode.Position.Y}) CurrentPos ({Position.X}, {Position.Y})");
                Rectangle tempRec = new Rectangle(Position.ToPoint() + new Point((int)BoxCollider.X / 4, 0), (BoxCollider / 2).ToPoint());
                if (tempRec.Contains(NextNode.Position) && OnGround)
                {
                    //Console.WriteLine("AT GOAL");
                    //If we have items left in the queue go to selecting node, if not we're at goal
                    CurrentState = currentPath.Count > 0 ? EnemyState.SelectingNode : EnemyState.AtGoal;
                }
                else
                {
                    MoveTowards(NextNode);
                }

                break;
            }
            }

            //Set OnGround to false
            OnGround = false;

            //Call RigidBody Update
            base.Update(gameTime);
        }
        /// <summary>
        /// Generate the Waypoints for the given platform row and connect them to the previous one at the given Y offset
        /// </summary>
        /// <param name="CurrentPlatformRow">An Array of the Current Plaform Row to generate Waypoints for</param>
        /// <param name="PreviousPlatformRow">An Array of the Previous Platform Row to connect the generated Waypoints to</param>
        /// <param name="waypointYOffset">The Y Offset to place the waypoints at</param>
        /// <returns>An array of WaypointNodes</returns>
        public WaypointNode[] GenerateWaypoints(Platform[] CurrentPlatformRow, Platform[] PreviousPlatformRow, float waypointYOffset)
        {
            //TODO:
            // Add safety incase either current row or previous row is null
            //Console.WriteLine("Waypoint");

            List <WaypointNode> waypointNodes = new List <WaypointNode>();

            float previousRowYDistance = PreviousPlatformRow[0].Position.Y - CurrentPlatformRow[0].Position.Y;

            //Add waypoints for the current platform row
            WaypointNode previousEdgeNode = new WaypointNode();

            for (int i = 0; i < CurrentPlatformRow.Length; i++)
            {
                //Generate a node at either side of each platform
                WaypointNode leftNode = new WaypointNode();
                leftNode.Position = new Vector2(CurrentPlatformRow[i].Position.X + WaypointEdgeBuffer,
                                                CurrentPlatformRow[i].Position.Y - waypointYOffset);
                leftNode.ConnectedPlatform = CurrentPlatformRow[i];

                WaypointNode rightNode = new WaypointNode();
                rightNode.Position = new Vector2(CurrentPlatformRow[i].Position.X + CurrentPlatformRow[i].Size.X - WaypointEdgeBuffer,
                                                 CurrentPlatformRow[i].Position.Y - waypointYOffset);
                rightNode.ConnectedPlatform = CurrentPlatformRow[i];

                //Connect them
                leftNode.ConnectedNodes.Add(rightNode);
                rightNode.ConnectedNodes.Add(leftNode);

                //Special case for a single platform on this row (Connect to the row below)
                if (CurrentPlatformRow.Length == 1)
                {
                    //Find closest waypoint on previous row and connect to it
                    WaypointNode closestToRightNode, closestToLeftNode;

                    //Find closest to Previous Edge Node
                    List <WaypointNode> sortedList = new List <WaypointNode>();
                    for (int j = 0; j < PreviousPlatformRow.Length; j++)
                    {
                        sortedList.AddRange(PreviousPlatformRow[j].ConnectedWaypoints);
                    }
                    //Sort by distance to right edge on X (to the right only)
                    sortedList.RemoveAll((n1) => n1.Position.X <= rightNode.Position.X);
                    if (sortedList.Count > 0)
                    {
                        sortedList.Sort((n1, n2) => (rightNode.Position.X - n1.Position.X).CompareTo(rightNode.Position.X - n2.Position.X));
                        closestToRightNode = sortedList[0];

                        if (Math.Abs((rightNode.Position - closestToRightNode.Position).Length()) < MaxConnectionLength)
                        {
                            rightNode.ConnectedNodes.Add(closestToRightNode);
                            closestToRightNode.ConnectedNodes.Add(rightNode);
                        }
                    }

                    //Find closest to left edge node
                    sortedList.Clear();
                    for (int j = 0; j < PreviousPlatformRow.Length; j++)
                    {
                        sortedList.AddRange(PreviousPlatformRow[j].ConnectedWaypoints);
                    }
                    //Sort by distance to previous edge on X (to the right only)
                    sortedList.RemoveAll((n1) => n1.Position.X >= leftNode.Position.X);
                    if (sortedList.Count > 0)
                    {
                        sortedList.Sort((n1, n2) => (n1.Position.X - leftNode.Position.X).CompareTo(n2.Position.X - leftNode.Position.X));
                        closestToLeftNode = sortedList[0];

                        if (Math.Abs((leftNode.Position - closestToLeftNode.Position).Length()) < MaxConnectionLength)
                        {
                            closestToLeftNode.ConnectedNodes.Add(leftNode);
                            leftNode.ConnectedNodes.Add(closestToLeftNode);
                        }
                    }
                }

                //Connect the platforms on each row together & to the row below it
                if (i > 0)
                {
                    previousEdgeNode.ConnectedNodes.Add(leftNode);
                    leftNode.ConnectedNodes.Add(previousEdgeNode);

                    //Add a waypoint in the middle of the two platforms on the row below to allow better jumping
                    Vector2 midPoint = (leftNode.Position + previousEdgeNode.Position) / 2;
                    midPoint = new Vector2(midPoint.X, midPoint.Y + previousRowYDistance);

                    //If it can find a platform at this point, put a waypoint there, if it can't then we asume it'll be able to
                    //connect via other means
                    try
                    {
                        Platform connectedPlatform = new List <Platform>(PreviousPlatformRow).Find((p) => midPoint.X >= p.Position.X && midPoint.X <= (p.Position.X + p.Size.X));

                        WaypointNode midNode = new WaypointNode();
                        midNode.Position          = midPoint;
                        midNode.ConnectedPlatform = connectedPlatform;

                        //Connect to all nearby nodes
                        midNode.ConnectedNodes.AddRange(connectedPlatform.ConnectedWaypoints);
                        for (int j = 0; j < connectedPlatform.ConnectedWaypoints.Count; j++)
                        {
                            connectedPlatform.ConnectedWaypoints[j].ConnectedNodes.Add(midNode);
                        }
                        midNode.ConnectedNodes.AddRange(new WaypointNode[] { previousEdgeNode, leftNode });
                        previousEdgeNode.ConnectedNodes.Add(midNode);
                        leftNode.ConnectedNodes.Add(midNode);
                        connectedPlatform.ConnectedWaypoints.Add(midNode);

                        //Add to our list
                        waypointNodes.Add(midNode);
                    }
                    catch
                    {
                        //Only do this if it's the first or last platform gap
                        //There's some weird behaviour if it tries to do it with the middle gaps
                        //When encountering certain platform compositions
                        //This is a bit of a hacky workaround

                        //With 4 platforms on the current row and 2 on the previous, the waypoint lines overlap
                        //the platforms, and not amount of intersection checks I am running can seem to detect this
                        //and remove it
                        //I suspect either due to the Monogame axis being inversed, or the scale of the drawing making
                        //it appear to overlap when it doesnt
                        //So in order to avoid this:

                        /*
                         *                 *
                         *  ____________            ____________
                         |__________|            |__________|
                         *
                         *                        *
                         * ___________                     ____________
                         * ___________|                    |___________
                         *
                         * It tries to connect the waypoints to the opposite top to allow you to jump up, however the waypoint line
                         * intersects with the platform, or it's so close it would ruin the AI but appears to intersect
                         * due to scale
                         */
                        //We do a hacky thing where it only makes paths up on the edges of the arena
                        if (i == 1 || i == CurrentPlatformRow.Length - 1)
                        {
                            //Find closest waypoint on previous row and connect to it
                            WaypointNode closestToPrevEdge, closestToLeftNode;

                            //Find closest to Previous Edge Node
                            List <WaypointNode> sortedList = new List <WaypointNode>();
                            for (int j = 0; j < PreviousPlatformRow.Length; j++)
                            {
                                sortedList.AddRange(PreviousPlatformRow[j].ConnectedWaypoints);
                            }
                            //Sort by distance to previous edge on X (to the right only)
                            sortedList.RemoveAll((n1) => n1.Position.X <= previousEdgeNode.ConnectedPlatform.Position.X + previousEdgeNode.ConnectedPlatform.BoxCollider.X);
                            if (sortedList.Count > 0)
                            {
                                sortedList.Sort((n1, n2) => (n1.Position.X - previousEdgeNode.Position.X).CompareTo(n2.Position.X - previousEdgeNode.Position.X));
                                closestToPrevEdge = sortedList[0];

                                previousEdgeNode.ConnectedNodes.Add(closestToPrevEdge);
                                closestToPrevEdge.ConnectedNodes.Add(previousEdgeNode);
                            }

                            //Find closest to left edge node
                            sortedList.Clear();
                            for (int j = 0; j < PreviousPlatformRow.Length; j++)
                            {
                                sortedList.AddRange(PreviousPlatformRow[j].ConnectedWaypoints);
                            }
                            //Sort by distance to previous edge on X (to the right only)
                            sortedList.RemoveAll((n1) => n1.Position.X >= leftNode.ConnectedPlatform.Position.X);
                            if (sortedList.Count > 0)
                            {
                                sortedList.Sort((n1, n2) => (leftNode.Position.X - n1.Position.X).CompareTo(leftNode.Position.X - n2.Position.X));
                                closestToLeftNode = sortedList[0];

                                //Connect to the visible waypoints on the area below
                                closestToLeftNode.ConnectedNodes.Add(leftNode);
                                leftNode.ConnectedNodes.Add(closestToLeftNode);
                            }
                        }
                    }
                }

                previousEdgeNode = rightNode;

                //Add to the waypoint nodes list & platform object
                waypointNodes.AddRange(new WaypointNode[] { leftNode, rightNode });
                CurrentPlatformRow[i].ConnectedWaypoints.AddRange(new WaypointNode[] { leftNode, rightNode });
            }

            return(waypointNodes.ToArray());
        }
Ejemplo n.º 7
0
        /// <summary>
        /// Uses A* to compute the shortest path from the start node to the goal node
        /// </summary>
        /// <returns>An ordered list of the path taken to reach the goal node</returns>
        public Queue <WaypointNode> AStarSearch(WaypointNode startNode, WaypointNode endNode)
        {
            List <WaypointNode> openList   = new List <WaypointNode>();
            List <WaypointNode> closedList = new List <WaypointNode>();

            //Make sure we have a goal
            if (endNode == null)
            {
                //Error - No Goal
                return(null);
            }

            //Add the start node to the open list
            openList.Add(startNode);
            WaypointNode currentNode = startNode;

            while (openList.Count > 0)
            {
                //Sort based on lowest heuristic
                openList.Sort((n1, n2) => (n1.H + n1.G).CompareTo((n2.H + n2.G)));

                //Get the next node
                currentNode = openList[0];
                closedList.Add(currentNode);
                openList.RemoveAt(0);

                //Check if we're at the goal node
                if (currentNode == endNode)
                {
                    //Break out of search and trace back
                    break;
                }

                //For each connected node
                for (int i = 0; i < currentNode.ConnectedNodes.Count; i++)
                {
                    //If it's on the closed list
                    if (closedList.Contains(currentNode.ConnectedNodes[i]))
                    {
                        //Ignore it
                        continue;
                    }
                    //If it's already in the open list, check to see if it's a better path
                    //and reassign the parent node
                    //If not, add it and assign parent node
                    else if (openList.Contains(currentNode.ConnectedNodes[i]))
                    {
                        float curF = currentNode.ConnectedNodes[i].ParentNode.G + currentNode.ConnectedNodes[i].ParentNode.H;
                        //If our F is less than it's current F, then we swap the parents over
                        if (currentNode.G + currentNode.H < curF)
                        {
                            int index = openList.FindIndex((n1) => n1 == currentNode.ConnectedNodes[i]);
                            openList[index].ParentNode = currentNode;
                            openList[index].CalculateG();
                        }
                        //Otherwise we ignore it
                    }
                    else
                    {
                        //Calculate Heuristics and add to open list
                        currentNode.ConnectedNodes[i].ParentNode = currentNode;
                        currentNode.ConnectedNodes[i].CalculateG();
                        currentNode.ConnectedNodes[i].CalculateH(endNode.Position);

                        openList.Add(currentNode.ConnectedNodes[i]);
                    }
                }
            }

            //If we found the end node
            if (endNode.ParentNode != null)
            {
                Queue <WaypointNode> path = new Queue <WaypointNode>();
                //Trace back through the end node to the start node
                WaypointNode pathNode = endNode;
                while (pathNode != null && pathNode != startNode)
                {
                    path.Enqueue(pathNode);
                    pathNode = pathNode.ParentNode;
                }
                //Reverse path & return
                path = new Queue <WaypointNode>(path.Reverse());
                return(path);
            }
            else
            {
                //Couldnt find a path
                return(null);
            }
        }