AnyAngleAlgorithm::AnyAngleAlgorithm(const std::vector<bool> &bits, int width, int height)
{
// Read the height and width of the map.
width_ = width + 2; // Add +2 for the frame of obstacles.
height_ = height + 2;
unblocked_cells_.clear();
// Generate the cells with the padding.
cells_ = new Cell*[width_];
for (unsigned int x = 0; x < width_; x++) {
cells_[x] = new Cell[height_];
}
// Make unblocked cells traversable.
int num_traversable = 0;
for (unsigned int x = 0; x < width_ - 2; x++) {
for (unsigned int y = 0; y < height_ - 2; y++) {
if (bits[y * width + x]) {
cells_[x + 1][y + 1].is_traversable = true;
unblocked_cells_.push_back(xyLoc(x,y));
}
}
}
#ifdef ANY_ANGLE_VERBOSE
std::cout << "Traversable cells: " << num_traversable << std::endl;
#endif
Initialize();
}
AnyAngleAlgorithm::AnyAngleAlgorithm(const MapEnvironment *env)
{
// Read the height and width of the map.
Map* map = env->GetMap();
width_ = map->GetMapWidth() + 2; // Add +2 for the frame of obstacles.
height_ = map->GetMapHeight() + 2;
unblocked_cells_.clear();
// Generate the cells with the padding.
cells_ = new Cell*[width_];
for (unsigned int x = 0; x < width_; x++) {
cells_[x] = new Cell[height_];
}
// Make unblocked cells traversable.
int num_traversable = 0;
for (unsigned int x = 0; x < width_ - 2; x++) {
for (unsigned int y = 0; y < height_ - 2; y++) {
if (map->GetTerrainType(x, y) == kGround) {
cells_[x+1][y+1].is_traversable = true;
num_traversable++;
unblocked_cells_.push_back(xyLoc(x,y));
}
}
}
#ifdef ANY_ANGLE_VERBOSE
std::cout << "Traversable cells: " << num_traversable << std::endl;
#endif
Initialize();
}
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);
}
