Optimising A* pathfinding, runs very slow. Possible bugs(?) visual c++ - visual-c++

Question

Hi I'm having a few problems with my A* pathfinding algorithm. The algorithm does successfully execute, however in a debug environment it executes in about 10 seconds, in release it will still take 2-3 seconds. This speed is way too slow. I suspect this is either due to a bug in the code, or the fact it isn't well optimised.
The map that pathfinding is being used on is a 30*30 grid, with each square being 10 unites away from one another.
I have noticed when running the algorithm, that when the open and closed list are searched to see if a node already exists, the node already stored in one of the lists always has a lower cost, so there is no updating of nodes. Not sure if this is normal or not. Also, I am not sure if quicksort is a good sort to be using in this situation.
Here is the code:
The coords struture used as a node:
struct coords
{
int x;
int z;
coords* parent;
int cost;
int score;
};
The sort compare function:
bool decompare(coords* o1, coords* o2)
{
return (o1->score < o2->score);
}
The main pathfind loop:
while (!goalFound) //While goal has not been found
{
current = openList.front(); //Retrieve current state from the open list
openList.pop_front();
for (int count = 1; count < 5; count++)
{
if (!goalFound)
{
coords* possibleState = new (coords); //Allocate new possible state
found = false;
if (count == 1)
{
possibleState->x = current->x;
possibleState->z = current->z + 10; //North
}
else if (count == 2)
{
possibleState->x = current->x + 10; //East
possibleState->z = current->z;
}
else if (count == 3)
{
possibleState->x = current->x; //South
possibleState->z = current->z - 10;
}
else if (count == 4)
{
possibleState->x = current->x - 10; //West
possibleState->z = current->z;
}
if (possibleState->x >-1 && possibleState->x <291 && possibleState->z >-1 && possibleState->z < 291) //If possible state is in game boundary
{
possibleState->cost = current->cost + 10; //Add 10 to current state to get cost of new possible state
int a = (possibleState->x / 10) + (30 * (possibleState->z / 10)); //get index of map
if (map[a] != wallTest) //Test possible state is not inside a wall
{
p = openList.begin();
while (p != openList.end() && !found) //Search open list to see if possible state already exists
{
if (possibleState->x == (*p)->x && possibleState->z == (*p)->z) //Already exists
{
found = true;
if (!possibleState->cost >= (*p)->cost) //Test possible state has lower cost
{
(*p)->parent = current; //Update existing with attributes of possible state
a = abs((*p)->x - goalState->x);
b = abs((*p)->z - goalState->z);
(*p)->cost = possibleState->cost;
(*p)->score = (possibleState->cost) + ((a)+(b));
}
}
else
{
found = false; //Set not found
}
p++;
}
q = closedList.begin();
while (q != closedList.end())
{
if (possibleState->x == (*q)->x && possibleState->z == (*q)->z)
{
found = true;
int a = (*q)->cost;
if (possibleState->cost < a) //Test if on closed list
{
(*q)->parent = current;
a = abs((*q)->x - goalState->x);
b = abs((*q)->z - goalState->z);
(*q)->cost = possibleState->cost;
(*q)->score = (possibleState->cost) + ((a)+(b)); //If cost lower push onto open list
coords* newcoord;
newcoord->x = (*q)->x;
newcoord->z = (*q)->z;
newcoord->score = (*q)->score;
newcoord->cost = (*q)->cost;
openList.push_back(newcoord);
closedList.erase(q);
}
}
q++;
}
if (!found) //If not found on either list
{
possibleState->parent = current; //Push onto open list
a = abs((possibleState)->x / 10 - goalState->x / 10);
b = abs((possibleState)->z / 10 - goalState->z / 10);
(possibleState)->score = (possibleState->cost) + ((a)+(b));
openList.push_back(possibleState);
}
sort(openList.begin(), openList.end(), decompare); // Sort the open list by score
}
if (possibleState->x == goalState->x && possibleState->z == goalState->z) //if goal found
{
openList.push_back(possibleState);
node = possibleState;
goalFound = true;
while (node != 0)
{
wayPoints.push_back(*node);
node = node->parent;
wayCount = wayPoints.size() - 1;
}
}
}
}
}
closedList.push_back(current);
}
player->setWayPoints(wayPoints);
wayPoints.clear();
player->setMoved(2);
player->setPath(1);
openList.clear();
closedList.clear();
goalFound = false;
player->setNewPath(1);
return true;
}
else {
return false;
}
}
Are there any bugs that need to be sorted in this code that anyone can see? Or is it just important optimizations that need making? Thanks