Statistics
| Revision:

root / branches / simulator / projects / simulator / simulator / core / world.c @ 1044

History | View | Annotate | Download (6.94 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
    create(ID_POLY,4,POLY_CONNECTED,pts);
28
}
29
int destroy_world()
30
{
31
    int i;
32
    for (i = 0; i < world.cur_objs; i++) {
33
        destroy(&world.objs[i]);
34
    }
35
    free(world.objs);
36
}
37

    
38
double (*collide_func[NUM_SHAPES])(ray_t *ray, object_t *obj) =
39
    {
40
    collide_poly
41
    };
42
int (*create_func[NUM_SHAPES])(object_t *obj, va_list ap) =
43
{
44
    create_poly
45
};
46
int (*destroy_func[NUM_SHAPES])(object_t *obj) = 
47
    {
48
    destroy_poly
49
    };
50

    
51

    
52

    
53
double collide_circle(ray_t *ray, object_t *obj)
54
{
55
    return -1;
56
}
57

    
58
double collide_seg(ray_t *ray, point_t p3, point_t p4)
59
{   
60
    double denom, nume_a, nume_b, ua, ub, xint, yint, dist;
61
    point_t p1 = ray->p;
62
    point_t p2 = {ray->p.x+cos(ray->d),ray->p.y+sin(ray->d)};
63

    
64
#if DEBUG
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
#endif
67
    denom = (p2.y-p1.y)*(p4.x-p3.x)-(p2.x-p1.x)*(p4.y-p3.y);
68
    if (denom == 0) {
69
        return RAY_MISS;
70
    }
71
    nume_a = (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x);
72
    nume_b = (p4.x-p3.x)*(p3.y-p1.y)-(p4.y-p3.y)*(p3.x-p1.x);
73

    
74
    ua = nume_a/denom;
75
    if (ua < 0 || ua > 1) {
76
        return RAY_MISS;
77
    }
78

    
79
    ub = nume_b/denom;
80
    if (ub < 0) {
81
        return RAY_MISS;
82
    }
83

    
84
    xint = p3.x+ua*(p4.x-p3.x);
85
    yint = p3.y+ua*(p4.y-p3.y);
86
    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));
87
#if DEBUG
88
    printf("Got: %g\n", dist);
89
#endif
90
    return dist;
91

    
92
}
93

    
94
double collide_poly(ray_t *ray, object_t *obj)
95
{
96

    
97
    int i;
98
    double min = RAY_MISS;
99
    double x;
100
    poly_t *p = (poly_t *) obj->props;
101
    
102
    if (obj->id != ID_POLY){
103
        return -1;
104
    }
105

    
106
    for (i = 0; i < p->num_pts - 1; i++) {
107
        if ((x = collide_seg(ray, p->pts[i], p->pts[i+1])) < min){
108
            min = x;
109
        }
110
    }
111
    if (p->type == POLY_CONNECTED) {
112
        if ((x = collide_seg(ray,p->pts[i],p->pts[0])) < min)
113
            min = x;
114
    }
115
    return min;
116
}
117

    
118
double collide(ray_t *ray, object_t *obj) 
119
{
120
    return collide_func[obj->id+ID_OFFSET](ray, obj);
121
}
122

    
123
double collide_world(ray_t *ray)
124
{
125
    double min = RAY_MISS;
126
    int i, x;
127
    for (i = 0; i < world.cur_objs; i++) {        
128
        x = collide(ray, &world.objs[i]);
129
        min = (x < min) ? x : min;
130
    }
131
    return min;
132
}
133

    
134

    
135

    
136
object_t *create(int id, ...)
137
{
138
    object_t *obj = &(world.objs[world.cur_objs++]);
139
    va_list args;
140

    
141
    va_start(args, id);
142
    if (create_func[id+ID_OFFSET](obj, args) < 0) {
143
        /* Cleanup ? */
144
    }
145
    va_end(args);
146
    return obj;
147
}
148

    
149
int destroy(object_t *obj) 
150
{
151
    if (obj->id == ID_NULL)
152
        return 0;
153
    destroy_func[obj->id+ID_OFFSET](obj);
154
    obj->id = ID_NULL;
155
}
156

    
157
/**
158
 *
159
 **/
160
int create_poly(object_t *obj, va_list ap) 
161
{
162
    int i;
163
    poly_t *p;
164

    
165
    int argc =  va_arg(ap, int);
166
    int poly_type = va_arg(ap, int);
167

    
168
    p = malloc(sizeof(poly_t));
169

    
170

    
171
    p->num_pts = argc;
172
    p->pts = malloc((argc) * sizeof(point_t));
173
    p->type = poly_type;
174

    
175
                double *pts = va_arg(ap, double*);
176

    
177
    for(i=0;i < argc; i++) {
178
                        p->pts[i].x = pts[2*i];
179
                        p->pts[i].y = pts[2*i+1];
180
    }
181
    obj->id = ID_POLY;
182
    obj->bbox = NULL;
183
    obj->props = p;
184
    return 1;
185
}
186

    
187
int create_rect(object_t *obj, va_list ap)
188
{
189
    int i;
190
    poly_t *p;
191
    int argc = va_arg(ap, int);
192
}
193

    
194
int destroy_poly (object_t *obj)
195
{
196
    poly_t *p;
197
    p =  (poly_t *)obj->props;
198
    if (p != NULL) free(p->pts);
199
    free(p);
200
    return 1;
201
}
202

    
203
void print_world(void)
204
{
205
    int i;
206
    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);
