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