pgRouting  2.2
pgRouting extends the PostGIS / PostgreSQL geospatial database to provide geospatial routing functionality.
 All Classes Functions Variables Pages
pgr_tsp.cpp
1 /*PGR-GNU*****************************************************************
2  * File: tsp_driver.cpp
3  *
4  * Generated with Template by:
5  * Copyright (c) 2015 pgRouting developers
6  * Mail: project@pgrouting.org
7  *
8  * Function's developer:
9  * Copyright (c) 2015 Celia Virginia Vergara Castillo
10  * Mail: vicky_vergara@hotmail.com
11  *
12  * ------
13  *
14  * This program is free software; you can redistribute it and/or modify
15  * it under the terms of the GNU General Public License as published by
16  * the Free Software Foundation; either version 2 of the License, or
17  * (at your option) any later version.
18  *
19  * This program is distributed in the hope that it will be useful,
20  * but WITHOUT ANY WARRANTY; without even the implied warranty of
21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22  * GNU General Public License for more details.
23  *
24  * You should have received a copy of the GNU General Public License
25  * along with this program; if not, write to the Free Software
26  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
27  *
28  * ********************************************************************PGR-GNU*/
29 
30 #ifdef __MINGW32__
31 #include <winsock2.h>
32 #include <windows.h>
33 #endif
34 
35 
36 
37 #include <vector>
38 #include <algorithm>
39 
40 #include "../../common/src/pgr_types.h"
41 #include "./pgr_tsp.hpp"
42 
43 
44 
45 static
46 size_t
47 rand(size_t n) {
48  return static_cast< size_t >(std::rand() * n);
49 }
50 
51 
52 static
53 size_t
54 succ(size_t i, size_t n) {
55  return static_cast<size_t>((i + 1) % n);
56 }
57 
58 static
59 size_t
60 MOD(size_t i, size_t n) {
61 #if 1
62 return succ(i, n);
63 #else
64 return ((i) % (n) > 0 ? (i) % (n) : (i) % (n) + (n));
65 #endif
66 }
67 
68 double
69 TSP::D(size_t i, size_t j) {
70  return dist[i][j];
71 }
72 
73 void
74 TSP::update(Ids new_order) {
75  auto blength = dist.pathCost(new_order);
76  if (bestCost < blength) {
77  border = new_order;
78  bestCost = blength;
79  }
80 }
81 
82 /*
83  * Prim's approximated TSP tour
84  * See also [Cristophides'92]
85  */
86 bool
87 TSP::findEulerianPath() {
88  Ids iorder(n);
89  Ids mst(n);
90  Ids arc(n);
91  std::vector < double > dis(n);
92  double d;
93 #if 0
94  int n, *iorder, *jorder;
95  DTYPE d;
96  DTYPE maxd;
97  DTYPE *dist;
98  DTYPE *dis;
99 
100  jorder = tsp->jorder;
101  iorder = tsp->iorder;
102  dist = tsp->dist;
103  maxd = tsp->maxd;
104  n = tsp->n;
105 
106  if (!(mst = (int*) palloc(n * sizeof(int))) ||
107  !(arc = (int*) palloc(n * sizeof(int))) ||
108  !(dis = (DTYPE*) palloc(n * sizeof(DTYPE))) )
109  {
110  elog(ERROR, "Failed to allocate memory!");
111  return -1;
112  }
113 #endif
114  // PGR_DBG("findEulerianPath: 1");
115 
116  size_t j(0);
117  double curr_maxd = maxd;
118  dis[0] = -1;
119 
120  for (size_t i = 1; i < n; ++i) {
121  dis[i] = dist[i][0];
122  arc[i] = 0;
123  if (curr_maxd > dis[i]) {
124  curr_maxd = dis[i];
125  j = i;
126  }
127  }
128  //PGR_DBG("findEulerianPath: j=%d", j);
129 
130  if (curr_maxd == maxd) {
131  // PGR_DBG("Error TSP fail to findEulerianPath, check your distance matrix is valid.");
132  return false;
133  }
134 
135  /*
136  * O(n^2) Minimum Spanning Trees by Prim and Jarnick
137  * for graphs with adjacency matrix.
138  */
139  for (size_t a = 0; a < n - 1; a++) {
140  size_t k(0);
141  mst[a] = j * n + arc[j]; /* join fragment j with MST */
142  dis[j] = -1;
143  d = maxd;
144  for (size_t i = 0; i < n; i++)
145  {
146  if (dis[i] >= 0) /* not connected yet */
147  {
148  if (dis[i] > dist[i][j])
149  {
150  dis[i] = dist[i][j];
151  arc[i] = j;
152  }
153  if (d > dis[i])
154  {
155  d = dis[i];
156  k = i;
157  }
158  }
159  }
160  j = k;
161  }
162  //PGR_DBG("findEulerianPath: 3");
163 
164  /*
165  * Preorder Tour of MST
166  */
167 #if 0
168 #define VISITED(x) jorder[x]
169 #define NQ(x) arc[l++] = x
170 #define DQ() arc[--l]
171 #define EMPTY (l==0)
172 #endif
173  for (auto &val : jorder) {
174  val = 0;
175  }
176 
177 #if 0
178  for (i = 0; i < n; i++) VISITED(i) = 0;
179 #endif
180 
181  size_t l = 0;
182  size_t k = 0;
183  d = 0;
184  arc[l++] = 0;
185  while (!(l == 0)) {
186  size_t i = arc[--l];
187 
188  if (!jorder[i]) {
189  iorder[k++] = i;
190  jorder[i] = 1;
191  /* push all kids of i */
192  for (size_t j = 0; j < n - 1; j++) {
193  if (i == mst[j] % n)
194  arc[l++] = mst[j] % n;
195  }
196  }
197  }
198 
199 #if 0
200  k = 0; l = 0; d = 0; NQ(0);
201  while (!EMPTY)
202  {
203  i = DQ();
204  if (!VISITED(i))
205  {
206  iorder[k++] = i;
207  VISITED(i) = 1;
208  for (j = 0; j < n - 1; j++) /* push all kids of i */
209  {
210  if (i == mst[j]%n) NQ(mst[j]/n);
211  }
212  }
213  }
214 #endif
215  //PGR_DBG("findEulerianPath: 4");
216 
217  update(iorder);
218  return true;
219 }
220 
221 /*
222  * Local Search Heuristics
223  * b-------a b a
224  * . . => .\ /.
225  * . d...e . . e...d .
226  * ./ \. . .
227  * c f c-------f
228  */
229 
230 double
231 TSP::getThreeWayCost(Path p) {
232  size_t a, b, c, d, e, f;
233 
234  a = iorder[MOD(p[0] - 1, n)];
235  b = iorder[p[0]];
236  c = iorder[p[1]];
237  d = iorder[MOD(p[1] + 1, n)];
238  e = iorder[p[2]];
239  f = iorder[MOD(p[2] + 1, n)];
240 
241  return (D(a,d) + D(e,b) + D(c,f) - D(a,b) - D(c,d) - D(e,f));
242  /* add cost between d and e if non symetric TSP */
243 }
244 
245 void
246 TSP::doThreeWay(Path p) {
247  size_t count, m1, m2, m3, a, b, c, d, e, f;
248 
249  a = MOD(p[0]-1,n);
250  b = p[0];
251  c = p[1];
252  d = MOD(p[1]+1,n);
253  e = p[2];
254  f = MOD(p[2]+1,n);
255 
256  m1 = MOD(n + c - b, n) + 1; /* num cities from b to c */
257  m2 = MOD(n + a - f, n) + 1; /* num cities from f to a */
258  m3 = MOD(n + e - d, n) + 1; /* num cities from d to e */
259 
260  count = 0;
261  /* [b..c] */
262  for (size_t i = 0; i < m1; i++)
263  jorder[count++] = iorder[MOD(i + b, n)];
264 
265  /* [f..a] */
266  for (size_t i = 0; i < m2; i++)
267  jorder[count++] = iorder[MOD(i+f,n)];
268 
269  /* [d..e] */
270  for (size_t i = 0; i < m3; i++)
271  jorder[count++] = iorder[MOD(i+d,n)];
272 
273  /* copy segment back into iorder */
274  for (size_t i = 0; i < n; i++) iorder[i] = jorder[i];
275 }
276 
277 
278 /*
279  * c..b c..b
280  * \/ => | |
281  * /\ | |
282  * a d a d
283  *
284  * a b 1 2 .. n-1 n c d
285  * a c n n-1 .. 2 1 c d
286  */
287 double
288 TSP::getReverseCost(Path p) {
289 
290  auto a = iorder[MOD(p[0] - 1, n)];
291  auto b = iorder[p[0]];
292  auto c = iorder[p[1]];
293  auto d = iorder[MOD(p[1] + 1, n)];
294 
295  return (D(d,b) + D(c,a) - D(a,b) - D(c,d));
296  /* add cost between c and b if non symetric TSP */
297 }
298 
299 void
300 TSP::doReverse(Path p) {
301 
302  /* reverse path b...c */
303  size_t nswaps = (MOD(p[1] - p[0], n) + 1) / 2;
304  for (size_t i = 0; i < nswaps; i++) {
305  size_t first = MOD(p[0]+i, n);
306  size_t last = MOD(p[1]-i, n);
307  std::swap(iorder[first], iorder[last]);
308 #if 0
309  tmp = iorder[first];
310  iorder[first] = iorder[last];
311  iorder[last] = tmp;
312 #endif
313  }
314 }
315 
316 
317 void
318 TSP::annealing() {
319  Path p;
320  size_t numOnPath, numNotOnPath;
321 
322  double pathCost(dist.pathCost(iorder));
323  const double T_INIT = 100.0;
324  const double FINAL_T = 0.1;
325  const double COOLING = 0.9; /* to lower down T (< 1) */
326  const size_t TRIES_PER_T(500 * n);
327  const size_t IMPROVED_PATH_PER_T = 60 * n;
328 
329  /* annealing schedule */
330  for (double T = T_INIT; FINAL_T < T; T *= COOLING) {
331  size_t pathchg = 0;
332  for (size_t j = 0; j < TRIES_PER_T; j++) {
333  do {
334  p[0] = rand(n);
335  p[1] = rand(n);
336  /* non-empty path */
337  if (p[0] == p[1])
338  p[1] = MOD(p[0] + 1, n);
339 
340  numOnPath = MOD(p[1] - p[0], n) + 1;
341  numNotOnPath = n - numOnPath;
342  } while (numOnPath < 2 || numNotOnPath < 2); /* non-empty path */
343 
344  if (rand(2)) {
345  /* threeWay */
346  do {
347  p[2] = MOD(rand(numNotOnPath) + p[1] + 1, n);
348  } while (p[0] == MOD(p[2] + 1, n)); /* avoids a non-change */
349 
350  auto energyChange = getThreeWayCost(p);
351  // if (energyChange < 0 || RREAL < exp(-energyChange / T) )
352  if (energyChange < 0 || std::rand() < exp(-energyChange / static_cast<double>(T)) ) {
353  pathchg++;
354  pathCost += energyChange;
355  doThreeWay(p);
356  }
357 
358  } else {
359  /* path Reverse */
360  auto energyChange = getReverseCost(p);
361  if (energyChange < 0 || std::rand() < exp(-energyChange / static_cast<double>(T)) ) {
362  pathchg++;
363  pathCost += energyChange;
364  doReverse(p);
365  }
366  }
367  // if the new length is better than best then save it as best
368  update(iorder);
369 #if 0
370  if (pathlen < tsp->bestlen) {
371  tsp->bestlen = pathlen;
372  for (i=0; i<tsp->n; i++) tsp->border[i] = tsp->iorder[i];
373  }
374 #endif
375  if (pathchg > IMPROVED_PATH_PER_T) break; /* finish early */
376  }
377  // PGR_DBG("T:%f L:%f B:%f C:%d", T, pathlen, tsp->bestlen, pathchg);
378  if (pathchg == 0) break; /* if no change then quit */
379  }
380 }
381