GRASS Programmer's Manual
6.4.2(2012)
|
00001 00020 #include <stdlib.h> 00021 #include <string.h> 00022 #include <fcntl.h> 00023 #include <grass/gis.h> 00024 #include <grass/dbmi.h> 00025 #include <grass/Vect.h> 00026 #include <grass/glocale.h> 00027 00028 static int From_node; /* from node set in SP and used by clipper for first arc */ 00029 00030 static int clipper(dglGraph_s * pgraph, 00031 dglSPClipInput_s * pargIn, 00032 dglSPClipOutput_s * pargOut, void *pvarg) 00033 { /* caller's pointer */ 00034 dglInt32_t cost; 00035 dglInt32_t from; 00036 00037 G_debug(3, "Net: clipper()"); 00038 00039 from = dglNodeGet_Id(pgraph, pargIn->pnNodeFrom); 00040 00041 G_debug(3, " Edge = %d NodeFrom = %d NodeTo = %d edge cost = %d", 00042 (int)dglEdgeGet_Id(pgraph, pargIn->pnEdge), 00043 (int)from, (int)dglNodeGet_Id(pgraph, pargIn->pnNodeTo), 00044 (int)pargOut->nEdgeCost); 00045 00046 if (from != From_node) { /* do not clip first */ 00047 if (dglGet_NodeAttrSize(pgraph) > 0) { 00048 memcpy(&cost, dglNodeGet_Attr(pgraph, pargIn->pnNodeFrom), 00049 sizeof(cost)); 00050 if (cost == -1) { /* closed, cannot go from this node except it is 'from' node */ 00051 G_debug(3, " closed node"); 00052 return 1; 00053 } 00054 else { 00055 G_debug(3, " EdgeCost += %d (node)", (int)cost); 00056 pargOut->nEdgeCost += cost; 00057 } 00058 } 00059 } 00060 else { 00061 G_debug(3, " don't clip first node"); 00062 } 00063 00064 return 0; 00065 } 00066 00090 int 00091 Vect_net_build_graph(struct Map_info *Map, 00092 int ltype, 00093 int afield, 00094 int nfield, 00095 const char *afcol, 00096 const char *abcol, 00097 const char *ncol, int geo, int algorithm) 00098 { 00099 int i, j, from, to, line, nlines, nnodes, ret, type, cat, skipped, cfound; 00100 int dofw, dobw; 00101 struct line_pnts *Points; 00102 struct line_cats *Cats; 00103 double dcost, bdcost, ll; 00104 int cost, bcost; 00105 dglGraph_s *gr; 00106 dglInt32_t opaqueset[16] = 00107 { 360000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; 00108 struct field_info *Fi; 00109 dbDriver *driver = NULL; 00110 dbHandle handle; 00111 dbString stmt; 00112 dbColumn *Column; 00113 dbCatValArray fvarr, bvarr; 00114 int fctype = 0, bctype = 0, nrec; 00115 00116 /* TODO int costs -> double (waiting for dglib) */ 00117 G_debug(1, "Vect_build_graph(): ltype = %d, afield = %d, nfield = %d", 00118 ltype, afield, nfield); 00119 G_debug(1, " afcol = %s, abcol = %s, ncol = %s", afcol, abcol, ncol); 00120 00121 G_message(_("Building graph...")); 00122 00123 Map->graph_line_type = ltype; 00124 00125 Points = Vect_new_line_struct(); 00126 Cats = Vect_new_cats_struct(); 00127 00128 ll = 0; 00129 if (G_projection() == 3) 00130 ll = 1; /* LL */ 00131 00132 if (afcol == NULL && ll && !geo) 00133 Map->cost_multip = 1000000; 00134 else 00135 Map->cost_multip = 1000; 00136 00137 nlines = Vect_get_num_lines(Map); 00138 nnodes = Vect_get_num_nodes(Map); 00139 00140 gr = &(Map->graph); 00141 00142 /* Allocate space for costs, later replace by functions reading costs from graph */ 00143 Map->edge_fcosts = (double *)G_malloc((nlines + 1) * sizeof(double)); 00144 Map->edge_bcosts = (double *)G_malloc((nlines + 1) * sizeof(double)); 00145 Map->node_costs = (double *)G_malloc((nnodes + 1) * sizeof(double)); 00146 /* Set to -1 initially */ 00147 for (i = 1; i <= nlines; i++) { 00148 Map->edge_fcosts[i] = -1; /* forward */ 00149 Map->edge_bcosts[i] = -1; /* backward */ 00150 } 00151 for (i = 1; i <= nnodes; i++) { 00152 Map->node_costs[i] = 0; 00153 } 00154 00155 if (ncol != NULL) 00156 dglInitialize(gr, (dglByte_t) 1, sizeof(dglInt32_t), (dglInt32_t) 0, 00157 opaqueset); 00158 else 00159 dglInitialize(gr, (dglByte_t) 1, (dglInt32_t) 0, (dglInt32_t) 0, 00160 opaqueset); 00161 00162 if (gr == NULL) 00163 G_fatal_error(_("Unable to build network graph")); 00164 00165 db_init_handle(&handle); 00166 db_init_string(&stmt); 00167 00168 if (abcol != NULL && afcol == NULL) 00169 G_fatal_error(_("Forward costs column not specified")); 00170 00171 /* --- Add arcs --- */ 00172 /* Open db connection */ 00173 if (afcol != NULL) { 00174 /* Get field info */ 00175 if (afield < 1) 00176 G_fatal_error(_("Arc field < 1")); 00177 Fi = Vect_get_field(Map, afield); 00178 if (Fi == NULL) 00179 G_fatal_error(_("Database connection not defined for layer %d"), 00180 afield); 00181 00182 /* Open database */ 00183 driver = db_start_driver_open_database(Fi->driver, Fi->database); 00184 if (driver == NULL) 00185 G_fatal_error(_("Unable to open database <%s> by driver <%s>"), 00186 Fi->database, Fi->driver); 00187 00188 /* Load costs to array */ 00189 if (db_get_column(driver, Fi->table, afcol, &Column) != DB_OK) 00190 G_fatal_error(_("Column <%s> not found in table <%s>"), 00191 afcol, Fi->table); 00192 00193 fctype = db_sqltype_to_Ctype(db_get_column_sqltype(Column)); 00194 00195 if (fctype != DB_C_TYPE_INT && fctype != DB_C_TYPE_DOUBLE) 00196 G_fatal_error(_("Data type of column <%s> not supported (must be numeric)"), 00197 afcol); 00198 00199 db_CatValArray_init(&fvarr); 00200 nrec = 00201 db_select_CatValArray(driver, Fi->table, Fi->key, afcol, NULL, 00202 &fvarr); 00203 G_debug(1, "forward costs: nrec = %d", nrec); 00204 00205 if (abcol != NULL) { 00206 if (db_get_column(driver, Fi->table, abcol, &Column) != DB_OK) 00207 G_fatal_error(_("Column <%s> not found in table <%s>"), 00208 abcol, Fi->table); 00209 00210 bctype = db_sqltype_to_Ctype(db_get_column_sqltype(Column)); 00211 00212 if (bctype != DB_C_TYPE_INT && bctype != DB_C_TYPE_DOUBLE) 00213 G_fatal_error(_("Data type of column <%s> not supported (must be numeric)"), 00214 abcol); 00215 00216 db_CatValArray_init(&bvarr); 00217 nrec = 00218 db_select_CatValArray(driver, Fi->table, Fi->key, abcol, NULL, 00219 &bvarr); 00220 G_debug(1, "backward costs: nrec = %d", nrec); 00221 } 00222 } 00223 00224 skipped = 0; 00225 00226 G_message(_("Registering arcs...")); 00227 00228 for (i = 1; i <= nlines; i++) { 00229 G_percent(i, nlines, 1); /* must be before any continue */ 00230 dofw = dobw = 1; 00231 Vect_get_line_nodes(Map, i, &from, &to); 00232 type = Vect_read_line(Map, Points, Cats, i); 00233 if (!(type & ltype & (GV_LINE | GV_BOUNDARY))) 00234 continue; 00235 00236 if (afcol != NULL) { 00237 if (!(Vect_cat_get(Cats, afield, &cat))) { 00238 G_debug(2, 00239 "Category of field %d not attached to the line %d -> line skipped", 00240 afield, i); 00241 skipped += 2; /* Both directions */ 00242 continue; 00243 } 00244 else { 00245 if (fctype == DB_C_TYPE_INT) { 00246 ret = db_CatValArray_get_value_int(&fvarr, cat, &cost); 00247 dcost = cost; 00248 } 00249 else { /* DB_C_TYPE_DOUBLE */ 00250 ret = 00251 db_CatValArray_get_value_double(&fvarr, cat, &dcost); 00252 } 00253 if (ret != DB_OK) { 00254 G_warning(_("Database record for line %d (cat = %d, " 00255 "forward/both direction(s)) not found " 00256 "(forward/both direction(s) of line skipped)"), 00257 i, cat); 00258 dofw = 0; 00259 } 00260 00261 if (abcol != NULL) { 00262 if (bctype == DB_C_TYPE_INT) { 00263 ret = 00264 db_CatValArray_get_value_int(&bvarr, cat, &bcost); 00265 bdcost = bcost; 00266 } 00267 else { /* DB_C_TYPE_DOUBLE */ 00268 ret = 00269 db_CatValArray_get_value_double(&bvarr, cat, 00270 &bdcost); 00271 } 00272 if (ret != DB_OK) { 00273 G_warning(_("Database record for line %d (cat = %d, " 00274 "backword direction) not found" 00275 "(direction of line skipped)"), i, cat); 00276 dobw = 0; 00277 } 00278 } 00279 else { 00280 if (dofw) 00281 bdcost = dcost; 00282 else 00283 dobw = 0; 00284 } 00285 } 00286 } 00287 else { 00288 if (ll) { 00289 if (geo) 00290 dcost = Vect_line_geodesic_length(Points); 00291 else 00292 dcost = Vect_line_length(Points); 00293 } 00294 else 00295 dcost = Vect_line_length(Points); 00296 00297 bdcost = dcost; 00298 } 00299 if (dofw && dcost != -1) { 00300 cost = (dglInt32_t) Map->cost_multip * dcost; 00301 G_debug(5, "Add arc %d from %d to %d cost = %d", i, from, to, 00302 cost); 00303 ret = 00304 dglAddEdge(gr, (dglInt32_t) from, (dglInt32_t) to, 00305 (dglInt32_t) cost, (dglInt32_t) i); 00306 Map->edge_fcosts[i] = dcost; 00307 if (ret < 0) 00308 G_fatal_error("Cannot add network arc"); 00309 } 00310 00311 G_debug(5, "bdcost = %f edge_bcosts = %f", bdcost, 00312 Map->edge_bcosts[i]); 00313 if (dobw && bdcost != -1) { 00314 bcost = (dglInt32_t) Map->cost_multip * bdcost; 00315 G_debug(5, "Add arc %d from %d to %d bcost = %d", -i, to, from, 00316 bcost); 00317 ret = 00318 dglAddEdge(gr, (dglInt32_t) to, (dglInt32_t) from, 00319 (dglInt32_t) bcost, (dglInt32_t) - i); 00320 Map->edge_bcosts[i] = bdcost; 00321 if (ret < 0) 00322 G_fatal_error(_("Cannot add network arc")); 00323 } 00324 } 00325 00326 if (afcol != NULL && skipped > 0) 00327 G_debug(2, "%d lines missing category of field %d skipped", skipped, 00328 afield); 00329 00330 if (afcol != NULL) { 00331 db_close_database_shutdown_driver(driver); 00332 db_CatValArray_free(&fvarr); 00333 00334 if (abcol != NULL) { 00335 db_CatValArray_free(&bvarr); 00336 } 00337 } 00338 00339 /* Set node attributes */ 00340 G_debug(2, "Register nodes"); 00341 if (ncol != NULL) { 00342 G_debug(2, "Set nodes' costs"); 00343 if (nfield < 1) 00344 G_fatal_error("Node field < 1"); 00345 00346 G_message(_("Setting node costs...")); 00347 00348 Fi = Vect_get_field(Map, nfield); 00349 if (Fi == NULL) 00350 G_fatal_error(_("Database connection not defined for layer %d"), 00351 nfield); 00352 00353 driver = db_start_driver_open_database(Fi->driver, Fi->database); 00354 if (driver == NULL) 00355 G_fatal_error(_("Unable to open database <%s> by driver <%s>"), 00356 Fi->database, Fi->driver); 00357 00358 /* Load costs to array */ 00359 if (db_get_column(driver, Fi->table, ncol, &Column) != DB_OK) 00360 G_fatal_error(_("Column <%s> not found in table <%s>"), 00361 ncol, Fi->table); 00362 00363 fctype = db_sqltype_to_Ctype(db_get_column_sqltype(Column)); 00364 00365 if (fctype != DB_C_TYPE_INT && fctype != DB_C_TYPE_DOUBLE) 00366 G_fatal_error(_("Data type of column <%s> not supported (must be numeric)"), 00367 ncol); 00368 00369 db_CatValArray_init(&fvarr); 00370 nrec = 00371 db_select_CatValArray(driver, Fi->table, Fi->key, ncol, NULL, 00372 &fvarr); 00373 G_debug(1, "node costs: nrec = %d", nrec); 00374 00375 for (i = 1; i <= nnodes; i++) { 00376 /* TODO: what happens if we set attributes of not existing node (skipped lines, 00377 * nodes without lines) */ 00378 00379 nlines = Vect_get_node_n_lines(Map, i); 00380 G_debug(2, " node = %d nlines = %d", i, nlines); 00381 cfound = 0; 00382 dcost = 0; 00383 for (j = 0; j < nlines; j++) { 00384 line = Vect_get_node_line(Map, i, j); 00385 G_debug(2, " line (%d) = %d", j, line); 00386 type = Vect_read_line(Map, NULL, Cats, line); 00387 if (!(type & GV_POINT)) 00388 continue; 00389 if (Vect_cat_get(Cats, nfield, &cat)) { /* point with category of field found */ 00390 /* Set costs */ 00391 if (fctype == DB_C_TYPE_INT) { 00392 ret = 00393 db_CatValArray_get_value_int(&fvarr, cat, &cost); 00394 dcost = cost; 00395 } 00396 else { /* DB_C_TYPE_DOUBLE */ 00397 ret = 00398 db_CatValArray_get_value_double(&fvarr, cat, 00399 &dcost); 00400 } 00401 if (ret != DB_OK) { 00402 G_warning(_("Database record for node %d (cat = %d) not found " 00403 "(cost set to 0)"), i, cat); 00404 } 00405 cfound = 1; 00406 break; 00407 } 00408 } 00409 if (!cfound) { 00410 G_debug(2, 00411 "Category of field %d not attached to any points in node %d" 00412 "(costs set to 0)", nfield, i); 00413 } 00414 if (dcost == -1) { /* closed */ 00415 cost = -1; 00416 } 00417 else { 00418 cost = (dglInt32_t) Map->cost_multip * dcost; 00419 } 00420 G_debug(3, "Set node's cost to %d", cost); 00421 dglNodeSet_Attr(gr, dglGetNode(gr, (dglInt32_t) i), 00422 (dglInt32_t *) (dglInt32_t) & cost); 00423 Map->node_costs[i] = dcost; 00424 } 00425 db_close_database_shutdown_driver(driver); 00426 db_CatValArray_free(&fvarr); 00427 } 00428 00429 G_message(_("Flattening the graph...")); 00430 ret = dglFlatten(gr); 00431 if (ret < 0) 00432 G_fatal_error(_("GngFlatten error")); 00433 00434 /* init SP cache */ 00435 /* disable to debug dglib cache */ 00436 dglInitializeSPCache(gr, &(Map->spCache)); 00437 00438 G_message(_("Graph was built")); 00439 00440 return 0; 00441 } 00442 00443 00462 int 00463 Vect_net_shortest_path(struct Map_info *Map, int from, int to, 00464 struct ilist *List, double *cost) 00465 { 00466 int i, line, *pclip, cArc, nRet; 00467 dglSPReport_s *pSPReport; 00468 dglInt32_t nDistance; 00469 int use_cache = 1; /* set to 0 to disable dglib cache */ 00470 00471 G_debug(3, "Vect_net_shortest_path(): from = %d, to = %d", from, to); 00472 00473 /* Note : if from == to dgl goes to nearest node and returns back (dgl feature) => 00474 * check here for from == to */ 00475 00476 if (List != NULL) 00477 Vect_reset_list(List); 00478 00479 /* Check if from and to are identical, otherwise dglib returns path to neares node and back! */ 00480 if (from == to) { 00481 if (cost != NULL) 00482 *cost = 0; 00483 return 0; 00484 } 00485 00486 From_node = from; 00487 00488 pclip = NULL; 00489 if (List != NULL) { 00490 if (use_cache) { 00491 nRet = 00492 dglShortestPath(&(Map->graph), &pSPReport, (dglInt32_t) from, 00493 (dglInt32_t) to, clipper, pclip, &(Map->spCache)); 00494 } 00495 else { 00496 nRet = 00497 dglShortestPath(&(Map->graph), &pSPReport, (dglInt32_t) from, 00498 (dglInt32_t) to, clipper, pclip, NULL); 00499 } 00500 } 00501 else { 00502 if (use_cache) { 00503 nRet = 00504 dglShortestDistance(&(Map->graph), &nDistance, (dglInt32_t) from, 00505 (dglInt32_t) to, clipper, pclip, &(Map->spCache)); 00506 } 00507 else { 00508 nRet = 00509 dglShortestDistance(&(Map->graph), &nDistance, (dglInt32_t) from, 00510 (dglInt32_t) to, clipper, pclip, NULL); 00511 } 00512 } 00513 00514 if (nRet == 0) { 00515 /* G_warning( "Destination node %d is unreachable from node %d\n" , to , from ); */ 00516 if (cost != NULL) 00517 *cost = PORT_DOUBLE_MAX; 00518 return -1; 00519 } 00520 else if (nRet < 0) { 00521 G_warning(_("dglShortestPath error: %s"), dglStrerror(&(Map->graph))); 00522 return -1; 00523 } 00524 00525 if (List != NULL) { 00526 for (i = 0; i < pSPReport->cArc; i++) { 00527 line = dglEdgeGet_Id(&(Map->graph), pSPReport->pArc[i].pnEdge); 00528 G_debug(2, "From %ld to %ld - cost %ld user %d distance %ld", pSPReport->pArc[i].nFrom, pSPReport->pArc[i].nTo, dglEdgeGet_Cost(&(Map->graph), pSPReport->pArc[i].pnEdge) / Map->cost_multip, /* this is the cost from clip() */ 00529 line, pSPReport->pArc[i].nDistance); 00530 Vect_list_append(List, line); 00531 } 00532 } 00533 00534 if (cost != NULL) { 00535 if (List != NULL) 00536 *cost = (double)pSPReport->nDistance / Map->cost_multip; 00537 else 00538 *cost = (double)nDistance / Map->cost_multip; 00539 } 00540 00541 if (List != NULL) { 00542 cArc = pSPReport->cArc; 00543 dglFreeSPReport(&(Map->graph), pSPReport); 00544 } 00545 else 00546 cArc = 0; 00547 00548 return (cArc); 00549 } 00550 00563 int 00564 Vect_net_get_line_cost(struct Map_info *Map, int line, int direction, 00565 double *cost) 00566 { 00567 /* dglInt32_t *pEdge; */ 00568 00569 G_debug(5, "Vect_net_get_line_cost(): line = %d, dir = %d", line, 00570 direction); 00571 00572 if (direction == GV_FORWARD) { 00573 /* V1 has no index by line-id -> array used */ 00574 /* 00575 pEdge = dglGetEdge ( &(Map->graph), line ); 00576 if ( pEdge == NULL ) return 0; 00577 *cost = (double) dglEdgeGet_Cost ( &(Map->graph), pEdge ); 00578 */ 00579 if (Map->edge_fcosts[line] == -1) { 00580 *cost = -1; 00581 return 0; 00582 } 00583 else 00584 *cost = Map->edge_fcosts[line]; 00585 } 00586 else if (direction == GV_BACKWARD) { 00587 /* 00588 pEdge = dglGetEdge ( &(Map->graph), -line ); 00589 if ( pEdge == NULL ) return 0; 00590 *cost = (double) dglEdgeGet_Cost ( &(Map->graph), pEdge ); 00591 */ 00592 if (Map->edge_bcosts[line] == -1) { 00593 *cost = -1; 00594 return 0; 00595 } 00596 else 00597 *cost = Map->edge_bcosts[line]; 00598 G_debug(5, "Vect_net_get_line_cost(): edge_bcosts = %f", 00599 Map->edge_bcosts[line]); 00600 } 00601 else { 00602 G_fatal_error(_("Wrong line direction in Vect_net_get_line_cost()")); 00603 } 00604 00605 return 1; 00606 } 00607 00617 int Vect_net_get_node_cost(struct Map_info *Map, int node, double *cost) 00618 { 00619 G_debug(3, "Vect_net_get_node_cost(): node = %d", node); 00620 00621 *cost = Map->node_costs[node]; 00622 00623 G_debug(3, " -> cost = %f", *cost); 00624 00625 return 1; 00626 } 00627 00646 int Vect_net_nearest_nodes(struct Map_info *Map, 00647 double x, double y, double z, 00648 int direction, double maxdist, 00649 int *node1, int *node2, int *ln, double *costs1, 00650 double *costs2, struct line_pnts *Points1, 00651 struct line_pnts *Points2, double *distance) 00652 { 00653 int line, n1, n2, nnodes; 00654 int npoints; 00655 int segment; /* nearest line segment (first is 1) */ 00656 static struct line_pnts *Points = NULL; 00657 double cx, cy, cz, c1, c2; 00658 double along; /* distance along the line to nearest point */ 00659 double length; 00660 00661 G_debug(3, "Vect_net_nearest_nodes() x = %f y = %f", x, y); 00662 00663 /* Reset */ 00664 if (node1) 00665 *node1 = 0; 00666 if (node2) 00667 *node2 = 0; 00668 if (ln) 00669 *ln = 0; 00670 if (costs1) 00671 *costs1 = PORT_DOUBLE_MAX; 00672 if (costs2) 00673 *costs2 = PORT_DOUBLE_MAX; 00674 if (Points1) 00675 Vect_reset_line(Points1); 00676 if (Points2) 00677 Vect_reset_line(Points2); 00678 if (distance) 00679 *distance = PORT_DOUBLE_MAX; 00680 00681 if (!Points) 00682 Points = Vect_new_line_struct(); 00683 00684 /* Find nearest line */ 00685 line = Vect_find_line(Map, x, y, z, Map->graph_line_type, maxdist, 0, 0); 00686 00687 if (line < 1) 00688 return 0; 00689 00690 Vect_read_line(Map, Points, NULL, line); 00691 npoints = Points->n_points; 00692 Vect_get_line_nodes(Map, line, &n1, &n2); 00693 00694 segment = 00695 Vect_line_distance(Points, x, y, z, 0, &cx, &cy, &cz, distance, NULL, 00696 &along); 00697 00698 G_debug(4, "line = %d n1 = %d n2 = %d segment = %d", line, n1, n2, 00699 segment); 00700 00701 /* Check first or last point and return one node in that case */ 00702 G_debug(4, "cx = %f cy = %f first = %f %f last = %f %f", cx, cy, 00703 Points->x[0], Points->y[0], Points->x[npoints - 1], 00704 Points->y[npoints - 1]); 00705 00706 if (Points->x[0] == cx && Points->y[0] == cy) { 00707 if (node1) 00708 *node1 = n1; 00709 if (ln) 00710 *ln = line; 00711 if (costs1) 00712 *costs1 = 0; 00713 if (Points1) { 00714 Vect_append_point(Points1, x, y, z); 00715 Vect_append_point(Points1, cx, cy, cz); 00716 } 00717 G_debug(3, "first node nearest"); 00718 return 1; 00719 } 00720 if (Points->x[npoints - 1] == cx && Points->y[npoints - 1] == cy) { 00721 if (node1) 00722 *node1 = n2; 00723 if (ln) 00724 *ln = line; 00725 if (costs1) 00726 *costs1 = 0; 00727 if (Points1) { 00728 Vect_append_point(Points1, x, y, z); 00729 Vect_append_point(Points1, cx, cy, cz); 00730 } 00731 G_debug(3, "last node nearest"); 00732 return 1; 00733 } 00734 00735 nnodes = 2; 00736 00737 /* c1 - costs to get from/to the first vertex */ 00738 /* c2 - costs to get from/to the last vertex */ 00739 if (direction == GV_FORWARD) { /* from point to net */ 00740 Vect_net_get_line_cost(Map, line, GV_BACKWARD, &c1); 00741 Vect_net_get_line_cost(Map, line, GV_FORWARD, &c2); 00742 } 00743 else { 00744 Vect_net_get_line_cost(Map, line, GV_FORWARD, &c1); 00745 Vect_net_get_line_cost(Map, line, GV_BACKWARD, &c2); 00746 } 00747 00748 if (c1 < 0) 00749 nnodes--; 00750 if (c2 < 0) 00751 nnodes--; 00752 if (nnodes == 0) 00753 return 0; /* both directions closed */ 00754 00755 length = Vect_line_length(Points); 00756 00757 if (ln) 00758 *ln = line; 00759 00760 if (nnodes == 1 && c1 < 0) { /* first direction is closed, return node2 as node1 */ 00761 if (node1) 00762 *node1 = n2; 00763 00764 if (costs1) { /* to node 2, i.e. forward */ 00765 *costs1 = c2 * (length - along) / length; 00766 } 00767 00768 if (Points1) { /* to node 2, i.e. forward */ 00769 int i; 00770 00771 if (direction == GV_FORWARD) { /* from point to net */ 00772 Vect_append_point(Points1, x, y, z); 00773 Vect_append_point(Points1, cx, cy, cz); 00774 for (i = segment; i < npoints; i++) 00775 Vect_append_point(Points1, Points->x[i], Points->y[i], 00776 Points->z[i]); 00777 } 00778 else { 00779 for (i = npoints - 1; i >= segment; i--) 00780 Vect_append_point(Points1, Points->x[i], Points->y[i], 00781 Points->z[i]); 00782 00783 Vect_append_point(Points1, cx, cy, cz); 00784 Vect_append_point(Points1, x, y, z); 00785 } 00786 } 00787 } 00788 else { 00789 if (node1) 00790 *node1 = n1; 00791 if (node2) 00792 *node2 = n2; 00793 00794 if (costs1) { /* to node 1, i.e. backward */ 00795 *costs1 = c1 * along / length; 00796 } 00797 00798 if (costs2) { /* to node 2, i.e. forward */ 00799 *costs2 = c2 * (length - along) / length; 00800 } 00801 00802 if (Points1) { /* to node 1, i.e. backward */ 00803 int i; 00804 00805 if (direction == GV_FORWARD) { /* from point to net */ 00806 Vect_append_point(Points1, x, y, z); 00807 Vect_append_point(Points1, cx, cy, cz); 00808 for (i = segment - 1; i >= 0; i--) 00809 Vect_append_point(Points1, Points->x[i], Points->y[i], 00810 Points->z[i]); 00811 } 00812 else { 00813 for (i = 0; i < segment; i++) 00814 Vect_append_point(Points1, Points->x[i], Points->y[i], 00815 Points->z[i]); 00816 00817 Vect_append_point(Points1, cx, cy, cz); 00818 Vect_append_point(Points1, x, y, z); 00819 } 00820 } 00821 00822 if (Points2) { /* to node 2, i.e. forward */ 00823 int i; 00824 00825 if (direction == GV_FORWARD) { /* from point to net */ 00826 Vect_append_point(Points2, x, y, z); 00827 Vect_append_point(Points2, cx, cy, cz); 00828 for (i = segment; i < npoints; i++) 00829 Vect_append_point(Points2, Points->x[i], Points->y[i], 00830 Points->z[i]); 00831 } 00832 else { 00833 for (i = npoints - 1; i >= segment; i--) 00834 Vect_append_point(Points2, Points->x[i], Points->y[i], 00835 Points->z[i]); 00836 00837 Vect_append_point(Points2, cx, cy, cz); 00838 Vect_append_point(Points2, x, y, z); 00839 } 00840 } 00841 } 00842 00843 return nnodes; 00844 } 00845 00864 int 00865 Vect_net_shortest_path_coor(struct Map_info *Map, 00866 double fx, double fy, double fz, double tx, 00867 double ty, double tz, double fmax, double tmax, 00868 double *costs, struct line_pnts *Points, 00869 struct ilist *List, struct line_pnts *FPoints, 00870 struct line_pnts *TPoints, double *fdist, 00871 double *tdist) 00872 { 00873 return Vect_net_shortest_path_coor2 ( Map, fx, fy, fz, tx, ty, tz, fmax, tmax, 00874 costs, Points, List, NULL, FPoints, TPoints, fdist, tdist ); 00875 } 00876 00896 int 00897 Vect_net_shortest_path_coor2(struct Map_info *Map, 00898 double fx, double fy, double fz, double tx, 00899 double ty, double tz, double fmax, double tmax, 00900 double *costs, struct line_pnts *Points, 00901 struct ilist *List, struct ilist *NodesList, 00902 struct line_pnts *FPoints, 00903 struct line_pnts *TPoints, double *fdist, 00904 double *tdist) 00905 { 00906 int fnode[2], tnode[2]; /* nearest nodes, *node[1] is 0 if only one was found */ 00907 double fcosts[2], tcosts[2], cur_cst; /* costs to nearest nodes on the network */ 00908 int nfnodes, ntnodes, fline, tline; 00909 static struct line_pnts *APoints, *SPoints, *fPoints[2], *tPoints[2]; 00910 static struct ilist *LList; 00911 static int first = 1; 00912 int reachable, shortcut; 00913 int i, j, fn = 0, tn = 0; 00914 00915 /* from/to_point_node is set if from/to point projected to line 00916 *falls exactly on node (shortcut -> fline == tline) */ 00917 int from_point_node = 0; 00918 int to_point_node = 0; 00919 00920 G_debug(3, "Vect_net_shortest_path_coor()"); 00921 00922 if (first) { 00923 APoints = Vect_new_line_struct(); 00924 SPoints = Vect_new_line_struct(); 00925 fPoints[0] = Vect_new_line_struct(); 00926 fPoints[1] = Vect_new_line_struct(); 00927 tPoints[0] = Vect_new_line_struct(); 00928 tPoints[1] = Vect_new_line_struct(); 00929 LList = Vect_new_list(); 00930 first = 0; 00931 } 00932 00933 /* Reset */ 00934 if (costs) 00935 *costs = PORT_DOUBLE_MAX; 00936 if (Points) 00937 Vect_reset_line(Points); 00938 if (fdist) 00939 *fdist = 0; 00940 if (tdist) 00941 *tdist = 0; 00942 if (List) 00943 List->n_values = 0; 00944 if (FPoints) 00945 Vect_reset_line(FPoints); 00946 if (TPoints) 00947 Vect_reset_line(TPoints); 00948 if (NodesList != NULL) 00949 Vect_reset_list(NodesList); 00950 00951 /* Find nearest nodes */ 00952 fnode[0] = fnode[1] = tnode[0] = tnode[1] = 0; 00953 00954 nfnodes = 00955 Vect_net_nearest_nodes(Map, fx, fy, fz, GV_FORWARD, fmax, &(fnode[0]), 00956 &(fnode[1]), &fline, &(fcosts[0]), 00957 &(fcosts[1]), fPoints[0], fPoints[1], fdist); 00958 if (nfnodes == 0) 00959 return 0; 00960 00961 if ( nfnodes == 1 && fPoints[0]->n_points < 3 ) { 00962 from_point_node = fnode[0]; 00963 } 00964 00965 ntnodes = 00966 Vect_net_nearest_nodes(Map, tx, ty, tz, GV_BACKWARD, tmax, 00967 &(tnode[0]), &(tnode[1]), &tline, &(tcosts[0]), 00968 &(tcosts[1]), tPoints[0], tPoints[1], tdist); 00969 if (ntnodes == 0) 00970 return 0; 00971 00972 if ( ntnodes == 1 && tPoints[0]->n_points < 3 ) { 00973 to_point_node = tnode[0]; 00974 } 00975 00976 00977 G_debug(3, "fline = %d tline = %d", fline, tline); 00978 00979 reachable = shortcut = 0; 00980 cur_cst = PORT_DOUBLE_MAX; 00981 00982 /* It may happen, that 2 points are at the same line. */ 00983 /* TODO?: it could also happen that fline != tline but both points are on the same 00984 * line if they fall on node but a different line was found. This case is correctly 00985 * handled as normal non shortcut, but it could be added here. In that case 00986 * NodesList collection must be changed */ 00987 if (fline == tline && (nfnodes > 1 || ntnodes > 1)) { 00988 double len, flen, tlen, c, fseg, tseg; 00989 double fcx, fcy, fcz, tcx, tcy, tcz; 00990 00991 Vect_read_line(Map, APoints, NULL, fline); 00992 len = Vect_line_length(APoints); 00993 00994 /* distance along the line */ 00995 fseg = 00996 Vect_line_distance(APoints, fx, fy, fz, 0, &fcx, &fcy, &fcz, NULL, 00997 NULL, &flen); 00998 tseg = 00999 Vect_line_distance(APoints, tx, ty, tz, 0, &tcx, &tcy, &tcz, NULL, 01000 NULL, &tlen); 01001 01002 Vect_reset_line(SPoints); 01003 if (flen == tlen) { 01004 cur_cst = 0; 01005 reachable = shortcut = 1; 01006 } 01007 else if (flen < tlen) { 01008 Vect_net_get_line_cost(Map, fline, GV_FORWARD, &c); 01009 if (c >= 0) { 01010 cur_cst = c * (tlen - flen) / len; 01011 01012 Vect_append_point(SPoints, fx, fy, fz); 01013 Vect_append_point(SPoints, fcx, fcy, fcz); 01014 for (i = fseg; i < tseg; i++) 01015 Vect_append_point(SPoints, APoints->x[i], APoints->y[i], 01016 APoints->z[i]); 01017 01018 Vect_append_point(SPoints, tcx, tcy, tcz); 01019 Vect_append_point(SPoints, tx, ty, tz); 01020 01021 reachable = shortcut = 1; 01022 } 01023 } 01024 else { /* flen > tlen */ 01025 Vect_net_get_line_cost(Map, fline, GV_BACKWARD, &c); 01026 if (c >= 0) { 01027 cur_cst = c * (flen - tlen) / len; 01028 01029 Vect_append_point(SPoints, fx, fy, fz); 01030 Vect_append_point(SPoints, fcx, fcy, fcz); 01031 for (i = fseg - 1; i >= tseg; i--) 01032 Vect_append_point(SPoints, APoints->x[i], APoints->y[i], 01033 APoints->z[i]); 01034 01035 Vect_append_point(SPoints, tcx, tcy, tcz); 01036 Vect_append_point(SPoints, tx, ty, tz); 01037 01038 reachable = shortcut = 1; 01039 } 01040 } 01041 } 01042 01043 /* Find the shortest variant from maximum 4 */ 01044 for (i = 0; i < nfnodes; i++) { 01045 for (j = 0; j < ntnodes; j++) { 01046 double ncst, cst; 01047 int ret; 01048 01049 G_debug(3, "i = %d fnode = %d j = %d tnode = %d", i, fnode[i], j, 01050 tnode[j]); 01051 01052 ret = 01053 Vect_net_shortest_path(Map, fnode[i], tnode[j], NULL, &ncst); 01054 if (ret == -1) 01055 continue; /* not reachable */ 01056 01057 cst = fcosts[i] + ncst + tcosts[j]; 01058 if (reachable == 0 || cst < cur_cst) { 01059 cur_cst = cst; 01060 fn = i; 01061 tn = j; 01062 shortcut = 0; 01063 } 01064 reachable = 1; 01065 } 01066 } 01067 01068 G_debug(3, "reachable = %d shortcut = %d cur_cst = %f", reachable, 01069 shortcut, cur_cst); 01070 if (reachable) { 01071 int ret; 01072 01073 if (shortcut) { 01074 if (Points) 01075 Vect_append_points(Points, SPoints, GV_FORWARD); 01076 if (NodesList) { 01077 /* Check if from/to point projected to line falls on node and 01078 *add it to the list */ 01079 if (from_point_node > 0) 01080 Vect_list_append(NodesList, from_point_node); 01081 01082 if (to_point_node > 0) 01083 Vect_list_append(NodesList, to_point_node); 01084 } 01085 } 01086 else { 01087 if (NodesList) { 01088 /* it can happen that starting point falls on node but SP starts 01089 * form the other node, add it in that case, 01090 * similarly for to point below */ 01091 if (from_point_node > 0 && from_point_node != fnode[fn]) { 01092 Vect_list_append(NodesList, from_point_node); 01093 } 01094 01095 /* add starting net SP search node */ 01096 Vect_list_append(NodesList, fnode[fn]); 01097 } 01098 ret = 01099 Vect_net_shortest_path(Map, fnode[fn], tnode[tn], LList, 01100 NULL); 01101 G_debug(3, "Number of lines %d", LList->n_values); 01102 01103 if (Points) 01104 Vect_append_points(Points, fPoints[fn], GV_FORWARD); 01105 01106 if (FPoints) 01107 Vect_append_points(FPoints, fPoints[fn], GV_FORWARD); 01108 01109 for (i = 0; i < LList->n_values; i++) { 01110 int line; 01111 01112 line = LList->value[i]; 01113 G_debug(3, "i = %d line = %d", i, line); 01114 01115 if (Points) { 01116 Vect_read_line(Map, APoints, NULL, abs(line)); 01117 01118 if (line > 0) 01119 Vect_append_points(Points, APoints, GV_FORWARD); 01120 else 01121 Vect_append_points(Points, APoints, GV_BACKWARD); 01122 } 01123 if (NodesList) { 01124 int node, node1, node2; 01125 Vect_get_line_nodes(Map, abs(line), &node1, &node2); 01126 /* add the second node, the first of first segmet was alread added */ 01127 if (line > 0) 01128 node = node2; 01129 else 01130 node = node1; 01131 01132 Vect_list_append(NodesList, node); 01133 } 01134 01135 if (List) 01136 Vect_list_append(List, line); 01137 } 01138 01139 if (Points) 01140 Vect_append_points(Points, tPoints[tn], GV_FORWARD); 01141 01142 if (TPoints) 01143 Vect_append_points(TPoints, tPoints[tn], GV_FORWARD); 01144 01145 if (NodesList) { 01146 if (to_point_node > 0 && to_point_node != tnode[tn]) { 01147 Vect_list_append(NodesList, to_point_node); 01148 } 01149 } 01150 } 01151 01152 if (costs) 01153 *costs = cur_cst; 01154 } 01155 01156 return reachable; 01157 }