Statistics
| Revision:

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

History | View | Annotate | Download (6.44 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
    world.max_objs = max_objs;
22
    world.cur_objs = 0;
23
    world.objs = calloc(max_objs,  sizeof(object_t));
24
    world.win = b;
25
    //PBBOX(b);
26
}
27
int destroy_world()
28
{
29
    int i;
30
    for (i = 0; i < world.cur_objs; i++) {
31
        destroy(&world.objs[i]);
32
    }
33
    free(world.objs);
34
}
35

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

    
49

    
50

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

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

    
62
    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);
63
    denom = (p2.y-p1.y)*(p4.x-p3.x)-(p2.x-p1.x)*(p4.y-p3.y);
64
    if (denom == 0) {
65
        return RAY_MISS;
66
    }
67
    nume_a = (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x);
68
    nume_b = (p4.x-p3.x)*(p3.y-p1.y)-(p4.y-p3.y)*(p3.x-p1.x);
69

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

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

    
80
    xint = p3.x+ua*(p4.x-p3.x);
81
    yint = p3.y+ua*(p4.y-p3.y);
82
    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));
83
    printf("Got: %g\n", dist);
84
    return dist;
85

    
86
}
87

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

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

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

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

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

    
128

    
129

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

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

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

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

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

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

    
164

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

    
169
    for(i=0;i < argc; i++) {
170
        p->pts[i].x = va_arg(ap, double);
171
        p->pts[i].y = va_arg(ap, double);
172
    }
173
    obj->id = ID_POLY;
174
    obj->bbox = NULL;
175
    obj->props = p;
176
    return 1;
177
}
178

    
179
int create_rect(object_t *obj, va_list ap)
180
{
181
    int i;
182
    poly_t *p;
183
    int argc = va_arg(ap, int);
184
}
185

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

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

    
224

    
225
/*
226
 * XXX: This shouldn't be here. Where should it be?
227
 */
228
/* These need to be measured... */
229
double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5};
230
void update_rangefinders(Robot *bot)
231
{
232
    //RangeFinder r = bot->shared->ranges;
233
    //ray_t rf = {bot->pose.x, bot->pose.y};
234
    ray_t rf = {0,0,0};
235
    int ir;
236
    double x;
237
    for (ir = 0; ir < 5; ir++)
238
    {
239
        rf.d = rf_thetas[ir];
240
        x = collide_poly(&rf, world.objs);
241
        printf("[%d] = %g\n", ir, x);
242
        //bot->shared->ranges.d[ir] = collide_world(&rf);
243
    }
244
}
245

    
246

    
247
int starts_with (const char* line, const char* word) {
248
  do {
249
    line++;
250
    word++;
251
  }while (*line == *word);
252
  if (*word == '\n' || *word == '\0') return 0;
253
  return 1;
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 (starts_with(line,"POLYGON") == 0){
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 (starts_with(line+i,"CONNECTED") == 0){
273
      i += strlen("CONNECTED") + 1;
274
      type = POLY_CONNECTED;
275
    }else if (starts_with(line+i,"DISCONNECTED") == 0){
276
      i += strlen("DISCONNECTED") + 1;
277
      type = POLY_DISCONNECTED;
278
    }else if (starts_with(line+i,"RECT") == 0){
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
      printf("%lf %lf\n",pts[2*j],pts[2*j+1]);
295
    }
296

    
297
    create(id,num,type,pts);
298
    free(pts);
299
  }
300
  return 0;
301
}
302

    
303
int load_world (const char* filename) {
304
  FILE* fin;
305
  char buf[BUF_SIZE];
306
  bbox_t bbox = {0,0,0,0};
307
  if ((fin = fopen(filename,"r"))==NULL){
308
    // open file failed
309
        perror("Fail to open file");
310
    return -1;
311
  }
312
  if (fgets(buf,512,fin) != NULL){
313
    sscanf(buf,"WORLD %lf %lf %lf %lf",&bbox.p1.x,&bbox.p1.y,&bbox.p2.x,&bbox.p2.y);
314
    init_world(MAX_OBJS, bbox);
315
    while (fgets(buf,512,fin) != NULL){
316
      load_object(buf);
317
    }
318
  }
319
  fclose(fin);
320
  return 0;
321
}
322