Full search only for t-1 to t+3 and best edge paths after that

matdb/mat/paths.cpp

@@ -97,18 +97,29 @@ inline CSourcePath path_append_front(const CSourcePath& inPath, CellID idx)
 }
 
 // this is the NEW workhorse!
-CellID bidirDepthFirstSearch(threadDB* thread_db, CSourcePath partialPath, CellID bestLGIdx = CellID(-1))
+CellID bidirDepthFirstSearch(threadDB* thread_db, CSourcePath partialPath, int t, CellID bestLGIdx = CellID(-1))
 {
 	// Get backward-looking edges from start of path
 	int lEdgeCount = 0;
 	if ( partialPath.startIdx > 0 )
 	{
+		int startCID = partialPath.indices[partialPath.startIdx];
+		int dtLeft = t - gHulls[getHullIdx(startCID)].get_time();
+
 		lEdgeCount = gccInEdges.count(partialPath.peek_front());
 		if ( lEdgeCount > 0 )
 		{
+			int minID = -1;
+			double minDist = dbltype::infinity();
 			for ( CellID leftID : gccInEdges[partialPath.peek_front()] )
 			{
-				bestLGIdx = bidirDepthFirstSearch(thread_db, path_append_front(partialPath, leftID), bestLGIdx);
+				if ( dtLeft <= gFullSearchLeft )
+					bestLGIdx = bidirDepthFirstSearch(thread_db, path_append_front(partialPath, leftID), t, bestLGIdx);
+				else if ( minDist > getHullDistance(thread_db, leftID, startCID) )
+				{
+					minID = leftID;
+					minDist = getHullDistance(thread_db, leftID, startCID);
+				}
 			}
 		}
 	}
@@ -117,12 +128,23 @@ CellID bidirDepthFirstSearch(threadDB* thread_db, CSourcePath partialPath, CellI
 	int rEdgeCount = 0;
 	if ( partialPath.endIdx < partialPath.indices.size() - 1 )
 	{
+		int endCID = partialPath.indices[partialPath.endIdx];
+		int dtRight = gHulls[getHullIdx(endCID)].get_time() - t;
+
 		rEdgeCount = gccOutEdges.count(partialPath.peek_back());
 		if ( rEdgeCount > 0 )
 		{
+			int minID = -1;
+			double minDist = dbltype::infinity();
 			for ( CellID rightID : gccOutEdges[partialPath.peek_back()] )
 			{
+				if ( dtRight <= gFullSearchRight )
 					bestLGIdx = bidirDepthFirstSearch(thread_db, path_append_back(partialPath, rightID), bestLGIdx);
+				else if ( minDist > getHullDistance(thread_db, rightID, endCID) )
+				{
+					minID = rightID;
+					minDist = getHullDistance(thread_db, rightID, endCID);
+				}
 			}
 		}
 	}
@@ -138,7 +160,7 @@ CellID bidirDepthFirstSearch(threadDB* thread_db, CSourcePath partialPath, CellI
 
 // bestEdges is returned as a pair <i1,i2> i1 is best incoming path for i2, i2 is the next hull we start bidirectional searc from
 // step is so each thread has a worklist based on stride step into the list of hulls in the t+1 frame
-void threadedBestPaths(threadDB* thread_db, std::vector<std::pair<CellID,CellID>>& bestEdges, int offset, int step)
+void threadedBestPaths(threadDB* thread_db, std::vector<std::pair<CellID,CellID>>& bestEdges, int t, int offset, int step)
 {
 	int rc;
 	
@@ -147,7 +169,7 @@ void threadedBestPaths(threadDB* thread_db, std::vector<std::pair<CellID,CellID>
 		CellID nextGIdx = bestEdges[i].second;
 
 		CSourcePath path(nextGIdx);
-		bestEdges[i].first = bidirDepthFirstSearch(thread_db, path);
+		bestEdges[i].first = bidirDepthFirstSearch(thread_db, path, t);
 	}
 }
 
@@ -221,7 +243,7 @@ void BuildBestPaths(std::map<CellID,CellID>& bestInEdges, int t)
 		if ( i >= bestEdges.size() )
 			break;
 
-		workers[i] = std::unique_ptr<std::thread>(new std::thread(threadedBestPaths, &gdbPool[i], std::ref(bestEdges), i, gnThreads));
+		workers[i] = std::unique_ptr<std::thread>(new std::thread(threadedBestPaths, &gdbPool[i], std::ref(bestEdges), t, i, gnThreads));
 	}
 
 	// Wait for threads to finish

matdb/matdb/matdb.cpp

@@ -20,6 +20,9 @@ std::map<CellID,CellID> gOverRideTrackIDs;
 char gszSqlCmd[1024];
 char *gszDB;
 
+int gFullSearchLeft = 0;
+int gFullSearchRight = 2;
+
 std::map<std::pair<CellID,CellID>,double> gccDistMap;
 std::unordered_map<CellID,std::set<CellID>> gccOutEdges;
 std::unordered_map<CellID,std::set<CellID>> gccInEdges;
@@ -119,7 +122,7 @@ static int getCCdistCallback(void* pvnull, int argc, char** argv, char** azColNa
 	gccDistMap[{cellID_src,cellID_dst}] = cost;
 
 	// MRW - Don't add edges for hulls outside the window (weren't read-in by readCells)
-	if ( gCellHullMap.count(cellID_src) > 0 || gCellHullMap.count(cellID_dst) > 0 )
+	if ( gCellHullMap.count(cellID_src) > 0 && gCellHullMap.count(cellID_dst) > 0 )
 	{
 		gccOutEdges[cellID_src].insert(cellID_dst);
 		gccInEdges[cellID_dst].insert(cellID_src);

matdb/matdb/matdb.h

@@ -31,6 +31,9 @@ extern sqlite3 *gdb;
 extern char* gszDB; // database full path
 extern char gszSqlCmd[1024];
 
+extern int gFullSearchLeft;
+extern int gFullSearchRight;
+
 extern std::unordered_map<CellID,std::set<CellID>> gccOutEdges;
 extern std::unordered_map<CellID,std::set<CellID>> gccInEdges;