207
    for (i = 0; i < world.cur_objs; i++) {
208
        print_object(&world.objs[i]);
209
    }
210
}
211
void print_object(object_t *obj)
212
{
213
    int i;
214
    poly_t *p;
215
    switch (obj->id) {
216
        case ID_NULL:
217
            printf("\tNULL\n");
218
        case ID_POLY:
219
            p = (poly_t *) obj->props;
220
            printf("\tPOLYGON (%d points, %s) { ", p->num_pts, p->type?"connected" : "disconnected");
221
            for (i = 0; i < p->num_pts; i++){
222
                printf("(%g, %g) ", p->pts[i].x, p->pts[i].y);
223
            }
224
            printf("}\n");
225
            break;
226
        default:
227
            break;
228
                    
229
    }
230
}
231

    
232

    
233
/*
234
 * XXX: This shouldn't be here. Where should it be?
235
 */
236
/* These need to be measured... */
237
double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5};
238
void update_rangefinders(Robot *bot)
239
{
240
    RangeFinder r = bot->shared->ranges;
241
    ray_t rf = {bot->pose.x, bot->pose.y, 0};
242
    /* Motion code has +y going down, I don't.
243
     * So to compensate, just reflect theta up to first quadrant.*/
244
    double theta = bot->pose.theta;
245
    theta = 2*M_PI - theta; 
246
    //ray_t rf = {0,0,0};
247
    int ir;
248
    double x;
249
    for (ir = 0; ir < 5; ir++)
250
    {
251
        rf.d = rf_thetas[ir];
252
        x = collide_world(&rf);
253
        printf("[%d] = %g\n", ir, x);
254
        bot->shared->ranges.d[ir] = x;
255
    }
256
}
257

    
258
int load_object (const char* line) {
259
  char buf[BUF_SIZE];
260
  int i = 0;
261
  int j;
262
  int id;
263
  if (strstr(line,"POLYGON") == line){
264
    int num;
265
    int type;
266
    double *pts;
267
    id = ID_POLY;
268
    i = strlen("POLYGON") + 1;
269

    
270
    sscanf(line+i,"%s",buf);
271
    num = strtol(buf,NULL,0);
272
    i += strlen(buf) + 1;
273

    
274
    if (strstr(line+i,"CONNECTED") == line+i){
275
      i += strlen("CONNECTED") + 1;
276
      type = POLY_CONNECTED;
277
    }else if (strstr(line+i,"DISCONNECTED") == line+i){
278
      i += strlen("DISCONNECTED") + 1;
279
      type = POLY_DISCONNECTED;
280
    }else if (strstr(line+i,"RECT") == line+i){
281
      i += strlen("RECT") + 1;
282
      type = POLY_RECT;
283
    }else{
284
      // invalid input
285
                        fprintf(stderr,"Invalid input for POLYGON\n");
286
      return -1;
287
    }
288

    
289
    char *c1,*c2;
290
    c1 = (char*)(line+i);
291
    c2 = (char*)(line+i);
292
    pts = (double*) malloc(sizeof(double)*num*2);
293
    for (j = 0;j < num;j++){
294
      pts[2*j] = strtod(c1,&c2);
295
      pts[2*j+1] = strtod(c2,&c1);
296
#if DEBUG
297
      printf("%lf %lf\n",pts[2*j],pts[2*j+1]);
298
#endif
299
    }
300

    
301
    create(id,num,type,pts);
302
    free(pts);
303
  }else if (strstr(line,"CIRCLE") == line){
304
                id = ID_CIRCLE;
305
                i = strlen("CIRCLE") + 1;
306
                //TODO
307
        }else if (strstr(line,"RECTANGLE") == line){
308
                id = ID_RECTANGLE;
309
                i = strlen("RECTANGLE") + 1;
310
                //TODO
311
        }else{
312
                //Invalid input
313
#if DEBUG
314
                fprintf(stderr,"Invalid object: %s",line);
315
#endif
316
        }
317
  return 0;
318
}
319

    
320
int load_world (const char* filename,int max_objs) {
321
  FILE* fin;
322
  char buf[BUF_SIZE];
323
  bbox_t bbox = {0,0,0,0};
324
  if ((fin = fopen(filename,"r"))==NULL){
325
    // open file failed
326
                perror("Fail to open file");
327
    return -1;
328
  }
329
  if (fgets(buf,512,fin) != NULL){
330
    sscanf(buf,"WORLD %lf %lf %lf %lf",&bbox.p1.x,&bbox.p1.y,&bbox.p2.x,&bbox.p2.y);
331
    init_world(max_objs, bbox);
332
    while (fgets(buf,512,fin) != NULL){
333
      load_object(buf);
334
    }
335
  }
336
  fclose(fin);
337
  return 0;
338
}
339