Colony

    xint = p3.x+ua*(p4.x-p3.x);

    yint = p3.y+ua*(p4.y-p3.y);

    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));

    return dist;

}

double collide_world(ray_t *ray)

{

    double min = RAY_MISS;

    for (i = 0; i < world.cur_objs; i++) {	

	x = collide(ray, &world.objs[i]);

	min = (x < min) ? x : min;

 */

/* These need to be measured... */

double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5};

void update_rangefinders(Robot *bot)

{

    RangeFinder r = bot->shared->ranges;

    /* Motion code has +y going down, I don't.

     * So to compensate, just reflect theta up to first quadrant.*/

    double theta = bot->pose.theta;

    int ir;

    double x;

    for (ir = 0; ir < 5; ir++)

    {

	x = collide_world(&rf);

	bot->shared->ranges.d[ir] = x;

    }

}

  fclose(fin);

  return 0;

}