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

83  83 
xint = p3.x+ua*(p4.xp3.x); 
84  84 
yint = p3.y+ua*(p4.yp3.y); 
85  85 
dist = sqrt((xintp1.x)*(xintp1.x)+(yintp1.y)*(yintp1.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