/* =============================================================================
* traceToNeighbor
* =============================================================================
*/
static void
traceToNeighbor (grid_t* myGridPtr,
point_t* currPtr,
point_t* movePtr,
bool_t useMomentum,
long bendCost,
point_t* nextPtr)
{
long x = currPtr->x + movePtr->x;
long y = currPtr->y + movePtr->y;
long z = currPtr->z + movePtr->z;
if (grid_isPointValid(myGridPtr, x, y, z) &&
!grid_isPointEmpty(myGridPtr, x, y, z) &&
!grid_isPointFull(myGridPtr, x, y, z))
{
long value = grid_getPoint(myGridPtr, x, y, z);
long b = 0;
if (useMomentum && (currPtr->momentum != movePtr->momentum)) {
b = bendCost;
}
if ((value + b) <= nextPtr->value) { /* '=' favors neighbors over current */
nextPtr->x = x;
nextPtr->y = y;
nextPtr->z = z;
nextPtr->value = value;
nextPtr->momentum = movePtr->momentum;
}
}
}
/* =============================================================================
* PexpandToNeighbor
* =============================================================================
*/
static void
PexpandToNeighbor (grid_t* myGridPtr,
long x, long y, long z, long value, queue_t* queuePtr)
{
if (grid_isPointValid(myGridPtr, x, y, z)) {
long* neighborGridPointPtr = grid_getPointRef(myGridPtr, x, y, z);
long neighborValue = *neighborGridPointPtr;
if (neighborValue == GRID_POINT_EMPTY) {
(*neighborGridPointPtr) = value;
PQUEUE_PUSH(queuePtr, (void*)neighborGridPointPtr);
} else if (neighborValue != GRID_POINT_FULL) {
/* We have expanded here before... is this new path better? */
if (value < neighborValue) {
(*neighborGridPointPtr) = value;
PQUEUE_PUSH(queuePtr, (void*)neighborGridPointPtr);
}
}
}
}
/* =============================================================================
* addToGrid
* =============================================================================
*/
static void
addToGrid (grid_t* gridPtr, vector_t* vectorPtr, char* type)
{
long i;
long n = vector_getSize(vectorPtr);
for (i = 0; i < n; i++) {
coordinate_t* coordinatePtr = (coordinate_t*)vector_at(vectorPtr, i);
if (!grid_isPointValid(gridPtr,
coordinatePtr->x,
coordinatePtr->y,
coordinatePtr->z))
{
fprintf(stderr, "Error: %s (%li, %li, %li) invalid\n",
type, coordinatePtr->x, coordinatePtr->y, coordinatePtr->z);
exit(1);
}
}
grid_addPath(gridPtr, vectorPtr);
}