Many candidate tile scores tie at the same level

I am currently modifying my A* implementation to include Cooperative mechanisms.
While I start running the app, it begins to freeze after a few secs . When I probe from the debugger, i find the open List is filled with candidates (about 10 seconds with 50000 candidates), then i know something is going very wrong. I check on the scores of each one and find all of them tie at 7 Initially, I design the loop so that when the current tile be the destination, it will break out of the loop. Because all scores are the same, it just chooses the one with the smallest id from the sorted set. Because the smallest id one never be the destination, so the open list continues to stack up. I wonder is my algorithm has some major flaw in it. Here is the attached code:


 TimeAStar::Path* TimeAStar::findPath(Node& from, Node& to, CObjects* unit, int depth)
 {
 
 bool solved = false;
 Node current;
 
 openList.clear();
 if (useBitSet)
 bitSetClosedList.reset();
 else
 intSetClosedList.clear();
 
 int maxOpenSize = 0; int maxCloseSize = 0; int reachedDepth = 0;
 
 from.transition = NULL;
 from.h = from.getActualTimelessCost(to);
 // error correction here
 from.g = 0;
 from.f = from.h;
 from.depth = 0;
 
 openList.insert(from);
 
 
 while (!openList.empty())
 {
 current = *openList.begin();
 openList.erase(openList.begin());
 
 
 if (useBitSet)
 {
 bitSetClosedList.set(current.id);
 }
 else
 {
 intSetClosedList.insert(current.id);
 }
 
 if (current.depth > reachedDepth)
 {
 reachedDepth = current.depth;
 
 if (reachedDepth == depth)
 {
 solved = true;
 break;
 }
 }
 
 if (current.x == to.x && current.z == to.z)
 {
 solved = true;
 break;
 }
 
 
 for (int i = -1; i getCost(unit);
 if (cost < 0)
 continue;
 
 if (useBitSet)
 {
 if (bitSetClosedList.at(node.id))
 continue;
 }
 else
 {
 bool found = intSetClosedList.find(node.id) != intSetClosedList.end();
 if (found)
 continue;
 }
 
 bool contains = openList.find(node) != openList.end();
 if (contains)
 {
 if (current.g + cost < node.g)
 {
 if (!openList.erase(node))
 assert(false);
 
 node.transition = t;
 node.g = current.g + cost;
 node.f = node.g + node.h;
 node.depth = current.depth + 1;
 
 openList.insert(node);
 }
 }
 else
 {
 
 // thePath.push(*node);
 node.transition = t;
 node.g = current.g + cost;
 node.h = node.getActualTimelessCost(to);
 node.f = node.g + node.h;
 node.depth = current.depth + 1;
 openList.insert(node);
 
 
 }
 }
 }
 }
 if (solved)
 {
 float totalCost = current.g;
 
 
 std::vector transitions;
 while (current.transition != NULL)
 {
 transitions.push_back((Transition*)current.transition);
 current = current.transition->FromNode();
 }
 return new Path(from, transitions, totalCost);
 }
 
 }

Could anyone give some advices on how this could be improved?
Or do you spot any bugs in this code snippet? Please let me know
I am currently stuck.
Thanks
Jack

TimeAStar.h


 #include "NodePool.h"
 #include
 #include
 #include
 
 class CObjects;
 
 extern NodePool pool;
 
 class TimeAStar
 {
 struct Comparator
 {
 bool operator() (const TimeAStarNode& a, const TimeAStarNode& b)
 {
 if (a.f >= b.f)
 {
 return false;
 
 }
 if (a.f < b.f)
 {
 return true;
 
 }
 
 }
 };
 public:
 class Path
 {
 public:
 std::vector transitions; // both abstract and solid classes okay
 
 Node startNode; // can't instantiate TimeAStarNode
 
 float cost;
 
 Path(const Node& startNode, const std::vector& transitions, float cost)
 {
 this->startNode = startNode;
 this->cost = cost;
 this->transitions = transitions;
 }
 
 
 };
 TimeAStar()
 {
 useBitSet = false;
 }
 
 
 private:
 std::multiset<Node, TimeAStar::Comparator> openList;
 std::bitset<8192> bitSetClosedList;
 std::multiset<int> intSetClosedList;
 bool useBitSet;
 
 public:
 Path* findPath(Node& from, Node& to, CObjects* unit, int depth);
 
 };
 
 class Coordinater
 {
 TimeAStar timeastar;
 std::map units;
 std::vector newUnits;
 int currentWindow;
 int currentTime;
 std::map window;
 int depth;
 
 public:
 Coordinater(int depth)
 {
 SetDepth(depth);
 currentTime = 0;
 
 
 }
 
 void SetDepth(int depth)
 {
 this->depth = depth;
 distributeUnits();
 }
 
 void distributeUnits();
 
 void findPath(CObjects* unit);
 
 void putUnitInWindow (CObjects* unit, int position);
 
 
 void addUnit(CObjects *unit);
 
 
 void Reset();
 
 int getDepth();
 
 
 void printReservations();
 
 
 bool iterate();
 
 void clearReservations(CObjects *unit);
 
 
 void reserveEmptyPath(CObjects *unit);
 
 };

