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