int main(int argc, char **argv)
{
int npoints = 100;
GRand *rng = g_rand_new ();
pointlist2d_t *candidates = pointlist2d_new (npoints);
for (int i=0; i<npoints; i++) {
candidates->points[i].x = g_rand_double_range (rng, 0, 100);
candidates->points[i].y = g_rand_double_range (rng, 0, 100);
}
for (int i=0; i<5000000; i++) {
pointlist2d_t *a = pointlist2d_new (g_rand_int_range (rng, 3, npoints / 3));
pointlist2d_t *b = pointlist2d_new (g_rand_int_range (rng, 3, npoints / 3));
for (int j=0; j<a->npoints; j++) {
a->points[j] =
candidates->points[g_rand_int_range (rng, 0, npoints)];
}
for (int j=0; j<b->npoints; j++) {
b->points[j] =
candidates->points[g_rand_int_range (rng, 0, npoints)];
}
pointlist2d_t *ah = convexhull_graham_scan_2d (a);
if (ah) {
pointlist2d_t *bh = convexhull_graham_scan_2d (b);
if (bh) {
pointlist2d_t *isect =
geom_convex_polygon_convex_polygon_intersect_2d (ah, bh);
if (isect)
pointlist2d_free (isect);
pointlist2d_free (bh);
}
pointlist2d_free (ah);
}
pointlist2d_free (a);
pointlist2d_free (b);
}
return 0;
}
// computes the relative position of b with respect to a
static void
_compute_lane_relative_placement (RndfOverlayLane *a, RndfOverlayLane *b)
{
assert (a != b);
assert (a->num_waypoints > 1);
// initialize a GPS linearization about a's first waypoint
gps_linearize_t gpslin;
RndfWaypoint *first_wp_a = a->waypoints[0].waypoint;
double first_wp_a_ll[] = { first_wp_a->lat, first_wp_a->lon };
gps_linearize_init (&gpslin, first_wp_a_ll);
// build a pointlist of a's waypoints
pointlist2d_t *pts_a = pointlist2d_new (a->num_waypoints);
for (int i=0; i<pts_a->npoints; i++) {
RndfOverlayWaypoint *owp_a = &a->waypoints[i];
double latlon_a[2] = { owp_a->waypoint->lat, owp_a->waypoint->lon };
gps_linearize_to_xy (&gpslin, latlon_a, (double*) &pts_a->points[i]);
}
// estimate which side of a that each point of b is on
int nleft = 0;
int nright = 0;
int nsamedir = 0;
int noppdir = 0;
for (int i=0; i<b->num_waypoints; i++) {
// linearize a waypoint of b
RndfOverlayWaypoint *owp_b = &b->waypoints[i];
double latlon_b[2] = { owp_b->waypoint->lat, owp_b->waypoint->lon };
point2d_t xy_b;
gps_linearize_to_xy (&gpslin, latlon_b, (double*) &xy_b);
// find the closest segment
int seg_ind = 0;
point2d_t seg_closest_pt = { 0, 0 };
geom_point_polyline_closest_point_2d (&xy_b, pts_a, &seg_ind, NULL,
&seg_closest_pt);
vec2d_t adir;
if (seg_ind >= pts_a->npoints - 1) {
adir.x = pts_a->points[seg_ind].x - pts_a->points[seg_ind - 1].x;
adir.y = pts_a->points[seg_ind].y - pts_a->points[seg_ind - 1].y;
} else {
adir.x = pts_a->points[seg_ind + 1].x - pts_a->points[seg_ind].x;
adir.y = pts_a->points[seg_ind + 1].y - pts_a->points[seg_ind].y;
}
// if it's too far, then ignore this waypoint
if (geom_point_point_distance_2d (&xy_b, &seg_closest_pt) >
50) continue;
// which side of the segment is the waypoint of b on?
int side = geom_handedness_2d (&pts_a->points[seg_ind],
&pts_a->points[seg_ind+1], &xy_b);
if (side < 0) nleft++;
if (side > 0) nright++;
// estimate the direction of travel at waypoint of b
if (b->num_waypoints < 2) continue;
point2d_t xy_nb = xy_b;
point2d_t xy_pb = xy_b;
vec2d_t bdir = { 0, 0 };
if (i > 0) {
RndfOverlayWaypoint *powp_b = &b->waypoints[i-1];
double latlon_pb[2] = {
powp_b->waypoint->lat,
powp_b->waypoint->lon
};
gps_linearize_to_xy (&gpslin, latlon_pb, (double*) &xy_pb);
vec2d_t pdir = { xy_b.x - xy_pb.x, xy_b.y - xy_pb.y };
geom_vec_normalize_2d (&pdir);
bdir.x = pdir.x / 2;
bdir.y = pdir.y / 2;
}
if (i < b->num_waypoints - 1) {
RndfOverlayWaypoint *nowp_b = &b->waypoints[i+1];
double latlon_nb[2] = {
nowp_b->waypoint->lat,
nowp_b->waypoint->lon
};
gps_linearize_to_xy (&gpslin, latlon_nb, (double*) &xy_nb);
vec2d_t ndir = { xy_nb.x - xy_b.x, xy_nb.y - xy_b.y };
geom_vec_normalize_2d (&ndir);
bdir.x += ndir.x / 2;
bdir.y += ndir.y / 2;
}
double dmag = geom_vec_magnitude_2d (&bdir);
assert (dmag > 0);
if (geom_vec_vec_dot_2d (&bdir, &adir) > 0) {
nsamedir ++;
} else {
noppdir ++;
}
}
pointlist2d_free (pts_a);
const char *rpos_str[] = {
"left", "right", "unknown"
};
RndfOverlayRelativePosition rpos =
(nleft > nright) ? RNDF_OVERLAY_RPOS_LEFT : RNDF_OVERLAY_RPOS_RIGHT;
if (!nleft && !nright) {
fprintf (stderr, "%s:%d Unable to determine position of lane %d.%d\n"
" with respect to %d.%d (%s)\n",
__FILE__, __LINE__,
b->parent->segment->id, b->lane->id,
a->parent->segment->id, a->lane->id,
a->parent->segment->name);
rpos = RNDF_OVERLAY_RPOS_UNKNOWN;
} else if ((nleft && nright) || (nleft == nright)) {
fprintf (stderr, "%s:%d Position of lane %d.%d with respect to %d.%d\n"
" is ambiguous (%s). Guessing %s. (%d / %d)\n",
__FILE__, __LINE__,
b->parent->segment->id, b->lane->id,
a->parent->segment->id, a->lane->id,
a->parent->segment->name,
rpos_str[rpos],
nleft, nright);
}
RndfOverlayRelativeDirection rdir;
if (!nsamedir && !noppdir) {
fprintf (stderr, "%s:%d Unable to determine direction of lane %d.%d\n"
" with respect to %d.%d (%s)\n",
__FILE__, __LINE__,
b->parent->segment->id, b->lane->id,
a->parent->segment->id, a->lane->id,
a->parent->segment->name);
rdir = RNDF_OVERLAY_RDIR_UNKNOWN;
} else if (nsamedir > noppdir) {
rdir = RNDF_OVERLAY_RDIR_SAME;
} else {
rdir = RNDF_OVERLAY_RDIR_OPPOSITE;
}
const char *rdir_str[] = { "same direction as", "opposite direction to",
"unknown direction" };
if ((nsamedir && noppdir) || (nsamedir == noppdir)) {
fprintf (stderr, "%s:%d Direction of lane %d.%d with respect to %d.%d\n"
" is ambiguous (%s). Guessing %s. (%d / %d)\n",
__FILE__, __LINE__,
b->parent->segment->id, b->lane->id,
a->parent->segment->id, a->lane->id,
a->parent->segment->name,
rdir_str[rdir],
nsamedir, noppdir);
} else {
dbg ("%d.%d %s of, %s %d.%d\n",
b->parent->segment->id, b->lane->id,
rpos_str[rpos], rdir_str[rdir],
a->parent->segment->id, a->lane->id);
}
RndfOverlayLaneRelativePlacement *rp =
_lane_relative_placement_new (rpos, rdir);
assert (!g_hash_table_lookup (a->relative_positions, b));
g_hash_table_insert (a->relative_positions, b, rp);
}
void setup_renderer_car(Viewer *viewer, int render_priority)
{
RendererCar *self = (RendererCar*) calloc (1, sizeof (RendererCar));
Renderer *renderer = &self->renderer;
renderer->draw = car_draw;
renderer->destroy = car_free;
renderer->widget = gtk_vbox_new(FALSE, 0);
renderer->name = RENDERER_NAME;
renderer->user = self;
renderer->enabled = 1;
EventHandler *ehandler = &self->ehandler;
ehandler->name = RENDERER_NAME;
ehandler->enabled = 1;
ehandler->pick_query = pick_query;
ehandler->key_press = key_press;
ehandler->hover_query = pick_query;
ehandler->mouse_press = mouse_press;
ehandler->mouse_release = mouse_release;
ehandler->mouse_motion = mouse_motion;
ehandler->user = self;
self->viewer = viewer;
self->lcm = globals_get_lcm ();
self->config = globals_get_config ();
self->pw = BOT_GTK_PARAM_WIDGET(bot_gtk_param_widget_new());
self->atrans = globals_get_atrans ();
self->path = bot_ptr_circular_new (MAX_POSES, free_path_element, NULL);
gtk_box_pack_start(GTK_BOX(renderer->widget), GTK_WIDGET(self->pw), TRUE,
TRUE, 0);
bot_gtk_param_widget_add_booleans (self->pw, 0, PARAM_FOLLOW_POS, 1, NULL);
bot_gtk_param_widget_add_booleans (self->pw, 0, PARAM_FOLLOW_YAW, 0, NULL);
char * model;
char path[256];
if (bot_conf_get_str (self->config, "renderer_car.chassis_model",
&model) == 0) {
snprintf (path, sizeof (path), "%s/%s", MODELS_DIR, model);
self->chassis_model = rwx_model_create (path);
}
if (bot_conf_get_str (self->config, "renderer_car.wheel_model",
&model) == 0) {
snprintf (path, sizeof (path), "%s/%s", MODELS_DIR, model);
self->wheel_model = rwx_model_create (path);
}
if (self->chassis_model)
bot_gtk_param_widget_add_booleans (self->pw, 0, PARAM_NAME_BLING, 1,
NULL);
if (self->wheel_model)
bot_gtk_param_widget_add_booleans (self->pw, 0, PARAM_NAME_WHEELS, 1,
NULL);
self->max_draw_poses = 1000;
bot_gtk_param_widget_add_int (self->pw, PARAM_MAXPOSES,
BOT_GTK_PARAM_WIDGET_SLIDER, 0, MAX_POSES, 100, self->max_draw_poses);
GtkWidget *find_button = gtk_button_new_with_label("Find");
gtk_box_pack_start(GTK_BOX(renderer->widget), find_button, FALSE, FALSE, 0);
g_signal_connect(G_OBJECT(find_button), "clicked",
G_CALLBACK (on_find_button), self);
GtkWidget *clear_button = gtk_button_new_with_label("Clear path");
gtk_box_pack_start(GTK_BOX(renderer->widget), clear_button, FALSE, FALSE,
0);
g_signal_connect(G_OBJECT(clear_button), "clicked",
G_CALLBACK (on_clear_button), self);
gtk_widget_show_all(renderer->widget);
g_signal_connect (G_OBJECT (self->pw), "changed",
G_CALLBACK (on_param_widget_changed), self);
on_param_widget_changed(self->pw, "", self);
botlcm_pose_t_subscribe(self->lcm, "POSE", on_pose, self);
viewer_add_renderer(viewer, &self->renderer, render_priority);
viewer_add_event_handler(viewer, &self->ehandler, render_priority);
self->footprint = pointlist2d_new (4);
fbconf_get_vehicle_footprint (self->config,
(double*)self->footprint->points);
g_signal_connect (G_OBJECT (viewer), "load-preferences",
G_CALLBACK (on_load_preferences), self);
g_signal_connect (G_OBJECT (viewer), "save-preferences",
G_CALLBACK (on_save_preferences), self);
}
