/// <summary>
/// Inserts a user RndfWayPoint between two Rndf Waypoints a certain distance from the beginning RndfWayPoint
/// </summary>
/// <param name="userWayPoint">UserWayPoint to add</param>
/// <param name="distanceFromBeginning">distance to add RndfWayPoint from the beginning RndfWayPoint of the LanePartition</param>
public void AddUserWayPoint(RndfWayPoint userWayPoint, double distanceFromBeginning)
{
throw new Exception("This method has not been implemented");
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialWaypoint">beginning waypoint of interconnect</param>
/// <param name="finalWaypoint">ending waypoint of interconnect</param>
public Interconnect(InterconnectID interconnectID, RndfWayPoint initialWaypoint, RndfWayPoint finalWaypoint)
{
this.initialWaypoint = initialWaypoint;
this.finalWaypoint = finalWaypoint;
this.interconnectID = interconnectID;
}
public static void NextStop(RndfNetwork rndf, RndfLocation vehicleLocation,
out RndfWayPoint nextStop, out double distance)
{
// set current as final waypoint on current partition
RndfWayPoint current = vehicleLocation.Partition.FinalWaypoint;
// set initial distance
distance = vehicleLocation.AbsolutePositionOnPartition.DistanceTo(current.Position);
// iterate over waypoints
while (!current.IsStop && current.NextLanePartition != null)
{
// update distance
distance += current.Position.DistanceTo(current.NextLanePartition.FinalWaypoint.Position);
// update current
current = current.NextLanePartition.FinalWaypoint;
}
if (current.IsStop)
{
nextStop = current;
// return current as stop or end of lane
return;
}
else
{
nextStop = null;
return;
}
}
/// <summary>
/// Checks if a specific vehicle is within generous vehicle-like area of an exit
/// </summary>
/// <param name="observedVehicle"></param>
/// <param name="stop"></param>
/// <returns></returns>
public static bool CheckVehicleAtExit(ObservedVehicle observedVehicle, RndfWayPoint exit)
{
throw new Exception("This method is not yet implemented");
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialWaypoint">beginning waypoint of interconnect</param>
/// <param name="finalWaypoint">ending waypoint of interconnect</param>
public Interconnect(InterconnectID interconnectID, RndfWayPoint initialWaypoint, RndfWayPoint finalWaypoint)
{
this.initialWaypoint = initialWaypoint;
this.finalWaypoint = finalWaypoint;
this.interconnectID = interconnectID;
}
/// <summary>
/// Generates a polygon representing the allowable area to perform a U-Turn
/// given Rndf Hazards and Allowed U-Turn Space
/// </summary>
/// <param name="exit"></param>
/// <param name="entry"></param>
/// <returns></returns>
public static List<BoundaryLine> GenerateUTurnPolygon(RndfWayPoint exit, RndfWayPoint entry)
{
// Generate the boundary points of the U-Turn
List<Coordinates> uTurnBounds = GenerateUTurnBounds(exit, entry);
// Turn the boundary points into a polygon using a jarvis march
List<BoundaryLine> uTurnPolygonBoundaries = JarvisMarch(uTurnBounds);
// Return the u turn polygon
return uTurnPolygonBoundaries;
}
/// <summary>
/// Generates the bounding waypoints of the acceptable U-Turn area given Rndf Hazards and specified exit and entry waypoints
/// </summary>
/// <param name="exit">The specified exit point of the U-Turn</param>
/// <param name="entry">The specified entry point of the U-Turn</param>
/// <returns></returns>
/// <remarks>See "Technical Evaluation Criteria: A.12. U-turn for specific definition of allowable space</remarks>
/// <remarks>Assumes the exit/entry coordinates are in the center of their respective lanes</remarks>
public static List<Coordinates> GenerateUTurnBounds(RndfWayPoint exit, RndfWayPoint entry)
{
// initialize the bounding box
List<Coordinates> boundingBox = new List<Coordinates>();
// get the length translation vector
Coordinates translation = exit.Position - exit.PreviousLanePartition.InitialWaypoint.Position;
translation = translation.Normalize(15);
// get the width lane shift length approximation
Coordinates approxWidthVector = (entry.Position - exit.Position);
double widthApprox = approxWidthVector.Length;
// get the width lane shift vector
Coordinates laneShift = translation.Rotate90().Normalize(widthApprox);
// get the center coordinate
Coordinates center = exit.Position + laneShift.Normalize(widthApprox/2);
// Calculate the bounding coordinates
Coordinates uR = center + translation + laneShift;
Coordinates lR = center + translation - laneShift;
Coordinates uL = center - translation + laneShift;
Coordinates lL = center - translation - laneShift;
// add coordinates to the bounding box
boundingBox.Add(uR);
boundingBox.Add(lR);
boundingBox.Add(uL);
boundingBox.Add(lL);
// return the bounding box
return boundingBox;
}
/// <summary>
/// Determine if one of the observed vehicles is stopped at a specified stop line
/// </summary>
/// <param name="stop"></param>
/// <param name="observedVehicles"></param>
/// <returns></returns>
/// <remarks>Uses a combination of determining if vehicle is within area of stop line and has a small enough velocity</remarks>
public static ObservedVehicle? FindVehicleStoppedAtStop(RndfWayPoint stop, List<ObservedVehicle> observedVehicles)
{
//foreach (ObservedVehicle ov in observedVehicles)
//{
// if (stop.Position.DistanceTo(ov.AbsolutePosition) < (ov.Length - ov.PositionOffsetFromRear) + 1 && ov.Speed < 1) //ANGLE FROM STOP POINT, NOT PERFECT
// return ov;
//}
//return null;
return null;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="goalNumber">id number of goal in checkpoint</param>
/// <param name="waypoint">rndf waypoint this checkpoint identifies</param>
public Goal(int goalNumber, RndfWayPoint waypoint)
{
this.goalNumber = goalNumber;
this.waypoint = waypoint;
}
/// <summary>
/// Inserts a user RndfWayPoint between two Rndf Waypoints a certain distance from the beginning RndfWayPoint
/// </summary>
/// <param name="userWayPoint">UserWayPoint to add</param>
/// <param name="distanceFromBeginning">distance to add RndfWayPoint from the beginning RndfWayPoint of the LanePartition</param>
public void AddUserWayPoint(RndfWayPoint userWayPoint, double distanceFromBeginning)
{
throw new Exception("This method has not been implemented");
}
