/// <summary>
/// Gets closest area in the road network
/// </summary>
/// <param name="c"></param>
/// <returns></returns>
public static IAreaSubtypeId GetClosest(ArbiterRoadNetwork arn, Coordinates c)
{
foreach (ArbiterZone az in arn.ArbiterZones.Values)
{
if (az.Perimeter.PerimeterPolygon.IsInside(c))
{
return(az.Perimeter.PerimeterId);
}
}
double curDist = double.MaxValue;
IAreaSubtypeId curId = null;
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
PointOnPath?closest = null;
if (al.IsInside(c))
{
closest = al.PartitionPath.GetClosest(c);
}
if (closest.HasValue)
{
double dist = closest.Value.pt.DistanceTo(c);
if (dist < curDist)
{
curDist = dist;
curId = al.LaneId;
}
}
}
}
/*
* foreach (ArbiterInterconnect ai in arn.ArbiterInterconnects.Values)
* {
* PointOnPath pop = ai.InterconnectPath.GetClosest(c);
* double dist = pop.pt.DistanceTo(c);
* if (dist < curDist && curDist - dist > 5)
* {
* curDist = dist;
* curId = ai.InterconnectId;
* }
* }*/
return(curId);
}
/// <summary>
/// Make a vehicle state from a sim vehicle state
/// </summary>
/// <param name="simState"></param>
/// <returns></returns>
public VehicleState VehicleStateFromSim(SimVehicleState simState)
{
// vehicle state
VehicleState vs = new VehicleState();
// area
vs.Area = new List <AreaEstimate>();
// ae
AreaEstimate ae = new AreaEstimate();
ae.Probability = 1;
// get string id
string id = "";
// set id
ae.AreaId = id;
ae.AreaType = StateAreaType.Interconnect;
// get area
IAreaSubtypeId iasi = this.GetAreaId(simState);
if (iasi is ArbiterPerimeterId)
{
id = ((ArbiterPerimeterId)iasi).ToString();
ae.AreaType = StateAreaType.Zone;
}
else if (iasi is ArbiterLaneId)
{
ae.AreaId = ((ArbiterLaneId)iasi).ToString() + ".1";
ae.AreaType = StateAreaType.Lane;
}
// add ae
vs.Area.Add(ae);
// set others
vs.Heading = simState.Heading;
vs.Position = simState.Position;
// return
return(vs);
}
/// <summary>
/// Distance to the next stop line
/// </summary>
/// <param name="simVehicleState"></param>
/// <returns></returns>
public double?DistanceToNextStop(SimVehicleState simVehicleState)
{
if (this.RoadNetwork == null)
{
return(null);
}
else
{
IAreaSubtypeId iasi = this.GetAreaId(simVehicleState);
if (iasi == null)
{
return(null);
}
else if (iasi is ArbiterPerimeterId)
{
return(null);
}
else
{
ArbiterLaneId ali = (ArbiterLaneId)iasi;
// get closest
ArbiterLanePartition alp = this.RoadNetwork.ArbiterSegments[ali.SegmentId].Lanes[ali].GetClosestPartition(simVehicleState.Position);
ArbiterWaypoint waypoint;
double distance;
RoadNetworkTools.NextStop(alp.Lane, alp, simVehicleState.Position, out waypoint, out distance);
if (waypoint == null)
{
return(null);
}
else
{
return(distance);
}
}
}
}
