cddcore.c

CheckMate is a MATLAB-based tool for modeling, simulating and investigating properties of hybrid dyn

    if (!dd_Equal((*RP1)->Ray[j - 1],(*RP2)->Ray[j - 1]))
      *equal = dd_FALSE;
    j++;
  }
  if (*equal)
    fprintf(stderr,"Equal records found !!!!\n");
}

void dd_CreateNewRay(dd_ConePtr cone, 
    dd_RayPtr Ptr1, dd_RayPtr Ptr2, dd_rowrange ii)
{
  /*Create a new ray by taking a linear combination of two rays*/
  dd_colrange j;
  mytype a1, a2, v1, v2;
  static dd_Arow NewRay;
  static dd_colrange last_d=0;
  dd_boolean localdebug=dd_debug;

  dd_init(a1); dd_init(a2); dd_init(v1); dd_init(v2);
  if (last_d!=cone->d){
    if (last_d>0) {
      for (j=0; j<last_d; j++) dd_clear(NewRay[j]);
      free(NewRay);
    }
    NewRay=(mytype*)calloc(cone->d,sizeof(mytype));
    for (j=0; j<cone->d; j++) dd_init(NewRay[j]);
    last_d=cone->d;
  }

  dd_AValue(&a1, cone->d, cone->A, Ptr1->Ray, ii);
  dd_AValue(&a2, cone->d, cone->A, Ptr2->Ray, ii);
  if (localdebug) {
    fprintf(stderr,"CreatNewRay: Ray1 ="); dd_WriteArow(stderr, Ptr1->Ray, cone->d);
    fprintf(stderr,"CreatNewRay: Ray2 ="); dd_WriteArow(stderr, Ptr2->Ray, cone->d);
  }
  dd_abs(v1,a1);
  dd_abs(v2,a2);
  if (localdebug){
    fprintf(stderr,"dd_AValue1 and ABS");  dd_WriteNumber(stderr,a1); dd_WriteNumber(stderr,v1); fprintf(stderr,"\n");
    fprintf(stderr,"dd_AValue2 and ABS");  dd_WriteNumber(stderr,a2); dd_WriteNumber(stderr,v2); fprintf(stderr,"\n");
  }
  for (j = 0; j < cone->d; j++){
    dd_LinearComb(NewRay[j], Ptr1->Ray[j],v2,Ptr2->Ray[j],v1);
  }
  if (localdebug) {
    fprintf(stderr,"CreatNewRay: New ray ="); dd_WriteArow(stderr, NewRay, cone->d);
  }
  dd_Normalize(cone->d, NewRay);
  if (localdebug) {
    fprintf(stderr,"CreatNewRay: dd_Normalized ray ="); dd_WriteArow(stderr, NewRay, cone->d);
  }
  dd_AddRay(cone, NewRay);
  dd_clear(a1); dd_clear(a2); dd_clear(v1); dd_clear(v2);
}

void dd_EvaluateARay1(dd_rowrange i, dd_ConePtr cone)
/* Evaluate the ith component of the vector  A x RD.Ray 
    and rearrange the linked list so that
    the infeasible rays with respect to  i  will be
    placed consecutively from First 
 */
{
  dd_colrange j;
  mytype temp,tnext;
  dd_RayPtr Ptr, PrevPtr, TempPtr;

  dd_init(temp); dd_init(tnext);
  Ptr = cone->FirstRay;
  PrevPtr = cone->ArtificialRay;
  if (PrevPtr->Next != Ptr) {
    fprintf(stderr,"Error.  Artificial Ray does not point to FirstRay!!!\n");
  }
  while (Ptr != NULL) {
    dd_set(temp,dd_purezero);
    for (j = 0; j < cone->d; j++){
      dd_mul(tnext,cone->A[i - 1][j],Ptr->Ray[j]);
      dd_add(temp,temp,tnext);
    }
    dd_set(Ptr->ARay,temp);
/*    if ( temp <= -zero && Ptr != cone->FirstRay) {*/
    if ( dd_Negative(temp) && Ptr != cone->FirstRay) {
      /* fprintf(stderr,"Moving an infeasible record w.r.t. %ld to FirstRay\n",i); */
      if (Ptr==cone->LastRay) cone->LastRay=PrevPtr;
      TempPtr=Ptr;
      Ptr = Ptr->Next;
      PrevPtr->Next = Ptr;
      cone->ArtificialRay->Next = TempPtr;
      TempPtr->Next = cone->FirstRay;
      cone->FirstRay = TempPtr;
    }
    else {
      PrevPtr = Ptr;
      Ptr = Ptr->Next;
    }
  }
  dd_clear(temp); dd_clear(tnext);
}

