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

Last change on this file since 1130 was 1130, checked in by Maciej Komosinski, 10 months ago

Used std::min(), std::max() explicitly to avoid compiler confusion. Used std::size() explicitly instead of the equivalent macro

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