source: cpp/frams/model/similarity/EMD/emd.c @ 1063

Last change on this file since 1063 was 1063, checked in by Maciej Komosinski, 2 months ago

Updated URL for Yossi Rubner's implementation of EMD

File size: 20.2 KB
Line 
1/*
2    emd.c
3
4    Last update: 3/14/98
5
6    An implementation of the Earth Movers Distance.
7    Based of the solution for the Transportation problem as described in
8    "Introduction to Mathematical Programming" by F. S. Hillier and
9    G. J. Lieberman, McGraw-Hill, 1990.
10
11    Copyright (C) 1998 Yossi Rubner
12    Computer Science Department, Stanford University
13    E-Mail: rubner@cs.stanford.edu   URL: http://robotics.stanford.edu/~rubner/emd/default.htm
14*/
15
16#include <stdio.h>
17#include <stdlib.h>
18#include <math.h>
19
20#include "emd.h"
21
22#define DEBUG_LEVEL 0
23/*
24 DEBUG_LEVEL:
25   0 = NO MESSAGES
26   1 = PRINT THE NUMBER OF ITERATIONS AND THE FINAL RESULT
27   2 = PRINT THE RESULT AFTER EVERY ITERATION
28   3 = PRINT ALSO THE FLOW AFTER EVERY ITERATION
29   4 = PRINT A LOT OF INFORMATION (PROBABLY USEFUL ONLY FOR THE AUTHOR)
30*/
31
32/* NEW TYPES DEFINITION */
33
34/* node1_t IS USED FOR SINGLE-LINKED LISTS */
35typedef struct node1_t {
36  int i;
37  double val;
38  struct node1_t *Next;
39} node1_t;
40
41/* node1_t IS USED FOR DOUBLE-LINKED LISTS */
42typedef struct node2_t {
43  int i, j;
44  double val;
45  struct node2_t *NextC;               /* NEXT COLUMN */
46  struct node2_t *NextR;               /* NEXT ROW */
47} node2_t;
48
49
50
51/* GLOBAL VARIABLE DECLARATION */
52
53/* VARIABLES TO HANDLE _X EFFICIENTLY */
54static node2_t *_EndX, *_EnterX;
55static double _maxW;
56static float _maxC;
57
58/* DECLARATION OF FUNCTIONS */
59static float init(signature_t *Signature1, signature_t *Signature2,
60                  float (*Dist)(feature_t *, feature_t *), int _n1, int _n2,
61        float **_CM, node2_t *_XV, char **_IsX, node2_t **_RowsX, node2_t **_ColsX);
62static void findBasicVariables(node1_t *U, node1_t *V, int _n1, int _n2, float **_CM, char **_IsX);
63static int isOptimal(node1_t *U, node1_t *V, int _n1, int _n2, float **_CM, char **_IsX);
64static int findLoop(node2_t **Loop, int _n1, int _n2, node2_t *_XV, node2_t **_RowsX, node2_t **_ColsX);
65static void newSol(int _n1, int _n2, node2_t *_XV, char **_IsX, node2_t **_RowsX, node2_t **_ColsX);
66static void russel(double *S, double *D, int _n1, int _n2, float **_CM, char **_IsX, node2_t **_RowsX, node2_t **_ColsX);
67static void addBasicVariable(int minI, int minJ, double *S, double *D,
68                             node1_t *PrevUMinI, node1_t *PrevVMinJ,
69                             node1_t *UHead, char **_IsX, node2_t **_RowsX, node2_t **_ColsX);
70#if DEBUG_LEVEL > 0
71static void printSolution();
72#endif
73
74
75/******************************************************************************
76float emd(signature_t *Signature1, signature_t *Signature2,
77          float (*Dist)(feature_t *, feature_t *), flow_t *Flow, int *FlowSize)
78 
79where
80
81   Signature1, Signature2  Pointers to signatures that their distance we want
82              to compute.
83   Dist       Pointer to the ground distance. i.e. the function that computes
84              the distance between two features.
85   Flow       (Optional) Pointer to a vector of flow_t (defined in emd.h)
86              where the resulting flow will be stored. Flow must have n1+n2-1
87              elements, where n1 and n2 are the sizes of the two signatures
88              respectively.
89              If NULL, the flow is not returned.
90   FlowSize   (Optional) Pointer to an integer where the number of elements in
91              Flow will be stored
92             
93******************************************************************************/
94
95float emd(signature_t *Signature1, signature_t *Signature2,
96          float (*Dist)(feature_t *, feature_t *),
97          flow_t *Flow, int *FlowSize, int _n1, int _n2)
98{
99  int itr;
100  int max_n = (_n1 > _n2) ? _n1 : _n2;
101  double totalCost;
102  float w;
103  node2_t *XP;
104  flow_t *FlowP;
105  node1_t U[max_n], V[max_n];
106  /* THE COST MATRIX */
107  float** _CM = new float*[_n1];
108  char** _IsX = new char*[_n1];
109  for(int k = 0; k < _n1; ++k)
110  {
111    _CM[k] = new float[_n2];
112    _IsX[k] = new char[_n2];
113  }
114  /* THE BASIC VARIABLES VECTOR */ 
115  node2_t *_XV = new node2_t[max_n*2];
116  node2_t **_RowsX = new node2_t*[max_n*2];
117  node2_t **_ColsX = new node2_t*[max_n*2];
118 
119  w = init(Signature1, Signature2, Dist, _n1, _n2, _CM, _XV, _IsX, _RowsX, _ColsX);
120
121#if DEBUG_LEVEL > 1
122  printf("\nINITIAL SOLUTION:\n");
123  printSolution();
124#endif
125 
126  if (_n1 > 1 && _n2 > 1)  /* IF _n1 = 1 OR _n2 = 1 THEN WE ARE DONE */
127    {
128      for (itr = 1; itr < MAX_ITERATIONS; itr++)
129        {
130          /* FIND BASIC VARIABLES */
131          findBasicVariables(U, V, _n1, _n2, _CM, _IsX);
132         
133          /* CHECK FOR OPTIMALITY */
134          if (isOptimal(U, V, _n1, _n2, _CM, _IsX))
135            break;
136         
137          /* IMPROVE SOLUTION */
138          newSol(_n1, _n2, _XV, _IsX, _RowsX, _ColsX);
139         
140#if DEBUG_LEVEL > 1
141          printf("\nITERATION # %d \n", itr);
142          printSolution();
143#endif
144        }
145
146      if (itr == MAX_ITERATIONS)
147        fprintf(stderr, "emd: Maximum number of iterations has been reached (%d)\n",
148                MAX_ITERATIONS);
149    }
150
151  /* COMPUTE THE TOTAL FLOW */
152  totalCost = 0;
153  if (Flow != NULL)
154    FlowP = Flow;
155  for(XP=_XV; XP < _EndX; XP++)
156    {
157      if (XP == _EnterX)  /* _EnterX IS THE EMPTY SLOT */
158        continue;
159      if (XP->i == Signature1->n || XP->j == Signature2->n)  /* DUMMY FEATURE */
160        continue;
161     
162      if (XP->val == 0)  /* ZERO FLOW */
163        continue;
164
165      totalCost += (double)XP->val * _CM[XP->i][XP->j];
166      if (Flow != NULL)
167        {
168          FlowP->from = XP->i;
169          FlowP->to = XP->j;
170          FlowP->amount = XP->val;
171          FlowP++;
172        }
173    }
174  if (Flow != NULL)
175    *FlowSize = FlowP-Flow;
176
177#if DEBUG_LEVEL > 0
178  printf("\n*** OPTIMAL SOLUTION (%d ITERATIONS): %f ***\n", itr, totalCost);
179#endif
180
181  for(int k = 0; k < _n1; ++k)
182  {
183    delete[] _CM[k];
184    delete[] _IsX[k];
185  }
186  delete[] _CM;
187  delete[] _IsX;
188  delete[] _XV;
189  delete[] _RowsX;
190  delete[] _ColsX; 
191 
192  /* RETURN THE NORMALIZED COST == EMD */   
193  return (float)(totalCost / w);
194}
195
196
197
198
199
200/**********************
201   init
202**********************/
203static float init(signature_t *Signature1, signature_t *Signature2,
204                  float (*Dist)(feature_t *, feature_t *), int _n1, int _n2,
205        float **_CM, node2_t *_XV, char **_IsX, node2_t **_RowsX, node2_t **_ColsX)
206{
207  int i, j;
208  int max_n = (_n1 > _n2) ? _n1 : _n2;
209  double sSum, dSum, diff;
210  feature_t *P1, *P2;
211  double S[max_n], D[max_n];
212
213 
214  /* COMPUTE THE DISTANCE MATRIX */
215  _maxC = 0;
216  for(i=0, P1=Signature1->Features; i < _n1; i++, P1++)
217    for(j=0, P2=Signature2->Features; j < _n2; j++, P2++)
218      {
219        _CM[i][j] = Dist(P1, P2);
220        if (_CM[i][j] > _maxC)
221          _maxC = _CM[i][j];
222      }
223       
224  /* SUM UP THE SUPPLY AND DEMAND */
225  sSum = 0.0;
226  for(i=0; i < _n1; i++)
227    {
228      S[i] = Signature1->Weights[i];
229      sSum += Signature1->Weights[i];
230      _RowsX[i] = NULL;
231    }
232  dSum = 0.0;
233  for(j=0; j < _n2; j++)
234    {
235      D[j] = Signature2->Weights[j];
236      dSum += Signature2->Weights[j];
237      _ColsX[j] = NULL;
238    }
239
240  /* IF SUPPLY DIFFERENT THAN THE DEMAND, ADD A ZERO-COST DUMMY CLUSTER */
241  diff = sSum - dSum;
242  if (fabs(diff) >= EPSILON * sSum)
243    {
244      if (diff < 0.0)
245        {
246          for (j=0; j < _n2; j++)
247            _CM[_n1][j] = 0;
248          S[_n1] = -diff;
249          _RowsX[_n1] = NULL;
250          _n1++;
251        }
252      else
253        {
254          for (i=0; i < _n1; i++)
255            _CM[i][_n2] = 0;
256          D[_n2] = diff;
257          _ColsX[_n2] = NULL;
258          _n2++;
259        }
260    }
261
262  /* INITIALIZE THE BASIC VARIABLE STRUCTURES */
263  for (i=0; i < _n1; i++)
264    for (j=0; j < _n2; j++)
265        _IsX[i][j] = 0;
266  _EndX = _XV;
267   
268  _maxW = sSum > dSum ? sSum : dSum;
269
270  /* FIND INITIAL SOLUTION */
271  russel(S, D, _n1, _n2, _CM, _IsX, _RowsX, _ColsX);
272
273  _EnterX = _EndX++;  /* AN EMPTY SLOT (ONLY _n1+_n2-1 BASIC VARIABLES) */
274
275  return sSum > dSum ? dSum : sSum;
276}
277
278
279/**********************
280    findBasicVariables
281 **********************/
282static void findBasicVariables(node1_t *U, node1_t *V, int _n1, int _n2, float **_CM, char **_IsX)
283{
284  int i, j, found;
285  int UfoundNum, VfoundNum;
286  node1_t u0Head, u1Head, *CurU, *PrevU;
287  node1_t v0Head, v1Head, *CurV, *PrevV;
288
289  /* INITIALIZE THE ROWS LIST (U) AND THE COLUMNS LIST (V) */
290  u0Head.Next = CurU = U;
291  for (i=0; i < _n1; i++)
292    {
293      CurU->i = i;
294      CurU->Next = CurU+1;
295      CurU++;
296    }
297  (--CurU)->Next = NULL;
298  u1Head.Next = NULL;
299
300  CurV = V+1;
301  v0Head.Next = _n2 > 1 ? V+1 : NULL;
302  for (j=1; j < _n2; j++)
303    {
304      CurV->i = j;
305      CurV->Next = CurV+1;
306      CurV++;
307    }
308  (--CurV)->Next = NULL;
309  v1Head.Next = NULL;
310
311  /* THERE ARE _n1+_n2 VARIABLES BUT ONLY _n1+_n2-1 INDEPENDENT EQUATIONS,
312     SO SET V[0]=0 */
313  V[0].i = 0;
314  V[0].val = 0;
315  v1Head.Next = V;
316  v1Head.Next->Next = NULL;
317
318  /* LOOP UNTIL ALL VARIABLES ARE FOUND */
319  UfoundNum=VfoundNum=0;
320  while (UfoundNum < _n1 || VfoundNum < _n2)
321    {
322
323#if DEBUG_LEVEL > 3
324      printf("UfoundNum=%d/%d,VfoundNum=%d/%d\n",UfoundNum,_n1,VfoundNum,_n2);
325      printf("U0=");
326      for(CurU = u0Head.Next; CurU != NULL; CurU = CurU->Next)
327        printf("[%d]",CurU-U);
328      printf("\n");
329      printf("U1=");
330      for(CurU = u1Head.Next; CurU != NULL; CurU = CurU->Next)
331        printf("[%d]",CurU-U);
332      printf("\n");
333      printf("V0=");
334      for(CurV = v0Head.Next; CurV != NULL; CurV = CurV->Next)
335        printf("[%d]",CurV-V);
336      printf("\n");
337      printf("V1=");
338      for(CurV = v1Head.Next; CurV != NULL; CurV = CurV->Next)
339        printf("[%d]",CurV-V);
340      printf("\n\n");
341#endif
342     
343      found = 0;
344      if (VfoundNum < _n2)
345        {
346          /* LOOP OVER ALL MARKED COLUMNS */
347          PrevV = &v1Head;
348          for (CurV=v1Head.Next; CurV != NULL; CurV=CurV->Next)
349            {
350              j = CurV->i;
351              /* FIND THE VARIABLES IN COLUMN j */
352              PrevU = &u0Head;
353              for (CurU=u0Head.Next; CurU != NULL; CurU=CurU->Next)
354                {
355                  i = CurU->i;
356                  if (_IsX[i][j])
357                    {
358                      /* COMPUTE U[i] */
359                      CurU->val = _CM[i][j] - CurV->val;
360                      /* ...AND ADD IT TO THE MARKED LIST */
361                      PrevU->Next = CurU->Next;
362                      CurU->Next = u1Head.Next != NULL ? u1Head.Next : NULL;
363                      u1Head.Next = CurU;
364                      CurU = PrevU;
365                    }
366                  else
367                    PrevU = CurU;
368                }
369              PrevV->Next = CurV->Next;
370              VfoundNum++;
371              found = 1;
372            }
373        }
374     if (UfoundNum < _n1)
375        {
376          /* LOOP OVER ALL MARKED ROWS */
377          PrevU = &u1Head;
378          for (CurU=u1Head.Next; CurU != NULL; CurU=CurU->Next)
379            {
380              i = CurU->i;
381              /* FIND THE VARIABLES IN ROWS i */
382              PrevV = &v0Head;
383              for (CurV=v0Head.Next; CurV != NULL; CurV=CurV->Next)
384                {
385                  j = CurV->i;
386                  if (_IsX[i][j])
387                    {
388                      /* COMPUTE V[j] */
389                      CurV->val = _CM[i][j] - CurU->val;
390                      /* ...AND ADD IT TO THE MARKED LIST */
391                      PrevV->Next = CurV->Next;
392                      CurV->Next = v1Head.Next != NULL ? v1Head.Next: NULL;
393                      v1Head.Next = CurV;
394                      CurV = PrevV;
395                    }
396                  else
397                    PrevV = CurV;
398                }
399              PrevU->Next = CurU->Next;
400              UfoundNum++;
401              found = 1;
402            }
403        }
404     if (! found)
405       {
406         fprintf(stderr, "emd: Unexpected error in findBasicVariables!\n");
407         fprintf(stderr, "This typically happens when the EPSILON defined in\n");
408         fprintf(stderr, "emd.h is not right for the scale of the problem.\n");
409         exit(1);
410       }
411    }
412}
413
414
415
416
417/**********************
418    isOptimal
419 **********************/
420static int isOptimal(node1_t *U, node1_t *V, int _n1, int _n2, float **_CM, char **_IsX)
421{   
422  double delta, deltaMin;
423  int i, j, minI, minJ;
424
425  /* FIND THE MINIMAL Cij-Ui-Vj OVER ALL i,j */
426  deltaMin = INFINITY;
427  for(i=0; i < _n1; i++)
428    for(j=0; j < _n2; j++)
429      if (! _IsX[i][j])
430        {
431          delta = _CM[i][j] - U[i].val - V[j].val;
432          if (deltaMin > delta)
433            {
434              deltaMin = delta;
435              minI = i;
436              minJ = j;
437            }
438        }
439
440#if DEBUG_LEVEL > 3
441  printf("deltaMin=%f\n", deltaMin);
442#endif
443
444   if (deltaMin == INFINITY)
445     {
446       fprintf(stderr, "emd: Unexpected error in isOptimal.\n");
447       exit(0);
448     }
449   
450   _EnterX->i = minI;
451   _EnterX->j = minJ;
452   
453   /* IF NO NEGATIVE deltaMin, WE FOUND THE OPTIMAL SOLUTION */
454   return deltaMin >= -EPSILON * _maxC;
455
456/*
457   return deltaMin >= -EPSILON;
458 */
459}
460
461
462/**********************
463    newSol
464**********************/
465static void newSol(int _n1, int _n2, node2_t * _XV, char **_IsX, node2_t **_RowsX, node2_t **_ColsX)
466{
467    int i, j, k, max_n = (_n1 > _n2) ? _n1 : _n2;
468    double xMin;
469    int steps;
470    node2_t *Loop[2*max_n], *CurX, *LeaveX;
471 
472#if DEBUG_LEVEL > 3
473    printf("EnterX = (%d,%d)\n", _EnterX->i, _EnterX->j);
474#endif
475
476    /* ENTER THE NEW BASIC VARIABLE */
477    i = _EnterX->i;
478    j = _EnterX->j;
479    _IsX[i][j] = 1;
480    _EnterX->NextC = _RowsX[i];
481    _EnterX->NextR = _ColsX[j];
482    _EnterX->val = 0;
483    _RowsX[i] = _EnterX;
484    _ColsX[j] = _EnterX;
485
486    /* FIND A CHAIN REACTION */
487    steps = findLoop(Loop, _n1, _n2, _XV, _RowsX, _ColsX);
488
489    /* FIND THE LARGEST VALUE IN THE LOOP */
490    xMin = INFINITY;
491    for (k=1; k < steps; k+=2)
492      {
493        if (Loop[k]->val < xMin)
494          {
495            LeaveX = Loop[k];
496            xMin = Loop[k]->val;
497          }
498      }
499
500    /* UPDATE THE LOOP */
501    for (k=0; k < steps; k+=2)
502      {
503        Loop[k]->val += xMin;
504        Loop[k+1]->val -= xMin;
505      }
506
507#if DEBUG_LEVEL > 3
508    printf("LeaveX = (%d,%d)\n", LeaveX->i, LeaveX->j);
509#endif
510
511    /* REMOVE THE LEAVING BASIC VARIABLE */
512    i = LeaveX->i;
513    j = LeaveX->j;
514    _IsX[i][j] = 0;
515    if (_RowsX[i] == LeaveX)
516      _RowsX[i] = LeaveX->NextC;
517    else
518      for (CurX=_RowsX[i]; CurX != NULL; CurX = CurX->NextC)
519        if (CurX->NextC == LeaveX)
520          {
521            CurX->NextC = CurX->NextC->NextC;
522            break;
523          }
524    if (_ColsX[j] == LeaveX)
525      _ColsX[j] = LeaveX->NextR;
526    else
527      for (CurX=_ColsX[j]; CurX != NULL; CurX = CurX->NextR)
528        if (CurX->NextR == LeaveX)
529          {
530            CurX->NextR = CurX->NextR->NextR;
531            break;
532          }
533
534    /* SET _EnterX TO BE THE NEW EMPTY SLOT */
535    _EnterX = LeaveX;
536}
537
538
539
540/**********************
541    findLoop
542**********************/
543static int findLoop(node2_t **Loop, int _n1, int _n2, node2_t *_XV, node2_t **_RowsX, node2_t **_ColsX)
544{
545  int i, steps, max_n = (_n1 > _n2) ? _n1 : _n2;
546  node2_t **CurX, *NewX;
547  char IsUsed[2*max_n];
548 
549  for (i=0; i < _n1+_n2; i++)
550    IsUsed[i] = 0;
551
552  CurX = Loop;
553  NewX = *CurX = _EnterX;
554  IsUsed[_EnterX-_XV] = 1;
555  steps = 1;
556
557  do
558    {
559      if (steps%2 == 1)
560        {
561          /* FIND AN UNUSED X IN THE ROW */
562          NewX = _RowsX[NewX->i];
563          while (NewX != NULL && IsUsed[NewX-_XV])
564            NewX = NewX->NextC;
565        }
566      else
567        {
568          /* FIND AN UNUSED X IN THE COLUMN, OR THE ENTERING X */
569          NewX = _ColsX[NewX->j];
570          while (NewX != NULL && IsUsed[NewX-_XV] && NewX != _EnterX)
571            NewX = NewX->NextR;
572          if (NewX == _EnterX)
573            break;
574        }
575
576     if (NewX != NULL)  /* FOUND THE NEXT X */
577       {
578         /* ADD X TO THE LOOP */
579         *++CurX = NewX;
580         IsUsed[NewX-_XV] = 1;
581         steps++;
582#if DEBUG_LEVEL > 3
583         printf("steps=%d, NewX=(%d,%d)\n", steps, NewX->i, NewX->j);   
584#endif
585       }
586     else  /* DIDN'T FIND THE NEXT X */
587       {
588         /* BACKTRACK */
589         do
590           {
591             NewX = *CurX;
592             do
593               {
594                 if (steps%2 == 1)
595                   NewX = NewX->NextR;
596                 else
597                   NewX = NewX->NextC;
598               } while (NewX != NULL && IsUsed[NewX-_XV]);
599             
600             if (NewX == NULL)
601               {
602                 IsUsed[*CurX-_XV] = 0;
603                 CurX--;
604                 steps--;
605               }
606         } while (NewX == NULL && CurX >= Loop);
607         
608#if DEBUG_LEVEL > 3
609         printf("BACKTRACKING TO: steps=%d, NewX=(%d,%d)\n",
610                steps, NewX->i, NewX->j);   
611#endif
612           IsUsed[*CurX-_XV] = 0;
613           *CurX = NewX;
614           IsUsed[NewX-_XV] = 1;
615       }     
616    } while(CurX >= Loop);
617 
618  if (CurX == Loop)
619    {
620      fprintf(stderr, "emd: Unexpected error in findLoop!\n");
621      exit(1);
622    }
623#if DEBUG_LEVEL > 3
624  printf("FOUND LOOP:\n");
625  for (i=0; i < steps; i++)
626    printf("%d: (%d,%d)\n", i, Loop[i]->i, Loop[i]->j);
627#endif
628
629  return steps;
630}
631
632
633
634/**********************
635    russel
636**********************/
637static void russel(double *S, double *D, int _n1, int _n2, float **_CM, char **_IsX, node2_t **_RowsX, node2_t **_ColsX)
638{
639  int i, j, found, minI, minJ, max_n = (_n1 > _n2) ? _n1 : _n2;
640  double deltaMin, oldVal, diff;
641  double** Delta = new double*[_n1];
642  for(int k = 0; k < _n1; ++k)
643    Delta[k] = new double[_n2];
644  node1_t Ur[max_n], Vr[max_n];
645  node1_t uHead, *CurU, *PrevU;
646  node1_t vHead, *CurV, *PrevV;
647  node1_t *PrevUMinI, *PrevVMinJ, *Remember;
648
649  /* INITIALIZE THE ROWS LIST (Ur), AND THE COLUMNS LIST (Vr) */
650  uHead.Next = CurU = Ur;
651  for (i=0; i < _n1; i++)
652    {
653      CurU->i = i;
654      CurU->val = -INFINITY;
655      CurU->Next = CurU+1;
656      CurU++;
657    }
658  (--CurU)->Next = NULL;
659 
660  vHead.Next = CurV = Vr;
661  for (j=0; j < _n2; j++)
662    {
663      CurV->i = j;
664      CurV->val = -INFINITY;
665      CurV->Next = CurV+1;
666      CurV++;
667    }
668  (--CurV)->Next = NULL;
669 
670  /* FIND THE MAXIMUM ROW AND COLUMN VALUES (Ur[i] AND Vr[j]) */
671  for(i=0; i < _n1 ; i++)
672    for(j=0; j < _n2 ; j++)
673      {
674        float v;
675        v = _CM[i][j];
676        if (Ur[i].val <= v)
677          Ur[i].val = v;
678        if (Vr[j].val <= v)
679          Vr[j].val = v;
680      }
681 
682  /* COMPUTE THE Delta MATRIX */
683  for(i=0; i < _n1 ; i++)
684    for(j=0; j < _n2 ; j++)
685      Delta[i][j] = _CM[i][j] - Ur[i].val - Vr[j].val;
686
687  /* FIND THE BASIC VARIABLES */
688  do
689    {
690#if DEBUG_LEVEL > 3
691      printf("Ur=");
692      for(CurU = uHead.Next; CurU != NULL; CurU = CurU->Next)
693        printf("[%d]",CurU-Ur);
694      printf("\n");
695      printf("Vr=");
696      for(CurV = vHead.Next; CurV != NULL; CurV = CurV->Next)
697        printf("[%d]",CurV-Vr);
698      printf("\n");
699      printf("\n\n");
700#endif
701 
702      /* FIND THE SMALLEST Delta[i][j] */
703      found = 0;
704      deltaMin = INFINITY;     
705      PrevU = &uHead;
706      for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next)
707        {
708          int i;
709          i = CurU->i;
710          PrevV = &vHead;
711          for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next)
712            {
713              int j;
714              j = CurV->i;
715              if (deltaMin > Delta[i][j])
716                {
717                  deltaMin = Delta[i][j];
718                  minI = i;
719                  minJ = j;
720                  PrevUMinI = PrevU;
721                  PrevVMinJ = PrevV;
722                  found = 1;
723                }
724              PrevV = CurV;
725            }
726          PrevU = CurU;
727        }
728     
729      if (! found)
730        break;
731
732      /* ADD X[minI][minJ] TO THE BASIS, AND ADJUST SUPPLIES AND COST */
733      Remember = PrevUMinI->Next;
734      addBasicVariable(minI, minJ, S, D, PrevUMinI, PrevVMinJ, &uHead, _IsX, _RowsX, _ColsX);
735
736      /* UPDATE THE NECESSARY Delta[][] */
737      if (Remember == PrevUMinI->Next)  /* LINE minI WAS DELETED */
738        {
739          for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next)
740            {
741              int j;
742              j = CurV->i;
743              if (CurV->val == _CM[minI][j])  /* COLUMN j NEEDS UPDATING */
744                {
745                  /* FIND THE NEW MAXIMUM VALUE IN THE COLUMN */
746                  oldVal = CurV->val;
747                  CurV->val = -INFINITY;
748                  for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next)
749                    {
750                      int i;
751                      i = CurU->i;
752                      if (CurV->val <= _CM[i][j])
753                        CurV->val = _CM[i][j];
754                    }
755                 
756                  /* IF NEEDED, ADJUST THE RELEVANT Delta[*][j] */
757                  diff = oldVal - CurV->val;
758                  if (fabs(diff) < EPSILON * _maxC)
759                    for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next)
760                      Delta[CurU->i][j] += diff;
761                }
762            }
763        }
764      else  /* COLUMN minJ WAS DELETED */
765        {
766          for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next)
767            {
768              int i;
769              i = CurU->i;
770              if (CurU->val == _CM[i][minJ])  /* ROW i NEEDS UPDATING */
771                {
772                  /* FIND THE NEW MAXIMUM VALUE IN THE ROW */
773                  oldVal = CurU->val;
774                  CurU->val = -INFINITY;
775                  for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next)
776                    {
777                      int j;
778                      j = CurV->i;
779                      if(CurU->val <= _CM[i][j])
780                        CurU->val = _CM[i][j];
781                    }
782                 
783                  /* If NEEDED, ADJUST THE RELEVANT Delta[i][*] */
784                  diff = oldVal - CurU->val;
785                  if (fabs(diff) < EPSILON * _maxC)
786                    for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next)
787                      Delta[i][CurV->i] += diff;
788                }
789            }
790        }
791    } while (uHead.Next != NULL || vHead.Next != NULL);
792    for(int k = 0; k < _n1; ++k)
793      delete[] Delta[k];
794    delete[] Delta;
795}
796
797
798
799
800/**********************
801    addBasicVariable
802**********************/
803static void addBasicVariable(int minI, int minJ, double *S, double *D,
804                             node1_t *PrevUMinI, node1_t *PrevVMinJ,
805                             node1_t *UHead, char **_IsX, node2_t **_RowsX, node2_t **_ColsX)
806{
807  double T;
808 
809  if (fabs(S[minI]-D[minJ]) <= EPSILON * _maxW)  /* DEGENERATE CASE */
810    {
811      T = S[minI];
812      S[minI] = 0;
813      D[minJ] -= T;
814    }
815  else if (S[minI] < D[minJ])  /* SUPPLY EXHAUSTED */
816    {
817      T = S[minI];
818      S[minI] = 0;
819      D[minJ] -= T;
820    }
821  else  /* DEMAND EXHAUSTED */
822    {
823      T = D[minJ];
824      D[minJ] = 0;
825      S[minI] -= T;
826    }
827
828  /* X(minI,minJ) IS A BASIC VARIABLE */
829  _IsX[minI][minJ] = 1;
830
831  _EndX->val = T;
832  _EndX->i = minI;
833  _EndX->j = minJ;
834  _EndX->NextC = _RowsX[minI];
835  _EndX->NextR = _ColsX[minJ];
836  _RowsX[minI] = _EndX;
837  _ColsX[minJ] = _EndX;
838  _EndX++;
839
840  /* DELETE SUPPLY ROW ONLY IF THE EMPTY, AND IF NOT LAST ROW */
841  if (S[minI] == 0 && UHead->Next->Next != NULL)
842    PrevUMinI->Next = PrevUMinI->Next->Next;  /* REMOVE ROW FROM LIST */
843  else
844    PrevVMinJ->Next = PrevVMinJ->Next->Next;  /* REMOVE COLUMN FROM LIST */
845}
Note: See TracBrowser for help on using the repository browser.