dijkstra.cpp

实现核ISOMAP算法,解决测试点问题和ROBUST问题

        }
     } while (Temp != NULL && Temp != Tree);
     mexPrintf( "\n" );

     Temp = Tree;
     do {
	mexPrintf( "Children of " );
	Temp->Print();
	mexPrintf( ": " );
	if (Temp->Child == NULL)
	     mexPrintf( "NONE\n" );
        else Print(Temp->Child, Temp);
	Temp = Temp->Right;
     } while (Temp!=NULL && Temp != Tree);

     if (theParent == NULL)
     {
     char ch;

	 mexPrintf( "\n\n\n" );
         cin >> ch;
     }
}

//===========================================================================
//===========================================================================

void FibHeap::_Exchange(FibHeapNode*& N1, FibHeapNode*& N2)
{
FibHeapNode *Temp;

     Temp = N1;
     N1 = N2;
     N2 = Temp;
}

//===========================================================================
// Consolidate()
//
// Internal function that reorganizes heap as part of an ExtractMin().
// We must find new minimum in heap, which could be anywhere along the
// root list.  The search could be O(n) since all nodes could be on
// the root list.  So, we reorganize the tree into more of a binomial forest
// structure, and then find the new minimum on the consolidated O(lg n) sized
// root list, and in the process set each Parent pointer to NULL, and count
// the number of resulting subtrees.
//
// Note that after a list of n inserts, there will be n nodes on the root
// list, so the first ExtractMin() will be O(n) regardless of whether or
// not we consolidate.  However, the consolidation causes subsequent
// ExtractMin() operations to be O(lg n).  Furthermore, the extra cost of
// the first ExtractMin() is covered by the higher amortized cost of
// Insert(), which is the real governing factor in how costly the first
// ExtractMin() will be.
//===========================================================================

void FibHeap::_Consolidate()
{
FibHeapNode *x, *y, *w;
FibHeapNode *A[1+8*sizeof(long)]; // 1+lg(n)
int  I=0, Dn = 1+8*sizeof(long);
short d;

// Initialize the consolidation detection array

     for (I=0; I < Dn; I++)
          A[I] = NULL;

// We need to loop through all elements on root list.
// When a collision of degree is found, the two trees
// are consolidated in favor of the one with the lesser
// element key value.  We first need to break the circle
// so that we can have a stopping condition (we can't go
// around until we reach the tree we started with
// because all root trees are subject to becoming a
// child during the consolidation).

     MinRoot->Left->Right = NULL;
     MinRoot->Left = NULL;
     w = MinRoot;

     do {
//cout << "Top of Consolidate's loop\n";
//Print(w);

	x = w;
        d = x->Degree;
        w = w->Right;

// We need another loop here because the consolidated result
// may collide with another large tree on the root list.

        while (A[d] != NULL)
        {
             y = A[d];
	     if (*y < *x)
		 _Exchange(x, y);
             if (w == y) w = y->Right;
             _Link(y, x);
             A[d] = NULL;
             d++;
//cout << "After a round of Linking\n";
//Print(x);
	}
	A[d] = x;

     } while (w != NULL);

// Now we rebuild the root list, find the new minimum,
// set all root list nodes' parent pointers to NULL and
// count the number of subtrees.

     MinRoot = NULL;
     NumTrees = 0;
     for (I = 0; I < Dn; I++)
          if (A[I] != NULL)
              _AddToRootList(A[I]);
}

//===========================================================================
// The node y is removed from the root list and becomes a subtree of node x.
//===========================================================================

void FibHeap::_Link(FibHeapNode *y, FibHeapNode *x)
{
// Remove node y from root list

     if (y->Right != NULL)
	 y->Right->Left = y->Left;
     if (y->Left != NULL)
         y->Left->Right = y->Right;
     NumTrees--;

// Make node y a singleton circular list with a parent of x

     y->Left = y->Right = y;
     y->Parent = x;

// If node x has no children, then list y is its new child list

     if (x->Child == NULL)
	 x->Child = y;

// Otherwise, node y must be added to node x's child list

     else
     {
         y->Left = x->Child;
         y->Right = x->Child->Right;
         x->Child->Right = y;
         y->Right->Left = y;
     }

// Increase the degree of node x because it's now a bigger tree

     x->Degree ++;

// Node y has just been made a child, so clear its mark

     if (y->Mark) NumMarkedNodes--;
     y->Mark = 0;
}

//===========================================================================
//===========================================================================

void FibHeap::_AddToRootList(FibHeapNode *x)
{
     if (x->Mark) NumMarkedNodes --;
     x->Mark = 0;

     NumNodes--;
     Insert(x);
}

