| gvSIG desktop 1

package org.gvsig.graph.solvers;

import java.util.Random;

import com.sun.org.apache.xpath.internal.operations.Mod;

public class TspSolverAnnealing {

	int n;

	int[] iorder;

	int[] jorder;

	float[] b = new float[4];

	double[][] odMatrix;

	boolean g_bVolverOrigen;

	int origenTSP, destinoTSP;

	int[] vTSP;

	static int T_INIT = 100;

	static double FINAL_T = 0.1;

	static double COOLING = 0.9; /* to lower down T (< 1) */

	int TRIES_PER_T = 500*n; // TODO: PONER ESTO BIEN.  

	int IMPROVED_PATH_PER_T = 60*n;  // TODO: PONER ESTO BIEN.  

//	static long A[56]= {-1};

//	long *rand_fptr = A;

//	#define mod_diff(x,y)   (((x)-(y))&0x7fffffff) 

//	private long flipCycle()

//	{

//		long *ii,*jj;

//		for (ii = &A[1], jj = &A[32]; jj <= &A[55]; ii++, jj++)

//		*ii= mod_diff (*ii, *jj);

//

//		for (jj = &A[1]; ii <= &A[55]; ii++, jj++)

//		*ii= mod_diff (*ii, *jj);

//		rand_fptr = &A[54];

//		return A[55];

//	}

//	typedef int Path[3];      /* specify how to change path */

	/*

	 * State vars

	 */

	int     verbose = 0;

//	 Point   *cities;

//	 int     *dist;

//	#define D(x,y) odMatrix[x][y]    //dist[(x)*n+y]		

	/* float calcula_dist_ordenacion(int v[], int bVolverOrigen)

	{

		float dist, distTot;

		int i;

	        distTot = odMatrix[origenTSP][v[0]]; // Origen al primer punto

	        for (i = 0; i< numElemTSP-1;i++)

			{

	            //frmDocMap.Distancia v(i), v(i + 1), dist, tiempo

				dist = odMatrix[v[i]][v[i+1]];

	            distTot = distTot + dist;

			}

	        //  desde y hasta el almacen (distancia al primero y al ?ltimo

	        // frmDocMap.Distancia idNodoOrigen, v(0), dist, tiempo

	        if (bVolverOrigen)

			{            

				dist = odMatrix[v[numElemTSP-1]][origenTSP];

	            distTot = distTot + dist;

	        }

			else

			{

				dist = odMatrix[v[numElemTSP-1]][destinoTSP];

				distTot = distTot + dist;

			}

			return distTot;

	} */

	double pathLength()

	{

		int i; 

		double len = 0;

		len = odMatrix[origenTSP][iorder[0]];

		for (i = 0; i < n-1; i++)

		{

			len += odMatrix[iorder[i]][iorder[i+1]];

		}

		if (g_bVolverOrigen)

			len += odMatrix[iorder[n-1]][origenTSP]; /* close path */

		else

			len += odMatrix[iorder[n-1]][destinoTSP]; /* close path */

		return (len);

	}

	/*

	 * Prim's approximated TSP tour

	 * See also [Cristophides'92]

	 */

	// TODO: NO LO USO => Borrar esta funci?n.

	void findEulerianPath()

	{

		int[] mst = new int[n];

		int[] arc = new int[n];	

		int i, j = 0, k = 0, l, a;

		double d, maxd;

		double[] dis = new double[n];

		maxd = Math.sqrt(b[1]-b[0])+ Math.sqrt(b[3]-b[2]);

		d    = maxd;

		dis[0] = -1;

		for (i = 1; i < n; i++)

		{

			dis[i] = odMatrix[i][0]; arc[i] = 0;

		        if (d > dis[i])

			{

				d = dis[i];

				j = i;

			}

		}

		/*

		 * O(n^2) Minimum Spanning Trees by Prim and Jarnick 

		 * for graphs with adjacency matrix. 

		 */

		for (a = 0; a < n - 1; a++)

		{

			mst[a] = j * n + arc[j]; /* join fragment j with MST */

			dis[j] = -1; 

			d = maxd;

			for (i = 0; i < n; i++)

			{

				if (dis[i] >= 0) /* not connected yet */

				{

					if (dis[i] > odMatrix[i][j])

					{

						dis[i] = odMatrix[i][j];

						arc[i] = j;

					}

					if (d > dis[i])

					{

						d = dis[i];

						k = i;

					}

				}

			}

			j = k;

		}

		/*

		 * Preorder Tour of MST

		 */

//	#define VISITED(x) jorder[x]

//	#define NQ(x) arc[l++] = x

//	#define DQ()  arc[--l]

//	#define EMPTY (l==0)

		for (i = 0; i < n; i++) jorder[i] = 0;

		k = 0; l = 0; d = 0; 

		arc[l++] = 0;

		while (l != 0)

		{

			i = arc[--l];

			if (!(jorder[i] == 0))

			{

				iorder[k++] = i;

				jorder[i]  = 1;			

				for (j = 0; j < n - 1; j++) /* push all kids of i */

				{

					if (i == mst[j]%n)

						arc[l++] = mst[j]/n; 

				}	

			}

		}

	}

