assignmentgraph.cpp

RoboCup 2D 仿真组老牌强队Mersad 2005的完整源代码

/*
 *  Copyright 2002-2005, Mersad Team, Allameh Helli High School (NODET).
 *
 *  This program is free software, you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation.
 *
 *  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 Library General Public License for more details.
 *
 *  This file is created by: Mohammad Salehe
 *  and is modified by: Mostafa Rokooey
 *
 *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include "AssignmentGraph.h"


AssignmentGraph::AssignmentGraph(int size)
{
	int i;
	dim = size;
	weights = new int*[dim];
	for (i = 0; i < dim; i++)
		weights[i] = new int[dim];
	answerRow = new int[dim];
	answerCol = new int[dim];
	u = new int[dim];
	v = new int[dim];
}

AssignmentGraph::~AssignmentGraph()
{
	int i;
	for (i = 0; i < dim; i++)
		delete [] weights[i];
	delete [] weights;
	delete [] answerRow;
	delete [] answerCol;
	delete [] u;
	delete [] v;
}

void AssignmentGraph::resetWeights()
{
	int i, j;
	for (i = 0; i < dim; i++)
		for (j = 0; j < dim; j++)
			weights[i][j] = 0;
}

void AssignmentGraph::setWeight(int row, int col, int weight)
{
	weights[row][col] = weight;
}

int AssignmentGraph::getWeight(int row,int col)
{
	return weights[row][col];
}
	
void AssignmentGraph::prepareMaximum()
{
	int max = -1;
	int i, j;
	for (i = 0; i < dim; i++)
		for (j = 0; j < dim; j++)
			if (weights[i][j] > max)
				max = weights[i][j];
	for (i = 0; i < dim; i++)
		for (j = 0; j < dim; j++)
			weights[i][j] = max - weights[i][j];
	maximumFlag = true;
}

void AssignmentGraph::prepareMinimum()
{
	maximumFlag = false;
}

void AssignmentGraph::solve()
{
/*
 * lap (Linear Assignment Problem). 
 * version 1.0 - 4 September 1996
 * R. Jonker and A. Volgenant, University of Amsterdam.
*/

	bool unassignedfound;
	int  i, imin, numfree = 0, prvnumfree, f, i0, k, freerow, *pred, *free;
	int  j, j1, j2 = 0, endofpath = 0, last = 0, low, up, *collist, *matches;
	int min = 0, h, umin, usubmin, v2, *d;

	free = new int[dim];
	collist = new int[dim];
	matches = new int[dim];
	d = new int[dim];
	pred = new int[dim];

	for (i = 0; i < dim; i++)  
		matches[i] = 0;

	for (j = dim-1; j >= 0; j--)
	{
		min = weights[0][j]; 
		imin = 0;
		for (i = 1; i < dim; i++)  
			if (weights[i][j] < min) 
			{ 
				min = weights[i][j]; 
				imin = i;
			}
		v[j] = min; 

		if (++matches[imin] == 1) 
		{ 
			answerRow[imin] = j; 
			answerCol[j] = imin; 
		}
		else
			answerCol[j] = -1;
	}

	for (i = 0; i < dim; i++) 
		if (matches[i] == 0)
			free[numfree++] = i;
		else
			if (matches[i] == 1)
			{
				j1 = answerRow[i]; 
				min = BIG;
				for (j = 0; j < dim; j++)  
					if (j != j1)
						if (weights[i][j] - v[j] < min) 
							min = weights[i][j] - v[j];
				v[j1] = v[j1] - min;
			}

	int loopcnt = 0;
	do
	{
		loopcnt++;
		k = 0; 
		prvnumfree = numfree; 
		numfree = 0;
		while (k < prvnumfree)
		{
			i = free[k]; 
			k++;

			umin = weights[i][0] - v[0]; 
			j1 = 0; 
			usubmin = BIG;
			for (j = 1; j < dim; j++) 
			{
				h = weights[i][j] - v[j];
				if (h < usubmin)
					if (h >= umin) 
					{ 
						usubmin = h; 
						j2 = j;
					}
					else 
					{ 
						usubmin = umin; 
						umin = h; 
						j2 = j1; 
						j1 = j;
					}
			}

			i0 = answerCol[j1];
			if (umin < usubmin) 
				v[j1] = v[j1] - (usubmin - umin);
			else
				if (i0 >= 0)
				{ 
					j1 = j2; 
					i0 = answerCol[j2];
				}

			answerRow[i] = j1; 
			answerCol[j1] = i;

			if (i0 >= 0)
				if (umin < usubmin) 
					free[--k] = i0; 
				else 
					free[numfree++] = i0; 
		}
	}
	while (loopcnt < 2);

	for (f = 0; f < numfree; f++) 
	{
		freerow = free[f];

		for (j = 0; j < dim; j++)  
		{ 
			d[j] = weights[freerow][j] - v[j]; 
			pred[j] = freerow;
			collist[j] = j;
		}

		low = 0;
		up = 0;
		unassignedfound = false;
		do
		{
			if (up == low)
			{
				last = low - 1; 

				min = d[collist[up++]]; 
				for (k = up; k < dim; k++) 
				{
					j = collist[k]; 
					h = d[j];
					if (h <= min)
					{
						if (h < min)
						{ 
							up = low;
							min = h;
						}
						collist[k] = collist[up]; 
						collist[up++] = j; 
					}
				}

				for (k = low; k < up; k++) 
					if (answerCol[collist[k]] < 0) 
					{
						endofpath = collist[k];
						unassignedfound = true;
						break;
					}
			}

			if (!unassignedfound) 
			{
				j1 = collist[low]; 
				low++; 
				i = answerCol[j1]; 
				h = weights[i][j1] - v[j1] - min;

				for (k = up; k < dim; k++) 
				{
					j = collist[k]; 
					v2 = weights[i][j] - v[j] - h;
					if (v2 < d[j])
					{
						pred[j] = i;
						if (v2 == min)
							if (answerCol[j] < 0) 
							{
								endofpath = j;
								unassignedfound = true;
								break;
							}
							else 
							{ 
								collist[k] = collist[up]; 
								collist[up++] = j; 
							}
						d[j] = v2;
					}
				}
			} 
		}
		while (!unassignedfound);

		for (k = 0; k <= last; k++)  
		{ 
			j1 = collist[k]; 
			v[j1] = v[j1] + d[j1] - min;
		}

		do
		{
			i = pred[endofpath]; 
			answerCol[endofpath] = i; 
			j1 = endofpath; 
			endofpath = answerRow[i]; 
			answerRow[i] = j1;
		}
		while (i != freerow);
	}

	int lapcost = 0;
	for (i = 0; i < dim; i++)  
	{
		j = answerRow[i];
		u[i] = weights[i][j] - v[j];
		lapcost = lapcost + weights[i][j]; 
	}

	delete[] pred;
	delete[] free;
	delete[] collist;
	delete[] matches;
	delete[] d;

	answerCost = lapcost;
}

int AssignmentGraph::getAnswerRow(int row)
{
	return answerRow[row];
}

int AssignmentGraph::getAnswerCol(int col)
{
	return answerCol[col];
}