/* check the neighbor and update the heap if necessary */
static void update_waiting(const Stack *stack, size_t r, size_t nbr_index,
double weight, Int_Arraylist *heap,
Int_Arraylist *result, Sp_Grow_Workspace *sgw)
{
if ((sgw->checked[nbr_index] != 1) &&
(sgw->flag[nbr_index] != SP_GROW_BARRIER)) {
double tmpdist = sgw->dist[r];
double eucdist = weight;
if ((sgw->wf != NULL) && (sgw->sp_option == 0)) { /* calculate weight */
sgw->argv[0] = weight;
sgw->argv[1] = Stack_Array_Value(stack, r);
sgw->argv[2] = Stack_Array_Value(stack, nbr_index);
weight = sgw->wf(sgw->argv);
}
if (sgw->sp_option == 1) {
tmpdist = imax2(tmpdist, -Stack_Array_Value(stack, r));
} else {
tmpdist += weight;
}
if (tmpdist < sgw->dist[nbr_index]) { /* update geodesic distance */
sgw->dist[nbr_index] = tmpdist;
sgw->path[nbr_index] = r;
if (sgw->length != NULL) {
sgw->length[nbr_index] = sgw->length[r] + eucdist;
}
}
if (sgw->sp_option == 1) {
if (tmpdist == sgw->dist[nbr_index]) {
if (sgw->path[nbr_index] >= 0) {
double v1 = Stack_Array_Value(stack, sgw->path[nbr_index]);
double v2 = Stack_Array_Value(stack, r);
if (v2 > v1) {
sgw->path[nbr_index] = r;
}
}
}
}
if (sgw->checked[nbr_index] > 1) { /* update heap */
Int_Heap_Update_I(heap, nbr_index, sgw->dist, sgw->checked);
} else if (sgw->checked[nbr_index] <= 0) { /* add to heap */
Int_Heap_Add_I(heap, nbr_index, sgw->dist, sgw->checked);
}
}
}
int* Stack_Graph_Shortest_Path(const Stack *stack, int start[], int end[],
Stack_Graph_Workspace *sgw)
{
int start_index = Stack_Util_Offset(start[0], start[1], start[2],
stack->width, stack->height,
stack->depth);
int end_index = Stack_Util_Offset(end[0], end[1], end[2],
stack->width, stack->height,
stack->depth);
int nvoxel = Stack_Voxel_Number(stack);
int *path = iarray_malloc(nvoxel);
double *dist = darray_malloc(nvoxel);
int *checked = iarray_malloc(nvoxel);
int i, j;
for (i = 0; i < nvoxel; i++) {
dist[i] = Infinity;
path[i] = -1;
checked[i] = 0;
}
dist[start_index] = 0;
path[start_index] = -1;
checked[start_index] = 1;
int prev_vertex = start_index;
int cur_vertex = start_index;
int updating_vertex;
double tmpdist;
int neighbor[26];
Stack_Neighbor_Offset(sgw->conn, stack->width, stack->height, neighbor);
const double *neighbor_dist = Stack_Neighbor_Dist(sgw->conn);
int is_in_bound[26];
int nbound;
Int_Arraylist *heap = Int_Arraylist_New(1, 0);
int cwidth = stack->width - 1;
int cheight = stack->height - 1;
int cdepth = stack->depth - 1;
void *argv[3];
double v1;
double v2;
double d;
argv[0] = &v1;
argv[1] = &v2;
argv[2] = &d;
for (i = 1; i < nvoxel; i++) {
prev_vertex = cur_vertex;
nbound = Stack_Neighbor_Bound_Test_I(sgw->conn, cwidth, cheight, cdepth,
cur_vertex, is_in_bound);
if (nbound == sgw->conn) {
for (j = 0; j < sgw->conn; j++) {
updating_vertex = cur_vertex + neighbor[j];
if (checked[updating_vertex] != 1) {
v1 = Stack_Array_Value(stack, cur_vertex);
v2 = Stack_Array_Value(stack, updating_vertex);
d = neighbor_dist[j];
double weight = sgw->wf(argv);
tmpdist = weight + dist[cur_vertex];
if (dist[updating_vertex] > tmpdist) {
dist[updating_vertex] = tmpdist;
path[updating_vertex] = cur_vertex;
if (checked[updating_vertex] > 1) {
Int_Heap_Update_I(heap, updating_vertex, dist, checked);
} else {
Int_Heap_Add_I(heap, updating_vertex, dist, checked);
}
}
}
}
} else {
for (j = 0; j < sgw->conn; j++) {
if (is_in_bound[j]) {
updating_vertex = cur_vertex + neighbor[j];
if (checked[updating_vertex] != 1) {
v1 = Stack_Array_Value(stack, cur_vertex);
v2 = Stack_Array_Value(stack, updating_vertex);
d = neighbor_dist[j];
double weight = sgw->wf(argv);
tmpdist = weight + dist[cur_vertex];
if (dist[updating_vertex] > tmpdist) {
dist[updating_vertex] = tmpdist;
path[updating_vertex] = cur_vertex;
if (checked[updating_vertex] > 1) {
Int_Heap_Update_I(heap, updating_vertex, dist, checked);
} else {
Int_Heap_Add_I(heap, updating_vertex, dist, checked);
}
}
}
}
}
}
cur_vertex = extract_min(dist, checked, nvoxel, heap);
if (cur_vertex == end_index) {
break;
}
if (cur_vertex < 0) {
break;
}
}
Kill_Int_Arraylist(heap);
free(checked);
free(dist);
return path;
}
