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()); }