void dd_EvaluateARay2(dd_rowrange i, dd_ConePtr cone)
/* Evaluate the ith component of the vector  A x RD.Ray 
   and rearrange the linked list so that
   the infeasible rays with respect to  i  will be
   placed consecutively from First. Also for all feasible rays,
   "positive" rays and "zero" rays will be placed consecutively.
 */
{
  dd_colrange j;
  mytype temp,tnext;
  dd_RayPtr Ptr, NextPtr;
  dd_boolean zerofound=dd_FALSE,negfound=dd_FALSE,posfound=dd_FALSE;

  if (cone==NULL || cone->TotalRayCount<=0) goto _L99;  
  dd_init(temp); dd_init(tnext);
  cone->PosHead=NULL;cone->ZeroHead=NULL;cone->NegHead=NULL;
  cone->PosLast=NULL;cone->ZeroLast=NULL;cone->NegLast=NULL;
  Ptr = cone->FirstRay;
  while (Ptr != NULL) {
    NextPtr=Ptr->Next;  /* remember the Next record */
    Ptr->Next=NULL;     /* then clear the Next pointer */
    dd_set(temp,dd_purezero);
    for (j = 0; j < cone->d; j++){
      dd_mul(tnext,cone->A[i - 1][j],Ptr->Ray[j]);
      dd_add(temp,temp,tnext);
    }
    dd_set(Ptr->ARay,temp);
/*    if ( temp < -zero) {*/
    if ( dd_Negative(temp)) {
      if (!negfound){
        negfound=dd_TRUE;
        cone->NegHead=Ptr;
        cone->NegLast=Ptr;
      }
      else{
        Ptr->Next=cone->NegHead;
        cone->NegHead=Ptr;
      }
    }
/*    else if (temp > zero){*/
    else if (dd_Positive(temp)){
      if (!posfound){
        posfound=dd_TRUE;
        cone->PosHead=Ptr;
        cone->PosLast=Ptr;
      }
      else{  
        Ptr->Next=cone->PosHead;
        cone->PosHead=Ptr;
       }
    }
    else {
      if (!zerofound){
        zerofound=dd_TRUE;
        cone->ZeroHead=Ptr;
        cone->ZeroLast=Ptr;
      }
      else{
        Ptr->Next=cone->ZeroHead;
        cone->ZeroHead=Ptr;
      }
    }
    Ptr=NextPtr;
  }
  /* joining three neg, pos and zero lists */
  if (negfound){                 /* -list nonempty */
    cone->FirstRay=cone->NegHead;
    if (posfound){               /* -list & +list nonempty */
      cone->NegLast->Next=cone->PosHead;
      if (zerofound){            /* -list, +list, 0list all nonempty */
        cone->PosLast->Next=cone->ZeroHead;
        cone->LastRay=cone->ZeroLast;
      } 
      else{                      /* -list, +list nonempty but  0list empty */
        cone->LastRay=cone->PosLast;      
      }
    }
    else{                        /* -list nonempty & +list empty */
      if (zerofound){            /* -list,0list nonempty & +list empty */
        cone->NegLast->Next=cone->ZeroHead;
        cone->LastRay=cone->ZeroLast;
      } 
      else {                      /* -list nonempty & +list,0list empty */
        cone->LastRay=cone->NegLast;
      }
    }
  }
  else if (posfound){            /* -list empty & +list nonempty */
    cone->FirstRay=cone->PosHead;
    if (zerofound){              /* -list empty & +list,0list nonempty */
      cone->PosLast->Next=cone->ZeroHead;
      cone->LastRay=cone->ZeroLast;
    } 
    else{                        /* -list,0list empty & +list nonempty */
      cone->LastRay=cone->PosLast;
    }
  }
  else{                          /* -list,+list empty & 0list nonempty */
    cone->FirstRay=cone->ZeroHead;
    cone->LastRay=cone->ZeroLast;
  }
  cone->ArtificialRay->Next=cone->FirstRay;
  cone->LastRay->Next=NULL;
  dd_clear(temp); dd_clear(tnext);
  _L99:;  
}

