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

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

Introduced EMD_LIMIT_WARNING_MESSAGES to limit the number of warning messages printed to stderr

File size: 23.3 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) //print a warning message
150          {
151#define EMD_LIMIT_WARNING_MESSAGES 10 //if thousands of such messages are printed, we group them to avoid spamming stderr. Undefine (comment out) to disable.
152#ifdef EMD_LIMIT_WARNING_MESSAGES
153                  static int warncounter = 0;
154                  static int warnnextlimit = 1;
155                  warncounter++;
156                  if (warncounter == warnnextlimit)
157                  {
158                          fprintf(stderr, "[repeated %dx in total, next warning at %dx] ", warncounter, warnnextlimit * EMD_LIMIT_WARNING_MESSAGES);
159#endif
160                          fprintf(stderr, "emd: Maximum number of iterations (%d) has been reached\n", MAX_ITERATIONS);
161#ifdef EMD_LIMIT_WARNING_MESSAGES
162                          warnnextlimit *= EMD_LIMIT_WARNING_MESSAGES;
163                  }
164#endif
165          }
166  }
167
168  /* COMPUTE THE TOTAL FLOW */
169  totalCost = 0;
170  if (Flow != NULL)
171    FlowP = Flow;
172  for(XP=_XV; XP < _EndX; XP++)
173    {
174      if (XP == _EnterX)  /* _EnterX IS THE EMPTY SLOT */
175        continue;
176      if (XP->i == Signature1->n || XP->j == Signature2->n)  /* DUMMY FEATURE */
177        continue;
178     
179      if (XP->val == 0)  /* ZERO FLOW */
180        continue;
181
182      totalCost += (double)XP->val * _CM[XP->i][XP->j];
183      if (Flow != NULL)
184        {
185          FlowP->from = XP->i;
186          FlowP->to = XP->j;
187          FlowP->amount = XP->val;
188          FlowP++;
189        }
190    }
191  if (Flow != NULL)
192    *FlowSize = FlowP-Flow;
193
194#if DEBUG_LEVEL > 0
195  printf("\n*** OPTIMAL SOLUTION (%d ITERATIONS): %f ***\n", itr, totalCost);
196#endif
197
198  for(int k = 0; k < _n1; ++k)
199  {
200    delete[] _CM[k];
201    delete[] _IsX[k];
202  }
203  delete[] _CM;
204  delete[] _IsX;
205  delete[] _XV;
206  delete[] _RowsX;
207  delete[] _ColsX; 
208
209  delete[] U;
210  delete[] V;
211 
212  /* RETURN THE NORMALIZED COST == EMD */   
213  return (float)(totalCost / w);
214}
215
216
217
218
219
220/**********************
221   init
222**********************/
223static float init(signature_t *Signature1, signature_t *Signature2,
224                  float (*Dist)(feature_t *, feature_t *), int _n1, int _n2,
225        float **_CM, node2_t *_XV, char **_IsX, node2_t **_RowsX, node2_t **_ColsX)
226{
227  int i, j;
228  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.
229  double sSum, dSum, diff;
230  feature_t *P1, *P2;
231  double *S=new double[max_n], *D=new double[max_n];
232
233 
234  /* COMPUTE THE DISTANCE MATRIX */
235  _maxC = 0;
236  for(i=0, P1=Signature1->Features; i < _n1; i++, P1++)
237    for(j=0, P2=Signature2->Features; j < _n2; j++, P2++)
238      {
239        _CM[i][j] = Dist(P1, P2);
240        if (_CM[i][j] > _maxC)
241          _maxC = _CM[i][j];
242      }
243       
244  /* SUM UP THE SUPPLY AND DEMAND */
245  sSum = 0.0;
246  for(i=0; i < _n1; i++)
247    {
248      S[i] = Signature1->Weights[i];
249      sSum += Signature1->Weights[i];
250      _RowsX[i] = NULL;
251    }
252  dSum = 0.0;
253  for(j=0; j < _n2; j++)
254    {
255      D[j] = Signature2->Weights[j];
256      dSum += Signature2->Weights[j];
257      _ColsX[j] = NULL;
258    }
259
260  /* IF SUPPLY DIFFERENT THAN THE DEMAND, ADD A ZERO-COST DUMMY CLUSTER */
261  diff = sSum - dSum;
262  if (fabs(diff) >= EPSILON * sSum)
263    {
264      if (diff < 0.0)
265        {
266          for (j=0; j < _n2; j++)
267            _CM[_n1][j] = 0;
268          S[_n1] = -diff;
269          _RowsX[_n1] = NULL;
270          _n1++;
271        }
272      else
273        {
274          for (i=0; i < _n1; i++)
275            _CM[i][_n2] = 0;
276          D[_n2] = diff;
277          _ColsX[_n2] = NULL;
278          _n2++;
279        }
280    }
281
282  /* INITIALIZE THE BASIC VARIABLE STRUCTURES */
283  for (i=0; i < _n1; i++)
284    for (j=0; j < _n2; j++)
285        _IsX[i][j] = 0;
286  _EndX = _XV;
287   
288  _maxW = sSum > dSum ? sSum : dSum;
289
290  /* FIND INITIAL SOLUTION */
291  russel(S, D, _n1, _n2, _CM, _IsX, _RowsX, _ColsX);
292
293  _EnterX = _EndX++;  /* AN EMPTY SLOT (ONLY _n1+_n2-1 BASIC VARIABLES) */
294
295  delete[] S;
296  delete[] D;
297
298  return sSum > dSum ? dSum : sSum;
299}
300
301
302/**********************
303    findBasicVariables
304 **********************/
305static void findBasicVariables(node1_t *U, node1_t *V, int _n1, int _n2, float **_CM, char **_IsX)
306{
307  int i, j, found;
308  int UfoundNum, VfoundNum;
309  node1_t u0Head, u1Head, *CurU, *PrevU;
310  node1_t v0Head, v1Head, *CurV, *PrevV;
311
312  /* INITIALIZE THE ROWS LIST (U) AND THE COLUMNS LIST (V) */
313  u0Head.Next = CurU = U;
314  for (i=0; i < _n1; i++)
315    {
316      CurU->i = i;
317      CurU->Next = CurU+1;
318      CurU++;
319    }
320  (--CurU)->Next = NULL;
321  u1Head.Next = NULL;
322
323  CurV = V+1;
324  v0Head.Next = _n2 > 1 ? V+1 : NULL;
325  for (j=1; j < _n2; j++)
326    {
327      CurV->i = j;
328      CurV->Next = CurV+1;
329      CurV++;
330    }
331  (--CurV)->Next = NULL;
332  v1Head.Next = NULL;
333
334  /* THERE ARE _n1+_n2 VARIABLES BUT ONLY _n1+_n2-1 INDEPENDENT EQUATIONS,
335     SO SET V[0]=0 */
336  V[0].i = 0;
337  V[0].val = 0;
338  v1Head.Next = V;
339  v1Head.Next->Next = NULL;
340
341  /* LOOP UNTIL ALL VARIABLES ARE FOUND */
342  UfoundNum=VfoundNum=0;
343  while (UfoundNum < _n1 || VfoundNum < _n2)
344    {
345
346#if DEBUG_LEVEL > 3
347      printf("UfoundNum=%d/%d,VfoundNum=%d/%d\n",UfoundNum,_n1,VfoundNum,_n2);
348      printf("U0=");
349      for(CurU = u0Head.Next; CurU != NULL; CurU = CurU->Next)
350        printf("[%d]",CurU-U);
351      printf("\n");
352      printf("U1=");
353      for(CurU = u1Head.Next; CurU != NULL; CurU = CurU->Next)
354        printf("[%d]",CurU-U);
355      printf("\n");
356      printf("V0=");
357      for(CurV = v0Head.Next; CurV != NULL; CurV = CurV->Next)
358        printf("[%d]",CurV-V);
359      printf("\n");
360      printf("V1=");
361      for(CurV = v1Head.Next; CurV != NULL; CurV = CurV->Next)
362        printf("[%d]",CurV-V);
363      printf("\n\n");
364#endif
365     
366      found = 0;
367      if (VfoundNum < _n2)
368        {
369          /* LOOP OVER ALL MARKED COLUMNS */
370          PrevV = &v1Head;
371          for (CurV=v1Head.Next; CurV != NULL; CurV=CurV->Next)
372            {
373              j = CurV->i;
374              /* FIND THE VARIABLES IN COLUMN j */
375              PrevU = &u0Head;
376              for (CurU=u0Head.Next; CurU != NULL; CurU=CurU->Next)
377                {
378                  i = CurU->i;
379                  if (_IsX[i][j])
380                    {
381                      /* COMPUTE U[i] */
382                      CurU->val = _CM[i][j] - CurV->val;
383                      /* ...AND ADD IT TO THE MARKED LIST */
384                      PrevU->Next = CurU->Next;
385                      CurU->Next = u1Head.Next != NULL ? u1Head.Next : NULL;
386                      u1Head.Next = CurU;
387                      CurU = PrevU;
388                    }
389                  else
390                    PrevU = CurU;
391                }
392              PrevV->Next = CurV->Next;
393              VfoundNum++;
394              found = 1;
395            }
396        }
397     if (UfoundNum < _n1)
398        {
399          /* LOOP OVER ALL MARKED ROWS */
400          PrevU = &u1Head;
401          for (CurU=u1Head.Next; CurU != NULL; CurU=CurU->Next)
402            {
403              i = CurU->i;
404              /* FIND THE VARIABLES IN ROWS i */
405              PrevV = &v0Head;
406              for (CurV=v0Head.Next; CurV != NULL; CurV=CurV->Next)
407                {
408                  j = CurV->i;
409                  if (_IsX[i][j])
410                    {
411                      /* COMPUTE V[j] */
412                      CurV->val = _CM[i][j] - CurU->val;
413                      /* ...AND ADD IT TO THE MARKED LIST */
414                      PrevV->Next = CurV->Next;
415                      CurV->Next = v1Head.Next != NULL ? v1Head.Next: NULL;
416                      v1Head.Next = CurV;
417                      CurV = PrevV;
418                    }
419                  else
420                    PrevV = CurV;
421                }
422              PrevU->Next = CurU->Next;
423              UfoundNum++;
424              found = 1;
425            }
426        }
427     if (! found)
428       {
429         fprintf(stderr, "emd: Unexpected error in findBasicVariables!\n");
430         fprintf(stderr, "This typically happens when the EPSILON defined in\n");
431         fprintf(stderr, "emd.h is not right for the scale of the problem.\n");
432         exit(1);
433       }
434    }
435}
436
437
438
439
440/**********************
441    isOptimal
442 **********************/
443static int isOptimal(node1_t *U, node1_t *V, int _n1, int _n2, float **_CM, char **_IsX)
444{   
445  double delta, deltaMin;
446  int i, j, minI, minJ;
447
448  /* FIND THE MINIMAL Cij-Ui-Vj OVER ALL i,j */
449  deltaMin = INFINITY;
450  for(i=0; i < _n1; i++)
451    for(j=0; j < _n2; j++)
452      if (! _IsX[i][j])
453        {
454          delta = _CM[i][j] - U[i].val - V[j].val;
455          if (deltaMin > delta)
456            {
457              deltaMin = delta;
458              minI = i;
459              minJ = j;
460            }
461        }
462
463#if DEBUG_LEVEL > 3
464  printf("deltaMin=%f\n", deltaMin);
465#endif
466
467   if (deltaMin == INFINITY)
468     {
469       fprintf(stderr, "emd: Unexpected error in isOptimal.\n");
470       exit(0);
471     }
472   
473   _EnterX->i = minI;
474   _EnterX->j = minJ;
475   
476   /* IF NO NEGATIVE deltaMin, WE FOUND THE OPTIMAL SOLUTION */
477   return deltaMin >= -EPSILON * _maxC;
478
479/*
480   return deltaMin >= -EPSILON;
481 */
482}
483
484
485/**********************
486    newSol
487**********************/
488static void newSol(int _n1, int _n2, node2_t * _XV, char **_IsX, node2_t **_RowsX, node2_t **_ColsX)
489{
490    int i, j, k;
491    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.
492    double xMin;
493    int steps;
494    node2_t **Loop=new node2_t*[2*max_n], *CurX, *LeaveX;
495 
496#if DEBUG_LEVEL > 3
497    printf("EnterX = (%d,%d)\n", _EnterX->i, _EnterX->j);
498#endif
499
500    /* ENTER THE NEW BASIC VARIABLE */
501    i = _EnterX->i;
502    j = _EnterX->j;
503    _IsX[i][j] = 1;
504    _EnterX->NextC = _RowsX[i];
505    _EnterX->NextR = _ColsX[j];
506    _EnterX->val = 0;
507    _RowsX[i] = _EnterX;
508    _ColsX[j] = _EnterX;
509
510    /* FIND A CHAIN REACTION */
511    steps = findLoop(Loop, _n1, _n2, _XV, _RowsX, _ColsX);
512
513    /* FIND THE LARGEST VALUE IN THE LOOP */
514    xMin = INFINITY;
515    for (k=1; k < steps; k+=2)
516      {
517        if (Loop[k]->val < xMin)
518          {
519            LeaveX = Loop[k];
520            xMin = Loop[k]->val;
521          }
522      }
523
524    /* UPDATE THE LOOP */
525    for (k=0; k < steps; k+=2)
526      {
527        Loop[k]->val += xMin;
528        Loop[k+1]->val -= xMin;
529      }
530
531#if DEBUG_LEVEL > 3
532    printf("LeaveX = (%d,%d)\n", LeaveX->i, LeaveX->j);
533#endif
534
535    /* REMOVE THE LEAVING BASIC VARIABLE */
536    i = LeaveX->i;
537    j = LeaveX->j;
538    _IsX[i][j] = 0;
539    if (_RowsX[i] == LeaveX)
540      _RowsX[i] = LeaveX->NextC;
541    else
542      for (CurX=_RowsX[i]; CurX != NULL; CurX = CurX->NextC)
543        if (CurX->NextC == LeaveX)
544          {
545            CurX->NextC = CurX->NextC->NextC;
546            break;
547          }
548    if (_ColsX[j] == LeaveX)
549      _ColsX[j] = LeaveX->NextR;
550    else
551      for (CurX=_ColsX[j]; CurX != NULL; CurX = CurX->NextR)
552        if (CurX->NextR == LeaveX)
553          {
554            CurX->NextR = CurX->NextR->NextR;
555            break;
556          }
557
558    /* SET _EnterX TO BE THE NEW EMPTY SLOT */
559    _EnterX = LeaveX;
560
561        delete[] Loop;
562}
563
564
565
566/**********************
567    findLoop
568**********************/
569static int findLoop(node2_t **Loop, int _n1, int _n2, node2_t *_XV, node2_t **_RowsX, node2_t **_ColsX)
570{
571  int i, steps;
572  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.
573  node2_t **CurX, *NewX;
574  char *IsUsed=new char[2*max_n];
575 
576  for (i=0; i < _n1+_n2; i++)
577    IsUsed[i] = 0;
578
579  CurX = Loop;
580  NewX = *CurX = _EnterX;
581  IsUsed[_EnterX-_XV] = 1;
582  steps = 1;
583
584  do
585    {
586      if (steps%2 == 1)
587        {
588          /* FIND AN UNUSED X IN THE ROW */
589          NewX = _RowsX[NewX->i];
590          while (NewX != NULL && IsUsed[NewX-_XV])
591            NewX = NewX->NextC;
592        }
593      else
594        {
595          /* FIND AN UNUSED X IN THE COLUMN, OR THE ENTERING X */
596          NewX = _ColsX[NewX->j];
597          while (NewX != NULL && IsUsed[NewX-_XV] && NewX != _EnterX)
598            NewX = NewX->NextR;
599          if (NewX == _EnterX)
600            break;
601        }
602
603     if (NewX != NULL)  /* FOUND THE NEXT X */
604       {
605         /* ADD X TO THE LOOP */
606         *++CurX = NewX;
607         IsUsed[NewX-_XV] = 1;
608         steps++;
609#if DEBUG_LEVEL > 3
610         printf("steps=%d, NewX=(%d,%d)\n", steps, NewX->i, NewX->j);   
611#endif
612       }
613     else  /* DIDN'T FIND THE NEXT X */
614       {
615         /* BACKTRACK */
616         do
617           {
618             NewX = *CurX;
619             do
620               {
621                 if (steps%2 == 1)
622                   NewX = NewX->NextR;
623                 else
624                   NewX = NewX->NextC;
625               } while (NewX != NULL && IsUsed[NewX-_XV]);
626             
627             if (NewX == NULL)
628               {
629                 IsUsed[*CurX-_XV] = 0;
630                 CurX--;
631                 steps--;
632               }
633         } while (NewX == NULL && CurX >= Loop);
634         
635#if DEBUG_LEVEL > 3
636         printf("BACKTRACKING TO: steps=%d, NewX=(%d,%d)\n",
637                steps, NewX->i, NewX->j);   
638#endif
639           IsUsed[*CurX-_XV] = 0;
640           *CurX = NewX;
641           IsUsed[NewX-_XV] = 1;
642       }     
643    } while(CurX >= Loop);
644 
645  if (CurX == Loop)
646    {
647      fprintf(stderr, "emd: Unexpected error in findLoop!\n");
648      exit(1);
649    }
650#if DEBUG_LEVEL > 3
651  printf("FOUND LOOP:\n");
652  for (i=0; i < steps; i++)
653    printf("%d: (%d,%d)\n", i, Loop[i]->i, Loop[i]->j);
654#endif
655
656  delete[] IsUsed;
657
658  return steps;
659}
660
661
662
663/**********************
664    russel
665**********************/
666static void russel(double *S, double *D, int _n1, int _n2, float **_CM, char **_IsX, node2_t **_RowsX, node2_t **_ColsX)
667{
668  int i, j, found, minI, minJ;
669  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.
670  double deltaMin, oldVal, diff;
671  double** Delta = new double*[_n1];
672  for(int k = 0; k < _n1; ++k)
673    Delta[k] = new double[_n2];
674  node1_t *Ur=new node1_t[max_n], *Vr=new node1_t[max_n];
675  node1_t uHead, *CurU, *PrevU;
676  node1_t vHead, *CurV, *PrevV;
677  node1_t *PrevUMinI, *PrevVMinJ, *Remember;
678
679  /* INITIALIZE THE ROWS LIST (Ur), AND THE COLUMNS LIST (Vr) */
680  uHead.Next = CurU = Ur;
681  for (i=0; i < _n1; i++)
682    {
683      CurU->i = i;
684      CurU->val = -INFINITY;
685      CurU->Next = CurU+1;
686      CurU++;
687    }
688  (--CurU)->Next = NULL;
689 
690  vHead.Next = CurV = Vr;
691  for (j=0; j < _n2; j++)
692    {
693      CurV->i = j;
694      CurV->val = -INFINITY;
695      CurV->Next = CurV+1;
696      CurV++;
697    }
698  (--CurV)->Next = NULL;
699 
700  /* FIND THE MAXIMUM ROW AND COLUMN VALUES (Ur[i] AND Vr[j]) */
701  for(i=0; i < _n1 ; i++)
702    for(j=0; j < _n2 ; j++)
703      {
704        float v;
705        v = _CM[i][j];
706        if (Ur[i].val <= v)
707          Ur[i].val = v;
708        if (Vr[j].val <= v)
709          Vr[j].val = v;
710      }
711 
712  /* COMPUTE THE Delta MATRIX */
713  for(i=0; i < _n1 ; i++)
714    for(j=0; j < _n2 ; j++)
715      Delta[i][j] = _CM[i][j] - Ur[i].val - Vr[j].val;
716
717  /* FIND THE BASIC VARIABLES */
718  do
719    {
720#if DEBUG_LEVEL > 3
721      printf("Ur=");
722      for(CurU = uHead.Next; CurU != NULL; CurU = CurU->Next)
723        printf("[%d]",CurU-Ur);
724      printf("\n");
725      printf("Vr=");
726      for(CurV = vHead.Next; CurV != NULL; CurV = CurV->Next)
727        printf("[%d]",CurV-Vr);
728      printf("\n");
729      printf("\n\n");
730#endif
731 
732      /* FIND THE SMALLEST Delta[i][j] */
733      found = 0;
734      deltaMin = INFINITY;     
735      PrevU = &uHead;
736      for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next)
737        {
738          int i;
739          i = CurU->i;
740          PrevV = &vHead;
741          for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next)
742            {
743              int j;
744              j = CurV->i;
745              if (deltaMin > Delta[i][j])
746                {
747                  deltaMin = Delta[i][j];
748                  minI = i;
749                  minJ = j;
750                  PrevUMinI = PrevU;
751                  PrevVMinJ = PrevV;
752                  found = 1;
753                }
754              PrevV = CurV;
755            }
756          PrevU = CurU;
757        }
758     
759      if (! found)
760        break;
761
762      /* ADD X[minI][minJ] TO THE BASIS, AND ADJUST SUPPLIES AND COST */
763      Remember = PrevUMinI->Next;
764      addBasicVariable(minI, minJ, S, D, PrevUMinI, PrevVMinJ, &uHead, _IsX, _RowsX, _ColsX);
765
766      /* UPDATE THE NECESSARY Delta[][] */
767      if (Remember == PrevUMinI->Next)  /* LINE minI WAS DELETED */
768        {
769          for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next)
770            {
771              int j;
772              j = CurV->i;
773              if (CurV->val == _CM[minI][j])  /* COLUMN j NEEDS UPDATING */
774                {
775                  /* FIND THE NEW MAXIMUM VALUE IN THE COLUMN */
776                  oldVal = CurV->val;
777                  CurV->val = -INFINITY;
778                  for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next)
779                    {
780                      int i;
781                      i = CurU->i;
782                      if (CurV->val <= _CM[i][j])
783                        CurV->val = _CM[i][j];
784                    }
785                 
786                  /* IF NEEDED, ADJUST THE RELEVANT Delta[*][j] */
787                  diff = oldVal - CurV->val;
788                  if (fabs(diff) < EPSILON * _maxC)
789                    for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next)
790                      Delta[CurU->i][j] += diff;
791                }
792            }
793        }
794      else  /* COLUMN minJ WAS DELETED */
795        {
796          for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next)
797            {
798              int i;
799              i = CurU->i;
800              if (CurU->val == _CM[i][minJ])  /* ROW i NEEDS UPDATING */
801                {
802                  /* FIND THE NEW MAXIMUM VALUE IN THE ROW */
803                  oldVal = CurU->val;
804                  CurU->val = -INFINITY;
805                  for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next)
806                    {
807                      int j;
808                      j = CurV->i;
809                      if(CurU->val <= _CM[i][j])
810                        CurU->val = _CM[i][j];
811                    }
812                 
813                  /* If NEEDED, ADJUST THE RELEVANT Delta[i][*] */
814                  diff = oldVal - CurU->val;
815                  if (fabs(diff) < EPSILON * _maxC)
816                    for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next)
817                      Delta[i][CurV->i] += diff;
818                }
819            }
820        }
821    } while (uHead.Next != NULL || vHead.Next != NULL);
822
823        delete[] Ur;
824        delete[] Vr;
825    for(int k = 0; k < _n1; ++k)
826      delete[] Delta[k];
827    delete[] Delta;
828}
829
830
831
832
833/**********************
834    addBasicVariable
835**********************/
836static void addBasicVariable(int minI, int minJ, double *S, double *D,
837                             node1_t *PrevUMinI, node1_t *PrevVMinJ,
838                             node1_t *UHead, char **_IsX, node2_t **_RowsX, node2_t **_ColsX)
839{
840  double T;
841 
842  if (fabs(S[minI]-D[minJ]) <= EPSILON * _maxW)  /* DEGENERATE CASE */
843    {
844      T = S[minI];
845      S[minI] = 0;
846      D[minJ] -= T;
847    }
848  else if (S[minI] < D[minJ])  /* SUPPLY EXHAUSTED */
849    {
850      T = S[minI];
851      S[minI] = 0;
852      D[minJ] -= T;
853    }
854  else  /* DEMAND EXHAUSTED */
855    {
856      T = D[minJ];
857      D[minJ] = 0;
858      S[minI] -= T;
859    }
860
861  /* X(minI,minJ) IS A BASIC VARIABLE */
862  _IsX[minI][minJ] = 1;
863
864  _EndX->val = T;
865  _EndX->i = minI;
866  _EndX->j = minJ;
867  _EndX->NextC = _RowsX[minI];
868  _EndX->NextR = _ColsX[minJ];
869  _RowsX[minI] = _EndX;
870  _ColsX[minJ] = _EndX;
871  _EndX++;
872
873  /* DELETE SUPPLY ROW ONLY IF THE EMPTY, AND IF NOT LAST ROW */
874  if (S[minI] == 0 && UHead->Next->Next != NULL)
875    PrevUMinI->Next = PrevUMinI->Next->Next;  /* REMOVE ROW FROM LIST */
876  else
877    PrevVMinJ->Next = PrevVMinJ->Next->Next;  /* REMOVE COLUMN FROM LIST */
878}
Note: See TracBrowser for help on using the repository browser.