cost QuickBucket::GetPath(xyLoc & s, xyLoc & g, std::vector<xyLoc> & path)
{
search++;
theHeap.clear();
bucketsInUse = 0;
#ifdef BUCKET_STATISTICS
int openSize = 1;
#endif
// Succesor buckets
int id[6]; // 0: 0, 1: 2C-D, 2: 2D-2C, 3:
id[0] = GetBucketId(abCost(0,0)); // Get the bucket for the start state
GenerateBucket(id[0]);
buckets[id[0]].contents.push_back(bucketElement(s, 8)); // Insert the start state to the bucket, mark its parent as 8 = no parent
buckets[id[0]].open = true;
AddToOpen(abCost(0,0)); // Insert the bucket into the open list
theMap[s.x][s.y].open = true; // Set the start state as open
theMap[s.x][s.y].generated = search;
theMap[g.x][g.y].open = true; // Also, set the goal state as open, but do not put it in the buckets yet
theMap[g.x][g.y].generated = search;
abCost currCost;
while (theMap[g.x][g.y].open && !theHeap.empty())
{
// Select a bucket to expand
currCost = theHeap[0];
PopMin();
#ifdef BUCKET_STATISTICS
bucketExpansions++;
#endif
// Get the bucket and its successor buckets
id[0] = GetBucketId(currCost);
buckets[id[0]].open = false;
for (int i = 1; i < 6; i++)
{
id[i] = GetBucketId(abCost(currCost.a + A[i], currCost.b + B[i]));
}
GenerateBucket(id[5]+4); // make sure we generate all the buckets with just one call
// Expand all the contents of the bucket
while (!buckets[id[0]].contents.empty())
{
bucketElement elem = buckets[id[0]].contents.back();
buckets[id[0]].contents.pop_back();
#ifdef BUCKET_STATISTICS
openSize--;
#endif
if (theMap[elem.loc.x][elem.loc.y].open)
{
#ifdef BUCKET_STATISTICS
elementExpansions++;
#endif
theMap[elem.loc.x][elem.loc.y].open = false;
theMap[elem.loc.x][elem.loc.y].parent = elem.parent;
//unsigned char neighbors = theMap[elem.loc.x][elem.loc.y].neighbors;
unsigned char neighbors = theMap[elem.loc.x][elem.loc.y].neighbors & parentMask[elem.parent];
unsigned char currNeighbor = 1;
int baseCase = GetBaseCase(elem.loc, g);
for (int d = 0; d < 8; d++) // look at 8 directions
{
if (currNeighbor & neighbors) // if there is a potential successor in a direction
{
xyLoc succ = xyLoc(elem.loc.x + X[d], elem.loc.y + Y[d]);
// generate it if necessary
if (theMap[succ.x][succ.y].generated != search)
{
theMap[succ.x][succ.y].generated = search;
theMap[succ.x][succ.y].open = true;
}
// if it was not already expanded
if (theMap[succ.x][succ.y].open)
{
buckets[id[cases[baseCase]]].contents.push_back(bucketElement(succ, (d+4)&7));
#ifdef BUCKET_STATISTICS
openSize++;
if (openSize > maxOpenSize)
maxOpenSize = openSize;
#endif
}
}
baseCase++;
currNeighbor = (currNeighbor << 1);
}
}
#ifdef BUCKET_STATISTICS
else
alreadyExpanded++;
#endif
}
// Bucket expanded, try to add new buckets to the open list
for (int i = 1; i < 6; i++)
{
if (!buckets[id[i]].open && !buckets[id[i]].contents.empty()) // if it is not already open and it contains at least one element
{
buckets[id[i]].open = true;
AddToOpen(abCost(currCost.a + A[i], currCost.b + B[i]));
}
}
#ifdef BUCKET_MEMORY_OPTIMIZATION
std::vector<bucketElement>().swap(buckets[id[0]].contents); // Free the memory for that bucket
buckets[id[0]].contents.reserve(DEFAULT_BUCKET_SIZE); // Allow it to have 256 elements so that we don't reallocate too often
#endif
}
if (!theMap[g.x][g.y].open)
{
path.clear();
xyLoc loc = g;
while (loc.x != s.x || loc.y != s.y)
{
path.push_back(loc);
int parent = theMap[loc.x][loc.y].parent;
loc.x += X[parent];
loc.y += Y[parent];
}
path.push_back(loc);
std::reverse(path.begin(), path.end());
return HCost(s,g) + currCost.GetCost();
}
return 0;//HCost(s,g);
}
/** Heuristic value between node and the stored goal. Asserts that the
goal is stored **/
double RubiksCube::HCost(const RubiksState &node)
{
return HCost(node, node);
}