void dd_DeleteNegativeRays(dd_ConePtr cone)
/* Eliminate the infeasible rays with respect to  i  which
   are supposed to be consecutive from the head of the dd_Ray list,
   and sort the zero list assumed to be consecutive at the
   end of the list.
 */
{
  dd_rowrange fii,fiitest;
  mytype temp;
  dd_RayPtr Ptr, PrevPtr,NextPtr,ZeroPtr1,ZeroPtr0;
  dd_boolean found, completed, zerofound=dd_FALSE,negfound=dd_FALSE,posfound=dd_FALSE;
  dd_boolean localdebug=dd_FALSE;
  
  dd_init(temp);
  cone->PosHead=NULL;cone->ZeroHead=NULL;cone->NegHead=NULL;
  cone->PosLast=NULL;cone->ZeroLast=NULL;cone->NegLast=NULL;

  /* Delete the infeasible rays  */
  PrevPtr= cone->ArtificialRay;
  Ptr = cone->FirstRay;
  if (PrevPtr->Next != Ptr) 
    fprintf(stderr,"Error at dd_DeleteNegativeRays: ArtificialRay does not point the FirstRay.\n");
  completed=dd_FALSE;
  while (Ptr != NULL && !completed){
/*    if ( (Ptr->ARay) < -zero ){ */
    if ( dd_Negative(Ptr->ARay)){
      dd_Eliminate(cone, &PrevPtr);
      Ptr=PrevPtr->Next;
    }
    else{
      completed=dd_TRUE;
    }
  }
  
  /* Sort the zero rays */
  Ptr = cone->FirstRay;
  cone->ZeroRayCount=0;
  while (Ptr != NULL) {
    NextPtr=Ptr->Next;  /* remember the Next record */
    dd_set(temp,Ptr->ARay);
    if (localdebug) {fprintf(stderr,"Ptr->ARay :"); dd_WriteNumber(stderr, temp);}
/*    if ( temp < -zero) {*/
    if ( dd_Negative(temp)) {
      if (!negfound){
        fprintf(stderr,"Error: An infeasible ray found after their removal\n");
        negfound=dd_TRUE;
      }
    }
/*    else if (temp > zero){*/
    else if (dd_Positive(temp)){
      if (!posfound){
        posfound=dd_TRUE;
        cone->PosHead=Ptr;
        cone->PosLast=Ptr;
      }
      else{  
        cone->PosLast=Ptr;
       }
    }
    else {
      (cone->ZeroRayCount)++;
      if (!zerofound){
        zerofound=dd_TRUE;
        cone->ZeroHead=Ptr;
        cone->ZeroLast=Ptr;
        cone->ZeroLast->Next=NULL;
      }
      else{/* Find a right position to store the record sorted w.r.t. FirstInfeasIndex */
        fii=Ptr->FirstInfeasIndex; 
        found=dd_FALSE;
        ZeroPtr1=NULL;
        for (ZeroPtr0=cone->ZeroHead; !found && ZeroPtr0!=NULL ; ZeroPtr0=ZeroPtr0->Next){
          fiitest=ZeroPtr0->FirstInfeasIndex;
          if (fiitest >= fii){
            found=dd_TRUE;
          }
          else ZeroPtr1=ZeroPtr0;
        }
        /* fprintf(stderr,"insert position found \n %d  index %ld\n",found, fiitest); */
        if (!found){           /* the new record must be stored at the end of list */
          cone->ZeroLast->Next=Ptr;
          cone->ZeroLast=Ptr;
          cone->ZeroLast->Next=NULL;
        }
        else{
          if (ZeroPtr1==NULL){ /* store the new one at the head, and update the head ptr */
            /* fprintf(stderr,"Insert at the head\n"); */
            Ptr->Next=cone->ZeroHead;
            cone->ZeroHead=Ptr;
          }
          else{                /* store the new one inbetween ZeroPtr1 and 0 */
            /* fprintf(stderr,"Insert inbetween\n");  */
            Ptr->Next=ZeroPtr1->Next;
            ZeroPtr1->Next=Ptr;
          }
        }
        /*
        Ptr->Next=cone->ZeroHead;
        cone->ZeroHead=Ptr;
        */
      }
    }
    Ptr=NextPtr;
  }
  /* joining the pos and zero lists */
  if (posfound){            /* -list empty & +list nonempty */
    cone->FirstRay=cone->PosHead;
    if (zerofound){              /* +list,0list nonempty */
      cone->PosLast->Next=cone->ZeroHead;
      cone->LastRay=cone->ZeroLast;
    } 
    else{                        /* 0list empty & +list nonempty */
      cone->LastRay=cone->PosLast;
    }
  }
  else{                          /* +list empty & 0list nonempty */
    cone->FirstRay=cone->ZeroHead;
    cone->LastRay=cone->ZeroLast;
  }
  cone->ArtificialRay->Next=cone->FirstRay;
  cone->LastRay->Next=NULL;
  dd_clear(temp);
}

