| /* AUTORIGHTS |
| This program is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU General Public License for more details. |
| |
| You should have received a copy of the GNU General Public License |
| along with this program; if not, write to the Free Software Foundation, |
| Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
| */ |
| |
| /* |
| * emd.c |
| * |
| * Last update: 3/14/98 |
| * |
| * An implementation of the Earth Movers Distance. |
| * Based of the solution for the Transportation problem as described in |
| * "Introduction to Mathematical Programming" by F. S. Hillier and |
| * G. J. Lieberman, McGraw-Hill, 1990. |
| * |
| * Copyright (C) 1998 Yossi Rubner |
| * Computer Science Department, Stanford University |
| * E-Mail: rubner@cs.stanford.edu URL: http://vision.stanford.edu/~rubner |
| * |
| * Bogus static data structures removed November 2005. |
| */ |
| |
| /* This file is modified by William K. Josephson and Wei Dong to by re-entrant |
| * and to work with the Ferret toolkit. Under the permit of the original author, |
| * this file is redistributed under the GPL license */ |
| |
| #include <math.h> |
| #include "emd.h" |
| |
| #define DEBUG_LEVEL 0 |
| /* |
| DEBUG_LEVEL: |
| 0 = NO MESSAGES |
| 1 = PRINT THE NUMBER OF ITERATIONS AND THE FINAL RESULT |
| 2 = PRINT THE RESULT AFTER EVERY ITERATION |
| 3 = PRINT ALSO THE FLOW AFTER EVERY ITERATION |
| 4 = PRINT A LOT OF INFORMATION (PROBABLY USEFUL ONLY FOR THE AUTHOR) |
| */ |
| |
| /* NEW TYPES DEFINITION */ |
| /* GLOBAL VARIABLE DECLARATION */ |
| |
| /* DECLARATION OF FUNCTIONS */ |
| static float emdinit(emd_state_t*, signature_t *Signature1, |
| signature_t *Signature2, |
| float (*Dist)(cass_size_t, feature_t, feature_t, void *), cass_size_t, void *); |
| static void findBasicVariables(emd_state_t*, emd_node1_t *U, emd_node1_t *V); |
| static int isOptimal(emd_state_t*, emd_node1_t *U, emd_node1_t *V); |
| static int findLoop(emd_state_t*, emd_node2_t **Loop); |
| static void newSol(emd_state_t*); |
| static void russel(emd_state_t*, double *S, double *D); |
| static void addBasicVariable(emd_state_t*, int minI, int minJ, double *S, |
| double *D, emd_node1_t *PrevUMinI, |
| emd_node1_t *PrevVMinJ, emd_node1_t *UHead); |
| #if DEBUG_LEVEL > 0 |
| static void printSolution(emd_state_t*); |
| #endif |
| |
| emd_state_t* |
| mkemdstate(void) |
| { |
| emd_state_t *state; |
| |
| state = malloc(sizeof *state); |
| bzero(state, sizeof *state); |
| return state; |
| } |
| |
| void |
| freeemdstate(emd_state_t *state) |
| { |
| free(state); |
| return; |
| } |
| |
| /****************************************************************************** |
| float emd(signature_t *Signature1, signature_t *Signature2, |
| float (*Dist)(feature_t *, feature_t *), flow_t *Flow, int *FlowSize) |
| |
| where |
| |
| Signature1, Signature2 Pointers to signatures that their distance we want |
| to compute. |
| Dist Pointer to the ground distance. i.e. the function that computes |
| the distance between two features. |
| Flow (Optional) Pointer to a vector of flow_t (defined in emd.h) |
| where the resulting flow will be stored. Flow must have n1+n2-1 |
| elements, where n1 and n2 are the sizes of the two signatures |
| respectively. |
| If NULL, the flow is not returned. |
| FlowSize (Optional) Pointer to an integer where the number of elements in |
| Flow will be stored |
| |
| ******************************************************************************/ |
| static __thread double **C = NULL; |
| |
| float |
| emd(signature_t *Signature1, signature_t *Signature2, float (*Dist)(cass_size_t, feature_t, feature_t, void *), cass_size_t dim, void *param, flow_t *Flow, int*FlowSize) |
| { |
| struct emd_state_t emd_state, *state = &emd_state; |
| int itr; |
| double totalCost; |
| float w; |
| emd_node2_t *XP; |
| flow_t *FlowP; |
| emd_node1_t U[MAX_SIG_SIZE1], V[MAX_SIG_SIZE1]; |
| |
| if (C == NULL) C = type_matrix_alloc(double, MAX_SIG_SIZE1, MAX_SIG_SIZE1); |
| |
| bzero(state, sizeof *state); |
| |
| state->C = C; |
| |
| w = emdinit(state, Signature1, Signature2, Dist, dim, param); |
| |
| if (w == EMD_INFINITY) { |
| // init failed, due to nr_reg too high, ignore this seg. |
| return EMD_INFINITY; |
| } |
| |
| #if DEBUG_LEVEL > 1 |
| print("\nINITIAL SOLUTION:\n"); |
| printSolution(state); |
| #endif |
| |
| if (state->n1 > 1 && state->n2 > 1) /* IF _n1 = 1 OR _n2 = 1 THEN WE ARE DONE */ |
| { |
| for (itr = 1; itr < MAX_ITERATIONS; itr++) |
| { |
| /* FIND BASIC VARIABLES */ |
| findBasicVariables(state, U, V); |
| |
| /* CHECK FOR OPTIMALITY */ |
| if (isOptimal(state, U, V)) |
| break; |
| |
| /* IMPROVE SOLUTION */ |
| newSol(state); |
| |
| #if DEBUG_LEVEL > 1 |
| print("\nITERATION # %d \n", itr); |
| printSolution(state); |
| #endif |
| } |
| |
| if (itr == MAX_ITERATIONS) |
| warn("emd: Maximum number of iterations has been reached (%d)\n", |
| MAX_ITERATIONS); |
| } |
| |
| /* COMPUTE THE TOTAL FLOW */ |
| totalCost = 0; |
| FlowP = Flow; |
| for(XP=state->X; XP < state->EndX; XP++) |
| { |
| if (XP == state->EnterX) /* _EnterX IS THE EMPTY SLOT */ |
| continue; |
| if (XP->i == Signature1->n || XP->j == Signature2->n) /* DUMMY FEATURE */ |
| continue; |
| |
| if (XP->val == 0) /* ZERO FLOW */ |
| continue; |
| |
| totalCost += (double)XP->val * state->C[XP->i][XP->j]; |
| if (Flow != NULL) |
| { |
| FlowP->from = XP->i; |
| FlowP->to = XP->j; |
| FlowP->amount = XP->val; |
| FlowP->cost = state->C[XP->i][XP->j]; |
| state->tot_flow_costs += FlowP->cost; |
| state->tot_flow ++; |
| FlowP++; |
| } |
| } |
| if (Flow != NULL) |
| *FlowSize = FlowP-Flow; |
| |
| #if DEBUG_LEVEL > 0 |
| print("\n*** OPTIMAL SOLUTION (%d ITERATIONS): %f ***\n", itr, totalCost); |
| #endif |
| |
| /* RETURN THE NORMALIZED COST == EMD */ |
| // print("avg_flow_costs: %g/%d = %g\n", state->tot_flow_costs, |
| // state->tot_flow, state->tot_flow_costs / state->tot_flow); |
| return (float)(totalCost / w); |
| } |
| |
| static float |
| emdinit(emd_state_t *state, signature_t *Signature1, signature_t *Signature2, float (*Dist)(cass_size_t, feature_t, feature_t, void *), cass_size_t dim, void *param) |
| { |
| int i, j; |
| double sSum, dSum, diff; |
| double S[MAX_SIG_SIZE1], D[MAX_SIG_SIZE1]; |
| |
| state->n1 = Signature1->n; |
| state->n2 = Signature2->n; |
| |
| if (state->n1 > MAX_SIG_SIZE || state->n2 > MAX_SIG_SIZE) |
| { |
| warn("emd: Signature size is limited to %d, n1: %d, n2: %d\n", MAX_SIG_SIZE, state->n1, state->n2); |
| return EMD_INFINITY; |
| } |
| |
| /* COMPUTE THE DISTANCE MATRIX */ |
| state->maxC = 0; |
| for(i=0; i < state->n1; i++) |
| for(j=0; j < state->n2; j++) |
| { |
| state->C[i][j] = Dist(dim, Signature1->Features[i], Signature2->Features[j], param); |
| if (state->C[i][j] > state->maxC) |
| state->maxC = state->C[i][j]; |
| } |
| |
| /* SUM UP THE SUPPLY AND DEMAND */ |
| sSum = 0.0; |
| for(i=0; i < state->n1; i++) |
| { |
| S[i] = Signature1->Weights[i]; |
| sSum += Signature1->Weights[i]; |
| state->RowsX[i] = NULL; |
| } |
| dSum = 0.0; |
| for(j=0; j < state->n2; j++) |
| { |
| D[j] = Signature2->Weights[j]; |
| dSum += Signature2->Weights[j]; |
| state->ColsX[j] = NULL; |
| } |
| |
| /* IF SUPPLY DIFFERENT THAN THE DEMAND, ADD A ZERO-COST DUMMY CLUSTER */ |
| diff = sSum - dSum; |
| if (fabs(diff) >= EPSILON * sSum) |
| { |
| if (diff < 0.0) |
| { |
| for (j=0; j < state->n2; j++) |
| state->C[state->n1][j] = 0; |
| S[state->n1] = -diff; |
| state->RowsX[state->n1] = NULL; |
| state->n1++; |
| } |
| else |
| { |
| for (i=0; i < state->n1; i++) |
| state->C[i][state->n2] = 0; |
| D[state->n2] = diff; |
| state->ColsX[state->n2] = NULL; |
| state->n2++; |
| } |
| } |
| |
| /* INITIALIZE THE BASIC VARIABLE STRUCTURES */ |
| for (i=0; i < state->n1; i++) |
| for (j=0; j < state->n2; j++) |
| state->IsX[i][j] = 0; |
| state->EndX = state->X; |
| |
| state->maxW = sSum > dSum ? sSum : dSum; |
| |
| /* FIND INITIAL SOLUTION */ |
| russel(state, S, D); |
| |
| state->EnterX = state->EndX++; /* AN EMPTY SLOT (ONLY _n1+_n2-1 BASIC VARIABLES) */ |
| |
| return sSum > dSum ? dSum : sSum; |
| } |
| |
| static void |
| findBasicVariables(emd_state_t *state, emd_node1_t *U, emd_node1_t *V) |
| { |
| int i, j, found; |
| int UfoundNum, VfoundNum; |
| emd_node1_t u0Head, u1Head, *CurU, *PrevU; |
| emd_node1_t v0Head, v1Head, *CurV, *PrevV; |
| |
| /* INITIALIZE THE ROWS LIST (U) AND THE COLUMNS LIST (V) */ |
| u0Head.Next = CurU = U; |
| for (i=0; i < state->n1; i++) |
| { |
| CurU->i = i; |
| CurU->Next = CurU+1; |
| CurU++; |
| } |
| (--CurU)->Next = NULL; |
| u1Head.Next = NULL; |
| |
| CurV = V+1; |
| v0Head.Next = state->n2 > 1 ? V+1 : NULL; |
| for (j=1; j < state->n2; j++) |
| { |
| CurV->i = j; |
| CurV->Next = CurV+1; |
| CurV++; |
| } |
| (--CurV)->Next = NULL; |
| v1Head.Next = NULL; |
| |
| /* THERE ARE _n1+_n2 VARIABLES BUT ONLY _n1+_n2-1 INDEPENDENT EQUATIONS, |
| SO SET V[0]=0 */ |
| V[0].i = 0; |
| V[0].val = 0; |
| v1Head.Next = V; |
| v1Head.Next->Next = NULL; |
| |
| /* LOOP UNTIL ALL VARIABLES ARE FOUND */ |
| UfoundNum=VfoundNum=0; |
| while (UfoundNum < state->n1 || VfoundNum < state->n2) |
| { |
| |
| #if DEBUG_LEVEL > 3 |
| print("UfoundNum=%d/%d,VfoundNum=%d/%d\n",UfoundNum,state->n1,VfoundNum,state->n2); |
| print("U0="); |
| for(CurU = u0Head.Next; CurU != NULL; CurU = CurU->Next) |
| print("[%d]",CurU-U); |
| print("\n"); |
| print("U1="); |
| for(CurU = u1Head.Next; CurU != NULL; CurU = CurU->Next) |
| print("[%d]",CurU-U); |
| print("\n"); |
| print("V0="); |
| for(CurV = v0Head.Next; CurV != NULL; CurV = CurV->Next) |
| print("[%d]",CurV-V); |
| print("\n"); |
| print("V1="); |
| for(CurV = v1Head.Next; CurV != NULL; CurV = CurV->Next) |
| print("[%d]",CurV-V); |
| print("\n\n"); |
| #endif |
| |
| found = 0; |
| if (VfoundNum < state->n2) |
| { |
| /* LOOP OVER ALL MARKED COLUMNS */ |
| PrevV = &v1Head; |
| for (CurV=v1Head.Next; CurV != NULL; CurV=CurV->Next) |
| { |
| j = CurV->i; |
| /* FIND THE VARIABLES IN COLUMN j */ |
| PrevU = &u0Head; |
| for (CurU=u0Head.Next; CurU != NULL; CurU=CurU->Next) |
| { |
| i = CurU->i; |
| if (state->IsX[i][j]) |
| { |
| /* COMPUTE U[i] */ |
| CurU->val = state->C[i][j] - CurV->val; |
| /* ...AND ADD IT TO THE MARKED LIST */ |
| PrevU->Next = CurU->Next; |
| CurU->Next = u1Head.Next != NULL ? u1Head.Next : NULL; |
| u1Head.Next = CurU; |
| CurU = PrevU; |
| } |
| else |
| PrevU = CurU; |
| } |
| PrevV->Next = CurV->Next; |
| VfoundNum++; |
| found = 1; |
| } |
| } |
| if (UfoundNum < state->n1) |
| { |
| /* LOOP OVER ALL MARKED ROWS */ |
| PrevU = &u1Head; |
| for (CurU=u1Head.Next; CurU != NULL; CurU=CurU->Next) |
| { |
| i = CurU->i; |
| /* FIND THE VARIABLES IN ROWS i */ |
| PrevV = &v0Head; |
| for (CurV=v0Head.Next; CurV != NULL; CurV=CurV->Next) |
| { |
| j = CurV->i; |
| if (state->IsX[i][j]) |
| { |
| /* COMPUTE V[j] */ |
| CurV->val = state->C[i][j] - CurU->val; |
| /* ...AND ADD IT TO THE MARKED LIST */ |
| PrevV->Next = CurV->Next; |
| CurV->Next = v1Head.Next != NULL ? v1Head.Next: NULL; |
| v1Head.Next = CurV; |
| CurV = PrevV; |
| } |
| else |
| PrevV = CurV; |
| } |
| PrevU->Next = CurU->Next; |
| UfoundNum++; |
| found = 1; |
| } |
| } |
| if (! found) |
| { |
| warn("emd: Unexpected error in findBasicVariables!\n"); |
| warn("This typically happens when the EPSILON defined in\n"); |
| warn("emd.h is not right for the scale of the problem.\n"); |
| fatal("emd: errr in findBasicVariables"); |
| } |
| } |
| } |
| |
| static int |
| isOptimal(emd_state_t *state, emd_node1_t *U, emd_node1_t *V) |
| { |
| double delta, deltaMin; |
| int i, j, minI, minJ; |
| |
| minI = minJ = 0; /* XXX: used but not set */ |
| |
| /* FIND THE MINIMAL Cij-Ui-Vj OVER ALL i,j */ |
| deltaMin = EMD_INFINITY; |
| for(i=0; i < state->n1; i++) |
| for(j=0; j < state->n2; j++) |
| if (! state->IsX[i][j]) |
| { |
| delta = state->C[i][j] - U[i].val - V[j].val; |
| if (deltaMin > delta) |
| { |
| deltaMin = delta; |
| minI = i; |
| minJ = j; |
| } |
| } |
| |
| #if DEBUG_LEVEL > 3 |
| print("deltaMin=%f\n", deltaMin); |
| #endif |
| |
| if (deltaMin == EMD_INFINITY) |
| { |
| warn("emd: Unexpected error in isOptimal.\n"); |
| fatal("emd: error in isOptimal"); |
| } |
| |
| state->EnterX->i = minI; |
| state->EnterX->j = minJ; |
| |
| /* IF NO NEGATIVE deltaMin, WE FOUND THE OPTIMAL SOLUTION */ |
| return deltaMin >= -EPSILON * state->maxC; |
| |
| /* |
| return deltaMin >= -EPSILON; |
| */ |
| } |
| |
| static void |
| newSol(emd_state_t *state) |
| { |
| int i, j, k; |
| double xMin; |
| int steps; |
| emd_node2_t *Loop[2*MAX_SIG_SIZE1], *CurX, *LeaveX; |
| |
| LeaveX = NULL; /* XXX: used but not set */ |
| |
| #if DEBUG_LEVEL > 3 |
| print("EnterX = (%d,%d)\n", state->EnterX->i, state->EnterX->j); |
| #endif |
| |
| /* ENTER THE NEW BASIC VARIABLE */ |
| i = state->EnterX->i; |
| j = state->EnterX->j; |
| state->IsX[i][j] = 1; |
| state->EnterX->NextC = state->RowsX[i]; |
| state->EnterX->NextR = state->ColsX[j]; |
| state->EnterX->val = 0; |
| state->RowsX[i] = state->EnterX; |
| state->ColsX[j] = state->EnterX; |
| |
| /* FIND A CHAIN REACTION */ |
| steps = findLoop(state, Loop); |
| |
| /* FIND THE LARGEST VALUE IN THE LOOP */ |
| xMin = EMD_INFINITY; |
| for (k=1; k < steps; k+=2) |
| { |
| if (Loop[k]->val < xMin) |
| { |
| LeaveX = Loop[k]; |
| xMin = Loop[k]->val; |
| } |
| } |
| |
| /* UPDATE THE LOOP */ |
| for (k=0; k < steps; k+=2) |
| { |
| Loop[k]->val += xMin; |
| Loop[k+1]->val -= xMin; |
| } |
| |
| #if DEBUG_LEVEL > 3 |
| print("LeaveX = (%d,%d)\n", LeaveX->i, LeaveX->j); |
| #endif |
| |
| /* REMOVE THE LEAVING BASIC VARIABLE */ |
| i = LeaveX->i; |
| j = LeaveX->j; |
| state->IsX[i][j] = 0; |
| if (state->RowsX[i] == LeaveX) |
| state->RowsX[i] = LeaveX->NextC; |
| else |
| for (CurX=state->RowsX[i]; CurX != NULL; CurX = CurX->NextC) |
| if (CurX->NextC == LeaveX) |
| { |
| CurX->NextC = CurX->NextC->NextC; |
| break; |
| } |
| if (state->ColsX[j] == LeaveX) |
| state->ColsX[j] = LeaveX->NextR; |
| else |
| for (CurX=state->ColsX[j]; CurX != NULL; CurX = CurX->NextR) |
| if (CurX->NextR == LeaveX) |
| { |
| CurX->NextR = CurX->NextR->NextR; |
| break; |
| } |
| |
| /* SET _EnterX TO BE THE NEW EMPTY SLOT */ |
| state->EnterX = LeaveX; |
| } |
| |
| static int |
| findLoop(emd_state_t *state, emd_node2_t **Loop) |
| { |
| int i, steps; |
| emd_node2_t **CurX, *NewX; |
| char IsUsed[2*MAX_SIG_SIZE1]; |
| |
| for (i=0; i < state->n1+state->n2; i++) |
| IsUsed[i] = 0; |
| |
| CurX = Loop; |
| NewX = *CurX = state->EnterX; |
| IsUsed[state->EnterX-state->X] = 1; |
| steps = 1; |
| |
| do |
| { |
| if (steps%2 == 1) |
| { |
| /* FIND AN UNUSED X IN THE ROW */ |
| NewX = state->RowsX[NewX->i]; |
| while (NewX != NULL && IsUsed[NewX-state->X]) |
| NewX = NewX->NextC; |
| } |
| else |
| { |
| /* FIND AN UNUSED X IN THE COLUMN, OR THE ENTERING X */ |
| NewX = state->ColsX[NewX->j]; |
| while (NewX != NULL && IsUsed[NewX-state->X] && NewX != state->EnterX) |
| NewX = NewX->NextR; |
| if (NewX == state->EnterX) |
| break; |
| } |
| |
| if (NewX != NULL) /* FOUND THE NEXT X */ |
| { |
| /* ADD X TO THE LOOP */ |
| *++CurX = NewX; |
| IsUsed[NewX-state->X] = 1; |
| steps++; |
| #if DEBUG_LEVEL > 3 |
| print("steps=%d, NewX=(%d,%d)\n", steps, NewX->i, NewX->j); |
| #endif |
| } |
| else /* DIDN'T FIND THE NEXT X */ |
| { |
| /* BACKTRACK */ |
| do |
| { |
| NewX = *CurX; |
| do |
| { |
| if (steps%2 == 1) |
| NewX = NewX->NextR; |
| else |
| NewX = NewX->NextC; |
| } while (NewX != NULL && IsUsed[NewX-state->X]); |
| |
| if (NewX == NULL) |
| { |
| IsUsed[*CurX-state->X] = 0; |
| CurX--; |
| steps--; |
| } |
| } while (NewX == NULL && CurX >= Loop); |
| |
| #if DEBUG_LEVEL > 3 |
| print("BACKTRACKING TO: steps=%d, NewX=(%d,%d)\n", |
| steps, NewX->i, NewX->j); |
| #endif |
| IsUsed[*CurX-state->X] = 0; |
| *CurX = NewX; |
| IsUsed[NewX-state->X] = 1; |
| } |
| } while(CurX >= Loop); |
| |
| if (CurX == Loop) |
| { |
| warn("emd: Unexpected error in findLoop!\n"); |
| fatal("emd: findLoop"); |
| } |
| #if DEBUG_LEVEL > 3 |
| print("FOUND LOOP:\n"); |
| for (i=0; i < steps; i++) |
| print("%d: (%d,%d)\n", i, Loop[i]->i, Loop[i]->j); |
| #endif |
| |
| return steps; |
| } |
| |
| static void |
| russel(emd_state_t *state, double *S, double *D) |
| { |
| int i, j, found, minI, minJ; |
| double deltaMin, oldVal, diff; |
| double Delta[MAX_SIG_SIZE1][MAX_SIG_SIZE1]; |
| emd_node1_t Ur[MAX_SIG_SIZE1], Vr[MAX_SIG_SIZE1]; |
| emd_node1_t uHead, *CurU, *PrevU; |
| emd_node1_t vHead, *CurV, *PrevV; |
| emd_node1_t *PrevUMinI, *PrevVMinJ, *Remember; |
| |
| PrevUMinI = PrevVMinJ = NULL; |
| |
| /* INITIALIZE THE ROWS LIST (Ur), AND THE COLUMNS LIST (Vr) */ |
| uHead.Next = CurU = Ur; |
| for (i=0; i < state->n1; i++) |
| { |
| CurU->i = i; |
| CurU->val = -EMD_INFINITY; |
| CurU->Next = CurU+1; |
| CurU++; |
| } |
| (--CurU)->Next = NULL; |
| |
| vHead.Next = CurV = Vr; |
| for (j=0; j < state->n2; j++) |
| { |
| CurV->i = j; |
| CurV->val = -EMD_INFINITY; |
| CurV->Next = CurV+1; |
| CurV++; |
| } |
| (--CurV)->Next = NULL; |
| |
| /* FIND THE MAXIMUM ROW AND COLUMN VALUES (Ur[i] AND Vr[j]) */ |
| for(i=0; i < state->n1 ; i++) |
| for(j=0; j < state->n2 ; j++) |
| { |
| float v; |
| v = state->C[i][j]; |
| if (Ur[i].val <= v) |
| Ur[i].val = v; |
| if (Vr[j].val <= v) |
| Vr[j].val = v; |
| } |
| |
| /* COMPUTE THE Delta MATRIX */ |
| for(i=0; i < state->n1 ; i++) |
| for(j=0; j < state->n2 ; j++) |
| Delta[i][j] = state->C[i][j] - Ur[i].val - Vr[j].val; |
| |
| /* FIND THE BASIC VARIABLES */ |
| do |
| { |
| #if DEBUG_LEVEL > 3 |
| print("Ur="); |
| for(CurU = uHead.Next; CurU != NULL; CurU = CurU->Next) |
| print("[%d]",CurU-Ur); |
| print("\n"); |
| print("Vr="); |
| for(CurV = vHead.Next; CurV != NULL; CurV = CurV->Next) |
| print("[%d]",CurV-Vr); |
| print("\n"); |
| print("\n\n"); |
| #endif |
| |
| /* FIND THE SMALLEST Delta[i][j] */ |
| found = 0; |
| minI = uHead.Next ? uHead.Next->i : 0; /* XXX: used but not set */ |
| minJ = vHead.Next ? vHead.Next->i : 0; /* XXX: used but not set */ |
| deltaMin = EMD_INFINITY; |
| PrevU = &uHead; |
| for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next) |
| { |
| int i; |
| i = CurU->i; |
| PrevV = &vHead; |
| for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next) |
| { |
| int j; |
| j = CurV->i; |
| if (deltaMin > Delta[i][j]) |
| { |
| deltaMin = Delta[i][j]; |
| minI = i; |
| minJ = j; |
| PrevUMinI = PrevU; |
| PrevVMinJ = PrevV; |
| found = 1; |
| } |
| PrevV = CurV; |
| } |
| PrevU = CurU; |
| } |
| |
| if (! found) |
| break; |
| |
| /* ADD X[minI][minJ] TO THE BASIS, AND ADJUST SUPPLIES AND COST */ |
| Remember = PrevUMinI->Next; |
| addBasicVariable(state, minI, minJ, S, D, PrevUMinI, PrevVMinJ, &uHead); |
| |
| /* UPDATE THE NECESSARY Delta[][] */ |
| if (Remember == PrevUMinI->Next) /* LINE minI WAS DELETED */ |
| { |
| for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next) |
| { |
| int j; |
| j = CurV->i; |
| if (CurV->val == state->C[minI][j]) /* COLUMN j NEEDS UPDATING */ |
| { |
| /* FIND THE NEW MAXIMUM VALUE IN THE COLUMN */ |
| oldVal = CurV->val; |
| CurV->val = -EMD_INFINITY; |
| for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next) |
| { |
| int i; |
| i = CurU->i; |
| if (CurV->val <= state->C[i][j]) |
| CurV->val = state->C[i][j]; |
| } |
| |
| /* IF NEEDED, ADJUST THE RELEVANT Delta[*][j] */ |
| diff = oldVal - CurV->val; |
| if (fabs(diff) < EPSILON * state->maxC) |
| for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next) |
| Delta[CurU->i][j] += diff; |
| } |
| } |
| } |
| else /* COLUMN minJ WAS DELETED */ |
| { |
| for (CurU=uHead.Next; CurU != NULL; CurU=CurU->Next) |
| { |
| int i; |
| i = CurU->i; |
| if (CurU->val == state->C[i][minJ]) /* ROW i NEEDS UPDATING */ |
| { |
| /* FIND THE NEW MAXIMUM VALUE IN THE ROW */ |
| oldVal = CurU->val; |
| CurU->val = -EMD_INFINITY; |
| for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next) |
| { |
| int j; |
| j = CurV->i; |
| if(CurU->val <= state->C[i][j]) |
| CurU->val = state->C[i][j]; |
| } |
| |
| /* If NEEDED, ADJUST THE RELEVANT Delta[i][*] */ |
| diff = oldVal - CurU->val; |
| if (fabs(diff) < EPSILON * state->maxC) |
| for (CurV=vHead.Next; CurV != NULL; CurV=CurV->Next) |
| Delta[i][CurV->i] += diff; |
| } |
| } |
| } |
| } while (uHead.Next != NULL || vHead.Next != NULL); |
| } |
| |
| static void |
| addBasicVariable(emd_state_t *state, int minI, int minJ, double *S, double *D, emd_node1_t *PrevUMinI, emd_node1_t *PrevVMinJ, emd_node1_t *UHead) |
| { |
| double T; |
| |
| if (fabs(S[minI]-D[minJ]) <= EPSILON * state->maxW) /* DEGENERATE CASE */ |
| { |
| T = S[minI]; |
| S[minI] = 0; |
| D[minJ] -= T; |
| } |
| else if (S[minI] < D[minJ]) /* SUPPLY EXHAUSTED */ |
| { |
| T = S[minI]; |
| S[minI] = 0; |
| D[minJ] -= T; |
| } |
| else /* DEMAND EXHAUSTED */ |
| { |
| T = D[minJ]; |
| D[minJ] = 0; |
| S[minI] -= T; |
| } |
| |
| /* X(minI,minJ) IS A BASIC VARIABLE */ |
| state->IsX[minI][minJ] = 1; |
| |
| state->EndX->val = T; |
| state->EndX->i = minI; |
| state->EndX->j = minJ; |
| state->EndX->NextC = state->RowsX[minI]; |
| state->EndX->NextR = state->ColsX[minJ]; |
| state->RowsX[minI] = state->EndX; |
| state->ColsX[minJ] = state->EndX; |
| state->EndX++; |
| |
| /* DELETE SUPPLY ROW ONLY IF THE EMPTY, AND IF NOT LAST ROW */ |
| if (S[minI] == 0 && UHead->Next->Next != NULL) |
| PrevUMinI->Next = PrevUMinI->Next->Next; /* REMOVE ROW FROM LIST */ |
| else |
| PrevVMinJ->Next = PrevVMinJ->Next->Next; /* REMOVE COLUMN FROM LIST */ |
| } |
| |
| #if DEBUG_LEVEL > 0 |
| static void |
| printSolution(emd_state_t *state) |
| { |
| emd_node2_t *P; |
| double totalCost; |
| |
| totalCost = 0; |
| |
| #if DEBUG_LEVEL > 2 |
| print("SIG1\tSIG2\tFLOW\tCOST\n"); |
| #endif |
| for(P=state->X; P < state->EndX; P++) |
| if (P != state->EnterX && state->IsX[P->i][P->j]) |
| { |
| #if DEBUG_LEVEL > 2 |
| print("%d\t%d\t%f\t%f\n", P->i, P->j, P->val, state->C[P->i][P->j]); |
| #endif |
| totalCost += (double)P->val * state->C[P->i][P->j]; |
| } |
| |
| print("COST = %f\n", totalCost); |
| } |
| #endif |