/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane">Lane of origination</param>
/// <param name="targetLane">Target lane of vehicle</param>
/// <param name="confidence">Confidence of this beign a proper estimate</param>
public LaneEstimate(LaneID initialLane, LaneID targetLane, double confidence, bool inIntersection)
{
this.initialLane = initialLane;
this.targetLane = targetLane;
this.confidence = confidence;
this.inIntersection = inIntersection;
}
/// <summary>
/// Constructor.
/// Assumes not in intersection and lane confidence 1
/// </summary>
/// <param name="initialLane"></param>
/// <param name="targetLane"></param>
public LaneEstimate(LaneID initialLane, LaneID targetLane)
{
this.initialLane = initialLane;
this.targetLane = targetLane;
this.confidence = 1;
this.inIntersection = false;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="stopType"></param>
public StoppingState(LaneID lane, RndfWaypointID waypoint, StopType stopType, LaneDescription currentLane)
: base(lane, lane)
{
this.stopType = stopType;
this.waypoint = waypoint;
this.currentLane = currentLane;
}
/// <summary>
/// Analyzes a lane to find the closest point to the coordinate passed in
/// </summary>
/// <param name="coordinate">coordinate to analyze the lane against</param>
/// <param name="lane">lane to reference the coordinate to</param>
/// <returns>vector consisting of relative and absolute rndf localization information</returns>
public static LocationAnalysis ClosestPartitionOnLane(Coordinates coordinate, LaneID laneId, RndfNetwork rndfNetwork)
{
// get the lane
Lane lane = rndfNetwork.Segments[laneId.WayID.SegmentID].Ways[laneId.WayID].Lanes[laneId];
// set the value to return
LocationAnalysis closest = null;
// values for comparing partitions
double offsetMin = Double.MaxValue;
// iterate over lanePartitions within the lane
foreach (LanePartition lanePartition in lane.LanePartitions)
{
// if there are user partitions, operate over them
if (lanePartition.UserPartitions != null && lanePartition.UserPartitions.Count > 1)
{
foreach (UserPartition userPartiton in lanePartition.UserPartitions)
{
throw new Exception("user partition relation to vehicle absolute coordinates not implemented yet");
}
}
// otherwise look at how close lane partition is
else
{
// analyze the partition
LocationAnalysis partitionAnalysis = AnalyzePartition(coordinate, (IWaypoint)lanePartition.InitialWaypoint, (IWaypoint)lanePartition.FinalWaypoint);
// if this partition has less of an offset from the vehicle than the current best, set as current
if (partitionAnalysis.Offset < offsetMin)
{
offsetMin = partitionAnalysis.Offset;
closest = partitionAnalysis;
// set partition of result
closest.Partition = lanePartition;
}
// otherwise, if the vehicle is relatively close to this partition
// and the closest Coordinates are not those of the initial final waypoitns of the lane
// and this error is less than the current best offset
else if (partitionAnalysis.Offset == 1234567
&& (partitionAnalysis.RelativeRndfPosition != lane.LanePartitions[0].InitialWaypoint.Position)
&& (partitionAnalysis.RelativeRndfPosition != lane.LanePartitions[lane.LanePartitions.Count - 1].FinalWaypoint.Position)
&& (partitionAnalysis.Error < offsetMin))
{
offsetMin = partitionAnalysis.Error;
closest = partitionAnalysis;
// set partition of result
closest.Partition = lanePartition;
}
}
}
// return closest value found
return closest;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
public ChangeLanesState(bool initialIsOncoming, LaneID initialLane, LaneID finalLane, Path changeLanesPath,
PointOnPath initialPosition, PointOnPath lowerBound, PointOnPath upperBound)
: base(initialLane, finalLane)
{
this.initialIsOncoming = initialIsOncoming;
this.ChangeLanesPath = changeLanesPath;
this.InitialPosition = initialPosition;
this.LowerBound = lowerBound;
this.UpperBound = upperBound;
}
public override bool Equals(object obj)
{
if (obj is LaneID)
{
LaneID lid = (LaneID)obj;
return(lid.laneNumber == laneNumber && lid.WayID.Equals(WayID));
}
else
{
return(false);
}
}
/// <summary>
/// Gets the closest position to the coordinates on a specific lane in the rndf network
/// </summary>
/// <param name="coordinate"></param>
/// <param name="lane"></param>
/// <param name="rndfNetwork"></param>
/// <returns></returns>
public static LocationAnalysis ClosestRndfRelativeParition(Coordinates coordinate, LaneID lane, RndfNetwork rndfNetwork)
{
// use point analysis tool
return PointAnalysis.ClosestPartitionOnLane(coordinate, lane, rndfNetwork);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane">Lane to stay in</param>
public ZoneState(LaneID inLane, LaneID outLane)
{
// set lane
this.inLane = inLane;
this.outLane = outLane;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane">Lane to stay in</param>
public StayInLaneState(LaneID lane)
: base(lane, lane)
{
}
/// <summary>
/// Constructor if we are to stay in the current lane
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
/// <param name="interconnect"></param>
public IntersectionState(LaneID initialLane, LaneID finalLane)
{
this.initialLane = initialLane;
this.finalLane = finalLane;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
/// <param name="closestGoodLane"></param>
public OpposingLaneState(LaneID lane, LaneID closestGoodLane)
: base(lane, lane)
{
this.closestGoodLane = closestGoodLane;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="laneID">Lane ident info</param>
/// <param name="waypointNumber">number of this rndf waypoint in lane</param>
public RndfWaypointID(LaneID laneID, int waypointNumber)
{
this.laneID = laneID;
this.waypointNumber = waypointNumber;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="laneID">Lane ident info</param>
/// <param name="waypointNumber">number of this rndf waypoint in lane</param>
public RndfWaypointID(LaneID laneID, int waypointNumber)
{
this.laneID = laneID;
this.waypointNumber = waypointNumber;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="laneID">Identification information for this lane</param>
/// <param name="way">Way this lane belongs to</param>
public Lane(LaneID laneID, Way way)
{
this.laneID = laneID;
this.way = way;
waypoints = new Dictionary <RndfWaypointID, RndfWayPoint>();
}
public UTurnState(LaneID initialLane, LaneID finalLane, InterconnectID interconnect, UTurnBehavior uTurn)
: base(initialLane, finalLane, interconnect, TurnDirection.Straight)
{
this.UTurn = uTurn;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="laneID">Identification information for this lane</param>
/// <param name="way">Way this lane belongs to</param>
public Lane(LaneID laneID, Way way)
{
this.laneID = laneID;
this.way = way;
waypoints = new Dictionary<RndfWaypointID, RndfWayPoint>();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
/// <param name="interconnect"></param>
public StoppedAtStopState(LaneID lane, LaneDescription currentLaneDescription)
: base(lane, lane)
{
this.currentLaneDescription = currentLaneDescription;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <param name="interconnect"></param>
/// <param name="turnDirection"></param>
public WaitingAtStopState(LaneID initial, LaneID final, InterconnectID interconnect, TurnDirection turnDirection)
: base(initial, final, interconnect)
{
this.TurnDirection = turnDirection;
}
/// <summary>
/// Get the Rndf Relative location of a Coordinate to a given LaneID
/// </summary>
/// <param name="laneEstimate"></param>
/// <param name="absolutePosition"></param>
/// <returns></returns>
/// <remarks>Algorithm already done, needs cleanup</remarks>
public static RndfLocation RndfLocation(LaneID laneEstimate, Coordinates absolutePosition)
{
throw new Exception("This method has not yet been implemented!");
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
/// <param name="interconnect"></param>
public StoppedAtExitState(LaneID initialLane, LaneID finalLane, InterconnectID interconnect, LaneDescription currentLaneDescription)
: base(initialLane, finalLane, interconnect)
{
this.currentLaneDescription = currentLaneDescription;
}
public InterconnectLanes(LaneID initial, LaneID final)
{
this.initial = initial;
this.final = final;
}
public InterconnectLanes(LaneID initial, LaneID final)
{
this.initial = initial;
this.final = final;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="laneID">ID of Lane the partition is a part of</param>
/// <param name="lanePartitionNumber">Number of LanePartition within the lane</param>
public LanePartitionID(LaneID laneID, int lanePartitionNumber)
{
this.laneID = laneID;
this.lanePartitionNumber = lanePartitionNumber;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
public TravelState(LaneID initialLane, LaneID finalLane)
{
this.initialLane = initialLane;
this.finalLane = finalLane;
}
/// <summary>
/// Determines the lane adjacency
/// </summary>
/// <param name="rndf"></param>
/// <returns></returns>
private RndfNetwork DetermineLaneAdjacency(RndfNetwork rndf)
{
// loop over segment
foreach (Segment segment in rndf.Segments.Values)
{
// make sure both ways valid
if (segment.Way1.IsValid && segment.Way2.IsValid)
{
// dictionary of lanes in the segment
Dictionary<LaneID, Lane> segmentLanes = new Dictionary<LaneID, Lane>();
// construct dictionary
foreach (Way way in segment.Ways.Values)
{
foreach (Lane lane in way.Lanes.Values)
{
segmentLanes.Add(lane.LaneID, lane);
}
}
// check sample lane in way1
Lane way1SampleLane = null;
foreach (Lane tmp in segment.Way1.Lanes.Values)
{
way1SampleLane = tmp;
}
// check sample lane in way2
Lane way2SampleLane = null;
foreach (Lane tmp in segment.Way2.Lanes.Values)
{
way2SampleLane = tmp;
}
// modifies to denote increasing or decreasing form way1 to way 2
int modifier = 1;
// check if way2 has lower numbers
if (way1SampleLane.LaneID.LaneNumber > way2SampleLane.LaneID.LaneNumber)
modifier = -1;
int i = 1;
LaneID currentLaneID = new LaneID(way1SampleLane.LaneID.WayID, i);
// loop over lanes
while (segmentLanes.ContainsKey(currentLaneID))
{
Lane currentLane = segmentLanes[currentLaneID];
// increasing lane
LaneID increasingLaneID1 = new LaneID(segment.Way1.WayID, i + (modifier * 1));
LaneID increasingLaneID2 = new LaneID(segment.Way2.WayID, i + (modifier * 1));
if (segmentLanes.ContainsKey(increasingLaneID1))
{
if (currentLaneID.WayID.WayNumber == 1)
currentLane.OnLeft = segmentLanes[increasingLaneID1];
else
currentLane.OnRight = segmentLanes[increasingLaneID1];
}
else if (segmentLanes.ContainsKey(increasingLaneID2))
{
if (currentLaneID.WayID.WayNumber == 1)
currentLane.OnLeft = segmentLanes[increasingLaneID2];
else
currentLane.OnRight = segmentLanes[increasingLaneID2];
}
// check for decreasing
// increasing lane
increasingLaneID1 = new LaneID(segment.Way1.WayID, i - (modifier * 1));
increasingLaneID2 = new LaneID(segment.Way2.WayID, i - (modifier * 1));
if (segmentLanes.ContainsKey(increasingLaneID1))
{
if (currentLaneID.WayID.WayNumber == 1)
currentLane.OnRight = segmentLanes[increasingLaneID1];
else
currentLane.OnLeft = segmentLanes[increasingLaneID1];
}
else if (segmentLanes.ContainsKey(increasingLaneID2))
{
if (currentLaneID.WayID.WayNumber == 1)
currentLane.OnRight = segmentLanes[increasingLaneID2];
else
currentLane.OnLeft = segmentLanes[increasingLaneID2];
}
if (currentLane.OnLeft != null)
{
Console.WriteLine("Lane: " + currentLane.LaneID.ToString() + ". On Left: " + currentLane.OnLeft.LaneID.ToString());
}
i++;
currentLaneID = new LaneID(segment.Way1.WayID, i);
if (segmentLanes.ContainsKey(new LaneID(segment.Way2.WayID, i)))
currentLaneID = new LaneID(segment.Way2.WayID, i);
}
}
else
{
// HACK
//throw new Exception("single way lane!");
}
}
return rndf;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
/// <param name="interconnect"></param>
public IntersectionState(LaneID initialLane, LaneID finalLane, InterconnectID interconnect)
{
this.initialLane = initialLane;
this.finalLane = finalLane;
this.interconnect = interconnect;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="laneID">ID of Lane the partition is a part of</param>
/// <param name="lanePartitionNumber">Number of LanePartition within the lane</param>
public LanePartitionID(LaneID laneID, int lanePartitionNumber)
{
this.laneID = laneID;
this.lanePartitionNumber = lanePartitionNumber;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
/// <param name="interconnect"></param>
public TurnState(LaneID initialLane, LaneID finalLane, InterconnectID interconnect, TurnDirection turnDirection)
: base(initialLane, finalLane, interconnect)
{
this.TurnDirection = turnDirection;
}