//===========================================================================
// Remove node x from the child list of its parent node y
//===========================================================================

void FibHeap::_Cut(FibHeapNode *x, FibHeapNode *y)
{
     if (y->Child == x)
         y->Child = x->Right;
     if (y->Child == x)
	 y->Child = NULL;

     y->Degree --;

     x->Left->Right = x->Right;
     x->Right->Left = x->Left;

     _AddToRootList(x);
}

//===========================================================================
// Cuts each node in parent list, putting successive ancestor nodes on the
// root list until we either arrive at the root list or until we find an
// ancestor that is unmarked.  When a mark is set (which only happens during
// a cascading cut), it means that one child subtree has been lost; if a
// second subtree is lost later during another cascading cut, then we move
// the node to the root list so that it can be re-balanced on the next
// consolidate. 
//===========================================================================

void FibHeap::_CascadingCut(FibHeapNode *y)
{
FibHeapNode *z = y->Parent;

     while (z != NULL)
     {
         if (y->Mark == 0)
         {
             y->Mark = 1;
             NumMarkedNodes++;
             z = NULL;
         }
         else
         {
             _Cut(y, z);
             y = z;
	     z = y->Parent;
         }
     }
}


class HeapNode : public FibHeapNode
{
      double   N;
      long int IndexV;

public:

      HeapNode() : FibHeapNode() { N = 0; };   

      virtual void operator =(FibHeapNode& RHS);
      virtual int  operator ==(FibHeapNode& RHS);
      virtual int  operator <(FibHeapNode& RHS);

      virtual void operator =(double NewKeyVal );
      virtual void Print();

      double GetKeyValue() { return N; };       /* !!!! */
      void SetKeyValue(double n) { N = n; };

      long int GetIndexValue() { return IndexV; };
      void SetIndexValue( long int v) { IndexV = v; };

};

void HeapNode::Print()
{
     FibHeapNode::Print();
     mexPrintf( "%f (%d)" , N , IndexV );
}

void HeapNode::operator =(double NewKeyVal)
{
     HeapNode Temp;
     Temp.N = N = NewKeyVal;
     FHN_Assign(Temp);
}

void HeapNode::operator =(FibHeapNode& RHS)
{
     FHN_Assign(RHS);
     N = ((HeapNode&) RHS).N;
}

int  HeapNode::operator ==(FibHeapNode& RHS)
{
     if (FHN_Cmp(RHS)) return 0;
     return N == ((HeapNode&) RHS).N ? 1 : 0;
}

int  HeapNode::operator <(FibHeapNode& RHS)
{
int X;

     if ((X=FHN_Cmp(RHS)) != 0)
	  return X < 0 ? 1 : 0;

     return N < ((HeapNode&) RHS).N ? 1 : 0;
};

int IntCmp(const void *pA, const void *pB)
{
int A, B;

    A = *((const int *) pA);
    B = *((const int *) pB);
    if (A < B) return -1;
    if (A == B) return 0;
    return 1; 
}

