root / branches / simulator / projects / simulator / simulator / core / world.c @ 1048
History | View | Annotate | Download (7 KB)
1 | 1022 | bpoole | /**
|
---|---|---|---|
2 | * @file world.c
|
||
3 | * @author Colony Project
|
||
4 | * @brief Simulator world code
|
||
5 | *
|
||
6 | * This is the world.
|
||
7 | **/
|
||
8 | |||
9 | #include <stdio.h> |
||
10 | 1026 | bpoole | #include <stdlib.h> |
11 | #include <math.h> |
||
12 | #include <stdarg.h> |
||
13 | 1022 | bpoole | #include "world.h" |
14 | 1042 | bpoole | #include "robot.h" |
15 | #include "robot_shared.h" |
||
16 | 1022 | bpoole | |
17 | 1040 | bpoole | world_t world; |
18 | 1022 | bpoole | |
19 | 1040 | bpoole | int init_world(int max_objs, bbox_t b) |
20 | { |
||
21 | 1044 | bpoole | double pts[8] = {b.p1.x, b.p1.y, b.p1.x, b.p2.y, b.p2.x,b.p2.y,b.p2.x,b.p1.y}; |
22 | 1040 | bpoole | world.max_objs = max_objs; |
23 | world.cur_objs = 0;
|
||
24 | world.objs = calloc(max_objs, sizeof(object_t));
|
||
25 | world.win = b; |
||
26 | 1044 | bpoole | /* Add a polygon surrounding the world */
|
27 | 1047 | bcoltin | // this devours the world - brian
|
28 | //create(ID_POLY,4,POLY_CONNECTED,pts);
|
||
29 | 1040 | bpoole | } |
30 | int destroy_world()
|
||
31 | { |
||
32 | int i;
|
||
33 | for (i = 0; i < world.cur_objs; i++) { |
||
34 | destroy(&world.objs[i]); |
||
35 | } |
||
36 | free(world.objs); |
||
37 | } |
||
38 | 1026 | bpoole | |
39 | double (*collide_func[NUM_SHAPES])(ray_t *ray, object_t *obj) =
|
||
40 | { |
||
41 | collide_poly |
||
42 | }; |
||
43 | int (*create_func[NUM_SHAPES])(object_t *obj, va_list ap) =
|
||
44 | { |
||
45 | create_poly |
||
46 | }; |
||
47 | 1040 | bpoole | int (*destroy_func[NUM_SHAPES])(object_t *obj) =
|
48 | { |
||
49 | destroy_poly |
||
50 | }; |
||
51 | |||
52 | |||
53 | |||
54 | 1026 | bpoole | double collide_circle(ray_t *ray, object_t *obj)
|
55 | { |
||
56 | 1022 | bpoole | return -1; |
57 | } |
||
58 | 1026 | bpoole | |
59 | double collide_seg(ray_t *ray, point_t p3, point_t p4)
|
||
60 | { |
||
61 | double denom, nume_a, nume_b, ua, ub, xint, yint, dist;
|
||
62 | point_t p1 = ray->p; |
||
63 | point_t p2 = {ray->p.x+cos(ray->d),ray->p.y+sin(ray->d)}; |
||
64 | |||
65 | 1045 | bpoole | //printf("(%g,%g) --> (%g,%g) with (%g,%g) --> (%g,%g)\n",p1.x,p1.y,p2.x,p2.y,p3.x,p3.y,p4.x,p4.y);
|
66 | 1026 | bpoole | denom = (p2.y-p1.y)*(p4.x-p3.x)-(p2.x-p1.x)*(p4.y-p3.y); |
67 | if (denom == 0) { |
||
68 | return RAY_MISS;
|
||
69 | } |
||
70 | nume_a = (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x); |
||
71 | nume_b = (p4.x-p3.x)*(p3.y-p1.y)-(p4.y-p3.y)*(p3.x-p1.x); |
||
72 | |||
73 | ua = nume_a/denom; |
||
74 | if (ua < 0 || ua > 1) { |
||
75 | return RAY_MISS;
|
||
76 | } |
||
77 | |||
78 | ub = nume_b/denom; |
||
79 | if (ub < 0) { |
||
80 | return RAY_MISS;
|
||
81 | } |
||
82 | |||
83 | xint = p3.x+ua*(p4.x-p3.x); |
||
84 | yint = p3.y+ua*(p4.y-p3.y); |
||
85 | dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y)); |
||
86 | 1048 | bpoole | //printf("Got: %g\n", dist);
|
87 | 1026 | bpoole | return dist;
|
88 | |||
89 | 1022 | bpoole | } |
90 | |||
91 | 1026 | bpoole | double collide_poly(ray_t *ray, object_t *obj)
|
92 | { |
||
93 | 1042 | bpoole | |
94 | 1026 | bpoole | int i;
|
95 | double min = RAY_MISS;
|
||
96 | double x;
|
||
97 | poly_t *p = (poly_t *) obj->props; |
||
98 | |||
99 | if (obj->id != ID_POLY){
|
||
100 | return -1; |
||
101 | } |
||
102 | |||
103 | for (i = 0; i < p->num_pts - 1; i++) { |
||
104 | if ((x = collide_seg(ray, p->pts[i], p->pts[i+1])) < min){ |
||
105 | min = x; |
||
106 | } |
||
107 | } |
||
108 | 1040 | bpoole | if (p->type == POLY_CONNECTED) {
|
109 | 1026 | bpoole | if ((x = collide_seg(ray,p->pts[i],p->pts[0])) < min) |
110 | min = x; |
||
111 | } |
||
112 | return min;
|
||
113 | } |
||
114 | |||
115 | double collide(ray_t *ray, object_t *obj)
|
||
116 | { |
||
117 | 1040 | bpoole | return collide_func[obj->id+ID_OFFSET](ray, obj);
|
118 | 1022 | bpoole | } |
119 | 1026 | bpoole | |
120 | 1042 | bpoole | double collide_world(ray_t *ray)
|
121 | { |
||
122 | double min = RAY_MISS;
|
||
123 | 1048 | bpoole | double x;
|
124 | int i;
|
||
125 | 1042 | bpoole | for (i = 0; i < world.cur_objs; i++) { |
126 | x = collide(ray, &world.objs[i]); |
||
127 | min = (x < min) ? x : min; |
||
128 | } |
||
129 | return min;
|
||
130 | } |
||
131 | 1026 | bpoole | |
132 | |||
133 | 1042 | bpoole | |
134 | 1026 | bpoole | object_t *create(int id, ...)
|
135 | { |
||
136 | 1040 | bpoole | object_t *obj = &(world.objs[world.cur_objs++]); |
137 | 1026 | bpoole | va_list args; |
138 | |||
139 | va_start(args, id); |
||
140 | 1040 | bpoole | if (create_func[id+ID_OFFSET](obj, args) < 0) { |
141 | /* Cleanup ? */
|
||
142 | 1026 | bpoole | } |
143 | va_end(args); |
||
144 | return obj;
|
||
145 | } |
||
146 | |||
147 | 1040 | bpoole | int destroy(object_t *obj)
|
148 | { |
||
149 | if (obj->id == ID_NULL)
|
||
150 | return 0; |
||
151 | destroy_func[obj->id+ID_OFFSET](obj); |
||
152 | obj->id = ID_NULL; |
||
153 | } |
||
154 | |||
155 | 1026 | bpoole | /**
|
156 | *
|
||
157 | **/
|
||
158 | int create_poly(object_t *obj, va_list ap)
|
||
159 | { |
||
160 | int i;
|
||
161 | poly_t *p; |
||
162 | |||
163 | int argc = va_arg(ap, int); |
||
164 | int poly_type = va_arg(ap, int); |
||
165 | |||
166 | p = malloc(sizeof(poly_t));
|
||
167 | |||
168 | |||
169 | p->num_pts = argc; |
||
170 | p->pts = malloc((argc) * sizeof(point_t));
|
||
171 | p->type = poly_type; |
||
172 | |||
173 | 1043 | chihsiuh | double *pts = va_arg(ap, double*); |
174 | |||
175 | 1026 | bpoole | for(i=0;i < argc; i++) { |
176 | 1043 | chihsiuh | p->pts[i].x = pts[2*i];
|
177 | p->pts[i].y = pts[2*i+1]; |
||
178 | 1026 | bpoole | } |
179 | obj->id = ID_POLY; |
||
180 | obj->bbox = NULL;
|
||
181 | obj->props = p; |
||
182 | return 1; |
||
183 | } |
||
184 | |||
185 | int create_rect(object_t *obj, va_list ap)
|
||
186 | { |
||
187 | int i;
|
||
188 | poly_t *p; |
||
189 | int argc = va_arg(ap, int); |
||
190 | } |
||
191 | |||
192 | 1040 | bpoole | int destroy_poly (object_t *obj)
|
193 | 1026 | bpoole | { |
194 | poly_t *p; |
||
195 | p = (poly_t *)obj->props; |
||
196 | if (p != NULL) free(p->pts); |
||
197 | free(p); |
||
198 | 1040 | bpoole | return 1; |
199 | } |
||
200 | 1026 | bpoole | |
201 | 1040 | bpoole | void print_world(void) |
202 | { |
||
203 | int i;
|
||
204 | printf("WORLD: %d object(s) in (%g,%g)->(%g,%g)\n", world.cur_objs, world.win.p1.x,world.win.p1.y,world.win.p2.x,world.win.p2.y);
|
||
205 | for (i = 0; i < world.cur_objs; i++) { |
||
206 | print_object(&world.objs[i]); |
||
207 | 1026 | bpoole | } |
208 | } |
||
209 | void print_object(object_t *obj)
|
||
210 | { |
||
211 | int i;
|
||
212 | poly_t *p; |
||
213 | switch (obj->id) {
|
||
214 | 1040 | bpoole | case ID_NULL:
|
215 | printf("\tNULL\n");
|
||
216 | 1026 | bpoole | case ID_POLY:
|
217 | p = (poly_t *) obj->props; |
||
218 | 1040 | bpoole | printf("\tPOLYGON (%d points, %s) { ", p->num_pts, p->type?"connected" : "disconnected"); |
219 | 1026 | bpoole | for (i = 0; i < p->num_pts; i++){ |
220 | printf("(%g, %g) ", p->pts[i].x, p->pts[i].y);
|
||
221 | } |
||
222 | printf("}\n");
|
||
223 | break;
|
||
224 | default:
|
||
225 | break;
|
||
226 | |||
227 | } |
||
228 | } |
||
229 | |||
230 | 1042 | bpoole | |
231 | /*
|
||
232 | * XXX: This shouldn't be here. Where should it be?
|
||
233 | */
|
||
234 | /* These need to be measured... */
|
||
235 | double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5}; |
||
236 | 1048 | bpoole | //double rf_thetas[5] = {0.0, 0.0, M_PI/2, M_PI, 3*M_PI/2};
|
237 | 1042 | bpoole | void update_rangefinders(Robot *bot)
|
238 | { |
||
239 | 1044 | bpoole | RangeFinder r = bot->shared->ranges; |
240 | ray_t rf = {bot->pose.x, bot->pose.y, 0};
|
||
241 | /* Motion code has +y going down, I don't.
|
||
242 | * So to compensate, just reflect theta up to first quadrant.*/
|
||
243 | double theta = bot->pose.theta;
|
||
244 | 1042 | bpoole | int ir;
|
245 | double x;
|
||
246 | for (ir = 0; ir < 5; ir++) |
||
247 | { |
||
248 | 1048 | bpoole | rf.d = theta + rf_thetas[ir]; |
249 | 1044 | bpoole | x = collide_world(&rf); |
250 | 1048 | bpoole | printf("@(%g) - [%d] = %g --> %d\n",rf.d, ir, x, (short)x); |
251 | 1044 | bpoole | bot->shared->ranges.d[ir] = x; |
252 | 1042 | bpoole | } |
253 | } |
||
254 | |||
255 | 1041 | chihsiuh | int load_object (const char* line) { |
256 | char buf[BUF_SIZE];
|
||
257 | int i = 0; |
||
258 | int j;
|
||
259 | int id;
|
||
260 | 1043 | chihsiuh | if (strstr(line,"POLYGON") == line){ |
261 | 1041 | chihsiuh | int num;
|
262 | int type;
|
||
263 | double *pts;
|
||
264 | id = ID_POLY; |
||
265 | 1043 | chihsiuh | i = strlen("POLYGON") + 1; |
266 | 1041 | chihsiuh | |
267 | sscanf(line+i,"%s",buf);
|
||
268 | num = strtol(buf,NULL,0); |
||
269 | i += strlen(buf) + 1;
|
||
270 | |||
271 | 1043 | chihsiuh | if (strstr(line+i,"CONNECTED") == line+i){ |
272 | 1041 | chihsiuh | i += strlen("CONNECTED") + 1; |
273 | type = POLY_CONNECTED; |
||
274 | 1043 | chihsiuh | }else if (strstr(line+i,"DISCONNECTED") == line+i){ |
275 | 1041 | chihsiuh | i += strlen("DISCONNECTED") + 1; |
276 | type = POLY_DISCONNECTED; |
||
277 | 1043 | chihsiuh | }else if (strstr(line+i,"RECT") == line+i){ |
278 | 1041 | chihsiuh | i += strlen("RECT") + 1; |
279 | type = POLY_RECT; |
||
280 | }else{
|
||
281 | // invalid input
|
||
282 | 1043 | chihsiuh | fprintf(stderr,"Invalid input for POLYGON\n");
|
283 | 1041 | chihsiuh | return -1; |
284 | } |
||
285 | |||
286 | char *c1,*c2;
|
||
287 | c1 = (char*)(line+i);
|
||
288 | c2 = (char*)(line+i);
|
||
289 | pts = (double*) malloc(sizeof(double)*num*2); |
||
290 | for (j = 0;j < num;j++){ |
||
291 | pts[2*j] = strtod(c1,&c2);
|
||
292 | pts[2*j+1] = strtod(c2,&c1); |
||
293 | 1043 | chihsiuh | #if DEBUG
|
294 | 1041 | chihsiuh | printf("%lf %lf\n",pts[2*j],pts[2*j+1]); |
295 | 1043 | chihsiuh | #endif
|
296 | 1041 | chihsiuh | } |
297 | |||
298 | create(id,num,type,pts); |
||
299 | free(pts); |
||
300 | 1043 | chihsiuh | }else if (strstr(line,"CIRCLE") == line){ |
301 | id = ID_CIRCLE; |
||
302 | i = strlen("CIRCLE") + 1; |
||
303 | //TODO
|
||
304 | }else if (strstr(line,"RECTANGLE") == line){ |
||
305 | id = ID_RECTANGLE; |
||
306 | i = strlen("RECTANGLE") + 1; |
||
307 | //TODO
|
||
308 | }else{
|
||
309 | //Invalid input
|
||
310 | #if DEBUG
|
||
311 | fprintf(stderr,"Invalid object: %s",line);
|
||
312 | #endif
|
||
313 | } |
||
314 | 1041 | chihsiuh | return 0; |
315 | } |
||
316 | |||
317 | 1043 | chihsiuh | int load_world (const char* filename,int max_objs) { |
318 | 1041 | chihsiuh | FILE* fin; |
319 | char buf[BUF_SIZE];
|
||
320 | bbox_t bbox = {0,0,0,0}; |
||
321 | if ((fin = fopen(filename,"r"))==NULL){ |
||
322 | // open file failed
|
||
323 | 1043 | chihsiuh | perror("Fail to open file");
|
324 | 1041 | chihsiuh | return -1; |
325 | } |
||
326 | if (fgets(buf,512,fin) != NULL){ |
||
327 | sscanf(buf,"WORLD %lf %lf %lf %lf",&bbox.p1.x,&bbox.p1.y,&bbox.p2.x,&bbox.p2.y);
|
||
328 | 1043 | chihsiuh | init_world(max_objs, bbox); |
329 | 1041 | chihsiuh | while (fgets(buf,512,fin) != NULL){ |
330 | load_object(buf); |
||
331 | } |
||
332 | } |
||
333 | fclose(fin); |
||
334 | return 0; |
||
335 | } |