00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <stdlib.h>
00018 #include <string.h>
00019 #include <grass/gis.h>
00020 #include <grass/dbmi.h>
00021 #include <grass/Vect.h>
00022
00023 static int From_node;
00024
00025 static int clipper ( dglGraph_s *pgraph ,
00026 dglSPClipInput_s * pargIn ,
00027 dglSPClipOutput_s * pargOut ,
00028 void * pvarg )
00029 {
00030 dglInt32_t cost;
00031 dglInt32_t from;
00032
00033 G_debug ( 3, "Net: clipper()" );
00034
00035 from = dglNodeGet_Id(pgraph, pargIn->pnNodeFrom);
00036
00037 G_debug ( 3, " Edge = %d NodeFrom = %d NodeTo = %d edge cost = %d",
00038 dglEdgeGet_Id (pgraph, pargIn->pnEdge),
00039 from, dglNodeGet_Id(pgraph, pargIn->pnNodeTo),
00040 pargOut->nEdgeCost);
00041
00042 if ( from != From_node ) {
00043 if ( dglGet_NodeAttrSize(pgraph) > 0 ) {
00044 memcpy( &cost, dglNodeGet_Attr(pgraph, pargIn->pnNodeFrom), sizeof(cost) );
00045 if ( cost == -1 ) {
00046 G_debug ( 3, " closed node" );
00047 return 1;
00048 } else {
00049 G_debug ( 3, " EdgeCost += %d (node)", cost );
00050 pargOut->nEdgeCost += cost;
00051 }
00052 }
00053 } else {
00054 G_debug ( 3, " don't clip first node" );
00055 }
00056
00057 return 0;
00058 }
00059
00088 int
00089 Vect_net_build_graph ( struct Map_info *Map,
00090 int ltype,
00091 int afield,
00092 int nfield,
00093 char *afcol,
00094 char *abcol,
00095 char *ncol,
00096 int geo,
00097 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 ] = { 360000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
00107 struct field_info *Fi;
00108 dbDriver *driver=NULL;
00109 dbHandle handle;
00110 dbString stmt;
00111 dbColumn *Column;
00112 dbCatValArray fvarr, bvarr;
00113 int fctype=0, bctype=0, nrec;
00114
00115
00116 G_debug (1, "Vect_build_graph(): ltype = %d, afield = %d, nfield = %d", ltype, afield, nfield);
00117 G_debug (1, " afcol = %s, abcol = %s, ncol = %s", afcol, abcol, ncol);
00118
00119 fprintf ( stderr, "Building graph:\n");
00120
00121 Map->graph_line_type = ltype;
00122
00123 Points = Vect_new_line_struct ();
00124 Cats = Vect_new_cats_struct ();
00125
00126 ll = 0;
00127 if( G_projection() == 3) ll = 1;
00128
00129 if ( afcol == NULL && ll && !geo ) Map->cost_multip = 1000000;
00130 else Map->cost_multip = 1000;
00131
00132 nlines = Vect_get_num_lines ( Map );
00133 nnodes = Vect_get_num_nodes ( Map );
00134
00135 gr = &(Map->graph);
00136
00137
00138 Map->edge_fcosts = (double*) G_malloc ( (nlines+1) * sizeof(double) );
00139 Map->edge_bcosts = (double*) G_malloc ( (nlines+1) * sizeof(double) );
00140 Map->node_costs = (double*) G_malloc ( (nnodes+1) * sizeof(double) );
00141
00142 for ( i = 1; i <= nlines; i++ ) {
00143 Map->edge_fcosts[i] = -1;
00144 Map->edge_bcosts[i] = -1;
00145 }
00146 for ( i = 1; i <= nnodes; i++ ) {
00147 Map->node_costs[i] = 0;
00148 }
00149
00150 if ( ncol != NULL )
00151 dglInitialize(gr, (dglByte_t)1, sizeof(dglInt32_t), (dglInt32_t)0, opaqueset);
00152 else
00153 dglInitialize(gr, (dglByte_t)1, (dglInt32_t)0, (dglInt32_t)0, opaqueset);
00154
00155 if ( gr == NULL ) G_fatal_error ("Cannot build network graph");
00156
00157 db_init_handle (&handle);
00158 db_init_string ( &stmt);
00159
00160 if ( abcol != NULL && afcol == NULL )
00161 G_fatal_error ("Forward costs column not specified");
00162
00163
00164
00165 if ( afcol != NULL ) {
00166
00167 if ( afield < 1 ) G_fatal_error ("Arc field < 1");
00168 Fi = Vect_get_field( Map, afield);
00169 if ( Fi == NULL ) G_fatal_error ("Cannot get field info");
00170
00171
00172 driver = db_start_driver_open_database ( Fi->driver, Fi->database );
00173 if ( driver == NULL )
00174 G_fatal_error("Cannot open database %s by driver %s", Fi->database, Fi->driver) ;
00175
00176
00177 if ( db_get_column ( driver, Fi->table, afcol, &Column ) != DB_OK)
00178 G_fatal_error("Cannot get column info");
00179
00180 fctype = db_sqltype_to_Ctype ( db_get_column_sqltype(Column) );
00181
00182 if ( fctype != DB_C_TYPE_INT && fctype != DB_C_TYPE_DOUBLE )
00183 G_fatal_error ( "Column type not supported" );
00184
00185 db_CatValArray_init ( &fvarr );
00186 nrec = db_select_CatValArray ( driver, Fi->table, Fi->key, afcol, NULL, &fvarr );
00187 G_debug (1, "forward costs: nrec = %d", nrec );
00188
00189 if ( abcol != NULL ) {
00190 if ( db_get_column ( driver, Fi->table, abcol, &Column ) != DB_OK)
00191 G_fatal_error("Cannot get column info");
00192
00193 bctype = db_sqltype_to_Ctype ( db_get_column_sqltype(Column) );
00194
00195 if ( bctype != DB_C_TYPE_INT && bctype != DB_C_TYPE_DOUBLE )
00196 G_fatal_error ( "Column type not supported" );
00197
00198 db_CatValArray_init ( &bvarr );
00199 nrec = db_select_CatValArray ( driver, Fi->table, Fi->key, abcol, NULL, &bvarr );
00200 G_debug (1, "backward costs: nrec = %d", nrec );
00201 }
00202 }
00203
00204 skipped = 0;
00205 fprintf ( stderr, "Registering arcs ... ");
00206 for ( i = 1; i <= nlines; i++ ) {
00207 G_percent ( i, nlines, 1 );
00208 dofw = dobw = 1;
00209 Vect_get_line_nodes ( Map, i, &from, &to );
00210 type = Vect_read_line ( Map, Points, Cats, i );
00211 if ( !(type & ltype & (GV_LINE | GV_BOUNDARY) ) ) continue;
00212
00213 if ( afcol != NULL ) {
00214 if ( !(Vect_cat_get(Cats, afield, &cat) ) ) {
00215 G_debug ( 2, "Category of field %d not attached to the line %d -> line skipped", afield, i);
00216 skipped += 2;
00217 continue;
00218 } else {
00219 if ( fctype == DB_C_TYPE_INT ) {
00220 ret = db_CatValArray_get_value_int ( &fvarr, cat, &cost );
00221 dcost = cost;
00222 } else {
00223 ret = db_CatValArray_get_value_double ( &fvarr, cat, &dcost );
00224 }
00225 if ( ret != DB_OK ) {
00226 G_warning ("Database record for line %d (cat = %d, forward/both direction(s)) not found "
00227 "(forward/both direction(s) of line skipped)", i, cat);
00228 dofw = 0;
00229 }
00230
00231 if ( abcol != NULL ) {
00232 if ( bctype == DB_C_TYPE_INT ) {
00233 ret = db_CatValArray_get_value_int ( &bvarr, cat, &bcost );
00234 bdcost = bcost;
00235 } else {
00236 ret = db_CatValArray_get_value_double ( &bvarr, cat, &bdcost );
00237 }
00238 if ( ret != DB_OK ) {
00239 G_warning ( "Database record for line %d (cat = %d, backword direction) not found"
00240 "(direction of line skipped)", i, cat);
00241 dobw = 0;
00242 }
00243 } else {
00244 if (dofw) bdcost = dcost;
00245 else dobw = 0;
00246 }
00247 }
00248 } else {
00249 if ( ll ) {
00250 if ( geo ) dcost = Vect_line_geodesic_length ( Points );
00251 else dcost = Vect_line_length ( Points );
00252 } else
00253 dcost = Vect_line_length ( Points );
00254
00255 bdcost = dcost;
00256 }
00257 if ( dofw && dcost != -1 ) {
00258 cost = (dglInt32_t) Map->cost_multip * dcost;
00259 G_debug (5, "Add arc %d from %d to %d cost = %d", i, from, to, cost);
00260 ret = dglAddEdge(gr, (dglInt32_t)from, (dglInt32_t)to, (dglInt32_t)cost, (dglInt32_t)i);
00261 Map->edge_fcosts[i] = dcost;
00262 if ( ret < 0 ) G_fatal_error ("Cannot add network arc");
00263 }
00264
00265 G_debug (5, "bdcost = %f edge_bcosts = %f", bdcost, Map->edge_bcosts[i]);
00266 if ( dobw && bdcost != -1 ) {
00267 bcost = (dglInt32_t) Map->cost_multip * bdcost;
00268 G_debug (5, "Add arc %d from %d to %d bcost = %d", -i, to, from, bcost);
00269 ret = dglAddEdge(gr, (dglInt32_t)to, (dglInt32_t)from, (dglInt32_t)bcost, (dglInt32_t)-i);
00270 Map->edge_bcosts[i] = bdcost;
00271 if ( ret < 0 ) G_fatal_error ("Cannot add network arc");
00272 }
00273 }
00274
00275 if ( afcol != NULL && skipped > 0 )
00276 G_debug ( 2, "%d lines missing category of field %d skipped", skipped, afield);
00277
00278 if ( afcol != NULL ) {
00279 db_close_database_shutdown_driver ( driver );
00280 db_CatValArray_free ( &fvarr);
00281
00282 if ( abcol != NULL ) {
00283 db_CatValArray_free ( &bvarr);
00284 }
00285 }
00286
00287
00288 G_debug ( 2, "Register nodes");
00289 if ( ncol != NULL ) {
00290 G_debug ( 2, "Set nodes' costs");
00291 if ( nfield < 1 ) G_fatal_error ("Node field < 1");
00292
00293 fprintf ( stderr, "Setting node costs ... ");
00294
00295 Fi = Vect_get_field( Map, nfield);
00296 if ( Fi == NULL ) G_fatal_error ("Cannot get field info");
00297
00298 driver = db_start_driver_open_database ( Fi->driver, Fi->database );
00299 if ( driver == NULL )
00300 G_fatal_error("Cannot open database %s by driver %s", Fi->database, Fi->driver) ;
00301
00302
00303 if ( db_get_column ( driver, Fi->table, ncol, &Column ) != DB_OK)
00304 G_fatal_error("Cannot get column info");
00305
00306 fctype = db_sqltype_to_Ctype ( db_get_column_sqltype(Column) );
00307
00308 if ( fctype != DB_C_TYPE_INT && fctype != DB_C_TYPE_DOUBLE )
00309 G_fatal_error ( "Column type not supported" );
00310
00311 db_CatValArray_init ( &fvarr );
00312 nrec = db_select_CatValArray ( driver, Fi->table, Fi->key, ncol, NULL, &fvarr );
00313 G_debug (1, "node costs: nrec = %d", nrec );
00314
00315 for ( i = 1; i <= nnodes; i++ ) {
00316
00317
00318
00319 nlines = Vect_get_node_n_lines ( Map, i );
00320 G_debug ( 2, " node = %d nlines = %d", i, nlines );
00321 cfound = 0;
00322 dcost = 0;
00323 for ( j = 0; j < nlines; j++ ) {
00324 line = Vect_get_node_line ( Map, i, j );
00325 G_debug ( 2, " line (%d) = %d", j, line );
00326 type = Vect_read_line ( Map, NULL, Cats, line);
00327 if ( !(type & GV_POINT) ) continue;
00328 if ( Vect_cat_get(Cats, nfield, &cat) ) {
00329
00330 if ( fctype == DB_C_TYPE_INT ) {
00331 ret = db_CatValArray_get_value_int ( &fvarr, cat, &cost );
00332 dcost = cost;
00333 } else {
00334 ret = db_CatValArray_get_value_double ( &fvarr, cat, &dcost );
00335 }
00336 if ( ret != DB_OK ) {
00337 G_warning ( "Database record for node %d (cat = %d) not found "
00338 "(cost set to 0)", i, cat);
00339 }
00340 cfound = 1;
00341 break;
00342 }
00343 }
00344 if ( !cfound ) {
00345 G_debug ( 2, "Category of field %d not attached to any points in node %d"
00346 "(costs set to 0)", nfield, i);
00347 }
00348 if ( dcost == -1 ) {
00349 cost = -1;
00350 } else {
00351 cost = (dglInt32_t) Map->cost_multip * dcost;
00352 }
00353 G_debug ( 3, "Set node's cost to %d", cost);
00354 dglNodeSet_Attr(gr, dglGetNode(gr, (dglInt32_t)i), (dglInt32_t *)(dglInt32_t)&cost);
00355 Map->node_costs[i] = dcost;
00356 }
00357 db_close_database_shutdown_driver ( driver );
00358 db_CatValArray_free ( &fvarr);
00359 fprintf ( stderr, "done.\n");
00360 }
00361
00362 fprintf ( stderr, "Flattening the graph ... ");
00363 ret = dglFlatten ( gr );
00364 if ( ret < 0 ) G_fatal_error ("GngFlatten error");
00365 fprintf ( stderr, "done.\n");
00366
00367
00368
00369
00370
00371
00372 fprintf ( stderr, "Graph was built.\n");
00373
00374 return 0;
00375 }
00376
00377
00387 #include<fcntl.h>
00388 int
00389 Vect_net_shortest_path ( struct Map_info *Map, int from, int to, struct ilist *List, double *cost )
00390 {
00391 int i, line, *pclip, cArc, nRet;
00392 dglSPReport_s * pSPReport;
00393 dglInt32_t nDistance;
00394
00395 G_debug (3, "Vect_net_shortest_path(): from = %d, to = %d", from, to );
00396
00397
00398
00399
00400 if ( List != NULL ) Vect_reset_list ( List);
00401
00402
00403 if ( from == to ) {
00404 if ( cost != NULL ) *cost = 0;
00405 return 0;
00406 }
00407
00408 From_node = from;
00409
00410 pclip = NULL;
00411 if ( List != NULL ) {
00412
00413 nRet = dglShortestPath(&(Map->graph), &pSPReport, (dglInt32_t)from, (dglInt32_t)to, clipper, pclip, NULL);
00414 } else {
00415
00416 nRet = dglShortestDistance(&(Map->graph), &nDistance, (dglInt32_t)from, (dglInt32_t)to, clipper, pclip, NULL);
00417 }
00418
00419 if ( nRet == 0 ) {
00420
00421 if ( cost != NULL )
00422 *cost = PORT_DOUBLE_MAX;
00423 return -1;
00424 }
00425 else if ( nRet < 0 ) {
00426 fprintf( stderr , "dglShortestPath error: %s\n", dglStrerror( &(Map->graph) ) );
00427 return -1;
00428 }
00429
00430 if ( List != NULL ) {
00431 for( i = 0 ; i < pSPReport->cArc ; i ++ ) {
00432 line = dglEdgeGet_Id(&(Map->graph), pSPReport->pArc[i].pnEdge);
00433 G_debug( 2, "From %ld to %ld - cost %ld user %d distance %ld" ,
00434 pSPReport->pArc[i].nFrom,
00435 pSPReport->pArc[i].nTo,
00436 dglEdgeGet_Cost(&(Map->graph),
00437 pSPReport->pArc[i].pnEdge) / Map->cost_multip,
00438 line,
00439 pSPReport->pArc[i].nDistance );
00440 Vect_list_append ( List, line );
00441 }
00442 }
00443
00444 if ( cost != NULL ) {
00445 if ( List != NULL )
00446 *cost = (double) pSPReport->nDistance / Map->cost_multip;
00447 else
00448 *cost = (double) nDistance / Map->cost_multip;
00449 }
00450
00451 if ( List != NULL ) {
00452 cArc = pSPReport->cArc;
00453 dglFreeSPReport( &(Map->graph), pSPReport );
00454 } else
00455 cArc = 0;
00456
00457 return (cArc);
00458 }
00459
00460
00461
00462
00463
00464
00465
00466 int
00467 Vect_net_get_line_cost ( struct Map_info *Map, int line, int direction, double *cost )
00468 {
00469
00470
00471 G_debug (5, "Vect_net_get_line_cost(): line = %d, dir = %d", line, direction );
00472
00473 if ( direction == GV_FORWARD ) {
00474
00475
00476
00477
00478
00479
00480 if ( Map->edge_fcosts[line] == -1 ) {
00481 *cost = -1;
00482 return 0;
00483 } else *cost = Map->edge_fcosts[line];
00484 } else if ( direction == GV_BACKWARD ) {
00485
00486
00487
00488
00489
00490 if ( Map->edge_bcosts[line] == -1 ) {
00491 *cost = -1;
00492 return 0;
00493 } else *cost = Map->edge_bcosts[line];
00494 G_debug (5, "Vect_net_get_line_cost(): edge_bcosts = %f", Map->edge_bcosts[line] );
00495 } else {
00496 G_fatal_error ("Wrong line direction in Vect_net_get_line_cost()" );
00497 }
00498
00499 return 1;
00500 }
00501
00502
00503
00504
00505
00506 int
00507 Vect_net_get_node_cost ( struct Map_info *Map, int node, double *cost )
00508 {
00509 G_debug (3, "Vect_net_get_node_cost(): node = %d", node );
00510
00511 *cost = Map->node_costs[node];
00512
00513 G_debug (3, " -> cost = %f", *cost );
00514
00515 return 1;
00516 }
00517
00541 int Vect_net_nearest_nodes ( struct Map_info *Map,
00542 double x, double y, double z,
00543 int direction, double maxdist,
00544 int *node1, int *node2, int *ln, double *costs1, double *costs2,
00545 struct line_pnts *Points1, struct line_pnts *Points2,
00546 double *distance )
00547 {
00548 int line, n1, n2, nnodes;
00549 int npoints;
00550 int segment;
00551 static struct line_pnts *Points = NULL;
00552 double cx, cy, cz, c1, c2;
00553 double along;
00554 double length;
00555
00556 G_debug (3, "Vect_net_nearest_nodes() x = %f y = %f", x, y );
00557
00558
00559 if ( node1 ) *node1 = 0;
00560 if ( node2 ) *node2 = 0;
00561 if ( ln ) *ln = 0;
00562 if ( costs1 ) *costs1 = PORT_DOUBLE_MAX;
00563 if ( costs2 ) *costs2 = PORT_DOUBLE_MAX;
00564 if ( Points1 ) Vect_reset_line ( Points1 );
00565 if ( Points2 ) Vect_reset_line ( Points2 );
00566 if ( distance ) *distance = PORT_DOUBLE_MAX;
00567
00568 if ( !Points ) Points = Vect_new_line_struct();
00569
00570
00571 line = Vect_find_line ( Map, x, y, z, Map->graph_line_type, maxdist, 0, 0 );
00572
00573 if ( line < 1 ) return 0;
00574
00575 Vect_read_line ( Map, Points, NULL, line );
00576 npoints = Points->n_points;
00577 Vect_get_line_nodes ( Map, line, &n1, &n2);
00578
00579 segment = Vect_line_distance ( Points, x, y, z, 0, &cx, &cy, &cz, distance, NULL, &along);
00580
00581 G_debug (4, "line = %d n1 = %d n2 = %d segment = %d", line, n1, n2, segment );
00582
00583
00584 G_debug (4, "cx = %f cy = %f first = %f %f last = %f %f", cx, cy, Points->x[0], Points->y[0],
00585 Points->x[npoints-1], Points->y[npoints-1] );
00586
00587 if ( Points->x[0] == cx && Points->y[0] == cy ) {
00588 if ( node1 ) *node1 = n1;
00589 if ( ln ) *ln = line;
00590 if ( costs1 ) *costs1 = 0;
00591 if ( Points1 ) {
00592 Vect_append_point ( Points1, x, y, z );
00593 Vect_append_point ( Points1, cx, cy, cz );
00594 }
00595 G_debug (3, "first node nearest");
00596 return 1;
00597 }
00598 if ( Points->x[npoints-1] == cx && Points->y[npoints-1] == cy ) {
00599 if ( node1 ) *node1 = n2;
00600 if ( ln ) *ln = line;
00601 if ( costs1 ) *costs1 = 0;
00602 if ( Points1 ) {
00603 Vect_append_point ( Points1, x, y, z );
00604 Vect_append_point ( Points1, cx, cy, cz );
00605 }
00606 G_debug (3, "last node nearest");
00607 return 1;
00608 }
00609
00610 nnodes = 2;
00611
00612
00613
00614 if ( direction == GV_FORWARD ) {
00615 Vect_net_get_line_cost ( Map, line, GV_BACKWARD, &c1 );
00616 Vect_net_get_line_cost ( Map, line, GV_FORWARD, &c2 );
00617 } else {
00618 Vect_net_get_line_cost ( Map, line, GV_FORWARD, &c1 );
00619 Vect_net_get_line_cost ( Map, line, GV_BACKWARD, &c2 );
00620 }
00621
00622 if ( c1 < 0 ) nnodes--;
00623 if ( c2 < 0 ) nnodes--;
00624 if ( nnodes == 0 ) return 0;
00625
00626 length = Vect_line_length ( Points );
00627
00628 if ( ln ) *ln = line;
00629
00630 if ( nnodes == 1 && c1 < 0 ) {
00631 if ( node1 ) *node1 = n2;
00632
00633 if ( costs1 ) {
00634 *costs1 = c2 * (length - along) / length;
00635 }
00636
00637 if ( Points1 ) {
00638 int i;
00639
00640 if ( direction == GV_FORWARD ) {
00641 Vect_append_point ( Points1, x, y, z );
00642 Vect_append_point ( Points1, cx, cy, cz );
00643 for ( i = segment; i < npoints; i++ )
00644 Vect_append_point ( Points1, Points->x[i], Points->y[i], Points->z[i] );
00645 } else {
00646 for ( i = npoints - 1; i >= segment; i-- )
00647 Vect_append_point ( Points1, Points->x[i], Points->y[i], Points->z[i] );
00648
00649 Vect_append_point ( Points1, cx, cy, cz );
00650 Vect_append_point ( Points1, x, y, z );
00651 }
00652 }
00653 } else {
00654 if ( node1 ) *node1 = n1;
00655 if ( node2 ) *node2 = n2;
00656
00657 if ( costs1 ) {
00658 *costs1 = c1 * along / length;
00659 }
00660
00661 if ( costs2 ) {
00662 *costs2 = c2 * (length - along) / length;
00663 }
00664
00665 if ( Points1 ) {
00666 int i;
00667
00668 if ( direction == GV_FORWARD ) {
00669 Vect_append_point ( Points1, x, y, z );
00670 Vect_append_point ( Points1, cx, cy, cz );
00671 for ( i = segment - 1; i >= 0; i-- )
00672 Vect_append_point ( Points1, Points->x[i], Points->y[i], Points->z[i] );
00673 } else {
00674 for ( i = 0; i < segment; i++ )
00675 Vect_append_point ( Points1, Points->x[i], Points->y[i], Points->z[i] );
00676
00677 Vect_append_point ( Points1, cx, cy, cz );
00678 Vect_append_point ( Points1, x, y, z );
00679 }
00680 }
00681
00682 if ( Points2 ) {
00683 int i;
00684
00685 if ( direction == GV_FORWARD ) {
00686 Vect_append_point ( Points2, x, y, z );
00687 Vect_append_point ( Points2, cx, cy, cz );
00688 for ( i = segment; i < npoints; i++ )
00689 Vect_append_point ( Points2, Points->x[i], Points->y[i], Points->z[i] );
00690 } else {
00691 for ( i = npoints - 1; i >= segment; i-- )
00692 Vect_append_point ( Points2, Points->x[i], Points->y[i], Points->z[i] );
00693
00694 Vect_append_point ( Points2, cx, cy, cz );
00695 Vect_append_point ( Points2, x, y, z );
00696 }
00697 }
00698 }
00699
00700 return nnodes;
00701 }
00702
00730 int
00731 Vect_net_shortest_path_coor ( struct Map_info *Map,
00732 double fx, double fy, double fz, double tx, double ty, double tz,
00733 double fmax, double tmax,
00734 double *costs, struct line_pnts *Points,
00735 struct ilist *List, struct line_pnts *FPoints, struct line_pnts *TPoints,
00736 double *fdist, double *tdist )
00737 {
00738 int fnode[2], tnode[2];
00739 double fcosts[2], tcosts[2], cur_cst;
00740 int nfnodes, ntnodes, fline, tline;
00741 static struct line_pnts *APoints, *SPoints, *fPoints[2], *tPoints[2];
00742 static struct ilist *LList;
00743 static int first = 1;
00744 int reachable, shortcut;
00745 int i, j, fn=0, tn=0;
00746
00747 G_debug (3, "Vect_net_shortest_path_coor()");
00748
00749 if ( first ) {
00750 APoints = Vect_new_line_struct();
00751 SPoints = Vect_new_line_struct();
00752 fPoints[0] = Vect_new_line_struct();
00753 fPoints[1] = Vect_new_line_struct();
00754 tPoints[0] = Vect_new_line_struct();
00755 tPoints[1] = Vect_new_line_struct();
00756 LList = Vect_new_list ();
00757 first = 0;
00758 }
00759
00760
00761 if ( costs ) *costs = PORT_DOUBLE_MAX;
00762 if ( Points ) Vect_reset_line ( Points );
00763 if ( fdist ) *fdist = 0;
00764 if ( tdist ) *tdist = 0;
00765 if ( List ) List->n_values = 0;
00766 if ( FPoints ) Vect_reset_line ( FPoints );
00767 if ( TPoints ) Vect_reset_line ( TPoints );
00768
00769
00770 fnode[0] = fnode[1] = tnode[0] = tnode[1] = 0;
00771
00772 nfnodes = Vect_net_nearest_nodes ( Map, fx, fy, fz, GV_FORWARD, fmax, &(fnode[0]), &(fnode[1]), &fline,
00773 &(fcosts[0]), &(fcosts[1]), fPoints[0], fPoints[1], fdist );
00774 if ( nfnodes == 0 ) return 0;
00775
00776 ntnodes = Vect_net_nearest_nodes ( Map, tx, ty, tz, GV_BACKWARD, tmax, &(tnode[0]), &(tnode[1]), &tline,
00777 &(tcosts[0]), &(tcosts[1]), tPoints[0], tPoints[1], tdist );
00778 if ( ntnodes == 0 ) return 0;
00779
00780 G_debug (3, "fline = %d tline = %d", fline, tline);
00781
00782 reachable = shortcut = 0;
00783 cur_cst = PORT_DOUBLE_MAX;
00784
00785
00786 if ( fline == tline && (nfnodes > 1 || ntnodes > 1) ) {
00787 double len, flen, tlen, c, fseg, tseg;
00788 double fcx, fcy, fcz, tcx, tcy, tcz;
00789
00790 Vect_read_line ( Map, APoints, NULL, fline );
00791 len = Vect_line_length ( APoints );
00792
00793
00794 fseg = Vect_line_distance ( APoints, fx, fy, fz, 0, &fcx, &fcy, &fcz, NULL, NULL, &flen);
00795 tseg = Vect_line_distance ( APoints, tx, ty, tz, 0, &tcx, &tcy, &tcz, NULL, NULL, &tlen);
00796
00797 Vect_reset_line ( SPoints );
00798 if ( flen == tlen ) {
00799 cur_cst = 0;
00800 reachable = shortcut = 1;
00801 } else if ( flen < tlen ) {
00802 Vect_net_get_line_cost ( Map, fline, GV_FORWARD, &c );
00803 if ( c >= 0 ) {
00804 cur_cst = c * (tlen - flen) / len;
00805
00806 Vect_append_point (SPoints, fx, fy, fz);
00807 Vect_append_point (SPoints, fcx, fcy, fcz);
00808 for ( i = fseg; i < tseg; i++ )
00809 Vect_append_point (SPoints, APoints->x[i], APoints->y[i], APoints->z[i]);
00810
00811 Vect_append_point (SPoints, tcx, tcy, tcz);
00812 Vect_append_point (SPoints, tx, ty, tz);
00813
00814 reachable = shortcut = 1;
00815 }
00816 } else {
00817 Vect_net_get_line_cost ( Map, fline, GV_BACKWARD, &c );
00818 if ( c >= 0 ) {
00819 cur_cst = c * (flen - tlen) / len;
00820
00821 Vect_append_point (SPoints, fx, fy, fz);
00822 Vect_append_point (SPoints, fcx, fcy, fcz);
00823 for ( i = fseg - 1; i >= tseg; i-- )
00824 Vect_append_point (SPoints, APoints->x[i], APoints->y[i], APoints->z[i]);
00825
00826 Vect_append_point (SPoints, tcx, tcy, tcz);
00827 Vect_append_point (SPoints, tx, ty, tz);
00828
00829 reachable = shortcut = 1;
00830 }
00831 }
00832 }
00833
00834
00835 for ( i = 0; i < nfnodes; i++ ) {
00836 for ( j = 0; j < ntnodes; j++ ) {
00837 double ncst, cst;
00838 int ret;
00839
00840 G_debug (3, "i = %d fnode = %d j = %d tnode = %d", i, fnode[i], j, tnode[j]);
00841
00842 ret = Vect_net_shortest_path ( Map, fnode[i], tnode[j], NULL, &ncst);
00843 if ( ret == -1 ) continue;
00844
00845 cst = fcosts[i] + ncst + tcosts[j];
00846 if ( reachable == 0 || cst < cur_cst ) {
00847 cur_cst = cst;
00848 fn = i;
00849 tn = j;
00850 shortcut = 0;
00851 }
00852 reachable = 1;
00853 }
00854 }
00855
00856 G_debug (3, "reachable = %d shortcut = %d cur_cst = %f", reachable, shortcut, cur_cst);
00857 if ( reachable ) {
00858 int ret;
00859
00860 if ( shortcut) {
00861 if ( Points )
00862 Vect_append_points ( Points, SPoints, GV_FORWARD );
00863 } else {
00864 ret = Vect_net_shortest_path ( Map, fnode[fn], tnode[tn], LList, NULL);
00865 G_debug (3, "Number of lines %d", LList->n_values);
00866
00867 if ( Points )
00868 Vect_append_points ( Points, fPoints[fn], GV_FORWARD );
00869
00870 if ( FPoints )
00871 Vect_append_points ( FPoints, fPoints[fn], GV_FORWARD );
00872
00873 for ( i = 0; i < LList->n_values; i++ ) {
00874 int line;
00875
00876 line = LList->value[i];
00877 G_debug (3, "i = %d line = %d", i, line);
00878
00879 if ( Points ) {
00880 Vect_read_line ( Map, APoints, NULL, abs(line) );
00881
00882 if ( line > 0 )
00883 Vect_append_points ( Points, APoints, GV_FORWARD );
00884 else
00885 Vect_append_points ( Points, APoints, GV_BACKWARD );
00886 }
00887
00888 if ( List ) Vect_list_append ( List, line );
00889 }
00890
00891 if ( Points )
00892 Vect_append_points ( Points, tPoints[tn], GV_FORWARD );
00893
00894 if ( TPoints )
00895 Vect_append_points ( TPoints, tPoints[tn], GV_FORWARD );
00896 }
00897
00898 if ( costs ) *costs = cur_cst;
00899 }
00900
00901 return reachable;
00902 }
00903