How can I speed up my A* Pathfinding?

Question

My path-finding works fine when moving with no obstacles or when moving from the top to the side of an obstacle but when it needs to find it's way from the top to the bottom of an obstacle it is too slow.

I'm fairly sure it's the way I am sorting each loop or looping though the closed set but I'm not sure how to avoid this as SortedLists are not available on windows phone 7

//This Vivid Destination means it doesn't have to find the exact location 
//which is helpful as it is continuous environment  
Rectangle vividDestination = new Rectangle((int)character.destination.X - 10, (int)character.destination.Y - 10, 20, 20);

while (!vividDestination.Contains(Maths.vectorToPoint(OpenNodes[0].position)))
{
    Node Current = OpenNodes[0];
    OpenNodes.RemoveAt(0);
    ClosedNodes.Add(Current);
    Current.calculateNeightbours();

    foreach (Node neighbour in Current.neighbours)
    {
        neighbour.parents = Current.parents + 1;
        //The cost of the node is the amount of parent nodes + the distance to destination
        neighbour.cost = calculateCost(neighbour.position, character.destination, neighbour.parents);

        for (int i = 0; i < OpenNodes.Count; i++)
        {
            //Round Vector rounds the floats in the Vector to the nearest integer
            if (Maths.roundVector(OpenNodes[i].position) == Maths.roundVector(neighbour.position) && OpenNodes[i].parents < neighbour.parents)
            {
                break;
            }
            else
            {
                OpenNodes.RemoveAt(i);
                OpenNodes.Add(neighbour);
                i--;
                break;
            }
        }

        bool closedNode = false;
        for (int i = 0; i < ClosedNodes.Count; i++)
        {
            if (Maths.roundVector(ClosedNodes[i].position) == Maths.roundVector(neighbour.position))
            {
                closedNode = true;
                break;
            }
        }

        if (!closedNode)
        {
            OpenNodes.Add(neighbour);
        }
    }

    OpenNodes = OpenNodes.OrderBy(item => item.cost).ToList();
}

Answer 1

What you are doing is horribly inefficient. The sorting of the list takes n*log(n) time, and you sort the list once for each vertex in the graph. This result in an algorithm that takes V^2*log(V) time, where V is the number of vertices. If you simply keep an unsorted list then you can extract the minimum in linear time by looping over all the items and keeping count of the lowest one so far. In this case, the time becomes V^2. This is only a tiny improvement. Of course, if you use a proper priority queue (like one based on a Binary Heap ) then the algorithm will run much, much faster still since then operations only cost log(n). Coding your own binary heap is not too difficult and it's something I would strongly recommend you do if the platform doesn't support one natively. In this case insertion and finding the minimum only take log(n) time, resulting in a E log V time (where E is the number of edges, which in a planar graph is proportional to V).