Browse Source

Neues schnelleres Routing

Lemmi hat meinen scanline basierten Algorithmus mal schneller gemacht
in dem er nicht alle Cluster (O(n^2) Laufzeit) anfassen muss, sondern
nur die relevanten Cluster aus dem Grid extrahiert und verwendet.
Patrick Kilian 9 years ago
parent
commit
635db0ebf2
4 changed files with 231 additions and 91 deletions
  1. 197
    85
      gitter/route.c
  2. 1
    0
      gitter/route.h
  3. 31
    4
      gitter/screen.c
  4. 2
    2
      gitter/screen.h

+ 197
- 85
gitter/route.c View File

@@ -51,77 +51,11 @@ waypoint_t* go_around(vector_t* A, vector_t* B, cluster_t* C, double r) {
51 51
 	return wp;
52 52
 }
53 53
 
54
-waypoint_t* route(vector_t* start, vector_t* stop, cluster_t* cluster, int n_points) {
55
-	int i;
56
-
57
-	int i_min = -1;
58
-	double r_min = 1;
59
-
60
-	for(i = 0; i < n_points; i++) {
61
-		double r = dividing_ratio(start, stop, &(cluster[i].center));
62
-		if (r > 0 && r < 1) {
63
-			double d = dist_to_line(start, stop, &(cluster[i].center));
64
-			if (fabs(d) < cluster[i].safety_radius) {
65
-				if(fabs(r-0.5) < fabs(r_min-0.5)) {
66
-					i_min = i;
67
-					r_min = r;
68
-				}
69
-			}
70
-		}
71
-	}
72
-
73
-	if(i_min >= 0) {
74
-		waypoint_t* wp = go_around(start, stop, &(cluster[i_min]), r_min);
75
-
76
-		waypoint_t* part1 = route(start, &(wp->point), cluster, n_points);
77
-		if(part1 == NULL) {
78
-			part1 = wp;
79
-		} else {
80
-			waypoint_t* t = part1;
81
-			while(t->next != NULL) {
82
-				t = t->next;
83
-			}
84
-			t->next = wp;
85
-		}
86
-		waypoint_t* part2 = route(&(wp->point), stop, cluster, n_points);
87
-		if(part2 != 0) {
88
-			waypoint_t* t = part1;
89
-			while(t->next != NULL) {
90
-				t = t->next;
91
-			}
92
-			t->next = part2;
93
-		}
94
-		return part1;
95
-	} else {
96
-		return NULL;
97
-	}
98
-}
99
-
100
-waypoint_t* plotCourse(vector_t* start, vector_t* stop, cluster_t* cluster, int n) {
101
-	int n_points = n*n;
102
-	waypoint_t* wp_start = malloc(sizeof(waypoint_t));
103
-	waypoint_t* wp_stop = malloc(sizeof(waypoint_t));
104
-	wp_start->point.x = start->x;
105
-	wp_start->point.y = start->y;
106
-	wp_stop->point.x = stop->x;
107
-	wp_stop->point.y = stop->y;
108
-	wp_start->next = route(start, stop, cluster, n_points);
109
-	wp_stop->next = NULL;
110
-	waypoint_t* t = wp_start;
111
-
112
-	while (t->next != NULL) {
113
-		t = t->next;
114
-	}
115
-
116
-	t->next = wp_stop;
117
-	return wp_start;
118
-}
119
-
120
-void get_surrounding_points(vector_t *surrounding_points, cluster_t *clusters, int n, int face_x, int face_y) {
121
-	surrounding_points[0] = clusters[(face_y - 1) * n + (face_x - 1)].center;
122
-	surrounding_points[1] = clusters[(face_y) * n + (face_x - 1)].center;
123
-	surrounding_points[2] = clusters[(face_y) * n + (face_x)].center;
124
-	surrounding_points[3] = clusters[(face_y - 1) * n + (face_x)].center;
54
+void get_surrounding_points(cluster_t **surrounding_points, cluster_t* clusters, int n, int face_x, int face_y) {
55
+	surrounding_points[0] = &(clusters[(face_y - 1) * n + (face_x - 1)]);
56
+	surrounding_points[1] = &(clusters[(face_y) * n + (face_x - 1)]);
57
+	surrounding_points[2] = &(clusters[(face_y) * n + (face_x)]);
58
+	surrounding_points[3] = &(clusters[(face_y - 1) * n + (face_x)]);
125 59
 }
126 60
 
127 61
 int get_line_intersection(vector_t *P0, vector_t *P1, vector_t *P2, vector_t *P3, vector_t *result)
@@ -148,24 +82,24 @@ int get_line_intersection(vector_t *P0, vector_t *P1, vector_t *P2, vector_t *P3
148 82
 	return 0;
149 83
 }
150 84
 
151
-void find_face(vector_t *p, cluster_t *cluster, int n, int *x, int *y) {
85
+void find_face(vector_t *p, cluster_t *clusters, int n, int *x, int *y) {
152 86
 	int face_x = p->x / (GLOBALS.WIDTH / (n + 1));
153 87
 	int face_y = p->y / (GLOBALS.HEIGHT / (n + 1));
154 88
 	int edge;
155 89
 	int retry = 1;
156 90
 	int count = 0;
157 91
 
158
-	vector_t surrounding[4];
92
+	cluster_t *surrounding[4];
159 93
 
160 94
 	while (retry && count < 4) {
161 95
 		retry = 0;
162
-		get_surrounding_points(surrounding, cluster, n, face_x, face_y);
96
+		get_surrounding_points(surrounding, clusters, n, face_x, face_y);
163 97
 
164 98
 		for (edge = 0; edge < 4; edge++) {
165
-			double r = dist_to_line(surrounding + edge, surrounding + (edge + 1) % 4, p);
99
+			double r = dist_to_line(&(surrounding[edge]->center), &(surrounding[(edge + 1) % 4]->center), p);
166 100
 			if (r > 0) {
167 101
 
168
-				fprintf(stderr, "edge: %d, r %f, x %d, y %d\n", edge, r, face_x, face_y);
102
+				//fprintf(stderr, "edge: %d, r %f, x %d, y %d\n", edge, r, face_x, face_y);
169 103
 				retry = 1;
170 104
 				count++;
171 105
 
@@ -194,6 +128,184 @@ void find_face(vector_t *p, cluster_t *cluster, int n, int *x, int *y) {
194 128
 
195 129
 }
196 130
 
131
+waypoint_t* route_scanline_gridbased(vector_t* start, vector_t* stop, cluster_t* clusters, int n) {
132
+	static int called = 0;
133
+	int i = -1;
134
+	int last_edge = -1;
135
+	int face_x, face_y, face_stop_x, face_stop_y; //should be passed as argument
136
+
137
+	cluster_t *min = NULL;
138
+	double r_min = 1;
139
+
140
+
141
+	find_face(start, clusters, n, &face_x, &face_y);
142
+	find_face(stop, clusters, n, &face_stop_x, &face_stop_y);
143
+
144
+	called++;
145
+
146
+	//fprintf(stderr, "going from (%.1f, %.1f) to (%.1f, %.1f)\n", start->x, start->y, stop->x, stop->y);
147
+	while (min == NULL && (face_x != face_stop_x || face_y != face_stop_y)) {
148
+		//printf("x %d y %d sx %d sy %d called: %d i: %d\n", face_x, face_y, face_stop_x, face_stop_y, called, i);
149
+		//getchar();
150
+		cluster_t *surrounding[4];
151
+		get_surrounding_points(surrounding, clusters, n, face_x, face_y);
152
+
153
+		for (i = 0; i < 4; i++) {
154
+			double r = dividing_ratio(start, stop, &(surrounding[i]->center));
155
+			if (r > 0 && r < 1) {
156
+				double d = dist_to_line(start, stop, &(surrounding[i]->center));
157
+				if (fabs(d) < surrounding[i]->safety_radius) {
158
+					min = surrounding[i];
159
+					r_min = r;
160
+					break;
161
+				}
162
+			}
163
+		}
164
+
165
+		if (min == NULL) {
166
+			for (i = 0; i < 4; i++) {
167
+				if (i == last_edge) {
168
+					continue;
169
+				}
170
+
171
+				if (get_line_intersection(&(surrounding[i]->center), &(surrounding[(i + 1)%4]->center), start, stop, NULL)) {
172
+					break;
173
+				}
174
+			}
175
+
176
+			switch (i) {
177
+				case 0 :
178
+					face_x--;
179
+					break;
180
+				case 1 :
181
+					face_y++;
182
+					break;
183
+				case 2 :
184
+					face_x++;
185
+					break;
186
+				case 3 :
187
+					face_y--;
188
+					break;
189
+				default:
190
+					break;
191
+			}
192
+
193
+			last_edge = (i + 2) % 4;
194
+		}
195
+	}
196
+
197
+	if(min != NULL) {
198
+		waypoint_t* wp = go_around(start, stop, min, r_min);
199
+
200
+		waypoint_t* part1 = route_scanline_gridbased(start, &(wp->point), clusters, n);
201
+		if(part1 == NULL) {
202
+			part1 = wp;
203
+		} else {
204
+			waypoint_t* t = part1;
205
+			while(t->next != NULL) {
206
+				t = t->next;
207
+			}
208
+			t->next = wp;
209
+		}
210
+		waypoint_t* part2 = route_scanline_gridbased(&(wp->point), stop, clusters, n);
211
+		if(part2 != 0) {
212
+			waypoint_t* t = part1;
213
+			while(t->next != NULL) {
214
+				t = t->next;
215
+			}
216
+			t->next = part2;
217
+		}
218
+		return part1;
219
+	} else {
220
+		return NULL;
221
+	}
222
+}
223
+
224
+waypoint_t* route(vector_t* start, vector_t* stop, cluster_t* clusters, int n_points) {
225
+	int i;
226
+
227
+	int i_min = -1;
228
+	double r_min = 1;
229
+
230
+	for(i = 0; i < n_points; i++) {
231
+		double r = dividing_ratio(start, stop, &(clusters[i].center));
232
+		if (r > 0 && r < 1) {
233
+			double d = dist_to_line(start, stop, &(clusters[i].center));
234
+			if (fabs(d) < clusters[i].safety_radius) {
235
+				if(fabs(r-0.5) < fabs(r_min-0.5)) {
236
+					i_min = i;
237
+					r_min = r;
238
+				}
239
+			}
240
+		}
241
+	}
242
+
243
+	if(i_min >= 0) {
244
+		waypoint_t* wp = go_around(start, stop, &(clusters[i_min]), r_min);
245
+
246
+		waypoint_t* part1 = route(start, &(wp->point), clusters, n_points);
247
+		if(part1 == NULL) {
248
+			part1 = wp;
249
+		} else {
250
+			waypoint_t* t = part1;
251
+			while(t->next != NULL) {
252
+				t = t->next;
253
+			}
254
+			t->next = wp;
255
+		}
256
+		waypoint_t* part2 = route(&(wp->point), stop, clusters, n_points);
257
+		if(part2 != 0) {
258
+			waypoint_t* t = part1;
259
+			while(t->next != NULL) {
260
+				t = t->next;
261
+			}
262
+			t->next = part2;
263
+		}
264
+		return part1;
265
+	} else {
266
+		return NULL;
267
+	}
268
+}
269
+
270
+waypoint_t* plotCourse_scanline_gridbased(vector_t* start, vector_t* stop, cluster_t* clusters, int n) {
271
+	//int n_points = n*n;
272
+	waypoint_t* wp_start = malloc(sizeof(waypoint_t));
273
+	waypoint_t* wp_stop = malloc(sizeof(waypoint_t));
274
+	wp_start->point.x = start->x;
275
+	wp_start->point.y = start->y;
276
+	wp_stop->point.x = stop->x;
277
+	wp_stop->point.y = stop->y;
278
+	wp_start->next = route_scanline_gridbased(start, stop, clusters, n);
279
+	wp_stop->next = NULL;
280
+	waypoint_t* t = wp_start;
281
+
282
+	while (t->next != NULL) {
283
+		t = t->next;
284
+	}
285
+
286
+	t->next = wp_stop;
287
+	return wp_start;
288
+}
289
+
290
+waypoint_t* plotCourse(vector_t* start, vector_t* stop, cluster_t* clusters, int n) {
291
+	waypoint_t* wp_start = malloc(sizeof(waypoint_t));
292
+	waypoint_t* wp_stop = malloc(sizeof(waypoint_t));
293
+	wp_start->point.x = start->x;
294
+	wp_start->point.y = start->y;
295
+	wp_stop->point.x = stop->x;
296
+	wp_stop->point.y = stop->y;
297
+	wp_start->next = route(start, stop, clusters, n * n);
298
+	wp_stop->next = NULL;
299
+	waypoint_t* t = wp_start;
300
+
301
+	while (t->next != NULL) {
302
+		t = t->next;
303
+	}
304
+
305
+	t->next = wp_stop;
306
+	return wp_start;
307
+}
308
+
197 309
 waypoint_t *smooth(waypoint_t *way, int res) {
198 310
 	waypoint_t *end = way;
199 311
 	waypoint_t *working_start;
@@ -253,7 +365,7 @@ waypoint_t *smooth(waypoint_t *way, int res) {
253 365
 			working = working->next;
254 366
 			working->point = s;
255 367
 		}
256
-		
368
+
257 369
 		working->next = end;
258 370
 		end = end->next;
259 371
 	}
@@ -261,14 +373,14 @@ waypoint_t *smooth(waypoint_t *way, int res) {
261 373
 	return working_start;
262 374
 }
263 375
 
264
-waypoint_t* plotCourse_gridbased(vector_t *start, vector_t *stop, cluster_t *cluster, int n) {
376
+waypoint_t* plotCourse_gridbased(vector_t *start, vector_t *stop, cluster_t *clusters, int n) {
265 377
 	int face_x, face_y, face_s_x, face_s_y;
266 378
 	int edge;
267 379
 	int last_edge = -1;
268 380
 
269
-	find_face(start, cluster, n, &face_x, &face_y);
270
-	find_face(stop, cluster, n, &face_s_x, &face_s_y);
271
-	vector_t surrounding[4];
381
+	find_face(start, clusters, n, &face_x, &face_y);
382
+	find_face(stop, clusters, n, &face_s_x, &face_s_y);
383
+	cluster_t *surrounding[4];
272 384
 	waypoint_t *wp_start = malloc (sizeof(waypoint_t));
273 385
 	wp_start->point = *start;
274 386
 	wp_start->next  = NULL;
@@ -277,15 +389,15 @@ waypoint_t* plotCourse_gridbased(vector_t *start, vector_t *stop, cluster_t *clu
277 389
 
278 390
 	while (face_x != face_s_x || face_y != face_s_y) {
279 391
 		vector_t c;
280
-		get_surrounding_points(surrounding, cluster, n, face_x, face_y);
392
+		get_surrounding_points(surrounding, clusters, n, face_x, face_y);
281 393
 
282 394
 		for (edge = 0; edge < 4; edge++) {
283 395
 			if (edge == last_edge) continue;
284 396
 			c = working->point;
285 397
 
286
-			if (get_line_intersection(surrounding + edge, surrounding + (edge + 1)%4, &c, stop, NULL)) {
287
-				c.x = (surrounding[edge].x + surrounding[(edge + 1) % 4].x) / 2;
288
-				c.y = (surrounding[edge].y + surrounding[(edge + 1) % 4].y) / 2;
398
+			if (get_line_intersection(&(surrounding[edge]->center), &(surrounding[(edge + 1)%4]->center), &c, stop, NULL)) {
399
+				c.x = (surrounding[edge]->center.x + surrounding[(edge + 1) % 4]->center.x) / 2;
400
+				c.y = (surrounding[edge]->center.y + surrounding[(edge + 1) % 4]->center.y) / 2;
289 401
 				break;
290 402
 			}
291 403
 		}

+ 1
- 0
gitter/route.h View File

@@ -5,6 +5,7 @@
5 5
 
6 6
 waypoint_t* smooth(waypoint_t *way, int res);
7 7
 waypoint_t* plotCourse(vector_t* start, vector_t* stop, cluster_t* clusters, int n);
8
+waypoint_t* plotCourse_scanline_gridbased(vector_t* start, vector_t* stop, cluster_t* clusters, int n);
8 9
 waypoint_t* plotCourse_gridbased(vector_t* start, vector_t* stop, cluster_t* clusters, int n);
9 10
 
10 11
 #endif  /*ROUTE_H*/

+ 31
- 4
gitter/screen.c View File

@@ -163,6 +163,8 @@ int main_loop() {
163 163
 	waypoint_t* route1s = NULL;
164 164
 	waypoint_t* route2 = NULL;
165 165
 	waypoint_t* route2s = NULL;
166
+	waypoint_t* route3 = NULL;
167
+	waypoint_t* route3s = NULL;
166 168
 	char needJsonOutput = 0;
167 169
 
168 170
 	struct timeval t1, t2;
@@ -185,12 +187,14 @@ int main_loop() {
185 187
 
186 188
 		if (view_waypoints) {
187 189
 			draw_route(route1, 0, 0, 255, 1);
188
-			draw_route(route2, 255, 0, 0, 1);
190
+			draw_route(route2, 0, 255, 0, 1);
191
+			draw_route(route3, 255, 0, 0, 1);
189 192
 		}
190 193
 
191 194
 		if (view_smoothed) {
192 195
 			draw_route(route1s, 0, 255, 255, 0);
193
-			draw_route(route2s, 255, 255, 0, 0);
196
+			draw_route(route2s, 128, 255, 128, 0);
197
+			draw_route(route3s, 255, 255, 0, 0);
194 198
 		}
195 199
 
196 200
 		if (start.x != 0 && start.y != 0) {
@@ -219,6 +223,14 @@ int main_loop() {
219 223
 						stop.y = e.button.y;
220 224
 						break;
221 225
 				}
226
+				if (start.x != 0 && start.y != 0) {
227
+					aacircleRGBA(screen, start.x, start.y, 2, 0, 255, 0, 255);
228
+				}
229
+
230
+				if (stop.x != 0 && stop.y != 0) {
231
+					aacircleRGBA(screen, stop.x, stop.y, 2, 255, 0, 0, 255);
232
+				}
233
+				SDL_Flip(screen);
222 234
 				if (start.x != 0 && start.y != 0 && stop.x != 0 && stop.y != 0) {
223 235
 					fprintf(stderr, "\n\nRoute from (%f,%f) to (%f,%f)\n", start.x, start.y, stop.x, stop.y);
224 236
 
@@ -237,17 +249,32 @@ int main_loop() {
237 249
 					fprintf(stderr, "smoth scanline based route took %f ms\n", elapsedTime);
238 250
 
239 251
 					gettimeofday(&t1, NULL);
240
-					route2 = plotCourse_gridbased(&start, &stop, clusters, n);
252
+					route2 = plotCourse_scanline_gridbased(&start, &stop, clusters, n);
241 253
 					gettimeofday(&t2, NULL);
242 254
 					elapsedTime = (t2.tv_sec - t1.tv_sec) * 1000.0;
243 255
 					elapsedTime += (t2.tv_usec - t1.tv_usec) / 1000.0;
244
-					fprintf(stderr, "plotCourse gridline based took %f ms\n", elapsedTime);
256
+					fprintf(stderr, "plotCourse scanline based gridbased took %f ms\n", elapsedTime);
245 257
 
246 258
 					gettimeofday(&t1, NULL);
247 259
 					route2s = smooth(route2, 5);
248 260
 					gettimeofday(&t2, NULL);
249 261
 					elapsedTime = (t2.tv_sec - t1.tv_sec) * 1000.0;
250 262
 					elapsedTime += (t2.tv_usec - t1.tv_usec) / 1000.0;
263
+					fprintf(stderr, "smoth scanline based gridbased route took %f ms\n", elapsedTime);
264
+
265
+
266
+					gettimeofday(&t1, NULL);
267
+					route3 = plotCourse_gridbased(&start, &stop, clusters, n);
268
+					gettimeofday(&t2, NULL);
269
+					elapsedTime = (t2.tv_sec - t1.tv_sec) * 1000.0;
270
+					elapsedTime += (t2.tv_usec - t1.tv_usec) / 1000.0;
271
+					fprintf(stderr, "plotCourse gridline based took %f ms\n", elapsedTime);
272
+
273
+					gettimeofday(&t1, NULL);
274
+					route3s = smooth(route3, 5);
275
+					gettimeofday(&t2, NULL);
276
+					elapsedTime = (t2.tv_sec - t1.tv_sec) * 1000.0;
277
+					elapsedTime += (t2.tv_usec - t1.tv_usec) / 1000.0;
251 278
 					fprintf(stderr, "smooth gridline based route took %f ms\n", elapsedTime);
252 279
 
253 280
 					if(needJsonOutput) {

+ 2
- 2
gitter/screen.h View File

@@ -16,8 +16,8 @@ typedef struct {
16 16
 typedef struct wp_t waypoint_t;
17 17
 
18 18
 struct wp_t {
19
-        vector_t point;
20
-        waypoint_t* next;
19
+	vector_t point;
20
+	waypoint_t* next;
21 21
 };
22 22
 
23 23
 void sdl_init();