From 7b358424ebad9349421acd533c2fa1cbf6cf3e3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B6rg=20Frings-F=C3=BCrst?= Date: Wed, 28 Dec 2016 16:52:56 +0100 Subject: Initial import of xtrkcad version 1:4.0.2-2 --- app/bin/shrtpath.c | 330 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 330 insertions(+) create mode 100644 app/bin/shrtpath.c (limited to 'app/bin/shrtpath.c') diff --git a/app/bin/shrtpath.c b/app/bin/shrtpath.c new file mode 100644 index 0000000..fa48408 --- /dev/null +++ b/app/bin/shrtpath.c @@ -0,0 +1,330 @@ +/* + * $Header: /home/dmarkle/xtrkcad-fork-cvs/xtrkcad/app/bin/shrtpath.c,v 1.1 2005-12-07 15:46:54 rc-flyer Exp $ + */ + +/* XTrkCad - Model Railroad CAD + * Copyright (C) 2005 Dave Bullis + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +#include "track.h" +#include "shrtpath.h" + +EXPORT int log_shortPath; +static int log_shortPathInitted; + +/************************************************************************** + * + * Dijkstra's shortest path + * + **************************************************************************/ + +typedef enum { Unknown, Working, Final } pathState_e; +typedef struct { + pathState_e state; + DIST_T dist; /* Distance from root to entry */ + track_p contTrk; /* continuation */ + EPINX_T contEP; + int inxBack; /* Previous node on shortest path */ + int inxTracks; /* List of tracks along this path */ + int numTracks; + } pathNode_t, *pathNode_p; + +static dynArr_t pathNode_da; +#define pathNode(N) DYNARR_N( pathNode_t, pathNode_da, N ) +typedef struct { + track_p trk; + EPINX_T ep1, ep2; + DIST_T dist; + } trackep_t, *trackep_p; +static dynArr_t trackep_da; +#define trackep(N) DYNARR_N( trackep_t, trackep_da, N ) + +static track_p shortPathTrk0, shortPathTrk1; +static EPINX_T shortPathEP0, shortPathEP1; + + +static int DoShortPathFunc( shortestPathFunc_p func, char * title, SPTF_CMD cmd, track_p trk, EPINX_T ep1, EPINX_T ep2, DIST_T dist, void * data ) +{ + int rc; +LOG( log_shortPath, 4, ( " %s: T%d:%d.%d D:%0.3f ", title, trk?GetTrkIndex(trk):-1, ep1, ep2, dist ) ) + rc = func( cmd, trk, ep1, ep2, dist, data ); +LOG( log_shortPath, 4, ( "-> %d\n", rc ) ) + return rc; +} + +static void DumpPaths( int pinx ) +{ + pathNode_p pPath; + trackep_p pTrackep; + int tinx; + + lprintf(" Current = %d\n", pinx ); + for (pinx=0; pinxstate==Unknown?'U':pPath->state==Working?'W':pPath->state==Final?'F':'?', + (pPath->contTrk?GetTrkIndex(pPath->contTrk):-1), + pPath->contEP, + pPath->dist, + pPath->inxTracks, + pPath->numTracks ); + if (pPath->inxBack>=0) { + lprintf(" B%d", pPath->inxBack ); + } + lprintf("\n "); + for (tinx=0; tinxnumTracks; tinx++) { + pTrackep = &trackep(pPath->inxTracks+tinx); + lprintf( " T%d:%d-%d=%0.1f", GetTrkIndex(pTrackep->trk), pTrackep->ep1, pTrackep->ep2, pTrackep->dist ); + } + lprintf("\n"); + } +} + + +static void AddTracksToPath( + int inxCurr, + shortestPathFunc_p func, + void * data ) +{ + pathNode_p pPath; + int tinx; + trackep_p pTrackep; + + while (inxCurr>=0) { + pPath = &pathNode(inxCurr); + for (tinx=pPath->numTracks-1;tinx>=0;tinx--) { + pTrackep = &trackep(pPath->inxTracks+tinx); + DoShortPathFunc( func, "ADDTRK", SPTC_ADD_TRK, pTrackep->trk, pTrackep->ep1, pTrackep->ep2, pTrackep->dist, data ); + } + inxCurr = pPath->inxBack; + } +} + + +static void AddTrackToNode( + track_p trk, + EPINX_T ep1, + EPINX_T ep2, + DIST_T dist) +{ + DYNARR_APPEND( trackep_t, trackep_da, 10 ); + trackep(trackep_da.cnt-1).trk = trk; + trackep(trackep_da.cnt-1).ep1 = ep1; + trackep(trackep_da.cnt-1).ep2 = ep2; + trackep(trackep_da.cnt-1).dist = dist; +} + + +static BOOL_T AddPath( + int inxCurr, + track_p trk0, + EPINX_T ep1, + EPINX_T ep2, + DIST_T dist, + shortestPathFunc_p func, + void * data ) +{ + EPINX_T epN; + track_p trk=trk0, trkN; + EPINX_T epCnt; + pathNode_p pNode; + int startTrack; + char * msg=NULL; + +LOG( log_shortPath, 2, ( " AddPath( T%d:%d.%d D=%0.3f B%d ) -> \n", GetTrkIndex(trk), ep1, ep2, dist, inxCurr ) ) + startTrack = trackep_da.cnt; + while (1) { + if ( ep2>=0 ) { + AddTrackToNode( trk, ep1, ep2, dist ); + dist += GetTrkLength( trk, ep1, -1 ) + GetTrkLength( trk, ep2, -1 ); + if ( DoShortPathFunc( func, "MATCH", SPTC_MATCH, trk, ep2, ep1, dist, data ) ) { + trk = NULL; + ep1 = -1; + msg = ""; + goto makeNode; + } + trkN = GetTrkEndTrk(trk,ep2); + if ( trkN == NULL ) { + /* dead end */ + msg = "dead end"; + goto skipNode; + } + if ( DoShortPathFunc( func, "IGNORE", SPTC_IGNNXTTRK, trk, ep2, ep1, dist, data ) ) { + msg = "ignore end"; + goto skipNode; + } + ep1 = GetEndPtConnectedToMe( trkN, trk ); + trk = trkN; + if ( (trk==shortPathTrk0 && ep1==shortPathEP0) || (trk==shortPathTrk1 && ep1==shortPathEP1) ) { + msg = "wrap around"; + goto skipNode; + } + } + epCnt = GetTrkEndPtCnt(trk); + if ( epCnt < 2 ) { + msg = "bumper track"; + goto skipNode; + } + if ( epCnt > 2 ) { + if ( (epN=DoShortPathFunc( func, "MATCHANY", SPTC_MATCHANY, trk, ep1, -1, dist, data )) >= 0 ) { + /* special match */ + /*dist += GetTrkLength( trk, ep1, epN );*/ + AddTrackToNode( trk, ep1, epN, dist ); + trk = NULL; + ep1 = -1; + msg = "ANY"; + } + goto makeNode; + } + ep2 = 1-ep1; + } + +makeNode: +if ( trk ) { +LOG( log_shortPath, 2, ( " -> FORK: [%d] T%d:%d", pathNode_da.cnt, GetTrkIndex(trk), ep1 ) ) +} else { +LOG( log_shortPath, 2, ( " -> MATCH%s: [%d]", msg, pathNode_da.cnt ) ) +} +LOG( log_shortPath, 2, ( " t%d D=%0.3f\n", startTrack, dist ) ) + + DYNARR_APPEND( pathNode_t, pathNode_da, 10 ); + pNode = &pathNode(pathNode_da.cnt-1); + pNode->state = Working; + pNode->dist = dist; + pNode->contTrk = trk; + pNode->contEP = ep1; + pNode->inxBack = inxCurr; + pNode->inxTracks = startTrack; + pNode->numTracks = trackep_da.cnt-startTrack; + if ( trk ) + SetTrkBits( trk, TB_SHRTPATH ); + return TRUE; + +skipNode: +LOG( log_shortPath, 2, ( " -> FAIL: %s @ T%d:%d.%d\n", msg, GetTrkIndex(trk), ep1, ep2 ) ) + trackep_da.cnt = startTrack; + return FALSE; +} + + + +int FindShortestPath( + track_p trkN, + EPINX_T epN, + BOOL_T bidirectional, + shortestPathFunc_p func, + void * data ) +{ + int inxCurr = 0; + pathNode_p pCurr; + pathNode_p pNext; + int pinx=0; + DIST_T minDist; + int count; + int rc = 0; + EPINX_T ep2, epCnt, ep3; + static dynArr_t ep_da; + #define ep(N) DYNARR_N( pathNode_p, ep_da, N ) + + DYNARR_RESET( pathNode_t, pathNode_da ); + DYNARR_RESET( trackep_t, trackep_da ); + count = 0; + + if ( !log_shortPathInitted ) { + log_shortPath = LogFindIndex( "shortPath" ); + log_shortPathInitted = TRUE; + } + +LOG( log_shortPath, 1, ( "FindShortestPath( T%d:%d, %s, ... )\n", GetTrkIndex(trkN), epN, bidirectional?"bidir":"unidir" ) ) + ClrAllTrkBits( TB_SHRTPATH ); + /* Note: trkN:epN is not tested for MATCH */ + shortPathTrk0 = trkN; + shortPathEP0 = epN; + shortPathTrk1 = GetTrkEndTrk( trkN, epN ); + if ( shortPathTrk1 != NULL ) + shortPathEP1 = GetEndPtConnectedToMe( shortPathTrk1, shortPathTrk0 ); + AddPath( -1, shortPathTrk0, shortPathEP0, -1, 0.0, func, data ); + if ( bidirectional && shortPathTrk1 != NULL ) + AddPath( -1, shortPathTrk1, shortPathEP1, -1, 0.0, func, data ); + + while (1) { + InfoMessage( "%d", ++count ); + + /* select next final node */ + minDist = 0.0; + inxCurr = -1; + for (pinx=0; pinxstate == Working && + (inxCurr < 0 || pNext->dist < minDist) ) { + minDist = pathNode(pinx).dist; + inxCurr = pinx; + } + } + if ( inxCurr < 0 ) + break; +if (log_shortPath>=4) DumpPaths(inxCurr); + pCurr = &pathNode(inxCurr); + pCurr->state = Final; + if ( pCurr->contTrk == NULL ) { + if ( !DoShortPathFunc( func, "VALID", SPTC_VALID, trackep(pCurr->inxTracks+pCurr->numTracks-1).trk, trackep(pCurr->inxTracks+pCurr->numTracks-1).ep2, -1, 0.0, data ) ) + continue; + AddTracksToPath( inxCurr, func, data ); + rc++; + if ( DoShortPathFunc( func, "TERMINATE", SPTC_TERMINATE, trackep(pCurr->inxTracks+pCurr->numTracks-1).trk, trackep(pCurr->inxTracks+pCurr->numTracks-1).ep2, -1, 0.0, data ) ) + break; + } else { + epCnt = GetTrkEndPtCnt(pCurr->contTrk); + DYNARR_SET( pathNode_p, ep_da, epCnt ); + memset( ep_da.ptr, 0, epCnt * sizeof pNext ); + if ( (GetTrkBits(pCurr->contTrk) & TB_SHRTPATH) ) { + for ( pinx=0; pinxcontTrk == pCurr->contTrk ) { + ep(pNext->contEP) = pNext; + } + } + } + for ( ep2=0; ep2contEP == ep2 ) continue; + /* no route to ep */ + if ( DoShortPathFunc( func, "IGNORE", SPTC_IGNNXTTRK, pCurr->contTrk, pCurr->contEP, ep2, pCurr->dist, data ) ) continue; + /* somebody got here first */ + if ( ep(ep2) ) continue; + /* there is already a path out via ep2 */ + for ( ep3=0; ep3contEP || ep3==ep2 ) continue; + if ( ep(ep3) == NULL ) continue; + if ( DoShortPathFunc( func, "IGNORE", SPTC_IGNNXTTRK, pCurr->contTrk, ep2, ep3, pCurr->dist, data ) ) continue; + if ( ep(ep3)->state == Final ) break; + } + if ( ep3 < epCnt ) continue; + AddPath( inxCurr, pCurr->contTrk, pCurr->contEP, ep2, pCurr->dist, func, data ); + } + } + } + +if (log_shortPath>=1) DumpPaths(inxCurr); + ClrAllTrkBits( TB_SHRTPATH ); + return rc; +} + + -- cgit v1.2.3