Statistics
| Revision:

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

History | View | Annotate | Download (7 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
    //printf("Got: %g\n", dist);
87
    return dist;
88

    
89
}
90

    
91
double collide_poly(ray_t *ray, object_t *obj)
92
{
93

    
94
    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
    if (p->type == POLY_CONNECTED) {
109
        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
    return collide_func[obj->id+ID_OFFSET](ray, obj);
118
}
119

    
120
double collide_world(ray_t *ray)
121
{
122
    double min = RAY_MISS;
123
    double x;
124
    int i;
125
    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

    
132

    
133

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

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

    
147
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
/**
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
                double *pts = va_arg(ap, double*);
174

    
175
    for(i=0;i < argc; i++) {
176
                        p->pts[i].x = pts[2*i];
177
                        p->pts[i].y = pts[2*i+1];
178
    }
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
int destroy_poly (object_t *obj)
193
{
194
    poly_t *p;
195
    p =  (poly_t *)obj->props;
196
    if (p != NULL) free(p->pts);
197
    free(p);
198
    return 1;
199
}
200

    
201
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
    }
208
}
209
void print_object(object_t *obj)
210
{
211
    int i;
212
    poly_t *p;
213
    switch (obj->id) {
214
        case ID_NULL:
215
            printf("\tNULL\n");
216
        case ID_POLY:
217
            p = (poly_t *) obj->props;
218
            printf("\tPOLYGON (%d points, %s) { ", p->num_pts, p->type?"connected" : "disconnected");
219
            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

    
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
//double rf_thetas[5] = {0.0, 0.0, M_PI/2, M_PI, 3*M_PI/2};
237
void update_rangefinders(Robot *bot)
238
{
239
    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
    int ir;
245
    double x;
246
    for (ir = 0; ir < 5; ir++)
247
    {
248
        rf.d = theta + rf_thetas[ir];
249
        x = collide_world(&rf);
250
        printf("@(%g) - [%d] = %g --> %d\n",rf.d, ir, x, (short)x);
251
        bot->shared->ranges.d[ir] = x;
252
    }
253
}
254

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

    
267
    sscanf(line+i,"%s",buf);
268
    num = strtol(buf,NULL,0);
269
    i += strlen(buf) + 1;
270

    
271
    if (strstr(line+i,"CONNECTED") == line+i){
272
      i += strlen("CONNECTED") + 1;
273
      type = POLY_CONNECTED;
274
    }else if (strstr(line+i,"DISCONNECTED") == line+i){
275
      i += strlen("DISCONNECTED") + 1;
276
      type = POLY_DISCONNECTED;
277
    }else if (strstr(line+i,"RECT") == line+i){
278
      i += strlen("RECT") + 1;
279
      type = POLY_RECT;
280
    }else{
281
      // invalid input
282
                        fprintf(stderr,"Invalid input for POLYGON\n");
283
      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
#if DEBUG
294
      printf("%lf %lf\n",pts[2*j],pts[2*j+1]);
295
#endif
296
    }
297

    
298
    create(id,num,type,pts);
299
    free(pts);
300
  }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
  return 0;
315
}
316

    
317
int load_world (const char* filename,int max_objs) {
318
  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
                perror("Fail to open file");
324
    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
    init_world(max_objs, bbox);
329
    while (fgets(buf,512,fin) != NULL){
330
      load_object(buf);
331
    }
332
  }
333
  fclose(fin);
334
  return 0;
335
}