Revision 1044

View differences:

branches/simulator/projects/simulator/simulator/core/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

  
branches/simulator/projects/simulator/simulator/core/main.c
28 28

  
29 29

  
30 30
	robot_create(argv[1]);
31
	/*robot_create(argv[1]);
31 32
	robot_create(argv[1]);
32 33
	robot_create(argv[1]);
33 34
	robot_create(argv[1]);
......
46 47
	robot_create(argv[1]);
47 48
	robot_create(argv[1]);
48 49
	robot_create(argv[1]);
49
	robot_create(argv[1]);
50
	*/
50 51

  
51 52

  
52 53
	sigset_t set;
branches/simulator/projects/simulator/simulator/core/robot.c
25 25

  
26 26
#include "robot.h"
27 27
#include "motion.h"
28
#include "world.h"
28 29
#include "gtk_gui.h"
29 30

  
30 31
void sig_chld_handler(int sig);
......
277 278
	if (r->id == -1)
278 279
		return;
279 280
	move_robot(r);
281
	update_rangefinders(r);
280 282
}
281 283

  

Also available in: Unified diff