snaphu_solver.c

phase unwrapping algorithm for SAR interferometry

      nodes[row][col].row=row;
      nodes[row][col].col=col;
    }
  }

  /* initialize the ground node */
  if(ground!=NULL){
    ground->row=GROUNDROW;
    ground->col=GROUNDCOL;
  }
}
      

/* function: InitBuckets()
 * -----------------------
 */
void InitBuckets(bucketT *bkts, nodeT *source, long nbuckets){
  
  long i;

  /* set up bucket array parameters */
  bkts->curr=0;
  bkts->wrapped=FALSE;

  /* initialize the buckets */
  for(i=0;i<nbuckets;i++){
    bkts->bucketbase[i]=NULL;
  }

  /* put the source in the zeroth distance index bucket */
  bkts->bucket[0]=source;
  source->next=NULL;
  source->prev=NULL;
  source->group=INBUCKET;
  source->outcost=0;
  
}


/* function: InitNodes()
 * ---------------------
 */
void InitNodes(long nnrow, long nncol, nodeT **nodes, nodeT *ground){

  long row, col;

  /* loop over each element and initialize values */
  for(row=0;row<nnrow;row++){
    for(col=0;col<nncol;col++){
      nodes[row][col].group=NOTINBUCKET;
      nodes[row][col].outcost=VERYFAR;
      nodes[row][col].pred=NULL;
    }
  }

  /* initialize the ground node */
  if(ground!=NULL){
    ground->group=NOTINBUCKET;
    ground->outcost=VERYFAR;
    ground->pred=NULL;
  }
  
}


/* function: BucketInsert()
 * ------------------------
 */
void BucketInsert(nodeT *node, long ind, bucketT *bkts){

  /* put node at beginning of bucket list */
  node->next=bkts->bucket[ind];
  if((bkts->bucket[ind])!=NULL){
    bkts->bucket[ind]->prev=node;
  }
  bkts->bucket[ind]=node;
  node->prev=NULL;

  /* mark node in bucket array */
  node->group=INBUCKET;

}

  
/* function: BucketRemove()
 * ------------------------
 */
void BucketRemove(nodeT *node, long ind, bucketT *bkts){
  
  /* remove node from doubly linked list */
  if((node->next)!=NULL){
    node->next->prev=node->prev;
  }
  if(node->prev!=NULL){
    node->prev->next=node->next;
  }else if(node->next==NULL){    
    bkts->bucket[ind]=NULL;
  }else{
    bkts->bucket[ind]=node->next;
  }

}


/* function: ClosestNode()
 * -----------------------
 */
nodeT *ClosestNode(bucketT *bkts){

  nodeT *node;

  /* find the first bucket with nodes in it */
  while(TRUE){

    /* see if we got to the last bucket */
    if((bkts->curr)>(bkts->maxind)){
	return(NULL);
    }

    /* see if we found a nonempty bucket; if so, return it */
    if((bkts->bucket[bkts->curr])!=NULL){
      node=bkts->bucket[bkts->curr];
      node->group=ONTREE;
      bkts->bucket[bkts->curr]=node->next;
      if((node->next)!=NULL){
	node->next->prev=NULL;
      }
      return(node);
    }

    /* move to next bucket */
    bkts->curr++;
  
  }
}


/* function: ClosestNodeCircular()
 * -------------------------------
 * Similar to ClosestNode(), but assumes circular buckets.  This
 * function should NOT be used if negative arc weights exist on the 
 * network; initial value of bkts->minind should always be zero.
 */
nodeT *ClosestNodeCircular(bucketT *bkts){

  nodeT *node;

  /* find the first bucket with nodes in it */
  while(TRUE){

    /* see if we got to the last bucket */
    if((bkts->curr+bkts->minind)>(bkts->maxind)){
      if(bkts->wrapped){
	bkts->wrapped=FALSE;
	bkts->curr=0;
	bkts->minind+=bkts->size;
	bkts->maxind+=bkts->size;
      }else{
	return(NULL);
      }
    }

    /* see if we found a nonempty bucket; if so, return it */
    if((bkts->bucket[bkts->curr])!=NULL){
      node=bkts->bucket[bkts->curr];
      node->group=ONTREE;
      bkts->bucket[bkts->curr]=node->next;
      if((node->next)!=NULL){
	node->next->prev=NULL;
      }
      return(node);
    }

    /* move to next bucket */
    bkts->curr++;
  
  }
}


