DTALink* CTLiteDoc::FindLinkFromSensorLocation(float x, float y, CString orientation)
{
double Min_distance = m_NetworkRect.Width()/100; // set the selection threshod
std::list<DTALink*>::iterator iLink;
int SelectedLinkID = -1;
for (iLink = m_LinkSet.begin(); iLink != m_LinkSet.end(); iLink++)
{
GDPoint p0, pfrom, pto;
p0.x = x; p0.y = y;
pfrom.x = (*iLink)->m_FromPoint.x; pfrom.y = (*iLink)->m_FromPoint.y;
pto.x = (*iLink)->m_ToPoint.x; pto.y = (*iLink)->m_ToPoint.y;
if(orientation.MakeUpper().Find('E')>0 && pfrom.x > pto.x) // East, Xfrom should be < XTo
continue; //skip
if(orientation.MakeUpper().Find('S')>0 && pfrom.y < pto.y) // South, Yfrom should be > YTo
continue;
if(orientation.MakeUpper().Find('W')>0 && pfrom.x < pto.x) // West, Xfrom should be > XTo
continue;
if(orientation.MakeUpper().Find('N')>0 && pfrom.y > pto.y) // North, Yfrom should be < YTo
continue;
float distance = g_GetPoint2LineDistance(p0, pfrom, pto);
if(distance >=0 && distance < Min_distance)
{
return (*iLink);
}
}
return NULL;
}
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);
}
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;
}