TimeAStar.cpp


 #include "stdafx.h"
 #include "TimeAStar.h"
 #include "Crowd.h"
 
 #include <stack>
 #include <assert.h>
 
 
 
 extern std::vector<Coord> g_vCoords;
 
 
 TimeAStar::Path* TimeAStar::findPath(Node& from, Node& to, CObjects* unit, int depth)
 {
 
 bool solved = false;
 Node current;
 
 openList.clear();
 if (useBitSet)
 bitSetClosedList.reset();
 else
 intSetClosedList.clear();
 
 int maxOpenSize = 0; int maxCloseSize = 0; int reachedDepth = 0;
 
 from.transition = NULL;
 from.h = from.getActualTimelessCost(to);
 // error correction here
 from.g = 0;
 from.f = from.h;
 from.depth = 0;
 
 openList.insert(from);
 
 
 while (!openList.empty())
 {
 current = *openList.begin();
 openList.erase(openList.begin());
 
 
 if (useBitSet)
 {
 bitSetClosedList.set(current.id);
 }
 else
 {
 intSetClosedList.insert(current.id);
 }
 
 if (current.depth > reachedDepth)
 {
 reachedDepth = current.depth;
 
 if (reachedDepth == depth)
 {
 solved = true;
 break;
 }
 }
 
 if (current.x == to.x && current.z == to.z)
 {
 solved = true;
 break;
 }
 
 
 for (int i = -1; i <= 1; i++)
 {
 for (int j = -1; j <= 1; j++)
 {
 int newx = current.x+i;
 int newy = current.z+j;
 if (newx <= g_startx || newx >= g_MaxX)
 continue;
 if (newy <= g_starty || newy >= g_MaxY)
 continue;
 int idx = Convert_XZ_vector(newx, newy);
 if (g_vCoords[idx].bBlocked == true)
 continue;
 // if (grid->grid[newx][newy].bBlocked == true)
 // continue;
 // Get neighbours for each
 Node node(newx, newy, current.t+1);
 // For unoccupied slot push it onto opened list
 //float cost = getCost(unit, &from, node);
 Transition* t = new Transition(current, node);
 float cost = t->getCost(unit);
 if (cost < 0)
 continue;
 
 if (useBitSet)
 {
 if (bitSetClosedList.at(node.id))
 continue;
 }
 else
 {
 bool found = intSetClosedList.find(node.id) != intSetClosedList.end();
 if (found)
 continue;
 }
 
 bool contains = openList.find(node) != openList.end();
 if (contains)
 {
 if (current.g + cost < node.g)
 {
 if (!openList.erase(node))
 assert(false);
 
 node.transition = t;
 node.g = current.g + cost;
 node.f = node.g + node.h;
 node.depth = current.depth + 1;
 
 openList.insert(node);
 }
 }
 else
 {
 
 // thePath.push(*node);
 node.transition = t;
 node.g = current.g + cost;
 node.h = node.getActualTimelessCost(to);
 node.f = node.g + node.h;
 node.depth = current.depth + 1;
 openList.insert(node);
 
 // What a mess, clean it up tomorrow
 }
 }
 }
 }
 if (solved)
 {
 float totalCost = current.g;
 
 
 std::vector transitions;
 while (current.transition != NULL)
 {
 transitions.push_back((Transition*)current.transition);
 current = current.transition->FromNode();
 }
 return new Path(from, transitions, totalCost);
 }
 
 
 
 }
 
 
 void Coordinater::distributeUnits()
 {
 if (units.empty())
 return;
 
 window.clear();
 
 std::vector remainingUnits;
 
 // for (int i = 0; i < units.size(); i++)
 // remainingUnits.push_back(units.at(i));
 std::map::iterator it, itEnd = units.end();
 for (it = units.begin(); it != itEnd; it++)
 {
 remainingUnits.push_back(it->second);
 }
 
 std::random_shuffle(remainingUnits.begin(), remainingUnits.end());
 
 while (remainingUnits.size() >= depth / 2)
 {
 for (int i = 0; i < depth / 2; i++) {
 putUnitInWindow(*remainingUnits.begin(), i);
 remainingUnits.erase(remainingUnits.begin());
 }
 
 
 float windowPerUnit = (float) depth / 2 / remainingUnits.size();
 for (int i = 0; i < remainingUnits.size(); i++)
 {
 int position = (int) (windowPerUnit * i);
 putUnitInWindow(remainingUnits, position);
 }
 }
 }
 
 void Coordinater::putUnitInWindow (CObjects* unit, int position)
 {
 std::vector unitsAtT = window[position];
 window[position] = unitsAtT;
 
 unitsAtT.push_back(unit);
 }
 
 void Coordinater::addUnit(CObjects *unit)
 {
 units[unit->m_id] = unit;
 newUnits.push_back(unit);
 
 }
 
 void Coordinater::Reset()
 {
 units.clear();
 newUnits.clear();
 pool.releaseAllNodes();
 pool.reclaimAll();
 
 }
 
 int Coordinater::getDepth()
 {
 return depth;
 }
 
 void Coordinater::printReservations()
 {
 char buff[256] = {0};
 std::map::iterator it, itEnd = pool.getReserved().end();
 for (it = pool.getReserved().begin(); it != itEnd; it++)
 {
 sprintf (buff, "Unit: %s %d\n", it->first.c_str(), it->second->m_id);
 OutputDebugStringA(buff);
 
 }
 }
 
 bool Coordinater::iterate()
 {
 char buff[256] = {0};
 sprintf (buff, "iterate time: %d\n", currentTime);
 OutputDebugStringA(buff);
 bool iterated = false;
 
 if (!newUnits.empty()) {
 distributeUnits();
 
 std::vector::iterator it, itEnd = newUnits.end();
 for (it = newUnits.begin(); it != itEnd; it++)
 reserveEmptyPath(*it);
 
 printReservations();
 std::vector::iterator it2, itEnd2 = newUnits.end();
 for (it2 = newUnits.begin(); it2 != itEnd2; it2++)
 {
 clearReservations(*it2);
 findPath(*it2);
 }
 
 iterated = true;
 }
 
 std::vector unitsToFindPath = window[currentWindow];
 if (!unitsToFindPath.empty())
 {
 std::vector::iterator it, itEnd = unitsToFindPath.end();
 for (it = unitsToFindPath.begin(); it != itEnd; it++)
 {
 clearReservations(*it);
 findPath(*it);
 
 iterated = true;
 
 }
 }
 
 currentTime++;
 currentWindow++;
 if (currentWindow == depth / 2)
 currentWindow = 0;
 
 return iterated;
 
 }
 
 void Coordinater::findPath(CObjects* unit)
 {
 pool.releaseAllNodes();
 
 D3DXVECTOR3 location = unit->GetPos();
 Node from = pool.acquireNode(location.x, location.z, currentTime);
 //SNodePool::SNode to = pool.acquireNode(unit->m_goal.x, unit->m_goal.z, 0);
 Node to = pool.acquireNode((int)unit->GetGoal().x,(int)unit->GetGoal().z, 0);
 std::vector unitPath;
 
 TimeAStar::Path *path = timeastar.findPath(from, to, unit, depth);
 
 if (!path->transitions.empty())
 {
 std::vector::iterator it, itEnd = path->transitions.end();
 for (it = path->transitions.begin(); it != itEnd; it++)
 {
 Node nextNode = (Node) (*it)->ToNode();
 pool.reserve(unit, nextNode);
 CObjects::PathPoint pt(nextNode.x, nextNode.z, nextNode.t);
 unitPath.push_back(pt);
 
 
 if (RESERVE_TWO)
 {
 Transition *trans = (Transition*) *it;
 if (trans->wait)
 {
 Node prevNode = (Node) (*it)->FromNode();
 pool.reserve(unit, nextNode.x, nextNode.z, nextNode.t - 1);
 
 }
 }
 }
 }
 
 /*
 std::vector::iterator it, itEnd = path->path.end();
 for (it = path->path.begin(); it != itEnd; it++)
 {
 Node nextNode = (Node) (*it);
 pool.reserve(unit, nextNode);
 unitPath.push_back(nextNode);
 
 
 if (RESERVE_TWO)
 {
 if (pool.wait)
 {
 SNodePool::SNode *prevNode = (SNodePool::SNode*) &(*it)-1;
 pool->reserve(unit, nextNode->x, nextNode->z, nextNode->t-1);
 
 
 }
 }
 }
 */
 // tomorrow
 printReservations();
 unit->setPath(unitPath);
 }
 
 
 void Coordinater::clearReservations(CObjects *unit)
 {
 if (RESERVE_TWO)
 {
 std::vector path = unit->GetPath();
 for (int i = 0; i < path.size(); i++)
 {
 CObjects::PathPoint point = path;
 pool.reclaim(point.x, point.z, point.t);
 
 if (i < path.size()-1) {
 CObjects::PathPoint next = path[i+1];
 if (!(point.isSamePlace(next)))
 pool.reclaim(next.x, next.z, next.t - 1);
 }
 }
 }
 else
 {
 ;
 /////TO DO
 }
 }
 
 void Coordinater::reserveEmptyPath(CObjects *unit)
 {
 D3DXVECTOR3 location = unit->getLocation();
 
 std::vector<CObjects::PathPoint> path;
 for (int i = 0; i <= depth; i++)
 {
 CObjects::PathPoint point(location.x, location.z, currentTime+i);
 // try
 pool.reserve(unit,point.x, point.z, point.t);
 path.push_back(point);
 
 unit->setPath(path);
 }
 
 
 
 
 }

