LAP.cpp

//
//  LAP.cpp
//  pickPocket
//
//  Created by Ambrish Roy on 11/11/15.
//  Copyright (c) 2015 Ambrish Roy. All rights reserved.
//
#include <stdio.h>
#include "LAP.h"
#include "Common.h"


/************************************************************************
 *
 *  lap.cpp
 version 1.0 - 4 September 1996
 author: Roy Jonker @ MagicLogic Optimization Inc.
 e-mail: roy_jonker@magiclogic.com
 
 Code for Linear Assignment Problem, according to
 
 "A Shortest Augmenting Path Algorithm for Dense and Sparse Linear
 Assignment Problems," Computing 38, 325-340, 1987
 
 by
 
 R. Jonker and A. Volgenant, University of Amsterdam.
 *
 *************************************************************************/


double lap(int dim, double **assigncost, col *rowsol, row *colsol, double *u, double *v){
    /*
     Input:
     dim        - problem size
     assigncost - cost matrix
     
     Output:
     rowsol     - column assigned to row in solution
     colsol     - row assigned to column in solution
     u          - dual variables, row reduction numbers
     v          - dual variables, column reduction numbers
     */
    
    bool   unassignedfound;
    row    i=0, imin=0, numfree = 0, prvnumfree=0, f=0, i0=0, k=0, freerow=0, *pred=0, *free=0;
    col    j=0, j1=0, j2=0, endofpath=0, last=0, low=0, up=0, *collist=0, *matches=0;
    double min=0, h=0, umin=0, usubmin=0, v2=0, *d=0;
    
    free    = new row[dim];       // list of unassigned rows.
    collist = new col[dim];       // list of columns to be scanned in various ways.
    matches = new col[dim];       // counts how many times a row could be assigned.
    d       = new double[dim];    // 'cost-distance' in augmenting path calculation.
    pred    = new row[dim];       // row-predecessor of column in augmenting/alternating path.
    
    // init how many times a row will be assigned in the column reduction.
    for (i = 0; i < dim; i++)
        matches[i] = 0;
    
    // COLUMN REDUCTION
    for(j = dim-1; j >= 0; j--){    // reverse order gives better results.
        // find minimum cost over rows.
        min = assigncost[0][j];
        imin = 0;
        for (i = 1; i < dim; i++)
            if (assigncost[i][j] < min){
                min = assigncost[i][j];
                imin = i;
            }
        v[j] = min;
        
        if (++matches[imin] == 1){
            // init assignment if minimum row assigned for first time.
            rowsol[imin] = j;
            colsol[j] = imin;
        }
        else
            colsol[j] = -1;        // row already assigned, column not assigned.
    }
    // REDUCTION TRANSFER
    for (i = 0; i < dim; i++)
        if (matches[i] == 0)     // fill list of unassigned 'free' rows.
            free[numfree++] = i;
        else
            if (matches[i] == 1){  // transfer reduction from rows that are assigned once.
                j1 = rowsol[i];
                min = BIG;
                for (j = 0; j < dim; j++)
                    if (j != j1)
                        if (assigncost[i][j] - v[j] < min)
                            min = assigncost[i][j] - v[j];
                v[j1] = v[j1] - min;
            }
    // AUGMENTING ROW REDUCTION
    int loopcnt = 0;           // do-loop to be done twice.
    do{
        loopcnt++;
        //cout << loopcnt << " 1" << endl;
        // scan all free rows.
        // in some cases, a free row may be replaced with another one to be scanned next.
        k = 0;
        prvnumfree = numfree;
        numfree = 0;             // start list of rows still free after augmenting row reduction.
        int cnt =0;
        while (k < prvnumfree){
            i = free[k];
            k++;
            cnt++;
            // find minimum and second minimum reduced cost over columns.
            umin = assigncost[i][0] - v[0];
            j1 = 0;
            usubmin = BIG;
            for (j = 1; j < dim; j++){
                h = assigncost[i][j] - v[j];
                if(cnt > 500){h = assigncost[i][j] - v[j] + rand(0,1);}
                if (h < usubmin){
                    if (h >= umin){
                        usubmin = h;
                        j2 = j;
                    }
                    else{
                        usubmin = umin;
                        umin = h;
                        j2 = j1;
                        j1 = j;
                    }
                }
            }
            i0 = colsol[j1];
            if (umin < usubmin)
                // change the reduction of the minimum column to increase the minimum
                // reduced cost in the row to the subminimum.
                v[j1] = v[j1] - (usubmin - umin);
            else                   // minimum and subminimum equal.
                if(i0 >= 0){         // minimum column j1 is assigned.
                    // swap columns j1 and j2, as j2 may be unassigned.
                    j1 = j2;
                    i0 = colsol[j2];
                }
            
            // (re-)assign i to j1, possibly de-assigning an i0.
            rowsol[i] = j1;
            colsol[j1] = i;
            
            if (i0 >= 0){           // minimum column j1 assigned earlier.
                if (umin < usubmin)
                    // put in current k, and go back to that k.
                    // continue augmenting path i - j1 with i0.
                    free[--k] = i0;
                else
                    // no further augmenting reduction possible.
                    // store i0 in list of free rows for next phase.
                    free[numfree++] = i0;
            }
        }
    }
    while (loopcnt < 2);       // repeat once.
    // AUGMENT SOLUTION for each free row.
    for (f = 0; f < numfree; f++){
        freerow = free[f];       // start row of augmenting path.
        
        // Dijkstra shortest path algorithm.
        // runs until unassigned column added to shortest path tree.
        for (j = 0; j < dim; j++){
            d[j] = assigncost[freerow][j] - v[j];
            pred[j] = freerow;
            collist[j] = j;        // init column list.
        }
        
        low = 0; // columns in 0..low-1 are ready, now none.
        up = 0;  // columns in low..up-1 are to be scanned for current minimum, now none.
        // columns in up..dim-1 are to be considered later to find new minimum,
        // at this stage the list simply contains all columns
        unassignedfound = FALSE;
        do{
            if(up == low){         // no more columns to be scanned for current minimum.
                last = low - 1;
                
                // scan columns for up..dim-1 to find all indices for which new minimum occurs.
                // store these indices between low..up-1 (increasing up).
                min = d[collist[up++]];
                for (k = up; k < dim; k++){
                    j = collist[k];
                    h = d[j];
                    if (h <= min){
                        if (h < min){     // new minimum.
                            up = low;      // restart list at index low.
                            min = h;
                        }
                        // new index with same minimum, put on undex up, and extend list.
                        collist[k] = collist[up];
                        collist[up++] = j;
                    }
                }
                
                // check if any of the minimum columns happens to be unassigned.
                // if so, we have an augmenting path right away.
                for (k = low; k < up; k++)
                    if (colsol[collist[k]] < 0){
                        endofpath = collist[k];
                        unassignedfound = TRUE;
                        break;
                    }
            }
            
            if (!unassignedfound){
                // update 'distances' between freerow and all unscanned columns, via next scanned column.
                j1 = collist[low];
                low++;
                i = colsol[j1];
                h = assigncost[i][j1] - v[j1] - min;
                
                for (k = up; k < dim; k++){
                    j = collist[k];
                    v2 = assigncost[i][j] - v[j] - h;
                    if(v2 < d[j]){
                        pred[j] = i;
                        if (v2 == min){   // new column found at same minimum value
                            if (colsol[j] < 0){
                                // if unassigned, shortest augmenting path is complete.
                                endofpath = j;
                                unassignedfound = TRUE;
                                break;
                            }
                            // else add to list to be scanned right away.
                            else{
                                collist[k] = collist[up];
                                collist[up++] = j;
                            }
                        }
                        d[j] = v2;
                    }
                }
            }
        }
        while (!unassignedfound);
        
        // update column prices.
        for (k = 0; k <= last; k++){
            j1 = collist[k];
            v[j1] = v[j1] + d[j1] - min;
        }
        
        // reset row and column assignments along the alternating path.
        do{
            i = pred[endofpath];
            colsol[endofpath] = i;
            j1 = endofpath;
            endofpath = rowsol[i];
            rowsol[i] = j1;
        }
        while (i != freerow);
    }
    
    // calculate optimal cost.
    cost lapcost = 0;
    for (i = 0; i < dim; i++)
    {
        j = rowsol[i];
        u[i] = assigncost[i][j] - v[j];
        lapcost = lapcost + assigncost[i][j];
    }
    
    // free reserved memory.
    delete[] pred;
    delete[] free;
    delete[] collist;
    delete[] matches;
    delete[] d;
    return lapcost;
}

