int main(void) { printf("Input: $"); scanf("%lf", &input); printf("Output:\n"); get_change(input); return (0); }
std::vector<gns::point> local_map::get_points() const { std::vector<gns::point> out; for (unsigned int i = 0; i < m_grid.size(); i++) for (unsigned int j = 0; j < m_grid.size(); j++) if (m_grid(i, j) == cell::NONFREE) out.push_back(get_change().toPoint(pointi(i, j))); return out; }
void local_map::set_robot(const gns::frame& r) { static bool first_time = true; m_moved = false; m_wXr = r; if (first_time) { m_center = r.zero(); first_time = false; } else if (gns::distance(r.zero(), m_center) > 1.0) { pointi delta = get_change().toCell(r.zero()); move(delta); m_center = r.zero(); m_moved = true; } }
void local_map::line_projection(double x, double y, double theta, double length, double sy, double stheta, double sytheta, grid<400>& out) { out.clear(); gns::frame line_frame(x, y, theta); gns::point line_begin = line_frame * gns::point(-length / 2, 0); gns::point line_end = line_frame * gns::point(length / 2, 0); change change = get_change(); pointi begin = change.toCell(line_begin); pointi end = change.toCell(line_end); my::line cells(begin, end); for (my::line::iterator i = cells.begin(); i != cells.end(); ++i) { gns::point p = (~line_frame) * change.toPoint(*i); double error = sy + 2 * sytheta * p.x + stheta * p.x * p.x; gns::frame pframe(p.x, p.y, theta + M_PI / 2); gns::point error_begin = line_frame * pframe * gns::point(-error, 0); gns::point error_end = line_frame * pframe * gns::point(error, 0); pointi ebegin = change.toCell(error_begin); pointi eend = change.toCell(error_end); out.drawline(ebegin, eend, cell::NONFREE); } }
void local_map::laser_projection(const gns::frame& f, const laser& laser, grid< 400>& out) { out.clear(); gns::frame wXl = f * laser.rXl; change change = get_change(); pointi lasercoord = change.toCell(wXl.zero()); for (unsigned int i = 0; i < laser.count(); i++) { gns::point p = wXl * gns::point::polar(laser.range(i), laser.angle(i)); if (gns::distance(p, m_wXr.zero()) <= m_robot_size) continue; pointi gcp = change.toCell(p); my::line theline(lasercoord, gcp); for (my::line::iterator j = theline.begin(); j != theline.end(); ++j) { if (out.valid(j->x, j->y) and out(j->x, j->y) == cell::UNKNOWN) out(j->x, j->y) = cell::FREE; } if (laser.range(i) < laser.max_range and out.valid(gcp.x, gcp.y)) out(gcp.x, gcp.y) = cell::NONFREE; } for (int i = int(laser.count()) - 1; i >= 0; i--) { gns::point p = wXl * gns::point::polar(laser.range(i), laser.angle(i)); if (gns::distance(p, m_wXr.zero()) <= m_robot_size) continue; pointi gcp = change.toCell(p); my::line theline(lasercoord, gcp); for (my::line::iterator j = theline.begin(); j != theline.end(); ++j) { if (out.valid(j->x, j->y) and out(j->x, j->y) == cell::UNKNOWN) out(j->x, j->y) = cell::FREE; } if (laser.range(i) < laser.max_range and out.valid(gcp.x, gcp.y)) out(gcp.x, gcp.y) = cell::NONFREE; } }
static gboolean parse_arg (GOptionContext *context, GOptionGroup *group, GOptionEntry *entry, const gchar *value, const gchar *option_name, GError **error) { Change *change; switch (entry->arg) { case G_OPTION_ARG_NONE: { change = get_change (context, G_OPTION_ARG_NONE, entry->arg_data); *(gboolean *)entry->arg_data = !(entry->flags & G_OPTION_FLAG_REVERSE); break; } case G_OPTION_ARG_STRING: { gchar *data; data = g_locale_to_utf8 (value, -1, NULL, NULL, error); if (!data) return FALSE; change = get_change (context, G_OPTION_ARG_STRING, entry->arg_data); g_free (change->allocated.str); change->prev.str = *(gchar **)entry->arg_data; change->allocated.str = data; *(gchar **)entry->arg_data = data; break; } case G_OPTION_ARG_STRING_ARRAY: { gchar *data; data = g_locale_to_utf8 (value, -1, NULL, NULL, error); if (!data) return FALSE; change = get_change (context, G_OPTION_ARG_STRING_ARRAY, entry->arg_data); if (change->allocated.array.len == 0) { change->prev.array = *(gchar ***)entry->arg_data; change->allocated.array.data = g_new (gchar *, 2); } else change->allocated.array.data = g_renew (gchar *, change->allocated.array.data, change->allocated.array.len + 2); change->allocated.array.data[change->allocated.array.len] = data; change->allocated.array.data[change->allocated.array.len + 1] = NULL; change->allocated.array.len ++; *(gchar ***)entry->arg_data = change->allocated.array.data; break; }
void local_map::clean_robot(const gns::frame& rf) { pointi c = get_change().toCell(rf.zero()); int r = int(round(m_robot_size / m_scale)); fill(m_updates, c, r); }
bool local_map::is_free(const gns::point& start, const gns::point& end) const { return m_grid.is_free(get_change().toCell(start), get_change().toCell(end)); }