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); }