void dd_FeasibilityIndices(long *fnum, long *infnum, dd_rowrange i, dd_ConePtr cone)
{
  /*Evaluate the number of feasible rays and infeasible rays*/
  /*  w.r.t the hyperplane  i*/
  dd_colrange j;
  mytype temp, tnext;
  dd_RayPtr Ptr;

  dd_init(temp); dd_init(tnext);
  *fnum = 0;
  *infnum = 0;
  Ptr = cone->FirstRay;
  while (Ptr != NULL) {
    dd_set(temp,dd_purezero);
    for (j = 0; j < cone->d; j++){
      dd_mul(tnext, cone->A[i - 1][j],Ptr->Ray[j]);
      dd_add(temp, temp, tnext);
    }
    if (dd_Nonnegative(temp))
      (*fnum)++;
    else
      (*infnum)++;
    Ptr = Ptr->Next;
  }
  dd_clear(temp); dd_clear(tnext);
}

dd_boolean dd_LexSmaller(mytype *v1, mytype *v2, long dmax)
{ /* dmax is the size of vectors v1,v2 */
  dd_boolean determined, smaller;
  dd_colrange j;

  smaller = dd_FALSE;
  determined = dd_FALSE;
  j = 1;
  do {
    if (!dd_Equal(v1[j - 1],v2[j - 1])) {  /* 086 */
      if (dd_Smaller(v1[j - 1],v2[j - 1])) {  /*086 */
	    smaller = dd_TRUE;
	  }
      determined = dd_TRUE;
    } else
      j++;
  } while (!(determined) && (j <= dmax));
  return smaller;
}


dd_boolean dd_LexLarger(mytype *v1, mytype *v2, long dmax)
{
  return dd_LexSmaller(v2, v1, dmax);
}

dd_boolean dd_LexEqual(mytype *v1, mytype *v2, long dmax)
{ /* dmax is the size of vectors v1,v2 */
  dd_boolean determined, equal;
  dd_colrange j;

  equal = dd_TRUE;
  determined = dd_FALSE;
  j = 1;
  do {
    if (!dd_Equal(v1[j - 1],v2[j - 1])) {  /* 093c */
	equal = dd_FALSE;
        determined = dd_TRUE;
    } else {
      j++;
    }
  } while (!(determined) && (j <= dmax));
  return equal;
}

