/* =============================================================================
* PdoExpansion
* =============================================================================
*/
static bool_t
PdoExpansion (router_t* routerPtr, grid_t* myGridPtr, queue_t* queuePtr,
coordinate_t* srcPtr, coordinate_t* dstPtr)
{
long xCost = routerPtr->xCost;
long yCost = routerPtr->yCost;
long zCost = routerPtr->zCost;
/*
* Potential Optimization: Make 'src' the one closest to edge.
* This will likely decrease the area of the emitted wave.
*/
PQUEUE_CLEAR(queuePtr);
long* srcGridPointPtr =
grid_getPointRef(myGridPtr, srcPtr->x, srcPtr->y, srcPtr->z);
PQUEUE_PUSH(queuePtr, (void*)srcGridPointPtr);
grid_setPoint(myGridPtr, srcPtr->x, srcPtr->y, srcPtr->z, 0);
grid_setPoint(myGridPtr, dstPtr->x, dstPtr->y, dstPtr->z, GRID_POINT_EMPTY);
long* dstGridPointPtr =
grid_getPointRef(myGridPtr, dstPtr->x, dstPtr->y, dstPtr->z);
bool_t isPathFound = FALSE;
while (!PQUEUE_ISEMPTY(queuePtr)) {
long* gridPointPtr = (long*)PQUEUE_POP(queuePtr);
if (gridPointPtr == dstGridPointPtr) {
isPathFound = TRUE;
break;
}
long x;
long y;
long z;
grid_getPointIndices(myGridPtr, gridPointPtr, &x, &y, &z);
long value = (*gridPointPtr);
/*
* Check 6 neighbors
*
* Potential Optimization: Only need to check 5 of these
*/
PexpandToNeighbor(myGridPtr, x+1, y, z, (value + xCost), queuePtr);
PexpandToNeighbor(myGridPtr, x-1, y, z, (value + xCost), queuePtr);
PexpandToNeighbor(myGridPtr, x, y+1, z, (value + yCost), queuePtr);
PexpandToNeighbor(myGridPtr, x, y-1, z, (value + yCost), queuePtr);
PexpandToNeighbor(myGridPtr, x, y, z+1, (value + zCost), queuePtr);
PexpandToNeighbor(myGridPtr, x, y, z-1, (value + zCost), queuePtr);
} /* iterate over work queue */
#if DEBUG
printf("Expansion (%li, %li, %li) -> (%li, %li, %li):\n",
srcPtr->x, srcPtr->y, srcPtr->z,
dstPtr->x, dstPtr->y, dstPtr->z);
grid_print(myGridPtr);
#endif /* DEBUG */
return isPathFound;
}
/* =============================================================================
* maze_checkPaths
* =============================================================================
*/
bool_t
maze_checkPaths (maze_t* mazePtr, list_t* pathVectorListPtr, bool_t doPrintPaths)
{
grid_t* gridPtr = mazePtr->gridPtr;
long width = gridPtr->width;
long height = gridPtr->height;
long depth = gridPtr->depth;
long i;
/* Mark walls */
grid_t* testGridPtr = grid_alloc(width, height, depth);
grid_addPath(testGridPtr, mazePtr->wallVectorPtr);
/* Mark sources */
vector_t* srcVectorPtr = mazePtr->srcVectorPtr;
long numSrc = vector_getSize(srcVectorPtr);
for (i = 0; i < numSrc; i++) {
coordinate_t* srcPtr = (coordinate_t*)vector_at(srcVectorPtr, i);
grid_setPoint(testGridPtr, srcPtr->x, srcPtr->y, srcPtr->z, 0);
}
/* Mark destinations */
vector_t* dstVectorPtr = mazePtr->dstVectorPtr;
long numDst = vector_getSize(dstVectorPtr);
for (i = 0; i < numDst; i++) {
coordinate_t* dstPtr = (coordinate_t*)vector_at(dstVectorPtr, i);
grid_setPoint(testGridPtr, dstPtr->x, dstPtr->y, dstPtr->z, 0);
}
/* Make sure path is contiguous and does not overlap */
long id = 0;
list_iter_t it;
list_iter_reset(&it, pathVectorListPtr);
while (list_iter_hasNext(&it)) {
vector_t* pathVectorPtr = (vector_t*)list_iter_next(&it);
long numPath = vector_getSize(pathVectorPtr);
long i;
for (i = 0; i < numPath; i++) {
id++;
vector_t* pointVectorPtr = (vector_t*)vector_at(pathVectorPtr, i);
/* Check start */
long* prevGridPointPtr = (long*)vector_at(pointVectorPtr, 0);
long x;
long y;
long z;
grid_getPointIndices(gridPtr, prevGridPointPtr, &x, &y, &z);
if (grid_getPoint(testGridPtr, x, y, z) != 0) {
grid_free(testGridPtr);
return FALSE;
}
coordinate_t prevCoordinate;
grid_getPointIndices(gridPtr,
prevGridPointPtr,
&prevCoordinate.x,
&prevCoordinate.y,
&prevCoordinate.z);
long numPoint = vector_getSize(pointVectorPtr);
long j;
for (j = 1; j < (numPoint-1); j++) { /* no need to check endpoints */
long* currGridPointPtr = (long*)vector_at(pointVectorPtr, j);
coordinate_t currCoordinate;
grid_getPointIndices(gridPtr,
currGridPointPtr,
&currCoordinate.x,
&currCoordinate.y,
&currCoordinate.z);
if (!coordinate_areAdjacent(&currCoordinate, &prevCoordinate)) {
grid_free(testGridPtr);
return FALSE;
}
prevCoordinate = currCoordinate;
long x = currCoordinate.x;
long y = currCoordinate.y;
long z = currCoordinate.z;
if (grid_getPoint(testGridPtr, x, y, z) != GRID_POINT_EMPTY) {
grid_free(testGridPtr);
return FALSE;
} else {
grid_setPoint(testGridPtr, x, y, z, id);
}
}
/* Check end */
long* lastGridPointPtr = (long*)vector_at(pointVectorPtr, j);
grid_getPointIndices(gridPtr, lastGridPointPtr, &x, &y, &z);
if (grid_getPoint(testGridPtr, x, y, z) != 0) {
grid_free(testGridPtr);
return FALSE;
}
} /* iteratate over pathVector */
} /* iterate over pathVectorList */
if (doPrintPaths) {
puts("\nRouted Maze:");
grid_print(testGridPtr);
}
grid_free(testGridPtr);
return TRUE;
}
