void CPathAI::constructPath() {
INT indexD = point2Index(destination);
INT indexO = point2Index(origin);
paths.push_back(indexD);
Entry* entry = entrys + indexD;
TCHAR szDebug[256] = {0};
#ifdef _DEBUG
OutputDebugString("Re-path is: \n");
wsprintf(szDebug, "(%02d, %02d)\n", destination.x, destination.y);
OutputDebugString(szDebug);
#endif
while (entry->lasts[0] != indexO) {
#ifdef _DEBUG
POINT pt = {-1, -1};
index2Point(entry->lasts[0], pt);
wsprintf(szDebug, "(%02d, %02d)\n", pt.x, pt.y);
OutputDebugString(szDebug);
#endif
paths.push_back(entry->lasts[0]);
entry = entrys + entry->lasts[0];
}
#ifdef _DEBUG
wsprintf(szDebug, "(%02d, %02d)\n", origin.x, origin.y);
OutputDebugString(szDebug);
#endif
paths.push_back(indexO);
return;
}
void addPointToIndex(Index *index, Point3D *point) {
int col,row;
point2Index(index->config, point, &row, &col);
assert(row < (index->rows) && col < (index->cols) && "col,row < cols,rows");
PointNode **binstart = (index->nodes)+(row*index->cols)+col;
addToBin(binstart, point);
}
Point3D* useNearest(Index *index, Point3D *pixel_rep) {
Point3D *nearest = NULL;
int i,j,
d=1,
row, col;
point2Index(index->config, pixel_rep, &row, &col);
int minj = MAX(0, row-d),
maxj = MIN(row+d, index->rows - 1),
mini = MAX(0, col-d),
maxi = MIN(col+d, index->cols - 1);
double shortestDist = 100000000000;
// Go through relevant bins
for (j = minj; j <= maxj; j++) {
for (i = mini; i <= maxi; i++) {
// Go through one bin
PointNode *node = *(index->nodes+j*(index->cols)+i);
while (node != NULL) {
Point3D *current = &(node->p);
double currentDist = dist(pixel_rep->x, pixel_rep->y, current->x, current->y);
if ( shortestDist > currentDist ) {
nearest = current;
shortestDist = currentDist;
}
node = node->next;
}
}
}
printf("row=%d col=%d ... mini=%d maxi=%d minj=%d maxj=%d\n", row, col, mini, maxi, minj, maxj);
return nearest;
}
std::vector <int>& CPathAI::getPaths(POINT& O, POINT& D) {
initEntrys();
INT offsets[] = {-COLS, 1, COLS, -1};
std::map <int, set <int>, less<int> > operations;
std::set <int> initOrigins;
initOrigins.insert(point2Index(origin));
operations[0] = initOrigins;
TCHAR szDebug[256] = {0};
for (INT i = 0; i < INT_MAX ; i++) {
std::set <INT>& pts = operations[i];
std::set <INT> nextStepOrigins;
#ifdef _DEBUG
wsprintf(szDebug, "-------------------------------\nNO. step: %02d\n", i);
OutputDebugString(szDebug);
#endif
bool runNext = false;
for (si = pts.begin(); si != pts.end(); si++) {
#ifdef _DEBUG
POINT p = {-1, -1};
index2Point(*si, p);
wsprintf(szDebug, "\nO (%02d, %02d)\n", p.x, p.y);
OutputDebugString(szDebug);
#endif
for (INT j = 0; j < 4; j++) {
INT index = *si + offsets[j];
#ifdef _DEBUG
index2Point(index, p);
wsprintf(szDebug, "%02d -> (%02d, %02d)\n", j, p.x, p.y);
OutputDebugString(szDebug);
#endif
if (index < 0 || index >= COLS * ROWS) {
continue;
}
if (s.find(index) != s.end()) {
continue;
}
if (entrys[*si].rights + entrys[index].right < entrys[index].rights) {
entrys[index].rights = entrys[*si].rights + entrys[index].right;
entrys[index].lasts[0] = *si;
nextStepOrigins.insert(index);
runNext = true;
}
}
}
if (!runNext) {
break;
}
for (si = nextStepOrigins.begin(); si != nextStepOrigins.end(); si++) {
if (*si == point2Index(destination)) {//destination reached
constructPath();
return paths;
}
s.insert(*si);
}
operations[i + 1] = nextStepOrigins;
}
return paths;
}