void checklap(int dim, double **assigncost, col *rowsol, row *colsol, double *u, double *v){
    row  i=0;
    col  j=0;
    double  redcost = 0;
    bool *matched;
//    char wait;
    
    matched = new bool[dim];
    
    for (i = 0; i < dim; i++)  
        for (j = 0; j < dim; j++)  
            if ((redcost = assigncost[i][j] - u[i] - v[j]) < 0){
                printf("\n");
                printf("negative reduced cost i %d j %d redcost %f\n", i, j, redcost);
                printf("\n\ndim %5d - press key\n", dim);
//                scanf("%s", &wait);
                break; 
            }
    
    for (i = 0; i < dim; i++)  
        if ((redcost = assigncost[i][rowsol[i]] - u[i] - v[rowsol[i]]) != 0){
            printf("\n");
            printf("non-null reduced cost i %d soli %d redcost %f\n", i, rowsol[i], redcost);
            printf("\n\ndim %5d - press key\n", dim);
//            scanf("%s", &wait);
            break; 
        }
    
    for (j = 0; j < dim; j++)  
        matched[j] = FALSE;
    
    for (i = 0; i < dim; i++)  
        if (matched[rowsol[i]]){
            printf("\n");
            printf("column matched more than once - i %d soli %d\n", i, rowsol[i]);
            printf("\n\ndim %5d - press key\n", dim);
//            scanf("%s", &wait);
            break; 
        }
        else
            matched[rowsol[i]] = TRUE;
    
    
    for (i = 0; i < dim; i++)  
        if (colsol[rowsol[i]] != i)
        {
            printf("\n");
            printf("error in row solution i %d soli %d solsoli %d\n", i, rowsol[i], colsol[rowsol[i]]);
            printf("\n\ndim %5d - press key\n", dim);
//            scanf("%s", &wait);
            break; 
        }
    
    for (j = 0; j < dim; j++)  
        if (rowsol[colsol[j]] != j){
            printf("\n");
            printf("error in col solution j %d solj %d solsolj %d\n", j, colsol[j], rowsol[colsol[j]]);
            printf("\n\ndim %5d - press key\n", dim);
//            scanf("%s", &wait);
            break; 
        }
    
    delete[] matched;
    return;
}