Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (6.17 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
    return -1;
58
}
59

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

    
66
    //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);
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
    //printf("Got: %g\n", dist);
88
    return dist;
89

    
90
}
91

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

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

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

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

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

    
133

    
134

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

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

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

    
155
    return 0;
156
}
157

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

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

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

    
171

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

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

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

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

    
194
        return 0;
195
}
196

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

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

    
235

    
236
int load_object (const char* line) {
237
  char buf[BUF_SIZE];
238
  int i = 0;
239
  int j;
240
  int id;
241
  if (strstr(line,"POLYGON") == line){
242
    int num;
243
    int type;
244
    double *pts;
245
    id = ID_POLY;
246
    i = strlen("POLYGON") + 1;
247

    
248
    sscanf(line+i,"%s",buf);
249
    num = strtol(buf,NULL,0);
250
    i += strlen(buf) + 1;
251

    
252
    if (strstr(line+i,"CONNECTED") == line+i){
253
      i += strlen("CONNECTED") + 1;
254
      type = POLY_CONNECTED;
255
    }else if (strstr(line+i,"DISCONNECTED") == line+i){
256
      i += strlen("DISCONNECTED") + 1;
257
      type = POLY_DISCONNECTED;
258
    }else if (strstr(line+i,"RECT") == line+i){
259
      i += strlen("RECT") + 1;
260
      type = POLY_RECT;
261
    }else{
262
      // invalid input
263
                        fprintf(stderr,"Invalid input for POLYGON\n");
264
      return -1;
265
    }
266

    
267
    char *c1,*c2;
268
    c1 = (char*)(line+i);
269
    c2 = (char*)(line+i);
270
    pts = (double*) malloc(sizeof(double)*num*2);
271
    for (j = 0;j < num;j++){
272
      pts[2*j] = strtod(c1,&c2);
273
      pts[2*j+1] = strtod(c2,&c1);
274
#if DEBUG
275
      printf("%lf %lf\n",pts[2*j],pts[2*j+1]);
276
#endif
277
    }
278

    
279
    create(id,num,type,pts);
280
    free(pts);
281
  }else if (strstr(line,"CIRCLE") == line){
282
                id = ID_CIRCLE;
283
                i = strlen("CIRCLE") + 1;
284
                //TODO
285
        }else if (strstr(line,"RECTANGLE") == line){
286
                id = ID_RECTANGLE;
287
                i = strlen("RECTANGLE") + 1;
288
                //TODO
289
        }else{
290
                //Invalid input
291
#if DEBUG
292
                fprintf(stderr,"Invalid object: %s",line);
293
#endif
294
        }
295
  return 0;
296
}
297

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