예제 #1
0
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;
}
예제 #2
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);
}
예제 #3
0
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);
}