Revision 1044
added update_rangefinders, which should be enough for the demo tomorrow
world.c | ||
---|---|---|
18 | 18 |
|
19 | 19 |
int init_world(int max_objs, bbox_t b) |
20 | 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}; |
|
21 | 22 |
world.max_objs = max_objs; |
22 | 23 |
world.cur_objs = 0; |
23 | 24 |
world.objs = calloc(max_objs, sizeof(object_t)); |
24 | 25 |
world.win = b; |
25 |
//PBBOX(b); |
|
26 |
/* Add a polygon surrounding the world */ |
|
27 |
create(ID_POLY,4,POLY_CONNECTED,pts); |
|
26 | 28 |
} |
27 | 29 |
int destroy_world() |
28 | 30 |
{ |
... | ... | |
235 | 237 |
double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5}; |
236 | 238 |
void update_rangefinders(Robot *bot) |
237 | 239 |
{ |
238 |
//RangeFinder r = bot->shared->ranges; |
|
239 |
//ray_t rf = {bot->pose.x, bot->pose.y}; |
|
240 |
ray_t rf = {0,0,0}; |
|
240 |
RangeFinder r = bot->shared->ranges; |
|
241 |
ray_t rf = {bot->pose.x, bot->pose.y, 0}; |
|
242 |
/* Motion code has +y going down, I don't. |
|
243 |
* So to compensate, just reflect theta up to first quadrant.*/ |
|
244 |
double theta = bot->pose.theta; |
|
245 |
theta = 2*M_PI - theta; |
|
246 |
//ray_t rf = {0,0,0}; |
|
241 | 247 |
int ir; |
242 | 248 |
double x; |
243 | 249 |
for (ir = 0; ir < 5; ir++) |
244 | 250 |
{ |
245 | 251 |
rf.d = rf_thetas[ir]; |
246 |
x = collide_poly(&rf, world.objs);
|
|
252 |
x = collide_world(&rf);
|
|
247 | 253 |
printf("[%d] = %g\n", ir, x); |
248 |
//bot->shared->ranges.d[ir] = collide_world(&rf);
|
|
254 |
bot->shared->ranges.d[ir] = x;
|
|
249 | 255 |
} |
250 | 256 |
} |
251 | 257 |
|
Also available in: Unified diff