Browse Source

vektor datentypen, grid optimierungstest

Johannes Kimmel 9 years ago
parent
commit
5533a00d18
3 changed files with 86 additions and 59 deletions
  1. 71
    53
      gitter/route.c
  2. 3
    3
      gitter/screen.c
  3. 12
    3
      gitter/screen.h

+ 71
- 53
gitter/route.c View File

@@ -4,9 +4,33 @@
4 4
 #include "route.h"
5 5
 #include "globals.h"
6 6
 
7
+// Quadrierter Abstand
8
+// Mit Vorzeichen
9
+double quaddist(vector_t* A, vector_t* B) {
10
+	vector_t t;
11
+	t.v = B->v - A->v;
12
+	t.v *= t.v;
13
+	return t.x + t.y;
14
+}
15
+
7 16
 // Abstand zwischen zwei Punkten
8 17
 double dist(vector_t* A, vector_t* B) {
9
-	return hypotf(B->x - A->x, B->y - A->y);
18
+	return sqrt(quaddist(A, B));
19
+}
20
+
21
+double left_of(const vector_t *A, const vector_t *B, const vector_t *C) {
22
+	vector_t t1, t2;
23
+	double t3;
24
+	t1.v = B->v - A->v;
25
+	t2.v = C->v - A->v;
26
+
27
+	t3 = t2.x;
28
+	t2.x = t2.y;
29
+	t2.y = t3;
30
+
31
+	t1.v *= t2.v;
32
+	t3 = t1.x - t1.y;
33
+	return t3;
10 34
 }
11 35
 
12 36
 // Wo auf der Linie zwischen A und B sitzt der Fußpunkt des Lots von C auf AB?