NodePool.h


 #ifndef NODEPOOL_H
 #define NODEPOOL_H
 
 #include <Windows.h>
 #include <sstream>
 #include <map>
 #include <vector>
 
 
 
 using namespace std;
 
 
 
 
 #define INFINITE_COST -1
 #define RESERVE_TWO true
 
 class NodePool;
 class CObjects;
 class TimeAStarTransition;
 class Transition;
 
 
 class TimeAStarNode
 { 
 public:
 float f;
 float g;
 float h;
 int depth;
 int id;
 Transition *transition; // changed from TimeAStarTransition
 
 int getID() { static int _id = 0; return _id++; }
 
 public:
 TimeAStarNode() { id = getID(); f = 0; g = 0; h = 0; depth = 0; transition = NULL; }
 
 virtual std::vector<TimeAStarTransition*>& getTransitions() = 0;
 virtual float getActualTimelessCost(const TimeAStarNode& dest) { return INFINITE_COST; }
 };
 
 class TimeAStarTransition
 {
 public:
 virtual TimeAStarNode& FromNode() = 0;
 virtual TimeAStarNode& ToNode() = 0;
 virtual float getCost(CObjects* unit) = 0;
 };
 
 class Node : public TimeAStarNode
 {
 public:
 int x;
 int z;
 long t;
 std::vector<TimeAStarTransition*> transitions;
 
 public:
 Node()
 {
 init(0,0,0);
 }
 
 Node(int x, int z, long t)
 {
 init(x,z,t);
 }
 
 void init(int x, int z, long t)
 {
 this->x = x;
 this->z = z;
 this->t = t;
 }
 
 float getActualTimelessCost(const Node& dest)
 {
 return (abs(x - dest.x) + abs(z - dest.z));
 }
 
 std::vector<TimeAStarTransition*>& getTransitions()
 {
 return transitions;
 }
 };
 
 class Transition : public TimeAStarTransition
 {
 public:
 Node fromNode;
 Node toNode;
 bool wait;
 
 
 public:
 Transition(const Node& fromNode, const Node& toNode)
 {
 
 this->fromNode = fromNode;
 this->toNode = toNode;
 this->wait = (fromNode.x == toNode.x) && (fromNode.z == toNode.z);
 }
 
 public:
 Node& FromNode()
 {
 return fromNode;
 }
 
 Node& ToNode()
 {
 return toNode;
 }
 
 float getCost(CObjects* unit);
 
 };
 
 class SPoint
 {
 public:
 int x;
 int z;
 SPoint(int x, int z)
 {
 this->x = x;
 this->z = z;
 }
 
 bool operator==(const SPoint& other)
 {
 return (this->x==other.x) && (this->z == other.z);
 }
 
 };
 
 class NodePool
 {
 
 public:
 
 std::map<std::string, Node> usedNodes;
 std::map<std::string, CObjects*> reserved;
 std::vector<Node> pool;
 std::map<int, CObjects*> units;
 
 
 NodePool() { }
 
 std::map<std::string, CObjects*>& getReserved()
 {
 return reserved;
 }
 
 void reserve(CObjects* obj, const Node& n)
 {
 reserve(obj, n.x, n.z, n.t);
 }
 
 void reserve(CObjects* obj, int x, int y, int t)
 {
 ostringstream strm;
 strm << x << ":" << y << ":" << t;
 string s = strm.str();
 
 bool b = reserved.find(s) != reserved.end();
 
 if (b == true)
 OutputDebugString("Already reserved\n");
 
 
 reserved[s] = obj;
 
 }
 
 bool isReserved(const Node& node)
 {
 return isReserved(node.x, node.z, node.t);
 }
 
 bool isReserved(int x, int z, int t)
 {
 ostringstream strm;
 strm << x << ":" << z << ":" << t;
 string s = strm.str();
 
 return reserved.find(s)!=reserved.end();
 
 }
 
 void reclaim (int x, int y, int t)
 {
 ostringstream strm;
 strm << x << ":" << y << ":" << t;
 string s = strm.str();
 
 reserved.erase(s);
 }
 
 void reclaimAll()
 {
 reserved.clear();
 }
 
 Node& acquireNode (int x, int y, int t)
 {
 ostringstream strm;
 strm << x << ":" << y << ":" << t;
 string s = strm.str();
 
 Node *node;
 bool found = usedNodes.find(s) != usedNodes.end();//Node *node = &usedNodes[s];
 //Node *node = new Node(usedNodes[s].x, usedNodes[s].z, usedNodes[s].t);
 if (!found)
 {
 if (pool.empty())
 {
 node = new Node(x,y,t);
 }
 else
 {
 node = &(*pool.begin());
 pool.erase(pool.begin());
 node->init(x,y,t);
 }
 usedNodes[s] = *node;
 }
 else
 node = new Node(usedNodes[s].x, usedNodes[s].z, usedNodes[s].t);
 
 return *node;
 
 }
 
 
 
 void releaseAllNodes()
 {
 //for (int i = 0; i < usedNodes.size(); i++)
 //{
 std::map<std::string, Node>::iterator it, itEnd = usedNodes.end();
 for (it = usedNodes.begin(); it != itEnd; it++)
 {
 pool.push_back(it->second);
 }
 usedNodes.clear();
 
 }
 };
 
 
 
 #endif

NodePool.cpp


 #include "stdafx.h"
 #include "NodePool.h"
 #include "CrowdEntity.h"
 
 
 NodePool pool;
 
 float Transition::getCost(CObjects* unit)
 {
 if (pool.isReserved(toNode))
 return INFINITE_COST;
 
 if (RESERVE_TWO)
 {
 if (!wait && pool.isReserved(toNode.x, toNode.z, toNode.t - 1))
 return INFINITE_COST;
 }
 
 if (wait && (unit->GetGoal().x == fromNode.x) &&
 (unit->GetGoal().z == fromNode.z))
 {
 OutputDebugStringA("-----------\n");
 return 0;
 }
 
 return 1;
 
 
 
 }

Here is the whole set of source code i use for cooperative pathfinding in which the definition of 'Node' is included
h is the manhattan heuristics.

Thanks a lot
Jack

Topic Locked