Revision 1048 branches/simulator/projects/simulator/simulator/core/world.c

View differences:

world.c
83 83
    xint = p3.x+ua*(p4.x-p3.x);
84 84
    yint = p3.y+ua*(p4.y-p3.y);
85 85
    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));
86
    //printf("Got: %g\n", dist);
86 87
    return dist;
87 88

  
88 89
}
......
119 120
double collide_world(ray_t *ray)
120 121
{
121 122
    double min = RAY_MISS;
122
    int i, x;
123
    double x;
124
    int i;
123 125
    for (i = 0; i < world.cur_objs; i++) {	
124 126
	x = collide(ray, &world.objs[i]);
125 127
	min = (x < min) ? x : min;
......
231 233
 */
232 234
/* These need to be measured... */
233 235
double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5};
236
//double rf_thetas[5] = {0.0, 0.0, M_PI/2, M_PI, 3*M_PI/2};
234 237
void update_rangefinders(Robot *bot)
235 238
{
236 239
    RangeFinder r = bot->shared->ranges;
......
238 241
    /* Motion code has +y going down, I don't.
239 242
     * So to compensate, just reflect theta up to first quadrant.*/
240 243
    double theta = bot->pose.theta;
241
    theta = 2*M_PI - theta; 
242
    //ray_t rf = {0,0,0};
243 244
    int ir;
244 245
    double x;
245 246
    for (ir = 0; ir < 5; ir++)
246 247
    {
247
	rf.d = rf_thetas[ir];
248
	rf.d = theta + rf_thetas[ir];
248 249
	x = collide_world(&rf);
249
	//printf("[%d] = %g\n", ir, x);
250
	printf("@(%g) - [%d] = %g --> %d\n",rf.d, ir, x, (short)x);
250 251
	bot->shared->ranges.d[ir] = x;
251 252
    }
252 253
}
......
332 333
  fclose(fin);
333 334
  return 0;
334 335
}
335

  

Also available in: Unified diff