/* function: MinOutCostNode()
 * --------------------------
 * Similar to ClosestNode(), but always returns closest node even if its
 * outcost is less than the minimum bucket index.  Does not handle circular
 * buckets.  Does not handle no nodes left condition (this should be handled 
 * by calling function).
 */
nodeT *MinOutCostNode(bucketT *bkts){

  long minoutcost;
  nodeT *node1, *node2;

  /* move to next non-empty bucket */
  while(bkts->curr<bkts->maxind && bkts->bucket[bkts->curr]==NULL){
    bkts->curr++;
  }

  /* scan the whole bucket if it is the overflow or underflow bag */
  if(bkts->curr==bkts->minind || bkts->curr==bkts->maxind){

    node2=bkts->bucket[bkts->curr];
    node1=node2;
    minoutcost=node1->outcost;
    while(node2!=NULL){
      if(node2->outcost<minoutcost){
	minoutcost=node2->outcost;
	node1=node2;
      }
      node2=node2->next;
    }
    BucketRemove(node1,bkts->curr,bkts);

  }else{

    node1=bkts->bucket[bkts->curr];
    bkts->bucket[bkts->curr]=node1->next;
    if(node1->next!=NULL){
      node1->next->prev=NULL;
    }

  }

  return(node1);

}


/* function: SelectSource()
 * ------------------------
 * If params->sourcemode is zero, the ground is returned as the source.  
 * Otherwise, the returned source is the endpoint of the longest chain of
 * arcs carrying at least nflow units of flow.  This function does
 * check for the case where two arcs both carry nflow into or out of a node,
 * but if there are flow cycles (not unexpected for nonlinear costs), the
 * longest chain is not guaranteed.  Which end of the longest chain is
 * determined by the sign of params->sourcemode (should be 1 or -1 if not 0).
 */