	int mod(int a, int b) {

		return (a % b);

	}

	double D(int f, int t) {

		return odMatrix[f][t];

	}

	/*

	 * Local Search Heuristics

	 *  b-------a        b       a

	 *  .       .   =>   .\     /.

	 *  . d...e .        . e...d .  

	 *  ./     \.        .       .

	 *  c       f        c-------f

	 */

	double getThreeWayCost (int[] p)

	{

		int a, b, c, d, e, f;

		a = iorder[mod(p[0]-1, n)]; b = iorder[p[0]];

		c = iorder[p[1]];   d = iorder[mod(p[1]+1,n)];

		e = iorder[p[2]];   f = iorder[mod(p[2]+1,n)];

		// b va a ser el nuevo origen => sumamos su distancia a nuestro origen fijo

		double Dant, Dnuevo, Ddiff = 0;

		Dant = odMatrix[origenTSP][iorder[0]];

		Dnuevo = odMatrix[origenTSP][b];

		Ddiff = Dnuevo - Dant;

		// tambi?n hay que mirar la distancia al destino desde el ?ltimo punto

		int fin=n-1;

		if (g_bVolverOrigen)

		{	

			// p[2] va a ser el pr?ximo ?ltimo punto 

			Dant = D(iorder[fin], origenTSP);

			Dnuevo = D(iorder[p[2]], origenTSP);

			Ddiff = Ddiff + Dnuevo - Dant;

		}

		else

		{

			Dant = D(iorder[fin], destinoTSP);

			Dnuevo = D(iorder[p[2]], destinoTSP);

			Ddiff = Ddiff + Dnuevo - Dant;

		}

		return (D(a,d) + D(e,b) + D(c,f) - D(a,b) - D(c,d) - D(e,f) + Ddiff); 

	        /* add cost between d and e if non symetric TSP */ 

		// A?ADIR DIFERENCIA DE COSTE A LOS NODOS INMOVILES. (ORIGEN Y ?DESTINO?)

	}

	void doThreeWay (int[] p)

	{

		int i, count, m1, m2, m3, a, b, c, d, e, f;

		int index;

		a = mod(p[0]-1,n); b = p[0];

		c = p[1];   d = mod(p[1]+1,n);

		e = p[2];   f = mod(p[2]+1,n);	

		m1 = mod(n+c-b,n)+1;  /* num cities from b to c */

		m2 = mod(n+a-f,n)+1;  /* num cities from f to a */

		m3 = mod(n+e-d,n)+1;  /* num cities from d to e */

		count = 0;

		/* [b..c] */

		for (i = 0; i < m1; i++)

		{

			index = mod(i+b,n);

			jorder[count++] = iorder[index];

		}

		/* [f..a] */

		for (i = 0; i < m2; i++)

		{

			index = mod(i+f,n);

			jorder[count++] = iorder[index];

		}

		/* [d..e] */

		for (i = 0; i < m3; i++)

		{

			index = mod(i+d,n);

			jorder[count++] = iorder[index];

		}

		/* copy segment back into iorder */

		for (i = 0; i < n; i++) iorder[i] = jorder[i];

	}

	/*

	 *   c..b       c..b

	 *    \/    =>  |  |

	 *    /\        |  |

	 *   a  d       a  d

	 */

	double getReverseCost (int[] p)

