/* 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_Arraylist* Stack_Sp_Grow(const Stack *stack, const size_t *seeds,
int nseed, const size_t *targets, int ntarget,
Sp_Grow_Workspace *sgw)
{
size_t nvoxel = Stack_Voxel_Number(stack);
sgw->width = stack->width;
sgw->height = stack->height;
sgw->depth = stack->depth;
/* allocate workspace */
if (sgw->size < nvoxel) {
sgw->dist = (double*) Guarded_Realloc(sgw->dist, sizeof(double) * nvoxel,
"Stack_Sp_Grow");
sgw->path = (int*) Guarded_Realloc(sgw->path, sizeof(int) * nvoxel,
"Stack_Sp_Grow");
sgw->checked = (int*) Guarded_Realloc(sgw->checked, sizeof(int) * nvoxel,
"Stack_Sp_Grow");
sgw->flag = (uint8_t*) Guarded_Realloc(sgw->flag, sizeof(uint8_t) * nvoxel,
"Stack_Sp_Grow");
if (sgw->lengthBufferEnabled) {
sgw->length = (double*) Guarded_Realloc(
sgw->length, sizeof(double) * nvoxel, "Stack_Sp_Grow");
}
} else {
if (sgw->dist == NULL) {
sgw->dist = (double*) Guarded_Malloc(sizeof(double) * nvoxel,
"Stack_Sp_Grow");
}
if (sgw->path == NULL) {
sgw->path = (int*) Guarded_Malloc(sizeof(int) * nvoxel,
"Stack_Sp_Grow");
}
if (sgw->checked == NULL) {
sgw->checked = (int*) Guarded_Malloc(sizeof(int) * nvoxel,
"Stack_Sp_Grow");
}
if (sgw->flag == NULL) {
sgw->flag = (uint8_t*) Guarded_Malloc(sizeof(uint8_t) * nvoxel,
"Stack_Sp_Grow");
}
if (sgw->lengthBufferEnabled) {
if (sgw->length == NULL) {
sgw->length = (double*) Guarded_Malloc(
sizeof(double) * nvoxel, "Stack_Sp_Grow");
}
}
}
sgw->size = nvoxel;
/* initialize */
size_t s;
for (s = 0; s < nvoxel; s++) {
sgw->dist[s] = Infinity;
if (sgw->length != NULL) {
sgw->length[s] = 0.0;
}
sgw->path[s] = -1;
sgw->checked[s] = 0;
sgw->flag[s] = 0;
}
if (sgw->mask != NULL) {
memcpy(sgw->flag, sgw->mask, nvoxel);
}
/* Recruit seeds (source) */
int i;
for (i = 0; i < nseed; i++) {
if (sgw->sp_option == 1) {
sgw->dist[seeds[i]] = -Stack_Array_Value(stack, seeds[i]);
} else {
sgw->dist[seeds[i]] = 0.0;
}
sgw->checked[seeds[i]] = 1;
sgw->flag[seeds[i]] = SP_GROW_SOURCE;
}
if (sgw->mask != NULL) {
for (s = 0; s < nvoxel; s++) {
if (sgw->flag[s] == SP_GROW_SOURCE) {
if (sgw->sp_option == 1) {
sgw->dist[s] = -Stack_Array_Value(stack, s);
} else {
sgw->dist[s] = 0.0;
}
sgw->checked[s] = 1;
}
}
}
Int_Arraylist *result = New_Int_Arraylist();
for (i = 0; i < ntarget; i++) {
if (sgw->flag[targets[i]] == 2) { /* overlap of source and target */
Int_Arraylist_Add(result, targets[i]);
sgw->value = 0.0;
return result;
}
sgw->flag[targets[i]] = SP_GROW_TARGET;
}
int width = Stack_Width(stack);
int height = Stack_Height(stack);
int depth = Stack_Depth(stack);
double dist[26];
Stack_Neighbor_Dist_R(sgw->conn, sgw->resolution, dist);
int is_in_bound[26];
int neighbors[26];
Stack_Neighbor_Offset(sgw->conn, Stack_Width(stack), Stack_Height(stack),
neighbors);
BOOL stop = FALSE;
/* Check neighbors of seeds */
int j;
// for (i = 0; i < nseed; i++) {
// size_t r = seeds[i];
/* alloc <heap> */
Int_Arraylist *heap = New_Int_Arraylist();
size_t r;
for (r = 0; r < nvoxel; r++) {
if (sgw->flag[r] == SP_GROW_SOURCE) { /* seeds */
int nbound = Stack_Neighbor_Bound_Test_I(sgw->conn, width, height, depth,
r, is_in_bound);
if (nbound == sgw->conn) { /* all neighbors are in bound */
for (j = 0; j < sgw->conn; j++) {
size_t nbr_index = r + neighbors[j];
if (sgw->checked[nbr_index] != 1) {
update_waiting(stack, r, nbr_index, dist[j], heap, result, sgw);
}
}
} else if (nbound > 0) {
for (j = 0; j < sgw->conn; j++) {
if (is_in_bound[j]) {
size_t nbr_index = r + neighbors[j];
if (sgw->checked[nbr_index] != 1) {
update_waiting(stack, r, nbr_index, dist[j], heap, result, sgw);
}
}
}
}
}
}
//Verify_Int_Heap_I(heap, sgw->dist);
ssize_t last_r = -1;
while (stop == FALSE) {
ssize_t r = extract_min(sgw->dist, sgw->checked, sgw->size, heap);
if (r >= 0) {
last_r = r;
if (sgw->flag[r] == 0) { /* normal voxel */
int nbound = Stack_Neighbor_Bound_Test_I(sgw->conn, width, height,
depth, r, is_in_bound);
if (nbound == sgw->conn) { /* all neighbors are in bound */
for (j = 0; j < sgw->conn; j++) {
size_t nbr_index = r + neighbors[j];
if (sgw->checked[nbr_index] != 1) {
update_waiting(stack, r, nbr_index, dist[j], heap, result, sgw);
}
}
} else if (nbound > 0) {
for (j = 0; j < sgw->conn; j++) {
if (is_in_bound[j]) {
size_t nbr_index = r + neighbors[j];
if (sgw->checked[nbr_index] != 1) {
update_waiting(stack, r, nbr_index, dist[j], heap, result, sgw);
}
}
}
}
//Print_Int_Heap(heap, "%d");
//Verify_Int_Heap_I(heap, sgw->dist);
} else if (sgw->flag[r] == SP_GROW_TARGET) { /* target reached */
//Int_Arraylist_Add(result, r);
stop = TRUE;
} else if (sgw->flag[r] == SP_GROW_CONDUCTOR) {
/* 0-distance region (super conductor) */
int nbound = Stack_Neighbor_Bound_Test_I(sgw->conn, width, height,
depth, r, is_in_bound);
size_t tail_index = r;
size_t old_tail_index = tail_index;
size_t cur_index = r;
#ifdef _DEBUG_2
printf("r: %lu\n", r);
#endif
#define UPDATE_SUPER_CONDUCTOR \
size_t nbr_index = cur_index + neighbors[j]; \
if (sgw->flag[nbr_index] == 4) { \
if (sgw->checked[nbr_index] > 1) { \
Int_Heap_Remove_I_At(heap, nbr_index, sgw->dist, \
sgw->checked); \
} \
tail_index = update_neighbor(cur_index, tail_index, nbr_index,sgw); \
} else { \
update_waiting(stack, cur_index, nbr_index, dist[j], heap, result,sgw); \
}
if (nbound == sgw->conn) { /* all neighbors are in bound */
for (j = 0; j < sgw->conn; j++) {
UPDATE_SUPER_CONDUCTOR
}
} else if (nbound > 0) {
for (j = 0; j < sgw->conn; j++) {
if (is_in_bound[j]) {
UPDATE_SUPER_CONDUCTOR
}
}
}
int count = 0;
while (tail_index != old_tail_index) {
old_tail_index = tail_index;
size_t cur_index = tail_index;
while (sgw->checked[cur_index] != 1) {
int nbound = Stack_Neighbor_Bound_Test_I(sgw->conn, width, height,
depth, cur_index,
is_in_bound);
if (nbound == sgw->conn) { /* all neighbors are in bound */
for (j = 0; j < sgw->conn; j++) {
UPDATE_SUPER_CONDUCTOR
}
} else if (nbound > 0) {
for (j = 0; j < sgw->conn; j++) {
if (is_in_bound[j]) {
UPDATE_SUPER_CONDUCTOR
}
}
}
sgw->checked[cur_index] = 1;
size_t tmp_index = cur_index;
cur_index = sgw->path[cur_index];
sgw->path[tmp_index] = r;
count++;
}
}
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;
}
