Initial import of xtrkcad version 1:4.0.2-2

1 files changed, 330 insertions, 0 deletions

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; pinx<pathNode_da.cnt; pinx++) {
+		pPath = &pathNode(pinx);
+		lprintf( "      %3d: S%c T%d:%d D%0.3f T%d:%d",
+				pinx,
+				pPath->state==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; tinx<pPath->numTracks; 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; pinx<pathNode_da.cnt; pinx++) {
+			 pNext = &pathNode(pinx);
+			 if (pNext->state == 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; pinx<pathNode_da.cnt; pinx++ ) {
+					pNext = &pathNode(pinx);
+					if ( pNext->contTrk == pCurr->contTrk ) {
+						ep(pNext->contEP) = pNext;
+					}
+				}
+			}
+			for ( ep2=0; ep2<epCnt; ep2++ ) {
+				pCurr = &pathNode(inxCurr);
+
+				/* don't point back at myself */
+				if ( pCurr->contEP == 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; ep3<epCnt; ep3++ ) {
+					if ( ep3==pCurr->contEP || 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;
+}
+
+