	{

		int a, b, c, d;

		a = iorder[mod(p[0]-1,n)]; b = iorder[p[0]];

		c = iorder[p[1]];   d = iorder[mod(p[1]+1,n)];

		double Dant = 0, Dnuevo = 0, Ddiff = 0;

		if (p[0]==0 || p[1]==0)

		{

			Dant = D(origenTSP,iorder[0]);

			if (p[0]==0)

				Dnuevo = D(origenTSP, c);

			else

				Dnuevo = D(origenTSP, b);

			Ddiff = Dnuevo-Dant;

		}

		int fin = n-1;

		if (g_bVolverOrigen) // Miramos la distancia al cero

		{		

			// iorder[p[1]] o iorder[p[0]] va a acabar en la ?ltima posici?n

			if (p[0]==fin || p[1] == fin) // tambi?n hay que mirar la distancia al destino desde el ?ltimo punto

			{

				Dant = D(iorder[fin], origenTSP);

				if (p[0]==fin)

					Dnuevo = D(c,origenTSP);

				else

					Dnuevo = D(b, origenTSP);

				Ddiff = Ddiff + Dnuevo - Dant;

			} 

		}

		else

		{

			if (p[0]==fin || p[1] == fin) // tambi?n hay que mirar la distancia al destino desde el ?ltimo punto

			{

				Dant = D(iorder[fin], destinoTSP);

				if (p[0]==fin)

					Dnuevo = D(c,destinoTSP);

				else

					Dnuevo = D(b, destinoTSP);

				Ddiff = Ddiff + Dnuevo - Dant;

			} 

		} 

		return (D(d,b) + D(c,a) - D(a,b) - D(c,d) + Ddiff);

	        /* add cost between c and b if non symetric TSP */ 

//	 A?ADIR DIFERENCIA DE COSTE A LOS NODOS INMOVILES. (ORIGEN Y ?DESTINO?)

	}

	void doReverse(int[] p)

	{

		int i, nswaps, first, last, tmp;

	        /* reverse path b...c */

		nswaps = (mod(p[1]-p[0],n)+1)/2;

		for (i = 0; i < nswaps; i++)

	        {

			first = mod(p[0]+i, n);

			last  = mod(p[1]-i, n);

			tmp   = iorder[first];

			iorder[first] = iorder[last];

			iorder[last]  = tmp;

	        }

	}

	double annealing()

	{

		int[] p;

		int    i=1, j, pathchg;

		int    numOnPath, numNotOnPath;

		double    pathlen, bestlen;

		double energyChange, T;

		/*

		 * Set up first eulerian path iorder to be improved by

		 * simulated annealing. 

		 */

		/* bool conEulerian = true;

		if (conEulerian)

		 	findEulerianPath();  */

		pathlen = pathLength(); // (iorder); 

		bestlen = pathlen;

		Random rnd = new Random();

		for (T = T_INIT; T > FINAL_T; T *= COOLING)  /* annealing schedule */

	        {

			pathchg = 0;

			for (j = 0; j < TRIES_PER_T; j++)

			{

				do {

					p[0] = rnd.nextInt(n);

					p[1] = rnd.nextInt(n);

					if (p[0] == p[1]) p[1] = mod(p[0]+1,n); /* non-empty path */

					numOnPath = mod(p[1]-p[0],n) + 1;

					numNotOnPath = n - numOnPath;

				} while (numOnPath < 2 || numNotOnPath < 2) ; /* non-empty path */

				if ((rnd.nextInt() % 2) == 0) /*  threeWay */

				{

					do {

						p[2] = mod(rnd.nextInt(numNotOnPath)+p[1]+1,n);

					} while (p[0] == mod(p[2]+1,n)); /* avoids a non-change */

					energyChange = getThreeWayCost (p);

					if ((energyChange < 0) || (RREAL < Math.exp(-energyChange/T)))

					{

						pathchg++;

						pathlen += energyChange;

						doThreeWay (p); 

					}

				}

				else            /* path Reverse */

				{

					energyChange = getReverseCost (p);

					if ((energyChange < 0) || (RREAL < Math.exp(-energyChange/T)))

					{

						pathchg++;

						pathlen += energyChange;

						doReverse(p); 

					}

				}

				if (pathlen < bestlen)

				{

					pathlen = pathLength(); // Calculamos la distancia de verdad, por si no interesa

											// hacer el cambio. pathlen es en realidad una estimaci?n

					if (pathlen < bestlen)

					{

						bestlen = pathlen;

						for (i=0; i< n; i++)

							vTSP[i+1] = iorder[i];

					}

				}

				if (pathchg > IMPROVED_PATH_PER_T) break; /* finish early */

			}   

			if (pathchg == 0) break;   /* if no change then quit */

	        }

		return bestlen;

	}

}