void Ndescs2Disk::points_callback(const normal_descriptor_node::ndesc_pc::ConstPtr& ndesc_pc_msg) { ROS_INFO("Received ndesc pointset"); //TODO: think of a better name generation tech. std::string file_name=this->prefix_out_+to_string(this->counter_)+".ndesc"; this->counter_++; int saved_points=0; FILE* pf=fopen(file_name.c_str(),"wb"); for(int ii=0;ii<ndesc_pc_msg->num;ii++) { if( (!isnan(ndesc_pc_msg->descriptors[ii].point3d.x) && !isnan(ndesc_pc_msg->descriptors[ii].point3d.y) && !isnan(ndesc_pc_msg->descriptors[ii].point3d.z)) || this->keep_nan_points_) { write_point(pf, ndesc_pc_msg->descriptors[ii]); saved_points++; } // fprintf(pf,"%d %d %f %f %f ",ndesc_pc_msg->descriptors[ii].u, // ndesc_pc_msg->descriptors[ii].v, // ndesc_pc_msg->descriptors[ii].point3d.x, // ndesc_pc_msg->descriptors[ii].point3d.y, // ndesc_pc_msg->descriptors[ii].point3d.z); // for(unsigned int j=0;j<ndesc_pc_msg->descriptors[ii].descriptor.size();j++) // fprintf(pf,"%f ",ndesc_pc_msg->descriptors[ii].descriptor[j]); // fprintf(pf,"\n"); } ROS_INFO("%d descriptors (out of %d) of size %d written to disk.\n",saved_points, ndesc_pc_msg->num, ndesc_pc_msg->descriptors[0].descriptor.size()); fclose(pf); //if(this->max_frames_<=this->counter_ && this->max_frames_!=0) // exit(0); }
void write_spv (const SearchPointVector& spv) { for (auto v = spv.begin(); v != spv.end(); ++v) { write_point(*v, v->get_flatLocation(), "spv"); } printf("spv\n"); fflush(stdout); }
void write_border (const AbstractAirspace& as) { const SearchPointVector& spv = as.GetPoints(); for (auto v = spv.begin(); v != spv.end(); ++v) write_point(*v, v->get_flatLocation(), "polygon"); printf("polygon\n"); write_spv(as.GetClearance()); fflush(stdout); }
int DxfMap::save(char *filename, int savefeatureNum, double scaleFactor) { FILE *fp; int i; double x, y; for (i = 0; i < savefeatureNum; i++) { x = feature[i].y / scaleFactor; y = feature[i].x / scaleFactor; feature[i].x = x; feature[i].y = y; x = feature[i].y2 / scaleFactor; y = feature[i].x2 / scaleFactor; feature[i].x2 = x; feature[i].y2 = y; } if ((fp = fopen(filename, "w")) == NULL) { printf("Can't open dxf file \"%s\"\n", filename); return -EIO; } write_head(fp); for (i = 0; i < savefeatureNum; i ++) { if (feature[i].l == 0) { write_point(fp, &feature[i]); } else { write_line(fp, &feature[i]); } } write_eof(fp); fclose(fp); return 0; }
bool LASwriter::write_point(double x, double y, double z) { LASpoint point; if (created_header) { if (p_count == 0) // then no header was provided { header->x_offset = 1000*((int)(x/1000)); header->y_offset = 1000*((int)(y/1000)); header->z_offset = 1000*((int)(z/1000)); header->min_x = header->max_x = x; header->min_y = header->max_y = y; header->min_z = header->max_z = z; } else { if (x < header->min_x) header->min_x = x; else if (x > header->max_x) header->max_x = x; if (y < header->min_y) header->min_y = y; else if (y > header->max_y) header->max_y = y; if (z < header->min_z) header->min_z = z; else if (z > header->max_z) header->max_z = z; } } point.return_number = 1; point.number_of_returns_of_given_pulse = 1; if (x > header->x_offset) point.x = (int)((x-header->x_offset)/header->x_scale_factor + 0.5); else point.x = (int)((x-header->x_offset)/header->x_scale_factor - 0.5); if (y > header->y_offset) point.y = (int)((y-header->y_offset)/header->y_scale_factor + 0.5); else point.y = (int)((y-header->y_offset)/header->y_scale_factor - 0.5); if (z > header->z_offset) point.z = (int)((z-header->z_offset)/header->z_scale_factor + 0.5); else point.z = (int)((z-header->z_offset)/header->z_scale_factor - 0.5); return write_point(&point); }
void decode_strike(char *line, nldn_file ltgf) { static int isinit=!0; int iret; char result[256], *cpos; int maxchar=sizeof(result)-1; int year, month, day, hour, minute, second; int mult; float slat,slon,sgnl,emaj,emin; nldn_flash flashdat; time_t obsclock; struct tm obstime; int loglev, numerr,axisor; static char errgrp[] = {"decode_strike"}; char errstr[LLMXLN]; int filver; /* Split line into result (date string) and cpos (leftovers) */ cpos = cst_split(line,',',maxchar,result,&iret); /* See if this look like a valid line (check if first char is 2[008] */ if ( ( iret != 0 ) || ( cpos == NULL ) || ( result[0] != '2' ) ) { printf("Not a strike line %s\n",line); return; } /* Check if file is old (filver = 4) or new (filver = 6) */ filver = sscanf(cpos,"%f,%f,%f,%f,%f,%d", &slat, &slon, &sgnl, &emaj, &emin, &axisor); switch ( filver ) { case 4: if ( (iret = sscanf(cpos,"%f,%f,%f,%d", &slat, &slon, &sgnl, &mult)) != 4 ) { sprintf(errstr,"Old: Not a strike line %s\0",line); loglev = 4; numerr = 1; dc_wclg(loglev, errgrp, numerr, errstr, &iret); return; } break; case 6: if ( (iret = sscanf(cpos,"%f,%f,%f,%f,%f,%d", &slat, &slon, &sgnl, &emaj, &emin, &axisor)) != 6 ) { sprintf(errstr,"New: Not a strike line %s\0",line); loglev = 4; numerr = 1; dc_wclg(loglev, errgrp, numerr, errstr, &iret); return; } break; } /* Check date string is formatted correctly */ if ( (iret = sscanf(result,"%d-%d-%dT%d:%d:%d", &year, &month, &day, &hour, &minute, &second )) != 6 ) { printf("Unexpected date string %s\n",result); return; } /* Initialize time zone information */ if(isinit) { isinit = 0; putenv("TZ=UTC0"); tzset(); } /* Convert time to seconds*/ obstime.tm_sec = second; obstime.tm_min = minute; obstime.tm_hour = hour; obstime.tm_mday = day; obstime.tm_mon = month - 1; obstime.tm_year = year - 1900; /* year since 1900 */ obstime.tm_wday = 0; obstime.tm_yday = 0; obstime.tm_isdst = 0; obsclock = mktime ( &obstime ); /* The following are present regardless of file version */ flashdat.sec = (int)obsclock; flashdat.nsec = 0; flashdat.lat = slat; flashdat.lon = slon; flashdat.sgnl = sgnl; switch ( filver ) { case 4: flashdat.mult = mult; flashdat.semimaj = RMISSD; flashdat.eccent = RMISSD; flashdat.angle = RMISSD; break; case 6: flashdat.mult = 1; flashdat.semimaj = emaj; flashdat.eccent = emin; flashdat.angle = axisor; break; } flashdat.chisqr = RMISSD; (void)write_point(ltgf, flashdat, &iret); if ( iret != 0 ) printf("%d look line %s %f %f %f %d [%d %d %d]\n",iret, result,slat,slon,sgnl,mult, obstime.tm_year, obstime.tm_mon, obstime.tm_mday); }
void report_ring_not_closed(osmium::Location end1, osmium::Location end2) override { write_point("ring_not_closed", m_object_id, 0, end1); write_point("ring_not_closed", m_object_id, 0, end2); }
void report_intersection(osmium::object_id_type way1_id, osmium::Location way1_seg_start, osmium::Location way1_seg_end, osmium::object_id_type way2_id, osmium::Location way2_seg_start, osmium::Location way2_seg_end, osmium::Location intersection) override { write_point("intersection", m_object_id, 0, intersection); write_line("intersection", m_object_id, way1_id, way1_seg_start, way1_seg_end); write_line("intersection", m_object_id, way2_id, way2_seg_start, way2_seg_end); }
void report_duplicate_node(osmium::object_id_type node_id1, osmium::object_id_type node_id2, osmium::Location location) override { write_point("duplicate_node", node_id1, node_id2, location); }
int main(int argc, char **argv) { int field, type, vertex_type; double dmax; char buf[DB_SQL_MAX]; struct { struct Option *input, *output, *type, *dmax, *lfield, *use; } opt; struct { struct Flag *table, *inter; } flag; struct GModule *module; struct Map_info In, Out; struct line_cats *LCats; struct line_pnts *LPoints; dbDriver *driver; struct field_info *Fi; dbString stmt; G_gisinit(argv[0]); module = G_define_module(); G_add_keyword(_("vector")); G_add_keyword(_("geometry")); G_add_keyword("3D"); G_add_keyword(_("node")); G_add_keyword(_("vertex")); module->description = _("Creates points along input lines in new vector map with 2 layers."); opt.input = G_define_standard_option(G_OPT_V_INPUT); opt.lfield = G_define_standard_option(G_OPT_V_FIELD); opt.lfield->key = "llayer"; opt.lfield->answer = "1"; opt.lfield->label = "Line layer number or name"; opt.lfield->guisection = _("Selection"); opt.type = G_define_standard_option(G_OPT_V3_TYPE); opt.type->answer = "point,line,boundary,centroid,face"; opt.type->guisection = _("Selection"); opt.output = G_define_standard_option(G_OPT_V_OUTPUT); opt.use = G_define_option(); opt.use->key = "use"; opt.use->type = TYPE_STRING; opt.use->required = NO; opt.use->description = _("Use line nodes or vertices only"); opt.use->options = "node,vertex"; opt.dmax = G_define_option(); opt.dmax->key = "dmax"; opt.dmax->type = TYPE_DOUBLE; opt.dmax->required = NO; opt.dmax->answer = "100"; opt.dmax->description = _("Maximum distance between points in map units"); flag.inter = G_define_flag(); flag.inter->key = 'i'; flag.inter->description = _("Interpolate points between line vertices (only for use=vertex)"); flag.table = G_define_standard_flag(G_FLG_V_TABLE); if (G_parser(argc, argv)) exit(EXIT_FAILURE); LCats = Vect_new_cats_struct(); LPoints = Vect_new_line_struct(); db_init_string(&stmt); type = Vect_option_to_types(opt.type); dmax = atof(opt.dmax->answer); vertex_type = 0; if (opt.use->answer) { if (opt.use->answer[0] == 'n') vertex_type = GV_NODE; else vertex_type = GV_VERTEX; } Vect_check_input_output_name(opt.input->answer, opt.output->answer, G_FATAL_EXIT); /* Open input lines */ Vect_set_open_level(2); if (Vect_open_old2(&In, opt.input->answer, "", opt.lfield->answer) < 0) G_fatal_error(_("Unable to open vector map <%s>"), opt.input->answer); Vect_set_error_handler_io(&In, &Out); field = Vect_get_field_number(&In, opt.lfield->answer); /* Open output segments */ if (Vect_open_new(&Out, opt.output->answer, Vect_is_3d(&In)) < 0) G_fatal_error(_("Unable to create vector map <%s>"), opt.output->answer); Vect_copy_head_data(&In, &Out); Vect_hist_copy(&In, &Out); Vect_hist_command(&Out); /* Table */ Fi = NULL; if (!flag.table->answer) { struct field_info *Fin; /* copy input table */ Fin = Vect_get_field(&In, field); if (Fin) { /* table defined */ int ret; Fi = Vect_default_field_info(&Out, 1, NULL, GV_MTABLE); Vect_map_add_dblink(&Out, 1, NULL, Fi->table, Fin->key, Fi->database, Fi->driver); ret = db_copy_table(Fin->driver, Fin->database, Fin->table, Fi->driver, Vect_subst_var(Fi->database, &Out), Fi->table); if (ret == DB_FAILED) { G_fatal_error(_("Unable to copy table <%s>"), Fin->table); } } Fi = Vect_default_field_info(&Out, 2, NULL, GV_MTABLE); Vect_map_add_dblink(&Out, 2, NULL, Fi->table, GV_KEY_COLUMN, Fi->database, Fi->driver); /* Open driver */ driver = db_start_driver_open_database(Fi->driver, Fi->database); if (driver == NULL) G_fatal_error(_("Unable to open database <%s> by driver <%s>"), Fi->database, Fi->driver); db_set_error_handler_driver(driver); if (field == -1) sprintf(buf, "create table %s ( cat int, along double precision )", Fi->table); else sprintf(buf, "create table %s ( cat int, lcat int, along double precision )", Fi->table); db_append_string(&stmt, buf); if (db_execute_immediate(driver, &stmt) != DB_OK) { G_fatal_error(_("Unable to create table: '%s'"), db_get_string(&stmt)); } if (db_create_index2(driver, Fi->table, GV_KEY_COLUMN) != DB_OK) G_warning(_("Unable to create index for table <%s>, key <%s>"), Fi->table, GV_KEY_COLUMN); if (db_grant_on_table (driver, Fi->table, DB_PRIV_SELECT, DB_GROUP | DB_PUBLIC) != DB_OK) G_fatal_error(_("Unable to grant privileges on table <%s>"), Fi->table); db_begin_transaction(driver); } if (type & (GV_POINTS | GV_LINES | GV_FACE)) { int line, nlines, nskipped; nskipped = 0; nlines = Vect_get_num_lines(&In); for (line = 1; line <= nlines; line++) { int ltype, cat; G_debug(3, "line = %d", line); G_percent(line, nlines, 2); ltype = Vect_read_line(&In, LPoints, LCats, line); if (!(ltype & type)) continue; if (!Vect_cat_get(LCats, field, &cat) && field != -1) { nskipped++; continue; } /* Assign CAT for layer 0 objects (i.e. boundaries) */ if (field == -1) cat = -1; if (LPoints->n_points <= 1) { write_point(&Out, LPoints->x[0], LPoints->y[0], LPoints->z[0], cat, 0.0, driver, Fi); } else { /* lines */ write_line(&Out, LPoints, cat, vertex_type, flag.inter->answer, dmax, driver, Fi); } } if (nskipped > 0) G_warning(_("%d features without category in layer <%d> skipped. " "Note that features without category (usually boundaries) are not " "skipped when '%s=-1' is given."), nskipped, field, opt.lfield->key); } if (type == GV_AREA) { int area, nareas, centroid, cat; nareas = Vect_get_num_areas(&In); for (area = 1; area <= nareas; area++) { int i, isle, nisles; G_percent(area, nareas, 2); centroid = Vect_get_area_centroid(&In, area); cat = -1; if (centroid > 0) { Vect_read_line(&In, NULL, LCats, centroid); if (!Vect_cat_get(LCats, field, &cat)) continue; } Vect_get_area_points(&In, area, LPoints); write_line(&Out, LPoints, cat, vertex_type, flag.inter->answer, dmax, driver, Fi); nisles = Vect_get_area_num_isles(&In, area); for (i = 0; i < nisles; i++) { isle = Vect_get_area_isle(&In, area, i); Vect_get_isle_points(&In, isle, LPoints); write_line(&Out, LPoints, cat, vertex_type, flag.inter->answer, dmax, driver, Fi); } } } if (!flag.table->answer) { db_commit_transaction(driver); db_close_database_shutdown_driver(driver); } Vect_build(&Out); /* Free, close ... */ Vect_close(&In); G_done_msg(_("%d points written to output vector map."), Vect_get_num_primitives(&Out, GV_POINT)); Vect_close(&Out); exit(EXIT_SUCCESS); }
void write_line(struct Map_info *Out, struct line_pnts *LPoints, int cat, int vertex, int interpolate, double dmax, int table) { if (vertex == GV_VERTEX || vertex == GV_NODE) { /* use line vertices */ double along; int vert; along = 0; for (vert = 0; vert < LPoints->n_points; vert++) { G_debug(3, "vert = %d", vert); if (vertex == GV_VERTEX || (vertex == GV_NODE && (vert == 0 || vert == LPoints->n_points - 1))) { write_point(Out, LPoints->x[vert], LPoints->y[vert], LPoints->z[vert], cat, along, table); } if (vert < LPoints->n_points - 1) { double dx, dy, dz, len; dx = LPoints->x[vert + 1] - LPoints->x[vert]; dy = LPoints->y[vert + 1] - LPoints->y[vert]; dz = LPoints->z[vert + 1] - LPoints->z[vert]; len = hypot(hypot(dx, dy), dz); /* interpolate segment */ if (interpolate && vert < (LPoints->n_points - 1)) { int i, n; double x, y, z, dlen; if (len > dmax) { n = len / dmax + 1; /* number of segments */ dx /= n; dy /= n; dz /= n; dlen = len / n; for (i = 1; i < n; i++) { x = LPoints->x[vert] + i * dx; y = LPoints->y[vert] + i * dy; z = LPoints->z[vert] + i * dz; write_point(Out, x, y, z, cat, along + i * dlen, table); } } } along += len; } } } else { /* do not use vertices */ int i, n; double len, dlen, along, x, y, z; len = Vect_line_length(LPoints); n = len / dmax + 1; /* number of segments */ dlen = len / n; /* length of segment */ G_debug(3, "n = %d len = %f dlen = %f", n, len, dlen); for (i = 0; i <= n; i++) { if (i > 0 && i < n) { along = i * dlen; Vect_point_on_line(LPoints, along, &x, &y, &z, NULL, NULL); } else { /* first and last vertex */ if (i == 0) { along = 0; x = LPoints->x[0]; y = LPoints->y[0]; z = LPoints->z[0]; } else { /* last */ along = len; x = LPoints->x[LPoints->n_points - 1]; y = LPoints->y[LPoints->n_points - 1]; z = LPoints->z[LPoints->n_points - 1]; } } G_debug(3, " i = %d along = %f", i, along); write_point(Out, x, y, z, cat, along, table); } } }
int main(int argc, char **argv) { int field, type, vertex_type; double dmax; struct Option *in_opt, *out_opt, *type_opt, *dmax_opt, *lfield_opt; struct Flag *inter_flag, *vertex_flag, *table_flag, *node_flag; struct GModule *module; char *mapset; struct Map_info In, Out; struct line_cats *LCats; struct line_pnts *LPoints; char buf[2000]; G_gisinit(argv[0]); module = G_define_module(); module->keywords = _("vector, geometry"); module->description = _("Create points along input lines in new vector with 2 layers."); in_opt = G_define_standard_option(G_OPT_V_INPUT); in_opt->description = _("Input vector map containing lines"); out_opt = G_define_standard_option(G_OPT_V_OUTPUT); out_opt->description = _("Output vector map where points will be written"); type_opt = G_define_standard_option(G_OPT_V_TYPE); type_opt->answer = "point,line,boundary,centroid"; lfield_opt = G_define_standard_option(G_OPT_V_FIELD); lfield_opt->key = "llayer"; lfield_opt->answer = "1"; lfield_opt->description = "Line layer"; node_flag = G_define_flag(); node_flag->key = 'n'; node_flag->description = _("Write line nodes"); vertex_flag = G_define_flag(); vertex_flag->key = 'v'; vertex_flag->description = _("Write line vertices"); inter_flag = G_define_flag(); inter_flag->key = 'i'; inter_flag->description = _("Interpolate points between line vertices"); dmax_opt = G_define_option(); dmax_opt->key = "dmax"; dmax_opt->type = TYPE_DOUBLE; dmax_opt->required = NO; dmax_opt->answer = "100"; dmax_opt->description = _("Maximum distance between points in map units"); table_flag = G_define_flag(); table_flag->key = 't'; table_flag->description = _("Do not create attribute table"); if (G_parser(argc, argv)) exit(EXIT_FAILURE); LCats = Vect_new_cats_struct(); PCats = Vect_new_cats_struct(); LPoints = Vect_new_line_struct(); PPoints = Vect_new_line_struct(); db_init_string(&stmt); field = atoi(lfield_opt->answer); type = Vect_option_to_types(type_opt); dmax = atof(dmax_opt->answer); if (node_flag->answer && vertex_flag->answer) G_fatal_error(_("Use either -n or -v flag, not both")); if (node_flag->answer) vertex_type = GV_NODE; else if (vertex_flag->answer) vertex_type = GV_VERTEX; else vertex_type = 0; Vect_check_input_output_name(in_opt->answer, out_opt->answer, GV_FATAL_EXIT); /* Open input lines */ mapset = G_find_vector2(in_opt->answer, NULL); if (mapset == NULL) G_fatal_error(_("Vector map <%s> not found"), in_opt->answer); Vect_set_open_level(2); Vect_open_old(&In, in_opt->answer, mapset); /* Open output segments */ Vect_open_new(&Out, out_opt->answer, Vect_is_3d(&In)); Vect_copy_head_data(&In, &Out); Vect_hist_copy(&In, &Out); Vect_hist_command(&Out); /* Table */ if (!table_flag->answer) { struct field_info *Fin; /* copy input table */ Fin = Vect_get_field(&In, field); if (Fin) { /* table defined */ int ret; Fi = Vect_default_field_info(&Out, 1, NULL, GV_MTABLE); Vect_map_add_dblink(&Out, 1, NULL, Fi->table, Fin->key, Fi->database, Fi->driver); ret = db_copy_table(Fin->driver, Fin->database, Fin->table, Fi->driver, Vect_subst_var(Fi->database, &Out), Fi->table); if (ret == DB_FAILED) { G_fatal_error(_("Unable to copy table <%s>"), Fin->table); } } Fi = Vect_default_field_info(&Out, 2, NULL, GV_MTABLE); Vect_map_add_dblink(&Out, 2, NULL, Fi->table, "cat", Fi->database, Fi->driver); /* Open driver */ driver = db_start_driver_open_database(Fi->driver, Fi->database); if (driver == NULL) G_fatal_error(_("Unable to open database <%s> by driver <%s>"), Fi->database, Fi->driver); sprintf(buf, "create table %s ( cat int, lcat int, along double precision )", Fi->table); db_append_string(&stmt, buf); if (db_execute_immediate(driver, &stmt) != DB_OK) { db_close_database_shutdown_driver(driver); G_fatal_error(_("Unable to create table: '%s'"), db_get_string(&stmt)); } if (db_create_index2(driver, Fi->table, "cat") != DB_OK) G_warning(_("Unable to create index for table <%s>, key <%s>"), Fi->table, "cat"); if (db_grant_on_table (driver, Fi->table, DB_PRIV_SELECT, DB_GROUP | DB_PUBLIC) != DB_OK) G_fatal_error(_("Unable to grant privileges on table <%s>"), Fi->table); db_begin_transaction(driver); } point_cat = 1; if (type & (GV_POINTS | GV_LINES)) { int line, nlines; nlines = Vect_get_num_lines(&In); for (line = 1; line <= nlines; line++) { int ltype, cat; G_debug(3, "line = %d", line); ltype = Vect_read_line(&In, LPoints, LCats, line); if (!(ltype & type)) continue; Vect_cat_get(LCats, field, &cat); if (LPoints->n_points <= 1) { write_point(&Out, LPoints->x[0], LPoints->y[0], LPoints->z[0], cat, 0.0, table_flag->answer); } else { /* lines */ write_line(&Out, LPoints, cat, vertex_type, inter_flag->answer, dmax, table_flag->answer); } G_percent(line, nlines, 2); } } if (type == GV_AREA) { int area, nareas, centroid, cat; nareas = Vect_get_num_areas(&In); for (area = 1; area <= nareas; area++) { int i, isle, nisles; centroid = Vect_get_area_centroid(&In, area); cat = -1; if (centroid > 0) { Vect_read_line(&In, NULL, LCats, centroid); Vect_cat_get(LCats, field, &cat); } Vect_get_area_points(&In, area, LPoints); write_line(&Out, LPoints, cat, vertex_type, inter_flag->answer, dmax, table_flag->answer); nisles = Vect_get_area_num_isles(&In, area); for (i = 0; i < nisles; i++) { isle = Vect_get_area_isle(&In, area, i); Vect_get_isle_points(&In, isle, LPoints); write_line(&Out, LPoints, cat, vertex_type, inter_flag->answer, dmax, table_flag->answer); } G_percent(area, nareas, 2); } } if (!table_flag->answer) { db_commit_transaction(driver); db_close_database_shutdown_driver(driver); } Vect_build(&Out); /* Free, close ... */ Vect_close(&In); Vect_close(&Out); G_done_msg(_("%d points written to output vector map"), point_cat - 1); exit(EXIT_SUCCESS); }
bool LASwriter::write_point(double* coordinates) { return write_point(coordinates[0], coordinates[1], coordinates[2]); }