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