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