/* =============================================================================
* 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;
}
/* =============================================================================
* grid_addPath
* =============================================================================
*/
void
grid_addPath (grid_t* gridPtr, vector_t* pointVectorPtr)
{
long i;
long n = vector_getSize(pointVectorPtr);
for (i = 0; i < n; i++) {
coordinate_t* coordinatePtr = (coordinate_t*)vector_at(pointVectorPtr, i);
long x = coordinatePtr->x;
long y = coordinatePtr->y;
long z = coordinatePtr->z;
grid_setPoint(gridPtr, x, y, z, GRID_POINT_FULL);
}
}
/* =============================================================================
* PdoTraceback
* =============================================================================
*/
static vector_t*
PdoTraceback (grid_t* gridPtr, grid_t* myGridPtr,
coordinate_t* dstPtr, long bendCost)
{
vector_t* pointVectorPtr = PVECTOR_ALLOC(1);
assert(pointVectorPtr);
point_t next;
next.x = dstPtr->x;
next.y = dstPtr->y;
next.z = dstPtr->z;
next.value = grid_getPoint(myGridPtr, next.x, next.y, next.z);
next.momentum = MOMENTUM_ZERO;
while (1) {
long* gridPointPtr = grid_getPointRef(gridPtr, next.x, next.y, next.z);
PVECTOR_PUSHBACK(pointVectorPtr, (void*)gridPointPtr);
grid_setPoint(myGridPtr, next.x, next.y, next.z, GRID_POINT_FULL);
/* Check if we are done */
if (next.value == 0) {
break;
}
point_t curr = next;
/*
* Check 6 neighbors
*
* Potential Optimization: Only need to check 5 of these
*/
traceToNeighbor(myGridPtr, &curr, &MOVE_POSX, TRUE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_POSY, TRUE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_POSZ, TRUE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_NEGX, TRUE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_NEGY, TRUE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_NEGZ, TRUE, bendCost, &next);
#if DEBUG
printf("(%li, %li, %li)\n", next.x, next.y, next.z);
#endif /* DEBUG */
/*
* Because of bend costs, none of the neighbors may appear to be closer.
* In this case, pick a neighbor while ignoring momentum.
*/
if ((curr.x == next.x) &&
(curr.y == next.y) &&
(curr.z == next.z))
{
next.value = curr.value;
traceToNeighbor(myGridPtr, &curr, &MOVE_POSX, FALSE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_POSY, FALSE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_POSZ, FALSE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_NEGX, FALSE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_NEGY, FALSE, bendCost, &next);
traceToNeighbor(myGridPtr, &curr, &MOVE_NEGZ, FALSE, bendCost, &next);
if ((curr.x == next.x) &&
(curr.y == next.y) &&
(curr.z == next.z))
{
PVECTOR_FREE(pointVectorPtr);
#if DEBUG
puts("[dead]");
#endif
return NULL; /* cannot find path */
}
}
}
#if DEBUG
puts("");
#endif /* DEBUG */
return pointVectorPtr;
}
/* =============================================================================
* 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;
}
