## Revision 1042

View differences:

branches/simulator/projects/simulator/simulator/core/world.c
11 11
```#include <math.h>
```
12 12
```#include <stdarg.h>
```
13 13
```#include "world.h"
```
14
```#include "robot.h"
```
15
```#include "robot_shared.h"
```
14 16

15 17
```world_t world;
```
16 18

......
57 59
```    point_t p1 = ray->p;
```
58 60
```    point_t p2 = {ray->p.x+cos(ray->d),ray->p.y+sin(ray->d)};
```
59 61

60
```   // 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);
```
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);
```
61 63
```    denom = (p2.y-p1.y)*(p4.x-p3.x)-(p2.x-p1.x)*(p4.y-p3.y);
```
62 64
```    if (denom == 0) {
```
63 65
```	return RAY_MISS;
```
......
78 80
```    xint = p3.x+ua*(p4.x-p3.x);
```
79 81
```    yint = p3.y+ua*(p4.y-p3.y);
```
80 82
```    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));
```
83
```    printf("Got: %g\n", dist);
```
81 84
```    return dist;
```
82 85

83 86
```}
```
84 87

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

87 91
```    int i;
```
88 92
```    double min = RAY_MISS;
```
89 93
```    double x;
```
......
107 111

108 112
```double collide(ray_t *ray, object_t *obj)
```
109 113
```{
```
110
```    if (ray == NULL || obj == NULL)
```
111
```    {
```
112
```	return -1;
```
113
```    }
```
114 114
```    return collide_func[obj->id+ID_OFFSET](ray, obj);
```
115 115
```}
```
116 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
```}
```
117 127

118 128

129

119 130
```object_t *create(int id, ...)
```
120 131
```{
```
121 132
```    object_t *obj = &(world.objs[world.cur_objs++]);
```
......
155 166
```    p->pts = malloc((argc) * sizeof(point_t));
```
156 167
```    p->type = poly_type;
```
157 168

158
```    double* pts = (double*) malloc((argc) * sizeof(double) * 2);
```
159
```    pts = va_arg(ap,double*);
```
160

161 169
```    for(i=0;i < argc; i++) {
```
162
```	p->pts[i].x = pts[2*i];
```
163
```	p->pts[i].y = pts[2*i+1];
```
170
```	p->pts[i].x = va_arg(ap, double);
```
171
```	p->pts[i].y = va_arg(ap, double);
```
164 172
```    }
```
165 173
```    obj->id = ID_POLY;
```
166 174
```    obj->bbox = NULL;
```
......
213 221
```    }
```
214 222
```}
```
215 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

216 247
```int starts_with (const char* line, const char* word) {
```
217 248
```  do {
```
218 249
```    line++;
```
branches/simulator/projects/simulator/simulator/core/world.h
30 30
```#define CREATE(id, ...) (create_func[id](__VA_ARGS__))
```
31 31
```#define PBBOX(b) {printf("(%g,%g) --> (%g, %g)\n",(b).p1.x,(b).p1.y,(b).p2.x,\
```
32 32
```						  (b).p2.y);}
```
33
```#define PRAY(v) {printf("(%g,%g), %g\n", (v).p.x, (v).p.y,(v).d);}
```
33 34

34 35
```#define MAX_OBJS 100
```
35 36
```#define BUF_SIZE 512
```
......
73 74
```double collide_circle(ray_t *ray, object_t *obj);
```
74 75
```double collide_rect(ray_t *ray, object_t *obj);
```
75 76
```double collide_poly(ray_t *ray, object_t *obj);
```
77
```double collide_world(ray_t *ray);
```
78

76 79
```/*  Array of function pointers to the specific collide functions.
```
77 80
``` *  Must be listed in the same order as IDs */
```
78 81
```extern double (*collide_func[NUM_SHAPES])(ray_t *ray, object_t *obj);
```

Also available in: Unified diff