pgRouting  2.2
pgRouting extends the PostGIS / PostgreSQL geospatial database to provide geospatial routing functionality.
 All Classes Functions Variables Pages
tsplib.c
1 /*PGR-MIT*****************************************************************
2 
3 * $Id: tsplib.c,v 1.1 2006/05/13 23:39:56 woodbri Exp $
4 *
5 * tsplib
6 * Copyright 2005,2013, Stephen Woodbridge, All rights Reserved
7 * This file is released under MIT-X license as part of pgRouting.
8 
9 ------
10 MIT/X license
11 
12 Permission is hereby granted, free of charge, to any person obtaining a copy
13 of this software and associated documentation files (the "Software"), to deal
14 in the Software without restriction, including without limitation the rights
15 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
16 copies of the Software, and to permit persons to whom the Software is
17 furnished to do so, subject to the following conditions:
18 
19 
20 The above copyright notice and this permission notice shall be included in
21 all copies or substantial portions of the Software.
22 
23 
24 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
25 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
26 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
27 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
28 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
29 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
30 THE SOFTWARE.
31 
32 ********************************************************************PGR-MIT*/
33 
34 /*
35 **********************************************************************
36 *
37 * tsplib.c
38 *
39 * Routine to calculate the node order for a Traveling Salesman Problem
40 * from a distance matrix.
41 *
42 * $Log: tsplib.c,v $
43 * Revision 1.1 2006/05/13 23:39:56 woodbri
44 * Initial checkin of IMT::TSP module.
45 *
46 * Initial revision created from router tsp2.c
47 *
48 **********************************************************************
49 *
50 * Simulated annealing and a non symetric
51 * Euclidian Traveling Salesman Problem.
52 *
53 * Solution based on local search heuristics for
54 * non-crossing paths and nearest neighbors
55 *
56 * Storage Requirements: 4n ints
57 *
58 * Problem: given an nXn matrix of costs for n cities in the plane,
59 * find a permutation pi_1, pi_2, ..., pi_n of 1, 2, ..., n that
60 * minimizes sum for 1<=i<n D(pi_i,pi_i+1), where D(i,j) is the
61 * euclidian distance between cities i and j from the matrix
62 *
63 * This means that the ith row is the "from" city and the jth column
64 * is the cost to the "to" city.
65 *
66 * NOTE: costs are assumed to be ints, so scaling them to meters or
67 * some other int would be advisable.
68 *
69 * NOTE: the internal cost calculations ASSUME a symetric matrix. There
70 * are notes of what needs to be updated, but all D(i,j)s need to be
71 * reviewed because order is important for asymetric cases.
72 *
73 * Note: with n cities, there is (n-1)!/2 possible tours.
74 * factorial(10)=3628800 factorial(50)=3E+64 factorial(150)=5.7E+262
75 * If we could check one tour per clock cycle on a 100 MHZ computer, we
76 * would still need to wait approximately 10^236 times the age of the
77 * universe to explore all tours for 150 cities.
78 *
79 * Derivied from code 1995 by Maugis Lionel (Sofreavia) on sid1
80 * that was placed in the public domain.
81 *************************************************************************/
82 
83 //#ifdef __MINGW64__
84 //#define ELOG_H
85 //#include <winsock2.h>
86 //#endif
87 #include <postgres.h>
88 #include <string.h> /* memcpy */
89 #include <math.h> /* exp */
90 
91 #include "tsp.h"
92 
93 #undef DEBUG
94 //#define DEBUG 1
95 #include "../../common/src/debug_macro.h"
96 
97 
98 
99 #define T_INIT 100
100 #define FINAL_T 0.1
101 #define COOLING 0.9 /* to lower down T (< 1) */
102 #define TRIES_PER_T 500*n
103 #define IMPROVED_PATH_PER_T 60*n
104 
105 /*
106  * MACHINE INDEPENDENT RANDOM NUMBER GENERATOR
107  * Written by: DIMACS (modified for TSP)
108 */
109 
110 #define PRANDMAX 1000000000
111 static int a;
112 static int b;
113 static int arr[55];
114 
115 static
116 int Rand(void);
117 
118 static
119 void initRand (int seed)
120 {
121  int i, ii;
122  int last, next;
123 
124  seed %= PRANDMAX;
125  if (seed < 0) seed += PRANDMAX;
126 
127  arr[0] = last = seed;
128  next = 1;
129  for (i = 1; i < 55; i++) {
130  ii = (21 * i) % 55;
131  arr[ii] = next;
132  next = last - next;
133  if (next < 0)
134  next += PRANDMAX;
135  last = arr[ii];
136  }
137  a = 0;
138  b = 24;
139  for (i = 0; i < 165; i++)
140  last = Rand ();
141 }
142 
143 static
144 int Rand (void)
145 {
146  int t;
147 
148  if (a-- == 0)
149  a = 54;
150  if (b-- == 0)
151  b = 54;
152 
153  t = arr[a] - arr[b];
154 
155  if (t < 0)
156  t += PRANDMAX;
157 
158  arr[a] = t;
159 
160  return t;
161 }
162 
163 #define RREAL ((double)Rand()/PRANDMAX)
164 #define RANDOM Rand
165 #define unifRand(n) (Rand()%n)
166 
167 
168 /*
169  * Defs
170  */
171 
172 typedef int Path[3]; /* specify how to change path */
173 
174 typedef struct tspstruct {
175  int n;
176  DTYPE maxd;
177  DTYPE *dist;
178  DTYPE bestlen;
179  int *iorder;
180  int *jorder;
181  int *border; // best order we find
182  float b[4];
183 } TSP;
184 
185 #define MOD(i,n) ((i) % (n) >= 0 ? (i) % (n) : (i) % (n) + (n))
186 #define D(x,y) dist[(x)*n+y]
187 
188 #define MIN(a,b) ((a)<(b)?(a):(b))
189 #define MAX(a,b) ((a)>(b)?(a):(b))
190 #define sqr(x) ((x)*(x))
191 
192 /*
193  * Prim's approximated TSP tour
194  * See also [Cristophides'92]
195  */
196 static
197 int findEulerianPath(TSP *tsp)
198 {
199  int *mst, *arc;
200  int i, j, k, l, a;
201  int n, *iorder, *jorder;
202  DTYPE d;
203  DTYPE maxd;
204  DTYPE *dist;
205  DTYPE *dis;
206 
207  jorder = tsp->jorder;
208  iorder = tsp->iorder;
209  dist = tsp->dist;
210  maxd = tsp->maxd;
211  n = tsp->n;
212 
213  if (!(mst = (int*) palloc((size_t) n * sizeof(int))) ||
214  !(arc = (int*) palloc((size_t) n * sizeof(int))) ||
215  !(dis = (DTYPE*) palloc((size_t) n * sizeof(DTYPE))) )
216  {
217  elog(ERROR, "Failed to allocate memory!");
218  return -1;
219  }
220  //PGR_DBG("findEulerianPath: 1");
221 
222  k = -1;
223  j = -1;
224  d = maxd;
225  dis[0] = -1;
226  for (i = 1; i < n; i++)
227  {
228  dis[i] = D(i,0);
229  arc[i] = 0;
230  if (d > dis[i])
231  {
232  d = dis[i];
233  j = i;
234  }
235  }
236  //PGR_DBG("findEulerianPath: j=%d", j);
237 
238  if (j == -1)
239  elog(ERROR, "Error TSP fail to findEulerianPath, check your distance matrix is valid.");
240 
241  /*
242  * O(n^2) Minimum Spanning Trees by Prim and Jarnick
243  * for graphs with adjacency matrix.
244  */
245  for (a = 0; a < n - 1; a++)
246  {
247  mst[a] = j * n + arc[j]; /* join fragment j with MST */
248  dis[j] = -1;
249  d = maxd;
250  for (i = 0; i < n; i++)
251  {
252  if (dis[i] >= 0) /* not connected yet */
253  {
254  if (dis[i] > D(i,j))
255  {
256  dis[i] = D(i,j);
257  arc[i] = j;
258  }
259  if (d > dis[i])
260  {
261  d = dis[i];
262  k = i;
263  }
264  }
265  }
266  j = k;
267  }
268  //PGR_DBG("findEulerianPath: 3");
269 
270  /*
271  * Preorder Tour of MST
272  */
273 #define VISITED(x) jorder[x]
274 #define NQ(x) arc[l++] = x
275 #define DQ() arc[--l]
276 #define EMPTY (l==0)
277 
278  for (i = 0; i < n; i++) VISITED(i) = 0;
279  k = 0; l = 0; d = 0; NQ(0);
280  while (!EMPTY)
281  {
282  i = DQ();
283  if (!VISITED(i))
284  {
285  iorder[k++] = i;
286  VISITED(i) = 1;
287  for (j = 0; j < n - 1; j++) /* push all kids of i */
288  {
289  if (i == mst[j]%n) NQ(mst[j]/n);
290  }
291  }
292  }
293  //PGR_DBG("findEulerianPath: 4");
294 
295  return 0;
296 }
297 
298 static
299 DTYPE pathLength (TSP *tsp)
300 {
301  unsigned int i;
302  DTYPE len = 0;
303 
304  int *iorder = tsp->iorder;
305  DTYPE *dist = tsp->dist;
306  int n = tsp->n;
307 
308  for (i = 0; i < n-1; i++)
309  {
310  len += D(iorder[i], iorder[i+1]);
311  }
312  len += D(iorder[n-1], iorder[0]); /* close path */
313  return (len);
314 }
315 
316 /*
317  * Local Search Heuristics
318  * b-------a b a
319  * . . => .\ /.
320  * . d...e . . e...d .
321  * ./ \. . .
322  * c f c-------f
323  */
324 static
325 DTYPE getThreeWayCost (TSP *tsp, Path p)
326 {
327  int a, b, c, d, e, f;
328  int *iorder = tsp->iorder;
329  DTYPE *dist = tsp->dist;
330  int n = tsp->n;
331 
332  a = iorder[MOD(p[0]-1,n)];
333  b = iorder[p[0]];
334  c = iorder[p[1]];
335  d = iorder[MOD(p[1]+1,n)];
336  e = iorder[p[2]];
337  f = iorder[MOD(p[2]+1,n)];
338 
339  return (D(a,d) + D(e,b) + D(c,f) - D(a,b) - D(c,d) - D(e,f));
340  /* add cost between d and e if non symetric TSP */
341 }
342 
343 static
344 void doThreeWay (TSP *tsp, Path p)
345 {
346  int i, count, m1, m2, m3, a, b, c, d, e, f;
347  int *iorder = tsp->iorder;
348  int *jorder = tsp->jorder;
349  int n = tsp->n;
350 
351  a = MOD(p[0]-1,n);
352  b = p[0];
353  c = p[1];
354  d = MOD(p[1]+1,n);
355  e = p[2];
356  f = MOD(p[2]+1,n);
357 
358  m1 = MOD(n+c-b,n)+1; /* num cities from b to c */
359  m2 = MOD(n+a-f,n)+1; /* num cities from f to a */
360  m3 = MOD(n+e-d,n)+1; /* num cities from d to e */
361 
362  count = 0;
363  /* [b..c] */
364  for (i = 0; i < m1; i++) jorder[count++] = iorder[MOD(i+b,n)];
365 
366  /* [f..a] */
367  for (i = 0; i < m2; i++) jorder[count++] = iorder[MOD(i+f,n)];
368 
369  /* [d..e] */
370  for (i = 0; i < m3; i++) jorder[count++] = iorder[MOD(i+d,n)];
371 
372  /* copy segment back into iorder */
373  for (i = 0; i < n; i++) iorder[i] = jorder[i];
374 }
375 
376 /*
377  * c..b c..b
378  * \/ => | |
379  * /\ | |
380  * a d a d
381  */
382 static
383 DTYPE getReverseCost (TSP *tsp, Path p)
384 {
385  int a, b, c, d;
386  int *iorder = tsp->iorder;
387  DTYPE *dist = tsp->dist;
388  int n = tsp->n;
389 
390  a = iorder[MOD(p[0]-1,n)];
391  b = iorder[p[0]];
392  c = iorder[p[1]];
393  d = iorder[MOD(p[1]+1,n)];
394 
395  return (D(d,b) + D(c,a) - D(a,b) - D(c,d));
396  /* add cost between c and b if non symetric TSP */
397 }
398 static
399 void doReverse(TSP *tsp, Path p)
400 {
401  int i, nswaps, first, last, tmp;
402  int *iorder = tsp->iorder;
403  int n = tsp->n;
404 
405 
406  /* reverse path b...c */
407  nswaps = (MOD(p[1]-p[0],n)+1)/2;
408  for (i = 0; i < nswaps; i++)
409  {
410  first = MOD(p[0]+i, n);
411  last = MOD(p[1]-i, n);
412  tmp = iorder[first];
413  iorder[first] = iorder[last];
414  iorder[last] = tmp;
415  }
416 }
417 
418 static
419 void annealing(TSP *tsp)
420 {
421  Path p;
422  int i, j, pathchg;
423  int numOnPath, numNotOnPath;
424  DTYPE pathlen;
425  int n = tsp->n;
426  double energyChange, T;
427 
428  pathlen = pathLength (tsp);
429 
430  for (T = T_INIT; T > FINAL_T; T *= COOLING) /* annealing schedule */
431  {
432  pathchg = 0;
433  for (j = 0; j < TRIES_PER_T; j++)
434  {
435  do {
436  p[0] = unifRand (n);
437  p[1] = unifRand (n);
438  /* non-empty path */
439  if (p[0] == p[1]) p[1] = MOD(p[0]+1,n);
440  numOnPath = MOD(p[1]-p[0],n) + 1;
441  numNotOnPath = n - numOnPath;
442  } while (numOnPath < 2 || numNotOnPath < 2); /* non-empty path */
443 
444  if (RANDOM() % 2) /* threeWay */
445  {
446  do {
447  p[2] = MOD(unifRand (numNotOnPath)+p[1]+1,n);
448  } while (p[0] == MOD(p[2]+1,n)); /* avoids a non-change */
449 
450  energyChange = getThreeWayCost (tsp, p);
451  if (energyChange < 0 || RREAL < exp(-energyChange/T) )
452  {
453  pathchg++;
454  pathlen += energyChange;
455  doThreeWay (tsp, p);
456  }
457  }
458  else /* path Reverse */
459  {
460  energyChange = getReverseCost (tsp, p);
461  if (energyChange < 0 || RREAL < exp(-energyChange/T))
462  {
463  pathchg++;
464  pathlen += energyChange;
465  doReverse(tsp, p);
466  }
467  }
468  // if the new length is better than best then save it as best
469  if (pathlen < tsp->bestlen) {
470  tsp->bestlen = pathlen;
471  for (i=0; i<tsp->n; i++) tsp->border[i] = tsp->iorder[i];
472  }
473  if (pathchg > IMPROVED_PATH_PER_T) break; /* finish early */
474  }
475  PGR_DBG("T:%f L:%f B:%f C:%d", T, pathlen, tsp->bestlen, pathchg);
476  if (pathchg == 0) break; /* if no change then quit */
477  }
478 }
479 
480 
481 static
482 void reverse(int num, int *ids)
483 {
484  int i, j, t;
485  for (i=0, j=num-1; i<j; i++, j--) {
486  t = ids[j];
487  ids[j] = ids[i];
488  ids[i] = t;
489  }
490 }
491 
492 
493 int find_tsp_solution(int num, DTYPE *cost, int *ids, int start, int end, DTYPE *total_len, char *err_msg)
494 {
495  int i, j;
496  int istart = 0;
497  int jstart = 0;
498  int iend = -1;
499  int jend = -1;
500  int rev = 0;
501  TSP tsp;
502  long seed = -314159L;
503  DTYPE blength;
504 
505  PGR_DBG("sizeof(long)=%d", (int)sizeof(long));
506 
507  initRand((int) seed);
508 
509 #ifdef DEBUG
510  char bufff[2048];
511  int nnn;
512  PGR_DBG("---------- Matrix[%d][%d] ---------------------\n", num, num);
513  for (i=0; i<num; i++) {
514  sprintf(bufff, "%d:", i);
515  nnn = 0;
516  for (j=0; j<num; j++) {
517  nnn += sprintf(bufff+nnn, "\t%.4f", cost[i*num+j]);
518  }
519  PGR_DBG("%s", bufff);
520  }
521 #endif
522 
523  /* initialize tsp struct */
524  tsp.n = num;
525  tsp.dist = NULL;
526  tsp.iorder = NULL;
527  tsp.jorder = NULL;
528  tsp.border = NULL;
529 
530  if (!(tsp.iorder = (int*) palloc ((size_t) tsp.n * sizeof(int))) ||
531  !(tsp.jorder = (int*) palloc ((size_t) tsp.n * sizeof(int))) ||
532  !(tsp.border = (int*) palloc ((size_t) tsp.n * sizeof(int))) ) {
533  elog(FATAL, "Memory allocation failed!");
534  return -1;
535  }
536 
537  tsp.dist = cost;
538  tsp.maxd = 0;
539  for (i=0; i<tsp.n*tsp.n; i++) {
540  tsp.maxd = MAX(tsp.maxd, cost[i]);
541  }
542 
543  /* identity permutation */
544  for (i = 0; i < tsp.n; i++) tsp.iorder[i] = i;
545 
546  tsp.bestlen = pathLength(&tsp);
547  for (i = 0; i < tsp.n; i++) tsp.border[i] = tsp.iorder[i];
548 
549  PGR_DBG("Initial Path Length: %.4f", tsp.bestlen);
550 
551  /*
552  * Set up first eulerian path iorder to be improved by
553  * simulated annealing.
554  */
555  if(findEulerianPath(&tsp))
556  return -1;
557 
558  blength = pathLength(&tsp);
559  if (blength < tsp.bestlen) {
560  tsp.bestlen = blength;
561  for (i = 0; i < tsp.n; i++) tsp.border[i] = tsp.iorder[i];
562  }
563 
564  PGR_DBG("Approximated Path Length: %.4f", blength);
565 
566  annealing(&tsp);
567 
568  *total_len = pathLength(&tsp);
569  PGR_DBG("Final Path Length: %.4f", *total_len);
570 
571  *total_len = tsp.bestlen;
572  for (i=0; i<tsp.n; i++) tsp.iorder[i] = tsp.border[i];
573  PGR_DBG("Best Path Length: %.4f", *total_len);
574 
575  // reorder ids[] with start as first
576 
577 #ifdef DEBUG
578  for (i=0; i<tsp.n; i++) {
579  PGR_DBG("i: %d, ids[i]: %d, io[i]: %d, jo[i]: %d, jo[io[i]]: %d",
580  i, ids[i], tsp.iorder[i], tsp.jorder[i], tsp.jorder[tsp.iorder[i]]);
581  }
582 #endif
583 
584  // get index of start node in ids
585  for (i=0; i < tsp.n; i++) {
586  if (ids[i] == start) istart = i;
587  if (ids[i] == end) iend = i;
588  }
589  PGR_DBG("istart: %d, iend: %d", istart, iend);
590 
591  // get the idex of start in iorder
592  for (i=0; i < tsp.n; i++) {
593  if (tsp.iorder[i] == istart) jstart = i;
594  if (tsp.iorder[i] == iend) jend = i;
595  }
596  PGR_DBG("jstart: %d, jend: %d", jstart, jend);
597 
598  /*
599  * If the end is specified and the end point and it follow start
600  * then we swap start and end and extract the list backwards
601  * and later we reverse the list for the desired order.
602  */
603  if ((jend > 0 && jend == jstart+1) || (jend == 0 && jstart == tsp.n-1)) {
604  int tmp = jend;
605  jend = jstart;
606  jstart = tmp;
607  rev = 1;
608  PGR_DBG("reversed start and end: jstart: %d, jend: %d", jstart, jend);
609  }
610 
611  // copy ids to tsp.jorder so we can rewrite ids
612  memcpy(tsp.jorder, ids, (size_t) tsp.n * sizeof(int));
613 
614  // write reordered ids into ids[]
615  // remember at this point jorder is our list if ids
616  for (i=jstart, j=0; i < tsp.n; i++, j++)
617  ids[j] = tsp.jorder[tsp.iorder[i]];
618 
619  for (i=0; i < jstart; i++, j++)
620  ids[j] =tsp.jorder[tsp.iorder[i]];
621 
622  // if we reversed the order above, now put it correct.
623  if (rev) {
624  int tmp = jend;
625  jend = jstart;
626  jstart = tmp;
627  reverse(tsp.n, ids);
628  }
629 
630 #ifdef DEBUG
631  PGR_DBG("ids getting returned!");
632  for (i=0; i<tsp.n; i++) {
633  PGR_DBG("i: %d, ids[i]: %d, io[i]: %d, jo[i]: %d",
634  i, ids[i], tsp.iorder[i], tsp.jorder[i]);
635  }
636 #endif
637 
638  PGR_DBG("tsplib: jstart=%d, jend=%d, n=%d, j=%d", jstart, jend, tsp.n, j);
639 
640  return 0;
641 }
642 
643 /* EOF */
Definition: pgr_tsp.hpp:35