void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray*prhs[] )
{
/* retrive arguments */
if( nrhs<5 )
mexErrMsgTxt("5 or 6 input arguments are required.");
if( nlhs<1 )
mexErrMsgTxt("1 or 2 output arguments are required.");
// first argument : weight list
n = mxGetM(prhs[0]);
p = mxGetN(prhs[0]);
W = mxGetPr(prhs[0]);
// second argument : start_points
start_points = mxGetPr(prhs[1]);
int tmp = mxGetM(prhs[1]);
nb_start_points = mxGetN(prhs[1]);
if( nb_start_points==0 || tmp!=2 )
mexErrMsgTxt("start_points must be of size 2 x nb_start_poins.");
// third argument : end_points
end_points = mxGetPr(prhs[2]);
tmp = mxGetM(prhs[2]);
nb_end_points = mxGetN(prhs[2]);
if( nb_end_points!=0 && tmp!=2 )
mexErrMsgTxt("end_points must be of size 2 x nb_end_poins.");
// fourth argument : center_point
center_point = mxGetPr(prhs[3]);
// fifth argument : nb_iter_max
nb_iter_max = (int) *mxGetPr(prhs[4]);
// sixth argument : heuristic
if( nrhs==6 )
{
H = mxGetPr(prhs[5]);
if( mxGetM(prhs[5])!=n || mxGetN(prhs[5])!=p )
mexErrMsgTxt("H must be of size n x p.");
}
else
H = NULL;
// first ouput : distance
plhs[0] = mxCreateDoubleMatrix(n, p, mxREAL);
D = mxGetPr(plhs[0]);
// second output : state
if( nlhs>=2 )
{
plhs[1] = mxCreateDoubleMatrix(n, p, mxREAL);
S = mxGetPr(plhs[1]);
}
else
{
S = new double[n*p];
}
// launch the propagation
perform_front_propagation_2d(callback_intert_node);
if( nlhs<2 )
GW_DELETEARRAY(S);
return;
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray*prhs[] )
{
/* retrive arguments */
if( nrhs<4 )
mexErrMsgTxt("4 or 5 input arguments are required.");
if( nlhs<1 )
mexErrMsgTxt("1 or 2 output arguments are required.");
/* dealing with sparse array */
W = mxGetPr(prhs[0]); // returns a pointer to the numerical values
irs = (mwIndex *)mxGetIr(prhs[0]); // returns a pointer to the row indices
jcs = (mwIndex *)mxGetJc(prhs[0]); // returns a pointer to the column pointer array
n = mxGetM(prhs[0]);
// second argument : start_points
start_points = mxGetPr(prhs[1]);
nb_start_points = mxGetN(prhs[1]);
// third argument : end_points
end_points = mxGetPr(prhs[2]);
nb_end_points = mxGetN(prhs[2]);
// third argument : nb_iter_max
nb_iter_max = (int) *mxGetPr(prhs[3]);
// second argument : heuristic
if( nrhs==5 )
{
H = mxGetPr(prhs[4]);
if( H!=NULL && (mxGetM(prhs[4])!=n || mxGetN(prhs[4])!=1) )
mexErrMsgTxt("H must be of size n x 1.");
}
else
H = NULL;
// first ouput : distance
plhs[0] = mxCreateDoubleMatrix(n, 1, mxREAL);
D = mxGetPr(plhs[0]);
// second output : state
if( nlhs>=2 )
{
plhs[1] = mxCreateDoubleMatrix(n, 1, mxREAL);
S = mxGetPr(plhs[1]);
}
else
{
S = new double[n];
}
// launch the propagation
perform_dijkstra_propagation();
if( nlhs<2 )
GW_DELETEARRAY(S);
return;
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray*prhs[] )
{
/* retrive arguments */
if( nrhs<4 )
mexErrMsgTxt("4 - 7 input arguments are required.");
if( nlhs<1 )
mexErrMsgTxt("1, 2 or 3 output arguments are required.");
// first argument : weight list
n = mxGetM(prhs[0]);
p = mxGetN(prhs[0]);
W = mxGetPr(prhs[0]);
// second argument : start_points
start_points = mxGetPr(prhs[1]);
int tmp = mxGetM(prhs[1]);
nb_start_points = mxGetN(prhs[1]);
if( nb_start_points==0 || tmp!=2 )
mexErrMsgTxt("start_points must be of size 2 x nb_start_poins.");
// third argument : end_points
end_points = mxGetPr(prhs[2]);
tmp = mxGetM(prhs[2]);
nb_end_points = mxGetN(prhs[2]);
if( nb_end_points!=0 && tmp!=2 )
mexErrMsgTxt("end_points must be of size 2 x nb_end_poins.");
// argument 4: nb_iter_max
nb_iter_max = (int) *mxGetPr(prhs[3]);
// argument 5: heuristic
if( nrhs>=5 )
{
H = mxGetPr(prhs[4]);
if( mxGetM(prhs[4])==0 && mxGetN(prhs[4])==0 )
H=NULL;
if( H!=NULL && (mxGetM(prhs[4])!=n || mxGetN(prhs[4])!=p) )
mexErrMsgTxt("H must be of size n x p.");
}
else
H = NULL;
// argument 6: constraint map
if( nrhs>=6 )
{
L = mxGetPr(prhs[5]);
if( mxGetM(prhs[5])==0 && mxGetN(prhs[5])==0 )
H=NULL;
if( L!=NULL && (mxGetM(prhs[5])!=n || mxGetN(prhs[5])!=p) )
mexErrMsgTxt("L must be of size n x p.");
}
else
L = NULL;
// argument 7: value list
if( nrhs>=7 )
{
values = mxGetPr(prhs[6]);
if( mxGetM(prhs[6])==0 && mxGetN(prhs[6])==0 )
values=NULL;
if( values!=NULL && (mxGetM(prhs[6])!=nb_start_points || mxGetN(prhs[6])!=1) )
mexErrMsgTxt("values must be of size nb_start_points x 1.");
}
else
values = NULL;
// first ouput : distance
plhs[0] = mxCreateDoubleMatrix(n, p, mxREAL);
D = mxGetPr(plhs[0]);
// second output : state
if( nlhs>=2 )
{
plhs[1] = mxCreateDoubleMatrix(n, p, mxREAL);
S = mxGetPr(plhs[1]);
}
else
{
S = new double[n*p];
}
// third output : index
if( nlhs>=3 )
{
plhs[2] = mxCreateDoubleMatrix(n, p, mxREAL);
Q = mxGetPr(plhs[2]);
}
else
{
Q = new double[n*p];
}
// launch the propagation
perform_front_propagation_2d();
if( nlhs<2 )
GW_DELETEARRAY(S);
if( nlhs<3 )
GW_DELETEARRAY(Q);
return;
}
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);
}
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);
}