@@ -15,7 +39,7 @@ double dist(vector_t* A, vector_t* B) {
15 39
 // Werte größer 1 bedeuten daß der Fußpunkt auf der Verlängerung von AB hinter B liegt
16 40
 // Wert den Code mit A=B aufruf ist doof und verdient daß alles platzt
17 41
 double dividing_ratio(vector_t* A, vector_t* B, vector_t* C) {
18
-	return ((C->x - A->x)*(B->x - A->x) + (C->y - A->y)*(B->y - A->y))/pow(dist(A, B),2);
42
+	return ((C->x - A->x)*(B->x - A->x) + (C->y - A->y)*(B->y - A->y))/quaddist(A, B);
19 43
 }
20 44
 
21 45
 // Was ist der Minimal-Abstand von C zum Linien AB?
@@ -41,13 +65,14 @@ double dist_to_seg(vector_t* A, vector_t* B, vector_t* C) {
41 65
 }
42 66
 
43 67
 waypoint_t* go_around(vector_t* A, vector_t* B, cluster_t* C, double r) {
44
-	vector_t X = { A->x + r*(B->x - A->x), A->y + r*(B->y - A->y) };
68
+	vector_t X;
69
+	X.v = A->v + (v2d) {r, r} * (B->v - A->v);;
45 70
 	double d = dist(&X, &(C->center));
46
-	vector_t W = {C->center.x + C->safety_radius*sqrt(2)*(X.x - C->center.x) / d, C->center.y + C->safety_radius*sqrt(2)*(X.y - C->center.y) / d};
71
+	vector_t W;// = {{C->center.x + C->safety_radius*sqrt(2)*(X.x - C->center.x) / d, C->center.y + C->safety_radius*sqrt(2)*(X.y - C->center.y) / d}};
72
+	W.v = C->center.v + (X.v - C->center.v) * (v2d) {M_SQRT2, M_SQRT2} * (v2d) {C->safety_radius,  C->safety_radius} / (v2d) {d, d};
47 73
 	waypoint_t* wp = malloc(sizeof(waypoint_t));
48 74
 	wp->next = NULL;
49
-	wp->point.x = W.x;
50
-	wp->point.y = W.y;
75
+	wp->point = W;
51 76
 	return wp;
52 77
 }
53 78
 
@@ -60,21 +85,19 @@ void get_surrounding_points(cluster_t **surrounding_points, cluster_t* clusters,
60 85
 
61 86
 int get_line_intersection(vector_t *P0, vector_t *P1, vector_t *P2, vector_t *P3, vector_t *result)
62 87
 {
63
-	float s1_x, s1_y, s2_x, s2_y;
64
-	s1_x = P1->x - P0->x;
65
-	s1_y = P1->y - P0->y;
66
-	s2_x = P3->x - P2->x;
67
-	s2_y = P3->y - P2->y;
88
+	vector_t s1, s2;
89
+	s1.v = P1->v - P0->v;
90
+	s2.v = P3->v - P2->v;
68 91
 
69
-	float s, t;
70
-	s = (-s1_y * (P0->x - P2->x) + s1_x * (P0->y - P2->y)) / (-s2_x * s1_y + s1_x * s2_y);
71
-	t = ( s2_x * (P0->y - P2->y) - s2_y * (P0->x - P2->x)) / (-s2_x * s1_y + s1_x * s2_y);
92
+	double s, t;
93
+	t = s1.x * s2.y - s2.x * s1.y;
94
+	s = (-s1.y * (P0->x - P2->x) + s1.x * (P0->y - P2->y)) / t;
95
+	t = ( s2.x * (P0->y - P2->y) - s2.y * (P0->x - P2->x)) / t;
72 96
 
73 97
 	if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
74 98
 	{
75 99
 		if (result) {
76
-			result->x = P0->x + (t * s1_x);
77
-			result->y = P0->y + (t * s1_y);
100
+			result->v = P0->v + ((v2d) {t, t} * s1.v);
78 101
 		}
79 102
 		return 1;
80 103
 	}
@@ -96,7 +119,7 @@ void find_face(vector_t *p, cluster_t *clusters, int n, int *x, int *y) {
96 119
 		get_surrounding_points(surrounding, clusters, n, face_x, face_y);
97 120
 
98 121
 		for (edge = 0; edge < 4; edge++) {
99
-			double r = dist_to_line(&(surrounding[edge]->center), &(surrounding[(edge + 1) % 4]->center), p);
122
+			double r = left_of(&(surrounding[edge]->center), &(surrounding[(edge + 1) % 4]->center), p);
100 123
 			if (r > 0) {
101 124
 
102 125
 				//fprintf(stderr, "edge: %d, r %f, x %d, y %d\n", edge, r, face_x, face_y);
@@ -128,21 +151,21 @@ void find_face(vector_t *p, cluster_t *clusters, int n, int *x, int *y) {
128 151
 
129 152
 }
130 153
 
131
-waypoint_t* route_scanline_gridbased(vector_t* start, vector_t* stop, cluster_t* clusters, int n) {
154
+waypoint_t* route_scanline_gridbased(vector_t* start, vector_t* stop, 
155
+		int face_start_x, int face_start_y, int face_stop_x, int face_stop_y, 
156
+		cluster_t* clusters, int n) {
132 157
 	static int called = 0;
133 158
 	int i = -1;
134 159
 	int last_edge = -1;
135
-	int face_x, face_y, face_stop_x, face_stop_y; //should be passed as argument
160
+	int face_x = face_start_x;
161
+	int face_y = face_start_y;
136 162
 
137 163
 	cluster_t *min = NULL;
138 164
 	double r_min = 1;
139 165
 
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 166
 	called++;
145 167
 
168
+	//fprintf(stderr, "going from (%d, %d) to (%d, %d)\n", face_start_x, face_start_y, face_stop_x, face_stop_y);
146 169
 	//fprintf(stderr, "going from (%.1f, %.1f) to (%.1f, %.1f)\n", start->x, start->y, stop->x, stop->y);
147 170
 	while (min == NULL && (face_x != face_stop_x || face_y != face_stop_y)) {
148 171
 		//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);
@@ -196,8 +219,12 @@ waypoint_t* route_scanline_gridbased(vector_t* start, vector_t* stop, cluster_t*
196 219
 
197 220
 	if(min != NULL) {
198 221
 		waypoint_t* wp = go_around(start, stop, min, r_min);
222
+		find_face(&(wp->point), clusters, n, &face_x, &face_y);
199 223
 
200
-		waypoint_t* part1 = route_scanline_gridbased(start, &(wp->point), clusters, n);
224
+		waypoint_t* part1 = NULL;
225
+		if (face_start_x != face_x || face_start_y != face_y ) {
226
+			part1 = route_scanline_gridbased(start, &(wp->point), face_start_x, face_start_y, face_x, face_y, clusters, n);
227
+		}
201 228
 		if(part1 == NULL) {
202 229
 			part1 = wp;
203 230
 		} else {
@@ -207,8 +234,11 @@ waypoint_t* route_scanline_gridbased(vector_t* start, vector_t* stop, cluster_t*
207 234
 			}
208 235
 			t->next = wp;
209 236
 		}
210
-		waypoint_t* part2 = route_scanline_gridbased(&(wp->point), stop, clusters, n);
211
-		if(part2 != 0) {
237
+		waypoint_t* part2 = NULL;
238
+		if (face_stop_x != face_x || face_stop_y != face_y ) {
239
+			 part2 = route_scanline_gridbased(&(wp->point), stop, face_x, face_y, face_stop_x, face_stop_y, clusters, n);
240
+		}
241
+		if(part2 != NULL) {
212 242
 			waypoint_t* t = part1;
213 243
 			while(t->next != NULL) {
214 244
 				t = t->next;
@@ -269,13 +299,14 @@ waypoint_t* route(vector_t* start, vector_t* stop, cluster_t* clusters, int n_po
269 299
 
270 300
 waypoint_t* plotCourse_scanline_gridbased(vector_t* start, vector_t* stop, cluster_t* clusters, int n) {
271 301
 	//int n_points = n*n;
302
+	int face_start_x, face_start_y, face_stop_x, face_stop_y;
303
+	find_face(start, clusters, n, &face_start_x, &face_start_y);
304
+	find_face(stop, clusters, n, &face_stop_x, &face_stop_y);
272 305
 	waypoint_t* wp_start = malloc(sizeof(waypoint_t));
273 306
 	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);
307
+	wp_start->point = *start;
308
+	wp_stop->point = *stop;
309
+	wp_start->next = route_scanline_gridbased(start, stop, face_start_x, face_start_y, face_stop_x, face_stop_y, clusters, n);
279 310
 	wp_stop->next = NULL;
280 311
 	waypoint_t* t = wp_start;
281 312
 
@@ -290,10 +321,8 @@ waypoint_t* plotCourse_scanline_gridbased(vector_t* start, vector_t* stop, clust
290 321
 waypoint_t* plotCourse(vector_t* start, vector_t* stop, cluster_t* clusters, int n) {
291 322
 	waypoint_t* wp_start = malloc(sizeof(waypoint_t));
292 323
 	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;
324
+	wp_start->point = *start;
325
+	wp_stop->point = *stop;
297 326
 	wp_start->next = route(start, stop, clusters, n * n);
298 327
 	wp_stop->next = NULL;
299 328
 	waypoint_t* t = wp_start;
@@ -340,26 +369,16 @@ waypoint_t *smooth(waypoint_t *way, int res) {
340 369
 		midp   = endp;
341 370
 		endp   = end->point;
342 371
 
343
-		t1.x = (startp.x + midp.x) / 2.;
344
-		t1.y = (startp.y + midp.y) / 2.;
345
-
372
+		t1.v = (startp.v + midp.v) * (v2d) {0.5, 0.5};
346 373
 		t2 = midp;
347
-
348
-		v1.x = (midp.x - startp.x) / 2. / res;
349
-		v1.y = (midp.y - startp.y) / 2. / res;
350
-
351
-		v2.x = (endp.x - midp.x) / 2. / res;
352
-		v2.y = (endp.y - midp.y) / 2. / res;
374
+		v1.v = (midp.v - startp.v) * (v2d) {0.5, 0.5} / (v2d) {res, res};
375
+		v2.v = (endp.v - midp.v) * (v2d) {0.5, 0.5} / (v2d) {res, res};
353 376
 
354 377
 		for (i = 0; i < res; i++) {
355
-			s.x = t1.x + ((t2.x - t1.x) * i) / res;
356
-			s.y = t1.y + ((t2.y - t1.y) * i) / res;
357
-
358
-			t1.x += v1.x;
359
-			t1.y += v1.y;
378
+			s.v = t1.v + ((t2.v - t1.v) * (v2d) {i, i}) / (v2d) {res, res};
360 379
 
361
-			t2.x += v2.x;
362
-			t2.y += v2.y;
380
+			t1.v += v1.v;
381
+			t2.v += v2.v;
363 382
 
364 383
 			working->next = malloc (sizeof(waypoint_t));
365 384
 			working = working->next;
@@ -396,8 +415,7 @@ waypoint_t* plotCourse_gridbased(vector_t *start, vector_t *stop, cluster_t *clu
396 415
 			c = working->point;
397 416
 
398 417
 			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;
418
+				c.v = (surrounding[edge]->center.v + surrounding[(edge + 1) % 4]->center.v) / (v2d){2, 2};
401 419
 				break;
402 420
 			}
403 421
 		}

+ 3
- 3
gitter/screen.c View File

@@ -106,7 +106,7 @@ void squirl(cluster_t *clusters, int n) {
106 106
 			float y = clusters[i*n + j].center.y;
107 107
 			float r = hypotf(x-GLOBALS.WIDTH / 2, y - GLOBALS.HEIGHT/2);
108 108
 			float phi = atan2f(y-GLOBALS.HEIGHT / 2, x - GLOBALS.WIDTH/2);
109
-			float angle = (1e-3 * r - 0.5);
109
+			float angle = (8e-4 * r - 0.35);
110 110
 			phi += angle;
111 111
 			x = GLOBALS.WIDTH/2 + r*cos(phi);
112 112
 			y = GLOBALS.HEIGHT/2 + r * sin(phi);
@@ -157,8 +157,8 @@ int main_loop() {
157 157
 	int safety_radius = GLOBALS.HEIGHT / (n + 1) / 5;
158 158
 	float fac = GLOBALS.WIDTH / (n + 1) * 0.3;
159 159
 	SDL_Event e;
160
-	vector_t start = {0, 0};
161
-	vector_t stop = {0, 0};
160
+	vector_t start = {{0, 0}};
161
+	vector_t stop = {{0, 0}};
162 162
 	waypoint_t* route1 = NULL;
163 163
 	waypoint_t* route1s = NULL;
164 164
 	waypoint_t* route2 = NULL;

+ 12
- 3
gitter/screen.h View File

@@ -3,9 +3,18 @@
3 3
 
4 4
 #include "globals.h"
5 5
 
6
-typedef struct {
7
-	float x;
8
-	float y;
6
+#ifdef __SSE__
7
+#include <x86intrin.h>
8
+#endif
9
+
10
+typedef double v2d __attribute__ ((vector_size (16)));
11
+
12
+typedef union {
13
+	v2d v;
14
+	struct {
15
+		double x;
16
+		double y;
17
+	};
9 18
 } vector_t;
10 19
 
11 20
 typedef struct {