int LightSensor :: update(const Posture& pos,
const boost::shared_ptr<Map>& map)
{
const std::vector<Map::ill_sw_t>& isv = map->get_illuminated_switches();
_activated = false;
for (size_t i = 0; i < isv.size(); ++i)
if (isv[i]->get_color() == get_color() && isv[i]->get_on())
{
float angle =
normalize_angle(atan2(isv[i]->get_y() - pos.y(),
isv[i]->get_x() - pos.x())
- pos.theta());
float x_res = 0, y_res = 0;// ignored
if (angle > normalize_angle(_angle - _range / 2.0f)
&& angle < normalize_angle(_angle + _range / 2.0f)
&& !map->check_inter_real(isv[i]->get_x(), isv[i]->get_y(),
pos.x(), pos.y(),
x_res, y_res))
{
_activated = true;
_num=i;
}
}
return _activated;
}
void LinearCamera :: update(const Posture& pos,
const boost::shared_ptr<Map>& map)
{
float inc = _angular_range / _pixels.size();
float r = -_angular_range / 2.0f;
for (size_t i = 0; i < _pixels.size(); ++i, r += inc)
{
float alpha = r + pos.theta();
float xr = cos(alpha) * 10000 + pos.x();//range = 10000
float yr = sin(alpha) * 10000 + pos.x();
assert(i < _pixels.size());
_pixels[i] = map->check_inter_is(pos.x(), pos.y(), xr, yr);
}
}
int Radar :: update(const Posture& pos,
const boost::shared_ptr<Map>& map) {
const Goal& g = map->get_goals()[_color];
float angle = normalize_angle_2pi(atan2(g.get_y() - pos.y(), g.get_x() - pos.x())
- pos.theta());
float x, y;
bool not_visible = _through_walls ? false :
map->check_inter_real(pos.x(), pos.y(), g.get_x(), g.get_y(), x, y);
if (not_visible && !_through_walls) {
_activated_slice = -1;
return -1;
}
float xi = _inc;
for (size_t i = 0; i < _nb_slices + 1; ++i) {
if (angle < xi)
return (_activated_slice = i % _nb_slices);
xi += _inc;
}
// assert(0);
return -1;
}
const Posture& move(float d_l, float d_r,
float wheels_dist)
{
Posture old_pos = *this;
float alpha = (d_r - d_l) / wheels_dist;
Posture p;
if (fabs(alpha) > 1e-10)
{
float r = (d_l / alpha) + wheels_dist / 2;
float d_x = (cos(alpha) - 1) * r;
float d_y = sin(alpha) * r;
p = Posture(d_x, d_y, alpha);
p.rotate(old_pos.theta() - M_PI / 2);
p.set_theta(normalize_angle(alpha));
}
else
p = Posture(d_l * cos(old_pos.theta()),
d_l * sin(old_pos.theta()),
0);
*this = p + old_pos;
return *this;
}
float Laser :: update(const Posture& pos,
const boost::shared_ptr<Map>& m) {
float x2 = cosf(_angle + pos.theta()) * _range + pos.x() + _gap_dist * cosf(_gap_angle + pos.theta());
float y2 = sinf(_angle + pos.theta()) * _range + pos.y() + _gap_dist * sinf(_gap_angle + pos.theta());
// convert to pixel
int xx1 = m->real_to_pixel(pos.x() + _gap_dist * cosf(_gap_angle + pos.theta()));
int yy1 = m->real_to_pixel(pos.y() + _gap_dist * sinf(_gap_angle + pos.theta()));
int xx2 = m->real_to_pixel(x2);
int yy2 = m->real_to_pixel(y2);
// check intersection
_x_pixel = m->real_to_pixel(x2), _y_pixel = m->real_to_pixel(y2);
bool inter = m->check_inter_pixel(xx1, yy1, xx2, yy2, _x_pixel, _y_pixel);
_x_pixel = std::min(std::max(0, _x_pixel), (int)m->get_pixel_w());
_y_pixel = std::min(std::max(0, _y_pixel), (int)m->get_pixel_h())
;
assert(_x_pixel >= 0);
assert(_y_pixel >= 0);
// convert back to real
_x = m->pixel_to_real(_x_pixel);
_y = m->pixel_to_real(_y_pixel);
//
if (!inter)
_dist = -1;
else {
float px = pos.x() + _gap_dist * cosf(_gap_angle + pos.theta()) - _x;
float py = pos.y() + _gap_dist * sinf(_gap_angle + pos.theta()) - _y;
_dist = sqrtf(px * px + py * py);
}
return _dist;
}
float dist_to(const Posture& p) const
{
return dist_to(p.x(), p.y());
}
