Statistics
| Revision:

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

History | View | Annotate | Download (6.88 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
    //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);
65
    denom = (p2.y-p1.y)*(p4.x-p3.x)-(p2.x-p1.x)*(p4.y-p3.y);
66
    if (denom == 0) {
67
        return RAY_MISS;
68
    }
69
    nume_a = (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x);
70
    nume_b = (p4.x-p3.x)*(p3.y-p1.y)-(p4.y-p3.y)*(p3.x-p1.x);
71

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

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

    
82
    xint = p3.x+ua*(p4.x-p3.x);
83
    yint = p3.y+ua*(p4.y-p3.y);
84
    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));
85
    return dist;
86

    
87
}
88

    
89
double collide_poly(ray_t *ray, object_t *obj)
90
{
91

    
92
    int i;
93
    double min = RAY_MISS;
94
    double x;
95
    poly_t *p = (poly_t *) obj->props;
96
    
97
    if (obj->id != ID_POLY){
98
        return -1;
99
    }
100

    
101
    for (i = 0; i < p->num_pts - 1; i++) {
102
        if ((x = collide_seg(ray, p->pts[i], p->pts[i+1])) < min){
103
            min = x;
104
        }
105
    }
106
    if (p->type == POLY_CONNECTED) {
107
        if ((x = collide_seg(ray,p->pts[i],p->pts[0])) < min)
108
            min = x;
109
    }
110
    return min;
111
}
112

    
113
double collide(ray_t *ray, object_t *obj) 
114
{
115
    return collide_func[obj->id+ID_OFFSET](ray, obj);
116
}
117

    
118
double collide_world(ray_t *ray)
119
{
120
    double min = RAY_MISS;
121
    int i, x;
122
    for (i = 0; i < world.cur_objs; i++) {        
123
        x = collide(ray, &world.objs[i]);
124
        min = (x < min) ? x : min;
125
    }
126
    return min;
127
}
128

    
129

    
130

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

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

    
144
int destroy(object_t *obj) 
145
{
146
    if (obj->id == ID_NULL)
147
        return 0;
148
    destroy_func[obj->id+ID_OFFSET](obj);
149
    obj->id = ID_NULL;
150
}
151

    
152
/**
153
 *
154
 **/
155
int create_poly(object_t *obj, va_list ap) 
156
{
157
    int i;
158
    poly_t *p;
159

    
160
    int argc =  va_arg(ap, int);
161
    int poly_type = va_arg(ap, int);
162

    
163
    p = malloc(sizeof(poly_t));
164

    
165

    
166
    p->num_pts = argc;
167
    p->pts = malloc((argc) * sizeof(point_t));
168
    p->type = poly_type;
169

    
170
                double *pts = va_arg(ap, double*);
171

    
172
    for(i=0;i < argc; i++) {
173
                        p->pts[i].x = pts[2*i];
174
                        p->pts[i].y = pts[2*i+1];
175
    }
176
    obj->id = ID_POLY;
177
    obj->bbox = NULL;
178
    obj->props = p;
179
    return 1;
180
}
181

    
182
int create_rect(object_t *obj, va_list ap)
183
{
184
    int i;
185
    poly_t *p;
186
    int argc = va_arg(ap, int);
187
}
188

    
189
int destroy_poly (object_t *obj)
190
{
191
    poly_t *p;
192
    p =  (poly_t *)obj->props;
193
    if (p != NULL) free(p->pts);
194
    free(p);
195
    return 1;
196
}
197

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

    
227

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

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

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

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

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

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

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