void dd_AddNewHalfspace1(dd_ConePtr cone, dd_rowrange hnew)
/* This procedure 1 must be used with PreorderedRun=dd_FALSE 
   This procedure is the most elementary implementation of
   DD and can be used with any type of ordering, including
   dynamic ordering of rows, e.g. MaxCutoff, MinCutoff.
   The memory requirement is minimum because it does not
   store any adjacency among the rays.
*/
{
  dd_RayPtr RayPtr0,RayPtr1,RayPtr2,RayPtr2s,RayPtr3;
  long pos1, pos2;
  double prevprogress, progress;
  mytype value1, value2;
  dd_boolean adj, equal, completed;

  dd_init(value1); dd_init(value2);
  dd_EvaluateARay1(hnew, cone);  
   /*Check feasibility of rays w.r.t. hnew 
     and put all infeasible ones consecutively */

  RayPtr0 = cone->ArtificialRay;   /*Pointer pointing RayPrt1*/
  RayPtr1 = cone->FirstRay;        /*1st hnew-infeasible ray to scan and compare with feasible rays*/
  dd_set(value1,cone->FirstRay->ARay);
  if (dd_Nonnegative(value1)) {
    if (cone->RayCount==cone->WeaklyFeasibleRayCount) cone->CompStatus=dd_AllFound;
    goto _L99;        /* Sicne there is no hnew-infeasible ray and nothing to do */
  }
  else {
    RayPtr2s = RayPtr1->Next;/* RayPtr2s must point the first feasible ray */
    pos2=1;
    while (RayPtr2s!=NULL && dd_Negative(RayPtr2s->ARay)) {
      RayPtr2s = RayPtr2s->Next;
      pos2++;
    }
  }
  if (RayPtr2s==NULL) {
    cone->FirstRay=NULL;
    cone->ArtificialRay->Next=cone->FirstRay;
    cone->RayCount=0;
    cone->CompStatus=dd_AllFound;
    goto _L99;   /* All rays are infeasible, and the computation must stop */
  }
  RayPtr2 = RayPtr2s;   /*2nd feasible ray to scan and compare with 1st*/
  RayPtr3 = cone->LastRay;    /*Last feasible for scanning*/
  prevprogress=-10.0;
  pos1 = 1;
  completed=dd_FALSE;
  while ((RayPtr1 != RayPtr2s) && !completed) {
    dd_set(value1,RayPtr1->ARay);
    dd_set(value2,RayPtr2->ARay);
    dd_CheckEquality(cone->d, &RayPtr1, &RayPtr2, &equal);
    if ((dd_Positive(value1) && dd_Negative(value2)) || (dd_Negative(value1) && dd_Positive(value2))){
      dd_CheckAdjacency(cone, &RayPtr1, &RayPtr2, &adj);
      if (adj) dd_CreateNewRay(cone, RayPtr1, RayPtr2, hnew);
    }
    if (RayPtr2 != RayPtr3) {
      RayPtr2 = RayPtr2->Next;
      continue;
    }
    if (dd_Negative(value1) || equal) {
      dd_Eliminate(cone, &RayPtr0);
      RayPtr1 = RayPtr0->Next;
      RayPtr2 = RayPtr2s;
    } else {
      completed=dd_TRUE;
    }
    pos1++;
    progress = 100.0 * ((double)pos1 / pos2) * (2.0 * pos2 - pos1) / pos2;
    if (progress-prevprogress>=10 && pos1%10==0 && dd_debug) {
      fprintf(stderr,"*Progress of iteration %5ld(/%ld):   %4ld/%4ld => %4.1f%% done\n",
	     cone->Iteration, cone->m, pos1, pos2, progress);
      prevprogress=progress;
    }
  }
  if (cone->RayCount==cone->WeaklyFeasibleRayCount) cone->CompStatus=dd_AllFound;
  _L99:;
  dd_clear(value1); dd_clear(value2);
}

void dd_AddNewHalfspace2(dd_ConePtr cone, dd_rowrange hnew)
/* This procedure must be used under PreOrderedRun mode */
{
  dd_RayPtr RayPtr0,RayPtr1,RayPtr2;
  dd_AdjacencyType *EdgePtr, *EdgePtr0;
  long pos1;
  dd_rowrange fii1, fii2;
  dd_boolean localdebug=dd_FALSE;

  dd_EvaluateARay2(hnew, cone);
   /* Check feasibility of rays w.r.t. hnew 
      and sort them. ( -rays, +rays, 0rays)*/

  if (cone->PosHead==NULL && cone->ZeroHead==NULL) {
    cone->FirstRay=NULL;
    cone->ArtificialRay->Next=cone->FirstRay;
    cone->RayCount=0;
    cone->CompStatus=dd_AllFound;
    goto _L99;   /* All rays are infeasible, and the computation must stop */
  }
  
  if (localdebug){
    pos1=0;
    fprintf(stderr,"(pos, FirstInfeasIndex, A Ray)=\n");
    for (RayPtr0=cone->FirstRay; RayPtr0!=NULL; RayPtr0=RayPtr0->Next){
      pos1++;
      fprintf(stderr,"(%ld,%ld,",pos1,RayPtr0->FirstInfeasIndex);
      dd_WriteNumber(stderr,RayPtr0->ARay); 
      fprintf(stderr,") ");
   }
    fprintf(stderr,"\n");
  }
  
  if (cone->ZeroHead==NULL) cone->ZeroHead=cone->LastRay;

  EdgePtr=cone->Edges[cone->Iteration];
  while (EdgePtr!=NULL){
    RayPtr1=EdgePtr->Ray1;
    RayPtr2=EdgePtr->Ray2;
    fii1=RayPtr1->FirstInfeasIndex;   
    dd_CreateNewRay(cone, RayPtr1, RayPtr2, hnew);
    fii2=cone->LastRay->FirstInfeasIndex;
    if (fii1 != fii2) 
      dd_ConditionalAddEdge(cone,RayPtr1,cone->LastRay,cone->PosHead);
    EdgePtr0=EdgePtr;
    EdgePtr=EdgePtr->Next;
    free(EdgePtr0);
    (cone->EdgeCount)--;