nodeT *SelectSource(nodeT **nodes, nodeT *ground, long nflow, 
		    short **flows, long ngroundarcs, 
		    long nrow, long ncol, paramT *params){

  long row, col, maxflowlength, arcnum, upperarcnum;
  long arcrow, arccol, arcdir, endptsign;
  signed char checknode;
  nodeT *source, *node1, *node2, *nextnode;
  nodesuppT **nodesupp;
  
  /* if sourcemode==0, return ground node; otherwise, it should be 1 or -1 */
  if(!params->sourcemode){
    return(ground);
  }else{
    endptsign=params->sourcemode;
  }

  /* initialize variables */
  /* group: 0=unvisited, 1=descended, 2=done */ 
  /* outcost: longest distance to a chain end */
  /* pred: parent node */
  nodesupp=NULL;
  source=ground;
  maxflowlength=0;
  ground->group=0;
  ground->outcost=0;
  ground->pred=NULL;
  for(row=0;row<nrow-1;row++){
    for(col=0;col<ncol-1;col++){
      nodes[row][col].group=0;
      nodes[row][col].outcost=0;
      nodes[row][col].pred=NULL;
    }
  }
  
  /* loop over all nodes (upper row limit is nrow-1 so we can check ground) */
  for(row=0;row<nrow;row++){
    for(col=0;col<ncol-1;col++){

      /* set the current node */
      if(row!=nrow-1){
	node1=&nodes[row][col];
      }else{
	if(col==0){
	  node1=ground;
	}else{
	  break;
	}
      }

      /* see if this node is an endpoint */
      checknode=FALSE;
      if(!node1->group){
	if(node1!=ground){
	  arcnum=-5;
	  upperarcnum=-1;
	}else{
	  arcnum=-1;
	  upperarcnum=ngroundarcs-1;
	}
	while(arcnum<upperarcnum){
	  node2=NeighborNode(node1,++arcnum,&upperarcnum,nodes,ground,
			     &arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
	  
	  /* node is not beginning of a chain (may be the end, though) */
	  if(-endptsign*arcdir*flows[arcrow][arccol] >= nflow){
	    checknode=FALSE;
	    break;
	  }

	  /* node may be beginning of a chain */
	  if(endptsign*arcdir*flows[arcrow][arccol] >= nflow){
	    checknode=TRUE;
	  }
	}
      }

      /* if it is an endpoint, trace the flow and determine longest chain */
      if(checknode){
      
	/* loop until we've walked the whole tree */
	nextnode=node1;
	while(TRUE){

	  node1=nextnode;
	  nextnode=NULL;

	  /* loop over all outgoing arcs */
	  if(node1!=ground){
	    arcnum=-5;
	    upperarcnum=-1;
	  }else{
	    arcnum=-1;
	    upperarcnum=ngroundarcs-1;
	  }
	  while(arcnum<upperarcnum){
	    node2=NeighborNode(node1,++arcnum,&upperarcnum,nodes,ground,
			     &arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);

	    /* see if next node is or should be on tree */
	    /* otherwise, keep node if it is predecessor, but keep looping */
	    if(endptsign*arcdir*flows[arcrow][arccol] >= nflow){
	      if(node2->group==2){
		if(node2->outcost+1 > node1->outcost){
		  node1->outcost=node2->outcost+1;
		}	    
	      }else if(node2->group==0){
		nextnode=node2;
		break;
	      }
	    }else if(node2==node1->pred){
	      nextnode=node2;
	    }
	  }

	  /* we are back to the root if we didn't find any eligible nodes */
	  if(nextnode==NULL){

	    /* see if the tree root should be the new source */
	    if(node1->outcost > maxflowlength){
	      source=node1;
	      maxflowlength=node1->outcost;
	    }
	    node1->group=2;
	    break; 
	  }

	  /* if nextnode is pred, mark current node and go back up the tree */
	  if(nextnode->group==1){
	    node1->group=2;
	  }else{
	    node1->group=1;
	    nextnode->pred=node1;
	  }
      	}
      }
    }
  }
  
  /* return source */
  return(source);

}


/* function: GetCost()
 * -------------------
 * Returns incremental flow cost for current flow increment dflow from
 * lookup array.  
 */
short GetCost(incrcostT **incrcosts, long arcrow, long arccol, 
	      long arcdir){

  /* look up cost and return it for the appropriate arc direction */
  /* we may want add a check here for clipped incremental costs */
  if(arcdir>0){
    return(incrcosts[arcrow][arccol].poscost);
  }else{
    return(incrcosts[arcrow][arccol].negcost);
  }
}


/* function: ReCalcCost()
 * ----------------------
 * Updates the incremental cost for an arc.
 */
long ReCalcCost(void **costs, incrcostT **incrcosts, long flow, 
		long arcrow, long arccol, long nflow, long nrow, 
		paramT *params){

  long poscost, negcost, iclipped;

  /* calculate new positive and negative nflow costs, as long ints */
  CalcCost(costs,flow,arcrow,arccol,nflow,nrow,params,
	   &poscost,&negcost);

  /* clip costs to short int */
  iclipped=0;
  if(poscost>LARGESHORT){
    incrcosts[arcrow][arccol].poscost=LARGESHORT;
    iclipped++;
  }else{
    if(poscost<-LARGESHORT){
      incrcosts[arcrow][arccol].poscost=-LARGESHORT;
      iclipped++;
    }else{
      incrcosts[arcrow][arccol].poscost=poscost;
    }
  }
  if(negcost>LARGESHORT){
    incrcosts[arcrow][arccol].negcost=LARGESHORT;
    iclipped++;
  }else{
    if(negcost<-LARGESHORT){
      incrcosts[arcrow][arccol].negcost=-LARGESHORT;
      iclipped++;
    }else{
      incrcosts[arcrow][arccol].negcost=negcost;
    }
  }

  /* return the number of clipped incremental costs (0, 1, or 2) */
  return(iclipped);
}


/* function: SetupIncrFlowCosts()
 * ------------------------------
 * Calculates the costs for positive and negative dflow flow increment
 * if there is zero flow on the arc.
 */
void SetupIncrFlowCosts(void **costs, incrcostT **incrcosts, short **flows,
			long nflow, long nrow, long narcrow, 
			short *narcsperrow, paramT *params){

  long arcrow, arccol, iclipped, narcs;
  char pl[2];


  /* loop over all rows and columns */
  narcs=0;
  iclipped=0;
  for(arcrow=0;arcrow<narcrow;arcrow++){
    narcs+=narcsperrow[arcrow];
    for(arccol=0;arccol<narcsperrow[arcrow];arccol++){

      /* calculate new positive and negative nflow costs, as long ints */
      iclipped+=ReCalcCost(costs,incrcosts,flows[arcrow][arccol],
			   arcrow,arccol,nflow,nrow,params);
    }
  }

  /* print overflow warning if applicable */
  if(iclipped){