PathPlan* PathingGraph::FindPath(const Vec3& startPoint, const Vec3& endPoint)
{
// Find the closest nodes to the start and end points. There really should be some ray-casting
// to ensure that we can actually make it to the closest node, but this is good enough for now.
PathingNode* pStart = FindClosestNode(startPoint);
PathingNode* pGoal = FindClosestNode(endPoint);
return FindPath(pStart,pGoal);
}
/// Returns the closest reachable node to the coords (x,y).
/// First checks if the node in this cell is reachable.
/// If not, checks the reachability of the surrounding 8 nodes.
/// If still no luck, just returns the closest node to (x,y)
SearchNode_t *BotNodes::GetClosestReachableNode(fixed_t x, fixed_t y)
{
SearchNode_t *temp;
int nx = x2PosX(x);
int ny = y2PosY(y);
if ((nx >= 0) && (nx < xSize) && (ny >= 0) && (ny < ySize))
{
temp = botNodeArray[nx][ny];
if (temp && DirectlyReachable(NULL, x, y, temp->mx, temp->my))
return temp;
}
for (int i = nx-1; i <= nx+1; i++)
for (int j = ny-1; j <= ny+1; j++)
if ((i >= 0) && (i < xSize) && (j >= 0) && (j < ySize))
{
temp = botNodeArray[i][j];
if (temp && DirectlyReachable(NULL, x, y, temp->mx, temp->my))
return temp;
}
return FindClosestNode(x, y);
}
void CTLiteDoc::OnToolsGpsmapmatching()
{
// create a network
// for each link, define the distance cost
// find the shortest path
// step 1: find out the origin zone and destination zone
std::list<DTAVehicle*>::iterator iVehicle;
std::list<DTALink*>::iterator iLink;
for (iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++) //m_LinkSet ?????????
{
(*iLink)->m_OriginalLength = (*iLink)->m_Length ; // keep the original link length
}
for (std::list<DTALink*>::iterator iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++)
{
(*iLink)->m_DisplayLinkID = -1;
}
// for each vehicle
//each vehicle GPS data are stored in the list called m_VehicleSet
for (iVehicle = m_VehicleSet.begin(); iVehicle != m_VehicleSet.end(); iVehicle++)
{
DTAVehicle* pVehicle = (*iVehicle);
if(m_pNetwork ==NULL) // we only build the network once
{
m_pNetwork = new DTANetworkForSP(m_NodeSet.size(), m_LinkSet.size(), 1, 1, m_AdjLinkSize); // network instance for single processor in multi-thread environment
}
if(pVehicle->m_GPSLocationVector.size()>=2)
{
//find OD nodes for each vehicle
m_OriginNodeID = FindClosestNode(pVehicle->m_GPSLocationVector[0]);
m_DestinationNodeID = FindClosestNode(pVehicle->m_GPSLocationVector[pVehicle->m_GPSLocationVector.size()-1]);
// set new length for links
std::list<DTALink*>::iterator iLink;
int max_link_id = 1;
for (iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++) // for each link in this network
{
double min_distance = 999999;
GDPoint pfrom = (*iLink)->m_FromPoint;
GDPoint pto = (*iLink)->m_ToPoint;
for(int i = 0; i < pVehicle->m_GPSLocationVector.size(); i++ )
{
float distance = g_GetPoint2LineDistance(pVehicle->m_GPSLocationVector[i], pfrom, pto); // go through each GPS location point
if(distance < min_distance)
{
min_distance = distance;
}
}
// determine the distance to GPS traces as length
(*iLink)->m_Length = min_distance; // keep the original link length
}
m_RandomRoutingCoefficient = 0; // no random cost
//build physical network
m_pNetwork->BuildPhysicalNetwork(&m_NodeSet, &m_LinkSet, m_RandomRoutingCoefficient, false);
int NodeNodeSum = 0;
int PathLinkList[MAX_NODE_SIZE_IN_A_PATH];//save link ids in a path(a path means the trajactory of a vehicle obtained from GPS)
float TotalCost;
bool distance_flag = true;
int NodeSize;
//get the total number of the nodes in the network we build,and give the link ids to PathLinkList
NodeSize= m_pNetwork->SimplifiedTDLabelCorrecting_DoubleQueue(m_OriginNodeID, 0, m_DestinationNodeID, 1, 10.0f,PathLinkList,TotalCost, distance_flag, false, false,m_RandomRoutingCoefficient); // Pointer to previous node (node)
//update m_PathDisplayList
if(NodeSize <= 1)
{
TRACE("error");
}
DTAPath path_element;
path_element.Init (NodeSize-1,g_Simulation_Time_Horizon);
std::vector<int> link_vector;// a vector to save link ids
for (int i=0 ; i < NodeSize-1; i++)
{
path_element.m_LinkVector [i] = PathLinkList[i] ; //starting from m_NodeSizeSP-2, to 0
link_vector.push_back (PathLinkList[i]);// save the link ids
DTALink* pLink = m_LinkNotoLinkMap[PathLinkList[i]];
if(pLink!=NULL)
{
path_element.m_Distance += m_LinkNotoLinkMap[PathLinkList[i]]->m_Length ;
pLink->m_bIncludedBySelectedPath = true; // mark this link as a link along the selected path
if(i==0) // first link
{
path_element.m_TravelTime = pLink->GetSimulatedTravelTime(g_Simulation_Time_Stamp);
}
else
{
path_element.m_TravelTime = path_element.m_TravelTime + pLink->GetSimulatedTravelTime(path_element.m_TravelTime);
}
}
}
HighlightPath(link_vector,m_PathDisplayList.size()); //highlight the path for each vehicle in different color.
m_PathDisplayList.push_back(path_element);
// do not know how to use link_id to find from and to nodes of link
for(int i=0;i<link_vector.size();i++)
{
int index=0;
int link_id=link_vector[i]; //link id
for (iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++) // for each link in this network
{
if(index==link_id)// find from and to nodes for the link
{
GDPoint LinkFromNode = (*iLink)->m_FromPoint;
GDPoint LinkToNode = (*iLink)->m_ToPoint;
// fprintf(st_meta_data, "%d,%d,%f,%f,%f,%f\n", vehicle_id,link_id,LinkFromNode.x,LinkFromNode.y,LinkToNode.x,LinkToNode.y);
}
index +=1;
}
}
// vehicle_id +=1;// update vehicle_id
}
}
//fclose(st_meta_data);
}
PathPlan* PathingGraph::FindPath(PathingNode* pStartNode, const Vec3& endPoint)
{
PathingNode* pGoal = FindClosestNode(endPoint);
return FindPath(pStartNode,pGoal);
}
PathPlan* PathingGraph::FindPath(const Vec3& startPoint, PathingNode* pGoalNode)
{
PathingNode* pStart = FindClosestNode(startPoint);
return FindPath(pStart,pGoalNode);
}
int main(int argc,char** argv)
{
Nodes *OSMNodes;
Segments *OSMSegments;
Ways *OSMWays;
Relations *OSMRelations;
Results *results[NWAYPOINTS+1]={NULL};
int point_used[NWAYPOINTS+1]={0};
double point_lon[NWAYPOINTS+1],point_lat[NWAYPOINTS+1];
index_t point_node[NWAYPOINTS+1]={NO_NODE};
double heading=-999;
int help_profile=0,help_profile_xml=0,help_profile_json=0,help_profile_pl=0;
char *dirname=NULL,*prefix=NULL;
char *profiles=NULL,*profilename=NULL;
char *translations=NULL,*language=NULL;
int exactnodes=0,reverse=0,loop=0;
Transport transport=Transport_None;
Profile *profile=NULL;
Translation *translation=NULL;
index_t start_node,finish_node=NO_NODE;
index_t join_segment=NO_SEGMENT;
int arg,nresults=0;
waypoint_t start_waypoint,finish_waypoint=NO_WAYPOINT;
waypoint_t first_waypoint=NWAYPOINTS,last_waypoint=1,waypoint;
int inc_dec_waypoint=1;
printf_program_start();
/* Parse the command line arguments */
if(argc<2)
print_usage(0,NULL,NULL);
/* Get the non-routing, general program options */
for(arg=1;arg<argc;arg++)
{
if(!strcmp(argv[arg],"--version"))
print_usage(-1,NULL,NULL);
else if(!strcmp(argv[arg],"--help"))
print_usage(1,NULL,NULL);
else if(!strcmp(argv[arg],"--help-profile"))
help_profile=1;
else if(!strcmp(argv[arg],"--help-profile-xml"))
help_profile_xml=1;
else if(!strcmp(argv[arg],"--help-profile-json"))
help_profile_json=1;
else if(!strcmp(argv[arg],"--help-profile-perl"))
help_profile_pl=1;
else if(!strncmp(argv[arg],"--dir=",6))
dirname=&argv[arg][6];
else if(!strncmp(argv[arg],"--prefix=",9))
prefix=&argv[arg][9];
else if(!strncmp(argv[arg],"--profiles=",11))
profiles=&argv[arg][11];
else if(!strncmp(argv[arg],"--translations=",15))
translations=&argv[arg][15];
else if(!strcmp(argv[arg],"--exact-nodes-only"))
exactnodes=1;
else if(!strncmp(argv[arg],"--reverse",9))
{
if(argv[arg][9]=='=')
reverse=atoi(&argv[arg][10]);
else
reverse=1;
}
else if(!strncmp(argv[arg],"--loop",6))
{
if(argv[arg][6]=='=')
loop=atoi(&argv[arg][7]);
else
loop=1;
}
else if(!strcmp(argv[arg],"--quiet"))
option_quiet=1;
else if(!strcmp(argv[arg],"--loggable"))
option_loggable=1;
else if(!strcmp(argv[arg],"--logtime"))
option_logtime=2;
else if(!strcmp(argv[arg],"--logmemory"))
option_logmemory=1;
else if(!strcmp(argv[arg],"--output-html"))
option_file_html=1;
else if(!strcmp(argv[arg],"--output-gpx-track"))
option_file_gpx_track=1;
else if(!strcmp(argv[arg],"--output-gpx-route"))
option_file_gpx_route=1;
else if(!strcmp(argv[arg],"--output-text"))
option_file_text=1;
else if(!strcmp(argv[arg],"--output-text-all"))
option_file_text_all=1;
else if(!strcmp(argv[arg],"--output-none"))
option_file_none=1;
else if(!strcmp(argv[arg],"--output-stdout"))
{ option_file_stdout=1; option_quiet=1; }
else if(!strncmp(argv[arg],"--profile=",10))
profilename=&argv[arg][10];
else if(!strncmp(argv[arg],"--language=",11))
language=&argv[arg][11];
else if(!strcmp(argv[arg],"--shortest"))
option_quickest=0;
else if(!strcmp(argv[arg],"--quickest"))
option_quickest=1;
else if(!strncmp(argv[arg],"--lon",5) && isdigit(argv[arg][5]))
{
int point;
char *p=&argv[arg][6];
while(isdigit(*p)) p++;
if(*p++!='=')
print_usage(0,argv[arg],NULL);
point=atoi(&argv[arg][5]);
if(point>NWAYPOINTS || point_used[point]&1)
print_usage(0,argv[arg],NULL);
point_lon[point]=degrees_to_radians(atof(p));
point_used[point]+=1;
if(point<first_waypoint)
first_waypoint=point;
if(point>last_waypoint)
last_waypoint=point;
}
else if(!strncmp(argv[arg],"--lat",5) && isdigit(argv[arg][5]))
{
int point;
char *p=&argv[arg][6];
while(isdigit(*p)) p++;
if(*p++!='=')
print_usage(0,argv[arg],NULL);
point=atoi(&argv[arg][5]);
if(point>NWAYPOINTS || point_used[point]&2)
print_usage(0,argv[arg],NULL);
point_lat[point]=degrees_to_radians(atof(p));
point_used[point]+=2;
if(point<first_waypoint)
first_waypoint=point;
if(point>last_waypoint)
last_waypoint=point;
}
else if(!strncmp(argv[arg],"--heading=",10))
{
double h=atof(&argv[arg][10]);
if(h>=-360 && h<=360)
{
heading=h;
if(heading<0) heading+=360;
}
}
else if(!strncmp(argv[arg],"--transport=",12))
{
transport=TransportType(&argv[arg][12]);
if(transport==Transport_None)
print_usage(0,argv[arg],NULL);
}
else
continue;
argv[arg]=NULL;
}
/* Check the specified command line options */
if(option_file_stdout && (option_file_html+option_file_gpx_track+option_file_gpx_route+option_file_text+option_file_text_all)!=1)
{
fprintf(stderr,"Error: The '--output-stdout' option requires exactly one other output option (but not '--output-none').\n");
exit(EXIT_FAILURE);
}
if(option_file_html==0 && option_file_gpx_track==0 && option_file_gpx_route==0 && option_file_text==0 && option_file_text_all==0 && option_file_none==0)
option_file_html=option_file_gpx_track=option_file_gpx_route=option_file_text=option_file_text_all=1;
/* Load in the selected profiles */
if(transport==Transport_None)
transport=Transport_Motorcar;
if(profiles)
{
if(!ExistsFile(profiles))
{
fprintf(stderr,"Error: The '--profiles' option specifies a file '%s' that does not exist.\n",profiles);
exit(EXIT_FAILURE);
}
}
else
{
profiles=FileName(dirname,prefix,"profiles.xml");
if(!ExistsFile(profiles))
{
char *defaultprofiles=FileName(ROUTINO_DATADIR,NULL,"profiles.xml");
if(!ExistsFile(defaultprofiles))
{
fprintf(stderr,"Error: The '--profiles' option was not used and the files '%s' and '%s' do not exist.\n",profiles,defaultprofiles);
exit(EXIT_FAILURE);
}
free(profiles);
profiles=defaultprofiles;
}
}
if(!profilename)
profilename=(char*)TransportName(transport);
if(ParseXMLProfiles(profiles,profilename,(help_profile_xml|help_profile_json|help_profile_pl)))
{
fprintf(stderr,"Error: Cannot read the profiles in the file '%s'.\n",profiles);
exit(EXIT_FAILURE);
}
profile=GetProfile(profilename);
if(!profile)
{
fprintf(stderr,"Error: Cannot find a profile called '%s' in the file '%s'.\n",profilename,profiles);
profile=(Profile*)calloc(1,sizeof(Profile));
profile->transport=transport;
}
/* Parse the other command line arguments that modify the profile */
for(arg=1;arg<argc;arg++)
{
if(!argv[arg])
continue;
else if(!strncmp(argv[arg],"--highway-",10))
{
Highway highway;
char *equal=strchr(argv[arg],'=');
char *string;
double p;
if(!equal)
print_usage(0,argv[arg],NULL);
string=strcpy((char*)malloc(strlen(argv[arg])),argv[arg]+10);
string[equal-argv[arg]-10]=0;
highway=HighwayType(string);
if(highway==Highway_None)
print_usage(0,argv[arg],NULL);
p=atof(equal+1);
if(p<0 || p>100)
print_usage(0,argv[arg],NULL);
profile->highway[highway]=(score_t)(p/100);
free(string);
}
else if(!strncmp(argv[arg],"--speed-",8))
{
Highway highway;
char *equal=strchr(argv[arg],'=');
char *string;
double s;
if(!equal)
print_usage(0,argv[arg],NULL);
string=strcpy((char*)malloc(strlen(argv[arg])),argv[arg]+8);
string[equal-argv[arg]-8]=0;
highway=HighwayType(string);
if(highway==Highway_None)
print_usage(0,argv[arg],NULL);
s=atof(equal+1);
if(s<0)
print_usage(0,argv[arg],NULL);
profile->speed[highway]=kph_to_speed(s);
free(string);
}
else if(!strncmp(argv[arg],"--property-",11))
{
Property property;
char *equal=strchr(argv[arg],'=');
char *string;
double p;
if(!equal)
print_usage(0,argv[arg],NULL);
string=strcpy((char*)malloc(strlen(argv[arg])),argv[arg]+11);
string[equal-argv[arg]-11]=0;
property=PropertyType(string);
if(property==Property_None)
print_usage(0,argv[arg],NULL);
p=atof(equal+1);
if(p<0 || p>100)
print_usage(0,argv[arg],NULL);
profile->props[property]=(score_t)(p/100);
free(string);
}
else if(!strncmp(argv[arg],"--oneway=",9))
profile->oneway=!!atoi(&argv[arg][9]);
else if(!strncmp(argv[arg],"--turns=",8))
profile->turns=!!atoi(&argv[arg][8]);
else if(!strncmp(argv[arg],"--weight=",9))
profile->weight=tonnes_to_weight(atof(&argv[arg][9]));
else if(!strncmp(argv[arg],"--height=",9))
profile->height=metres_to_height(atof(&argv[arg][9]));
else if(!strncmp(argv[arg],"--width=",8))
profile->width=metres_to_width(atof(&argv[arg][8]));
else if(!strncmp(argv[arg],"--length=",9))
profile->length=metres_to_length(atof(&argv[arg][9]));
else
print_usage(0,argv[arg],NULL);
}
/* Print one of the profiles if requested */
if(help_profile)
{
PrintProfile(profile);
exit(EXIT_SUCCESS);
}
else if(help_profile_xml)
{
PrintProfilesXML();
exit(EXIT_SUCCESS);
}
else if(help_profile_json)
{
PrintProfilesJSON();
exit(EXIT_SUCCESS);
}
else if(help_profile_pl)
{
PrintProfilesPerl();
exit(EXIT_SUCCESS);
}
/* Load in the selected translation */
if(option_file_html || option_file_gpx_route || option_file_gpx_track || option_file_text || option_file_text_all)
{
if(translations)
{
if(!ExistsFile(translations))
{
fprintf(stderr,"Error: The '--translations' option specifies a file '%s' that does not exist.\n",translations);
exit(EXIT_FAILURE);
}
}
else
{
translations=FileName(dirname,prefix,"translations.xml");
if(!ExistsFile(translations))
{
char *defaulttranslations=FileName(ROUTINO_DATADIR,NULL,"translations.xml");
if(!ExistsFile(defaulttranslations))
{
fprintf(stderr,"Error: The '--translations' option was not used and the files '%s' and '%s' do not exist.\n",translations,defaulttranslations);
exit(EXIT_FAILURE);
}
free(translations);
translations=defaulttranslations;
}
}
if(ParseXMLTranslations(translations,language,0))
{
fprintf(stderr,"Error: Cannot read the translations in the file '%s'.\n",translations);
exit(EXIT_FAILURE);
}
if(language)
{
translation=GetTranslation(language);
if(!translation)
{
fprintf(stderr,"Warning: Cannot find a translation called '%s' in the file '%s'.\n",language,translations);
exit(EXIT_FAILURE);
}
}
else
{
translation=GetTranslation("");
if(!translation)
{
fprintf(stderr,"Warning: No translations in '%s'.\n",translations);
exit(EXIT_FAILURE);
}
}
}
/* Check the waypoints are valid */
for(waypoint=1;waypoint<=NWAYPOINTS;waypoint++)
if(point_used[waypoint]==1 || point_used[waypoint]==2)
print_usage(0,NULL,"All waypoints must have latitude and longitude.");
if(first_waypoint>=last_waypoint)
print_usage(0,NULL,"At least two waypoints must be specified.");
/* Load in the data - Note: No error checking because Load*List() will call exit() in case of an error. */
if(!option_quiet)
printf_first("Loading Files:");
OSMNodes=LoadNodeList(FileName(dirname,prefix,"nodes.mem"));
OSMSegments=LoadSegmentList(FileName(dirname,prefix,"segments.mem"));
OSMWays=LoadWayList(FileName(dirname,prefix,"ways.mem"));
OSMRelations=LoadRelationList(FileName(dirname,prefix,"relations.mem"));
if(!option_quiet)
printf_last("Loaded Files: nodes, segments, ways & relations");
/* Check the profile is valid for use with this database */
if(UpdateProfile(profile,OSMWays))
{
fprintf(stderr,"Error: Profile is invalid or not compatible with database.\n");
exit(EXIT_FAILURE);
}
/* Find all waypoints */
for(waypoint=first_waypoint;waypoint<=last_waypoint;waypoint++)
{
distance_t distmax=km_to_distance(MAXSEARCH);
distance_t distmin;
index_t segment=NO_SEGMENT;
index_t node1,node2,node=NO_NODE;
if(point_used[waypoint]!=3)
continue;
/* Find the closest point */
if(!option_quiet)
printf_first("Finding Closest Point: Waypoint %d",waypoint);
if(exactnodes)
node=FindClosestNode(OSMNodes,OSMSegments,OSMWays,point_lat[waypoint],point_lon[waypoint],distmax,profile,&distmin);
else
{
distance_t dist1,dist2;
segment=FindClosestSegment(OSMNodes,OSMSegments,OSMWays,point_lat[waypoint],point_lon[waypoint],distmax,profile,&distmin,&node1,&node2,&dist1,&dist2);
if(segment!=NO_SEGMENT)
node=CreateFakes(OSMNodes,OSMSegments,waypoint,LookupSegment(OSMSegments,segment,1),node1,node2,dist1,dist2);
}
if(!option_quiet)
printf_last("Found Closest Point: Waypoint %d",waypoint);
if(node==NO_NODE)
{
fprintf(stderr,"Error: Cannot find node close to specified point %d.\n",waypoint);
exit(EXIT_FAILURE);
}
if(!option_quiet)
{
double lat,lon;
if(IsFakeNode(node))
GetFakeLatLong(node,&lat,&lon);
else
GetLatLong(OSMNodes,node,NULL,&lat,&lon);
if(IsFakeNode(node))
printf("Waypoint %d is segment %"Pindex_t" (node %"Pindex_t" -> %"Pindex_t"): %3.6f %4.6f = %2.3f km\n",waypoint,segment,node1,node2,
radians_to_degrees(lon),radians_to_degrees(lat),distance_to_km(distmin));
else
printf("Waypoint %d is node %"Pindex_t": %3.6f %4.6f = %2.3f km\n",waypoint,node,
radians_to_degrees(lon),radians_to_degrees(lat),distance_to_km(distmin));
}
point_node[waypoint]=node;
}
/* Check for reverse direction */
if(reverse)
{
waypoint_t temp;
temp=first_waypoint;
first_waypoint=last_waypoint;
last_waypoint=temp;
inc_dec_waypoint=-1;
}
/* Loop through all pairs of waypoints */
if(loop && reverse)
{
finish_node=point_node[last_waypoint];
finish_waypoint=last_waypoint;
}
for(waypoint=first_waypoint;waypoint!=(last_waypoint+inc_dec_waypoint);waypoint+=inc_dec_waypoint)
{
if(point_used[waypoint]!=3)
continue;
start_node=finish_node;
finish_node=point_node[waypoint];
start_waypoint=finish_waypoint;
finish_waypoint=waypoint;
if(start_node==NO_NODE)
continue;
if(heading!=-999 && join_segment==NO_SEGMENT)
join_segment=FindClosestSegmentHeading(OSMNodes,OSMSegments,OSMWays,start_node,heading,profile);
/* Calculate the route */
if(!option_quiet)
printf("Routing from waypoint %d to waypoint %d\n",start_waypoint,finish_waypoint);
results[nresults]=CalculateRoute(OSMNodes,OSMSegments,OSMWays,OSMRelations,profile,start_node,join_segment,finish_node,start_waypoint,finish_waypoint);
if(!results[nresults])
exit(EXIT_FAILURE);
join_segment=results[nresults]->last_segment;
nresults++;
}
if(loop && !reverse)
{
start_node=finish_node;
finish_node=point_node[first_waypoint];
start_waypoint=finish_waypoint;
finish_waypoint=first_waypoint;
/* Calculate the route */
if(!option_quiet)
printf("Routing from waypoint %d to waypoint %d\n",start_waypoint,finish_waypoint);
results[nresults]=CalculateRoute(OSMNodes,OSMSegments,OSMWays,OSMRelations,profile,start_node,join_segment,finish_node,start_waypoint,finish_waypoint);
if(!results[nresults])
exit(EXIT_FAILURE);
nresults++;
}
if(!option_quiet)
{
printf("Routed OK\n");
fflush(stdout);
}
/* Print out the combined route */
if(!option_quiet)
printf_first("Generating Result Outputs");
if(!option_file_none)
PrintRoute(results,nresults,OSMNodes,OSMSegments,OSMWays,OSMRelations,profile,translation);
if(!option_quiet)
printf_last("Generated Result Outputs");
/* Destroy the remaining results lists and data structures */
#ifdef DEBUG_MEMORY_LEAK
for(waypoint=0;waypoint<nresults;waypoint++)
FreeResultsList(results[waypoint]);
DestroyNodeList(OSMNodes);
DestroySegmentList(OSMSegments);
DestroyWayList(OSMWays);
DestroyRelationList(OSMRelations);
FreeXMLProfiles();
FreeXMLTranslations();
#endif
if(!option_quiet)
printf_program_end();
exit(EXIT_SUCCESS);
}
bool CTLiteDoc::TransitTripMatching()
{
FILE* st;
fopen_s(&st,m_PT_network.m_ProjectDirectory +"input_transit_trip.csv","w");
if(st==NULL)
{
AfxMessageBox("Error: File input_transit_trip.csv cannot be opened.\nIt might be currently used and locked by EXCEL.");
}
if(st!=NULL)
{
fprintf(st,"route_id,trip_id,link_sequence_no,from_node_id,to_node_id,street_name,stop_id,stop_sequence,arrival_time,departure_time\n");
std::list<DTALink*>::iterator iLink;
// pass 0: initialization
for (iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++) // for each link in this network
{
(*iLink)-> m_OriginalLength = (*iLink)->m_Length;
}
int count = 0;
std::map<int, int> RouteMap;
// step 2: assemble data
std::map<int, PT_Trip>::iterator iPT_TripMap;
for ( iPT_TripMap= m_PT_network.m_PT_TripMap.begin() ; iPT_TripMap != m_PT_network.m_PT_TripMap.end(); iPT_TripMap++ )
{
if(m_pNetwork ==NULL) // we only build the network once
{
m_pNetwork = new DTANetworkForSP(m_NodeSet.size(), m_LinkSet.size(), 1, 1, m_AdjLinkSize); // network instance for single processor in multi-thread environment
}
count++;
if(count>=50)
break;
int stop_time_size = (*iPT_TripMap).second .m_PT_StopTimeVector.size();
if(stop_time_size>=2)
{
//find OD nodes for each vehicle
m_OriginNodeID = FindClosestNode((*iPT_TripMap).second .m_PT_StopTimeVector[0].pt);
m_DestinationNodeID = FindClosestNode((*iPT_TripMap).second .m_PT_StopTimeVector[stop_time_size-1].pt);
// set new length for links
if(m_OriginNodeID==-1 || m_DestinationNodeID==-1)
continue;
// pass 0: initialization
for (iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++) // for each link in this network
{
(*iLink)->m_StopTimeRecord.stop_id = -1;
(*iLink)->m_Length = 999999;
}
int max_link_id = 1;
// first pass: link distance
for (iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++) // for each link in this network
{
//// if( m_LinkTypeMap[(*iLink)->m_link_type].IsTransit())
// {
//
// (*iLink)->m_Length = 999999; // // not using light rail link
//
// }else
{
double min_p_to_link_distance = 999999;
GDPoint pfrom = (*iLink)->m_FromPoint;
GDPoint pto = (*iLink)->m_ToPoint;
GDPoint mid_point;
mid_point.x = (pfrom.x + pto.x)/2;
mid_point.y = (pfrom.y + pto.y)/2;
int Intersection_Count = 0;
for(int i = 0; i < stop_time_size; i++ )
{
GDPoint pt0 = (*iPT_TripMap).second .m_PT_StopTimeVector[i].pt;
GDPoint pt1 = (*iPT_TripMap).second .m_PT_StopTimeVector[min(i+1,stop_time_size-1)].pt;
GDPoint pt2 = (*iPT_TripMap).second .m_PT_StopTimeVector[min(i+2,stop_time_size-1)].pt;
float distance;
distance = (g_GetPoint2Point_Distance(pfrom, pt0) + g_GetPoint2Point_Distance(mid_point, pt1) + g_GetPoint2Point_Distance(pto, pt2))/3;
if(distance < min_p_to_link_distance)
{
min_p_to_link_distance = distance;
}
}
// determine the distance to GPS traces as length
double dis_in_feet = min_p_to_link_distance /m_UnitFeet;
(*iLink)->m_Length = dis_in_feet; // keep the original link length
}
}
m_RandomRoutingCoefficient = 0; // no random cost
//build physical network
m_pNetwork->BuildPhysicalNetwork(&m_NodeSet, &m_LinkSet, m_RandomRoutingCoefficient, false);
int NodeNodeSum = 0;
int PathLinkList[MAX_NODE_SIZE_IN_A_PATH];//save link ids in a path(a path means the trajactory of a vehicle obtained from GPS)
float TotalCost;
bool distance_flag = true;
int NodeSize;
//get the total number of the nodes in the network we build,and give the link ids to PathLinkList
NodeSize= m_pNetwork->SimplifiedTDLabelCorrecting_DoubleQueue(m_OriginNodeID, 0, m_DestinationNodeID, 1, 10.0f,PathLinkList,TotalCost, distance_flag, false, false,m_RandomRoutingCoefficient); // Pointer to previous node (node)
//update m_PathDisplayList
if(NodeSize <= 1)
{
TRACE("error");
}
NodeSize = min(NodeSize, MAX_NODE_SIZE_IN_A_PATH);
std::vector<int> link_vector;// a vector to save link ids
for (int i=0 ; i < NodeSize-1; i++)
{
link_vector.push_back (PathLinkList[i]);// save the link ids
DTALink* pLink = m_LinkNotoLinkMap[PathLinkList[i]];
if(pLink!=NULL)
{
//if(i==0)
// (*iPT_TripMap).second .m_PathNodeVector.push_back (pLink->m_FromNodeID );
//
//(*iPT_TripMap).second .m_PathNodeVector.push_back (pLink->m_ToNodeID );
fprintf(st,"%d,%d,%d,%d,%d,%s,",(*iPT_TripMap).second.route_id, (*iPT_TripMap).second.trip_id ,
i,pLink->m_FromNodeNumber , pLink->m_ToNodeNumber, pLink->m_Name.c_str ());
for(int s_i = 0; s_i < stop_time_size; s_i++ )
{
GDPoint pfrom = pLink->m_FromPoint;
GDPoint pto = pLink->m_ToPoint;
float distance = g_GetPoint2LineDistance((*iPT_TripMap).second .m_PT_StopTimeVector[s_i].pt, pfrom, pto,m_UnitMile); // go through each GPS location point
if(distance < 90) // with intersection
{
PT_StopTime element = (*iPT_TripMap).second .m_PT_StopTimeVector[s_i];
fprintf(st,"%d,%d,%d,%d,", element.stop_id, element.stop_sequence ,element.arrival_time , element.departure_time);
break;
}
}
fprintf(st,"\n");
}
} // for all links
}
} // for each transit trip
for (iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++) // for each link in this network
{
(*iLink)->m_Length = (*iLink)-> m_OriginalLength;
}
fclose(st);
}
return true;
}