void perform_front_propagation_2d(T_callback_intert_node callback_insert_node)
{
// create the Fibonacci heap
struct fibheap* open_heap = fh_makeheap();
fh_setcmp(open_heap, compare_points);
double h = 1.0/n;
// initialize points
for( int i=0; i<n; ++i )
for( int j=0; j<p; ++j )
{
D_(i,j) = GW_INFINITE;
S_(i,j) = kFar;
Q_(i,j) = -1;
}
// record all the points
heap_pool = new fibheap_el*[n*p];
memset( heap_pool, NULL, n*p*sizeof(fibheap_el*) );
// inialize open list
point_list existing_points;
for( int k=0; k<nb_start_points; ++k )
{
int i = (int) start_points_(0,k);
int j = (int) start_points_(1,k);
if( D_( i,j )==0 )
ERROR_MSG("start_points should not contain duplicates.");
point* pt = new point( i,j );
existing_points.push_back( pt ); // for deleting at the end
heap_pool_(i,j) = fh_insert( open_heap, pt ); // add to heap
if( values==NULL )
D_( i,j ) = 0;
else
D_( i,j ) = values[k];
S_( i,j ) = kOpen;
Q_(i,j) = k;
}
// perform the front propagation
int num_iter = 0;
bool stop_iteration = GW_False;
while( !fh_isempty(open_heap) && num_iter<nb_iter_max && !stop_iteration )
{
num_iter++;
// current point
point& cur_point = * ((point*) fh_extractmin( open_heap ));
int i = cur_point.i;
int j = cur_point.j;
heap_pool_(i,j) = NULL;
S_(i,j) = kDead;
stop_iteration = end_points_reached(i,j);
/*
char msg[200];
sprintf(msg, "Cool %f", Q_(i,j) );
WARN_MSG( msg );
*/
CHECK_HEAP;
// recurse on each neighbor
int nei_i[4] = {i+1,i,i-1,i};
int nei_j[4] = {j,j+1,j,j-1};
for( int k=0; k<4; ++k )
{
int ii = nei_i[k];
int jj = nei_j[k];
bool bInsert = true;
if( callback_insert_node!=NULL )
bInsert = callback_insert_node(i,j,ii,jj);
// check that the contraint distance map is ok
if( ii>=0 && jj>=0 && ii<n && jj<p && bInsert )
{
double P = h/W_(ii,jj);
// compute its neighboring values
double a1 = GW_INFINITE;
int k1 = -1;
if( ii<n-1 )
{
bool bParticipate = true;
if( callback_insert_node!=NULL )
bParticipate = callback_insert_node(ii,jj,ii+1,jj);
if( bParticipate )
{
a1 = D_(ii+1,jj);
k1 = Q_(ii+1,jj);
}
}
if( ii>0 )
{
bool bParticipate = true;
if( callback_insert_node!=NULL )
bParticipate = callback_insert_node(ii,jj,ii-1,jj);
if( bParticipate )
{
if( D_(ii-1,jj)<a1 )
k1 = Q_(ii-1,jj);
a1 = GW_MIN( a1, D_(ii-1,jj) );
}
}
double a2 = GW_INFINITE;
int k2 = -1;
if( jj<p-1 )
{
bool bParticipate = true;
if( callback_insert_node!=NULL )
bParticipate = callback_insert_node(ii,jj,ii,jj+1);
if( bParticipate )
{
a2 = D_(ii,jj+1);
k2 = Q_(ii,jj+1);
}
}
if( jj>0 )
{
bool bParticipate = true;
if( callback_insert_node!=NULL )
bParticipate = callback_insert_node(ii,jj,ii,jj-1);
if( bParticipate )
{
if( D_(ii,jj-1)<a2 )
k2 = Q_(ii,jj-1);
a2 = GW_MIN( a2, D_(ii,jj-1) );
}
}
if( a1>a2 ) // swap so that a1<a2
{
double tmp = a1; a1 = a2; a2 = tmp;
int tmpi = k1; k1 = k2; k2 = tmpi;
}
// update its distance
// now the equation is (a-a1)^2+(a-a2)^2 = P, with a >= a2 >= a1.
double A1 = 0;
if( P*P > (a2-a1)*(a2-a1) )
{
double delta = 2*P*P-(a2-a1)*(a2-a1);
A1 = (a1+a2+sqrt(delta))/2.0;
}
else
A1 = a1 + P;
if( ((int) S_(ii,jj)) == kDead )
{
// check if action has change. Should not happen for FM
// if( A1<D_(ii,jj) )
// WARN_MSG("The update is not monotone");
#if 1
if( A1<D_(ii,jj) ) // should not happen for FM
{
D_(ii,jj) = A1;
// update the value of the closest starting point
//if( GW_ABS(a1-A1)<GW_ABS(a2-A1) && k1>=0 )
Q_(ii,jj) = k1;
//else
// Q_(ii,jj) = k2;
//Q_(ii,jj) = Q_(i,j);
}
#endif
}
else if( ((int) S_(ii,jj)) == kOpen )
{
// check if action has change.
if( A1<D_(ii,jj) )
{
D_(ii,jj) = A1;
// update the value of the closest starting point
//if( GW_ABS(a1-A1)<GW_ABS(a2-A1) && k1>=0 )
Q_(ii,jj) = k1;
//else
// Q_(ii,jj) = k2;
//Q_(ii,jj) = Q_(i,j);
// Modify the value in the heap
fibheap_el* cur_el = heap_pool_(ii,jj);
if( cur_el!=NULL )
fh_replacedata( open_heap, cur_el, cur_el->fhe_data ); // use same data for update
else
ERROR_MSG("Error in heap pool allocation.");
}
}
else if( ((int) S_(ii,jj)) == kFar )
{
if( D_(ii,jj)!=GW_INFINITE )
ERROR_MSG("Distance must be initialized to Inf");
if( L==NULL || A1<=L_(ii,jj) )
{
S_(ii,jj) = kOpen;
// distance must have change.
D_(ii,jj) = A1;
// update the value of the closest starting point
//if( GW_ABS(a1-A1)<GW_ABS(a2-A1) && k1>=0 )
Q_(ii,jj) = k1;
//else
// Q_(ii,jj) = k2;
//Q_(ii,jj) = Q_(i,j);
// add to open list
point* pt = new point(ii,jj);
existing_points.push_back( pt );
heap_pool_(ii,jj) = fh_insert( open_heap, pt ); // add to heap
}
}
else
ERROR_MSG("Unkwnown state.");
} // end switch
} // end for
} // end while
// char msg[200];
// sprintf(msg, "Cool %f", Q_(100,100) );
// WARN_MSG( msg );
// free heap
fh_deleteheap(open_heap);
// free point pool
for( point_list::iterator it = existing_points.begin(); it!=existing_points.end(); ++it )
GW_DELETE( *it );
// free fibheap pool
GW_DELETEARRAY(heap_pool);
}
boolean IsHeapEmpty ( FibHeap * heap )
{
return fh_isempty ( heap );
}
void perform_dijkstra_propagation(T_callback_insert_node callback_insert_node = NULL)
{
// create the Fibonacci heap
struct fibheap* open_heap = fh_makeheap();
fh_setcmp(open_heap, compare_points);
// initialize points
for( int i=0; i<n; ++i )
{
D[i] = GW_INFINITE;
S[i] = kFar;
}
// record all the points
heap_pool = new fibheap_el*[n];
memset( heap_pool, NULL, n*sizeof(fibheap_el*) );
// inialize open list
for( int k=0; k<nb_start_points; ++k )
{
int i = (int) start_points[k];
if( D[i]==0 )
mexErrMsgTxt("start_points should not contain duplicates.");
heap_pool[i] = fh_insert( open_heap, (void*) i ); // add to heap
D[i] = 0;
S[i] = kOpen;
}
// perform the front propagation
int num_iter = 0;
bool stop_iteration = GW_False;
while( !fh_isempty(open_heap) && num_iter<nb_iter_max && !stop_iteration )
{
num_iter++;
// current point
int i = (int) fh_extractmin( open_heap );
heap_pool[i] = NULL;
S[i] = kDead;
stop_iteration = end_points_reached(i);
CHECK_HEAP;
// index in irs[jcs[k]]...irs[jcs[k+1]-1] are the node connected to node k
// values are W[jcs[k]]...W[jcs[k]-1]
// recurse on each neighbor of i
for( int k=jcs[i]; k<jcs[i+1]; ++k )
{
int ii = irs[k];
double P = W[k]; // graph weight
bool bInsert = true;
if( callback_insert_node!=NULL )
bInsert = callback_insert_node(ii,ii);
if( ii>=0 && ii<n && bInsert )
{
// compute its neighboring values
double a1 = D[i] + P;
if( ((int) S[ii]) == kDead )
{
// check if action has change. Should not happen for Dijkstra
if( a1<D[ii] )
mexWarnMsgTxt("The update is not monotone");
#if 1
if( a1<D[ii] ) // should not happen for FM
D[ii] = a1;
#endif
}
else if( ((int) S[ii]) == kOpen )
{
// check if action has change.
if( a1<D[ii] )
{
D[ii] = a1;
// Modify the value in the heap
fibheap_el* cur_el = heap_pool[ii];
if( cur_el!=NULL )
fh_replacedata( open_heap, cur_el, cur_el->fhe_data ); // use same data for update
else
mexErrMsgTxt("Error in heap pool allocation.");
}
}
else if( ((int) S[ii]) == kFar )
{
if( D[ii]!=GW_INFINITE )
mexErrMsgTxt("Distance must be initialized to Inf");
S[ii] = kOpen;
// distance must have change.
D[ii] = a1;
// add to open list
heap_pool[ii] = fh_insert( open_heap, (void*) ii ); // add to heap
}
else
mexErrMsgTxt("Unkwnown state.");
} // end switch
} // end for
} // end while
// free heap
fh_deleteheap(open_heap);
// free fibheap pool
GW_DELETEARRAY(heap_pool);
}
