2-opt法の実現をするために訂正してほしい（巡回セールスマン問題）

実現したいこと

NN法からの2-opt法を確実なものにしたい

前提

実行自体はできるのですが、グラフ化したときに2-opt法をしたのにも関わらず、51この点を回る巡回セールスマン問題を実行した際に交差している場所が何箇所か見られます。
それをなくすようなプログラムにするために改善点を教えていただきたいです。

発生している問題・エラーメッセージ

なし

該当のソースコード

C言語
1#include <stdio.h>
2#include <stdlib.h>
3#include <string.h>
4#include <math.h>
5#include <time.h>
6
7#define INF 1048576
8#define NUM 110000
9//#define STA city_data_num-1
10#define STA 24
11
12struct City {
13  int id;
14  float x;
15  float y;
16};
17
18struct City city_data[1000000];
19int city_data_num = 0;
20
21
22double norm(double x0, double y0, double x1, double y1) {
23  return sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1));
24}
25
26/* This is the main function where TSP solver is implemented.
27   If you would like to use another algorithm to solve TSP,
28   you can freely change this function. */
29   
30void do_travel(void) {
31  int i, j, p, time, minp;
32  int pid, cid, minid, staid;
33  float px, py, cx, cy, minx, miny, stax, stay;
34  float travel_distance[NUM] = {0.0};
35  float min = 100000, total_distance = 0, last_distance = 0;
36  int use[NUM] = {0};
37  int log[NUM] = {city_data_num};
38  float logx[NUM] = {city_data[STA].x};
39  float logy[NUM] = {city_data[STA].y};
40  int* q = malloc(sizeof(int));
41  *q = 1;
42
43  /* set the initial city you visit */
44  pid = city_data[STA].id;
45  px = city_data[STA].x;
46  py = city_data[STA].y;
47
48  staid = city_data[STA].id;
49  stax = city_data[STA].x;
50  stay = city_data[STA].y;
51  use[STA] = 1;
52
53  for (time = 0; time < city_data_num - 1; time++) {
54    for (i = 0; i < city_data_num; i++) {
55      if (use[i] == 0) {
56        cid = city_data[i].id;
57        cx = city_data[i].x;
58        cy = city_data[i].y;
59
60        /* calculate distance between the previous city and the current one */
61        travel_distance[i] = norm(cx, cy, px, py);
62        if (min > travel_distance[i]) {
63          if (travel_distance[i] != 0) {
64            min = travel_distance[i];
65            minid = cid;
66            minx = cx;
67            miny = cy;
68          }
69        }
70      }
71    }
72
73    total_distance += min;
74    printf("<%02d>(%f, %f) => <%02d>(%f, %f) : cumulative distance = %f\n", pid, px, py, minid, minx, miny, min);
75    min = 1000000;
76    use[minid - 1] = 1;
77    log[*q] = minid;
78    logx[*q] = minx;
79    logy[*q] = miny;
80    (*q)++;
81
82    /* set the current city as the previous one */
83    pid = minid;
84    px = minx;
85    py = miny;
86  }
87  
88   logx[*q] = stax;
89   logy[*q] = stay;
90  last_distance = norm(px, py, stax, stay);
91  printf("<%02d>(%f, %f) => <%02d>(%f, %f) : cumulative distance = %f\n", pid, px, py, staid, stax, stay, last_distance);
92  printf("result......%f\n", total_distance + last_distance);
93
94  
95  int* next = malloc(sizeof(int) * (*q));
96  for (int i = 1; i < (*q); i++) {
97    next[i - 1] = i;
98  }
99  next[(*q) - 1] = 0;
100
101  int iteration = 0;
102
103  for (int i = 0; i < (*q-1); i++) {
104    for (int j = 0; j < (*q-1); j++) {
105      if (i == j || next[i] == j || next[j] == i) {
106        continue;
107      }
108
109      double abcd = norm(logx[i], logy[i], logx[next[i]], logy[next[i]]) + norm(logx[j], logy[j], logx[next[j]], logy[next[j]]);
110      double acbd = norm(logx[i], logy[i], logx[j], logy[j]) + norm(logx[next[j]], logy[next[j]], logx[next[i]], logy[next[i]]);
111      
112      if (abcd > acbd) {
113        int len = 0;
114        for (int k = next[i]; j != k; k = next[k]) {
115          len++;
116        }
117        
118        int* tmp = malloc(sizeof(int) * len);
119        for (int k = next[i], l = 0; j != k; k = next[k], l++) {
120          tmp[l] = k;
121        }
122
123        next[next[i]] = next[j];
124        for (int k = 0, tmpj = j; k < len; k++, tmpj = next[tmpj]) {
125          next[tmpj] = tmp[len - k - 1];
126        }
127        next[i] = j;
128
129        double total_d = 0.0;
130        for (int i = 0; i < (*q); i++) {
131          total_d += norm(logx[i], logy[i], logx[next[i]], logy[next[i]]);
132        }
133
134        printf("iteration: %6d, total distance: %f\n", ++iteration, total_d);
135       
136      }
137    }
138  }
139   FILE* fp = fopen("output12.txt", "a");
140  if (fp == NULL) {
141    printf("The output file cannot be opened!\n");
142    exit(1);
143  }
144  for (i = 0; i <= city_data_num; i++) {
145    fprintf(fp, "%.0f %.0f \n", logx[i], logy[i]);
146  }
147  fclose(fp);
148
149}
150
151/* The program starts here */
152int main(int argc, char** argv) {
153  char line[512];
154  FILE* fp;
155  clock_t start_clock, end_clock;
156  start_clock = clock();
157
158  /* check the number of arguments */
159  if (argc != 2) {
160    printf("usage: ./a.out input_file_name\n");
161    exit(1);
162  }
163
164  /* check whether the file can be opened */
165  fp = fopen(argv[1], "r");
166  if (fp == NULL) {
167    printf("The file (%s) cannot be opened!\n", argv[1]);
168    exit(1);
169  }
170
171  /* read city data and write the position of each city into the array "city_data" */
172  while (fgets(line, sizeof(line), fp)) {
173    char* line_r;
174    line_r = strtok(line, " \t\n");
175    city_data[city_data_num].id = (int)strtol(line_r, NULL, 10);
176    line_r = strtok(NULL, " \t\n");
177    city_data[city_data_num].x = (float)strtod(line_r, NULL);
178    line_r = strtok(NULL, " \t\n");
179    city_data[city_data_num].y = (float)strtod(line_r, NULL);
180
181    city_data_num++;
182  }
183
184  fclose(fp);
185
186  /* solve TSP */
187  do_travel();
188
189  end_clock = clock();
190  printf("processing time is %f [sec]\n", (double)(end_clock - start_clock) / CLOCKS_PER_SEC);
191
192  return 0;
193}
194
195

試したこと

なし

補足情報（FW/ツールのバージョンなど）

写真を添付します。

7 38
12 42
21 47
25 55
30 48
30 40
32 39
31 32
25 32
20 26
27 23
32 22
36 16
30 15
21 10
13 13
10 17
5 25
17 33
8 52
16 57
17 63
27 68
31 62
37 69
43 67
52 64
57 58
62 63
63 69
58 48
62 42
56 37
52 33
48 28
51 21
58 27
61 33
52 41
49 49
42 41
38 46
37 52
42 57
45 35
40 30
39 10
46 10
59 15
5 6
5 64
7 38