void dodijk_sparse( 
             long int M,
             long int N,
             long int S,
             double   *D,
             double   *sr,
             int      *irs,
             int      *jcs,
             HeapNode *A,
             FibHeap  *theHeap  )
{
   int      finished;
   long int i,startind,endind,whichneighbor,ndone,index,switchwith,closest,closesti;
   long int *INDICES;
   double   closestD,arclength; 
   double   INF,SMALL,olddist;
   HeapNode *Min;
   HeapNode Temp;

   INF   = mxGetInf();
   SMALL = mxGetEps();

   /* initialize */
   for (i=0; i<M; i++) 
   {
      if (i!=S) A[ i ] = (double) INF; else A[ i ] = (double) SMALL;
      if (i!=S) D[ i ] = (double) INF; else D[ i ] = (double) SMALL;
	  theHeap->Insert( &A[i] );
      A[ i ].SetIndexValue( (long int) i );
   }
   

   // Insert 0 then extract it.  This will cause the
   // Fibonacci heap to get balanced.

   theHeap->Insert(&Temp);
   theHeap->ExtractMin();

   /*theHeap->Print();
   for (i=0; i<M; i++)
   {
      closest = A[ i ].GetIndexValue();
      closestD = A[ i ].GetKeyValue();
      mexPrintf( "Index at i=%d =%d  value=%f\n" , i , closest , closestD );
   }*/   

   /* loop over nonreached nodes */
   finished = 0;
   ndone    = 0;
   while ((finished==0) && (ndone < M))
   {
      //if ((ndone % 100) == 0) mexPrintf( "Done with node %d\n" , ndone );

      Min = (HeapNode *) theHeap->ExtractMin();
      closest  = Min->GetIndexValue();
      closestD = Min->GetKeyValue();

      if ((closest<0) || (closest>=M)) mexErrMsgTxt( "Minimum Index out of bound..." );

      //theHeap->Print();
      //mexPrintf( "EXTRACTED MINIMUM  NDone=%d S=%d closest=%d closestD=%f\n" , ndone , S , closest , closestD );
      //mexErrMsgTxt( "Exiting..." );

      D[ closest ] = closestD;

      if (closestD == INF) finished=1; else
      {
         /* add the closest to the determined list */
         ndone++;         
          
         /* relax all nodes adjacent to closest */
         startind = jcs[ closest   ];
         endind   = jcs[ closest+1 ] - 1;

         if (startind!=endind+1)
         for (i=startind; i<=endind; i++)
         {
            whichneighbor = irs[ i ];
            arclength = sr[ i ];
            olddist   = D[ whichneighbor ];

            //mexPrintf( "INSPECT NEIGHBOR #%d  olddist=%f newdist=%f\n" , whichneighbor , olddist , closestD+arclength );            

            if ( olddist > ( closestD + arclength ))
            {
               D[ whichneighbor ] = closestD + arclength;

	           Temp = A[ whichneighbor ];
	           Temp.SetKeyValue( closestD + arclength );
               theHeap->DecreaseKey( &A[ whichneighbor ], Temp );

               //mexPrintf( "UPDATING NODE #%d  olddist=%f newdist=%f\n" , whichneighbor , olddist , closestD+arclength );
            }
         }

      }
      
   }
}


void mexFunction(
		 int          nlhs,
		 mxArray      *plhs[],
		 int          nrhs,
		 const mxArray *prhs[]
		 )
{
  double    *sr,*D,*P,*SS,*Dsmall,*Psmall;
  int       *irs,*jcs;
  long int  M,N,S,MS,NS,i,j,in;

  HeapNode *A = NULL;
  FibHeap  *theHeap = NULL;
  
  if (nrhs != 2)
  {
      mexErrMsgTxt( "Only 2 input arguments allowed." );
  }
      else if (nlhs != 1) 
   {
      mexErrMsgTxt( "Only 1 output argument allowed." );
   }
   
   M = mxGetM( prhs[0] );
   N = mxGetN( prhs[0] );
   
   if (M != N) mexErrMsgTxt( "Input matrix needs to be square." );
    
   SS = mxGetPr(prhs[1]);
   MS = mxGetM( prhs[1] );
   NS = mxGetN( prhs[1] );
     
   if ((MS==0) || (NS==0) || ((MS>1) && (NS>1))) mexErrMsgTxt( "Source nodes are specified in one dimensional matrix only" );
   if (NS>MS) MS=NS;
     
   plhs[0] = mxCreateDoubleMatrix( MS,M, mxREAL);
   D = mxGetPr(plhs[0]);
    
   Dsmall = (double *) mxCalloc( M , sizeof( double ));

   if (mxIsSparse( prhs[ 0 ] ) == 1)
   {
     /* dealing with sparse array */
     sr      = mxGetPr(prhs[0]);
     irs     = mxGetIr(prhs[0]);
     jcs     = mxGetJc(prhs[0]);

     // Setup for the Fibonacci heap

     

     for (i=0; i<MS; i++)
     {
        if ((theHeap = new FibHeap) == NULL || (A = new HeapNode[M+1]) == NULL )
        {
	      mexErrMsgTxt( "Memory allocation failed-- ABORTING.\n" );
        }

        theHeap->ClearHeapOwnership();

        S = (long int) *( SS + i );
        S--;

        if ((S < 0) || (S > M-1)) mexErrMsgTxt( "Source node(s) out of bound" );

        /* -------------------------------------------------------------------------------------------------
                                    run the dijkstra code 
           ------------------------------------------------------------------------------------------------- */

        //mexPrintf( "Working on i=%d\n" , i );

        dodijk_sparse( M,N,S,Dsmall,sr,irs,jcs,A,theHeap );

        for (j=0; j<M; j++) 
        {
           *( D + j*MS + i ) = *( Dsmall + j );

         //mexPrintf( "Distance i=%d to j=%d =%f\n" , S+1 , j , *( Dsmall + j ) ); 
        }
         
        
       
        /* -------------------------------------------------------------------------------------------------
                                    end of the dijkstra code 
           ------------------------------------------------------------------------------------------------- */
        
        delete theHeap;
        delete[] A;
     } 

     

   } else mexErrMsgTxt( "Function not implemented for full arrays" );

}