virtual void Set(double value) const {
double red, green, blue;
if(absval(value - vmax) < 0.3)
red = 0.6;
else
red = boundval(0.0, 0.5 * (value / vmax), 0.5);
if(absval(value) < 0.3)
green = 0.6;
else
green = boundval(0.0, 0.5 * ((vmax - value) / vmax), 0.5);
if(absval(value - vmax / 2) < 0.3)
blue = 0.6;
else
blue = 0;
glColor3d(red, green, blue);
}
/** \todo Could cache a lot of the intermediate values, but this is
only called during init. */
int Mapper2d::
ComputeCost(double dist_from_obstacle) const
{
if (dist_from_obstacle >= grown_safe_distance)
return m_travmap->freespace;
int cost(m_travmap->obstacle);
if (dist_from_obstacle > grown_robot_radius) {
double const normdist((dist_from_obstacle - grown_robot_radius) / buffer_zone);
double const vv(m_travmap->freespace + 1
+ (*m_cost_decay)(normdist) * (m_travmap->obstacle - m_travmap->freespace - 2));
cost = boundval(m_travmap->freespace + 1, static_cast<int>(rint(vv)), m_travmap->obstacle - 1);
}
return cost;
}
