/// <summary>
/// Generates a path around a turn
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <param name="turn"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleRelative"></param>
/// <returns></returns>
public static IPath TurnPath(Lane initial, Lane final, Interconnect turn, VehicleState vehicleState, bool vehicleRelative)
{
// 1. list of absolute positions of lane coordinates
List<Coordinates> absoluteCoordinates = new List<Coordinates>();
// make sure not turning onto same lane
if(!initial.LaneID.Equals(final.LaneID))
{
// 2. loop through initial lane's waypoints
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(initial, true));
// 3. check if the turn has user partitions
if (turn.UserPartitions != null)
{
// 3.1 loop through turn's waypoints if not rndfwaypoints (As will be included by lane coordinate generation
foreach (UserPartition partition in turn.UserPartitions)
{
// add if not an rndf waypoint
if (!(partition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
absoluteCoordinates.Add(partition.InitialWaypoint.Position);
}
}
}
// 4. add the next lane's waypoints
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(final, true));
}
else
{
// get lanegth of lane / 4
// HACK
double length = RoadToolkit.LanePath(initial, vehicleState, true).Length/4;
// 2. loop through initial lane's waypoints
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(initial, true, length, false));
// 3. check if the turn has user partitions
if (turn.UserPartitions != null)
{
// 3.1 loop through turn's waypoints if not rndfwaypoints (As will be included by lane coordinate generation
foreach (UserPartition partition in turn.UserPartitions)
{
// add if not an rndf waypoint
if (!(partition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
absoluteCoordinates.Add(partition.InitialWaypoint.Position);
}
}
}
// 4. loop through final lane's waypoints
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(final, true, length, true));
}
// 5. Generate path
return GeneratePathFromCoordinates(absoluteCoordinates, vehicleState.Position, vehicleState.Heading, vehicleRelative);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lanePartitionID">Identification information</param>
/// <param name="lane">Lane this partition is a part of</param>
public LanePartition(LanePartitionID lanePartitionID, Lane lane)
{
this.lanePartitionID = lanePartitionID;
this.lane = lane;
}
/// <summary>
/// Gets a path for this lane relative to the vehicle (as if the vehicle was travelling to (1,0)
/// But only upto a certain distance if the lane goes that far
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleRelative"></param>
/// <returns></returns>
public static IPath LanePath(Lane lane, VehicleState vehicleState, double distance, bool forwards, bool vehicleRelative)
{
// 1. list of absolute positions of lane coordinates
List<Coordinates> absoluteCoordinates = new List<Coordinates>();
// 2. get lane coordinates
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(lane, true, distance, forwards));
// 3. Generate path
return GeneratePathFromCoordinates(absoluteCoordinates, vehicleState.Position, vehicleState.Heading, vehicleRelative);
}
/// <summary>
/// Preprocesses the lane paths
/// </summary>
/// <param name="lane"></param>
/// <param name="forwardsPath"></param>
/// <param name="backwardsPath"></param>
public static void PreprocessLanePaths(Lane lane,
out Path forwardsPath, out Path backwardsPath, out List<CubicBezier> forwardSpline)
{
// 1. list of absolute positions of lane coordinates
List<Coordinates> forwardCoordinates = new List<Coordinates>();
// 2. loop through waypoints
foreach (RndfWayPoint rwp in lane.Waypoints.Values)
{
// add position
forwardCoordinates.Add(rwp.Position);
}
// 3. generate spline
CubicBezier[] bez = SmoothingSpline.BuildC2Spline(forwardCoordinates.ToArray(), null, null, 0.5);
// 4. generate path
List<IPathSegment> forwardPathSegments = new List<IPathSegment>();
// spline generation
List<CubicBezier> tmpForwardSpline = new List<CubicBezier>();
// 5. loop through individual beziers
foreach (CubicBezier cb in bez)
{
// add to spline
tmpForwardSpline.Add(cb);
// add to final spline
forwardPathSegments.Add(new BezierPathSegment(cb, null, false));
}
// set spline
forwardSpline = tmpForwardSpline;
// 6. Create the forward path
forwardsPath = new Path(forwardPathSegments, CoordinateMode.AbsoluteProjected);
// 7. list of backwards coordinates
List<Coordinates> backwardsCoordinates = forwardCoordinates;
backwardsCoordinates.Reverse();
// 8. generate spline
bez = SmoothingSpline.BuildC2Spline(backwardsCoordinates.ToArray(), null, null, 0.5);
// 9. generate path
List<IPathSegment> backwardPathSegments = new List<IPathSegment>();
// 10. loop through individual beziers
foreach (CubicBezier cb in bez)
{
// add to final spline
backwardPathSegments.Add(new BezierPathSegment(cb, null, false));
}
// 11. generate backwards path
backwardsPath = new Path(backwardPathSegments, CoordinateMode.AbsoluteProjected);
}
/// <summary>
/// Gets up to a certain distance of the lane's coordinates
/// </summary>
/// <param name="lane"></param>
/// <param name="includeUserWaypoint"></param>
/// <param name="d"></param>
/// <param name="forwards">whether or not to get the front or back of a lane</param>
/// <returns></returns>
public static List<Coordinates> GetLaneWaypointCoordinates(Lane lane, bool includeUserWaypoints, double d, bool forwards)
{
// 1. list of absolute positions of lane coordinates
List<Coordinates> absoluteCoordinates = new List<Coordinates>();
if (forwards)
{
RndfWayPoint current = lane.LanePartitions[0].InitialWaypoint;
double distance = 0;
// loop over waypoints while distance within params
while (current != null && distance <= d)
{
// add rndf waypoint
absoluteCoordinates.Add(current.Position);
// update a coarse distance
if (current.NextLanePartition != null)
{
// update distance
distance += current.NextLanePartition.FinalWaypoint.Position.DistanceTo(current.Position);
// check to see if we're including user waypoints
if (includeUserWaypoints)
{
// add the user waypoints
foreach (UserPartition userPartition in current.NextLanePartition.UserPartitions)
{
// check if not an rndfwaypoint
if (!(userPartition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
absoluteCoordinates.Add(userPartition.InitialWaypoint.Position);
}
}
}
// iterate
current = (current.NextLanePartition.FinalWaypoint);
}
else
{
return absoluteCoordinates;
}
}
return absoluteCoordinates;
}
else
{
RndfWayPoint current = lane.LanePartitions[lane.LanePartitions.Count-1].FinalWaypoint;
double distance = 0;
// loop over waypoints while distance within params
while (current != null && distance <= d)
{
// add rndf waypoint
absoluteCoordinates.Add(current.Position);
// update a coarse distance
if (current.PreviousLanePartition != null)
{
// update distance
distance += current.PreviousLanePartition.InitialWaypoint.Position.DistanceTo(current.Position);
// check to see if we're including user waypoints
if (includeUserWaypoints && current.NextLanePartition != null)
{
// add the user waypoints
foreach (UserPartition userPartition in current.NextLanePartition.UserPartitions)
{
List<Coordinates> forwardUserCoords = new List<Coordinates>();
// check if not an rndfwaypoint
if (!(userPartition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
forwardUserCoords.Add(userPartition.InitialWaypoint.Position);
}
forwardUserCoords.Reverse();
absoluteCoordinates.AddRange(forwardUserCoords);
}
}
// iterate
current = (current.PreviousLanePartition.InitialWaypoint);
}
else
{
absoluteCoordinates.Reverse();
return absoluteCoordinates;
}
}
absoluteCoordinates.Reverse();
return absoluteCoordinates;
}
}
/// <summary>
/// Gets the coordinates of a lane's waypoints
/// </summary>
/// <param name="includeuserWaypoints"></param>
/// <returns></returns>
public static List<Coordinates> GetLaneWaypointCoordinates(Lane lane, bool includeuserWaypoints)
{
// 1. list of absolute positions of lane coordinates
List<Coordinates> absoluteCoordinates = new List<Coordinates>();
// 2. loop through initial lane's waypoints
foreach (RndfWayPoint waypoint in lane.Waypoints.Values)
{
// add the waypoint's position
absoluteCoordinates.Add(waypoint.Position);
// check to see if we're including user waypoints
if (includeuserWaypoints && waypoint.NextLanePartition != null)
{
// add the user waypoints
foreach (UserPartition userPartition in waypoint.NextLanePartition.UserPartitions)
{
// check if not an rndfwaypoint
if (!(userPartition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
absoluteCoordinates.Add(userPartition.InitialWaypoint.Position);
}
}
}
}
// 3. return coordinates
return absoluteCoordinates;
}
/// <summary>
/// Generates a UTurn Behavior
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <returns></returns>
public static UTurnBehavior GenerateDefaultUTurnBehavior(Lane initial, Lane final, Interconnect turn, VehicleState vehicleState, bool relative)
{
// generate polygon
/*List<BoundaryLine> boundaryPolygon = GenerateUTurnPolygon(turn.InitialWaypoint, turn.FinalWaypoint);
List<Coordinates> boundaryInOrder = new List<Coordinates>();
foreach(BoundaryLine b in boundaryPolygon)
boundaryInOrder.Add(b.p1);
Polygon polygon = new Polygon(boundaryInOrder, CoordinateMode.AbsoluteProjected);
// generate paths
IPath initialPath = RoadToolkit.LanePath(initial, vehicleState, relative);
IPath finalPath = RoadToolkit.LanePath(final, vehicleState, relative);
// generate speeds
SpeedCommand initialSpeed = new ScalarSpeedCommand(initial.Way.Segment.SpeedInformation.MaxSpeed);
SpeedCommand finalSpeed = new ScalarSpeedCommand(final.Way.Segment.SpeedInformation.MaxSpeed);
// generate path behaviors
PathFollowingBehavior initialPathBehavior = new PathFollowingBehavior(initialPath, initialSpeed);
PathFollowingBehavior finalPathBehavior = new PathFollowingBehavior(finalPath, finalSpeed);
// generate U-Turn
return new UTurnBehavior(initialPathBehavior, finalPathBehavior, polygon);*/
return null;
}
/// <summary>
/// Generates a default turn behavior
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <param name="turn"></param>
/// <param name="vehicleState"></param>
/// <param name="relative"></param>
/// <returns></returns>
public static PathFollowingBehavior GenerateDefaultTurnBehavior(Lane initial, Lane final, Interconnect turn, bool relative)
{
CubicBezier[] spline;
if (turn.UserPartitions.Count == 2)
{
Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position;
Coordinates p1 = turn.InitialWaypoint.Position;
Coordinates p2 = turn.UserPartitions[0].FinalWaypoint.Position;
Coordinates p3 = turn.FinalWaypoint.Position;
Coordinates p4 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position;
Coordinates[] pts = { p0, p1, p2, p3, p4 };
spline = SmoothingSpline.BuildC2Spline(pts, null, null, 0.5);
}
else if (turn.UserPartitions.Count > 2)
{
Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position;
Coordinates p1 = turn.InitialWaypoint.Position;
Coordinates p0head = p0 - p1;
p0 = p1 + p0head.Normalize(4);
List<Coordinates> middleUsers = new List<Coordinates>();
for (int i = 0; i < turn.UserPartitions.Count - 1; i++)
{
middleUsers.Add(turn.UserPartitions[i].FinalWaypoint.Position);
}
Coordinates p2 = turn.FinalWaypoint.Position;
Coordinates p3 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position;
Coordinates p3head = p3 - p2;
p3 = p2 + p3head.Normalize(4);
List<Coordinates> finalList = new List<Coordinates>();
finalList.Add(p0);
finalList.Add(p1);
finalList.AddRange(middleUsers);
finalList.Add(p2);
finalList.Add(p3);
spline = SmoothingSpline.BuildC2Spline(finalList.ToArray(), null, null, 0.5);
}
else
{
Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position;
Coordinates p1 = turn.InitialWaypoint.Position;
Coordinates p3 = turn.FinalWaypoint.Position;
Coordinates p4 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position;
Coordinates[] pts = { p0, p1, p3, p4 };
spline = SmoothingSpline.BuildC2Spline(pts, null, null, 0.5);
}
// Create the Path Segments
List<IPathSegment> bezierPathSegments = new List<IPathSegment>();
foreach (CubicBezier bezier in spline)
{
bezierPathSegments.Add(new BezierPathSegment(bezier, null, false));
}
// get the method from the road toolkit
//IPath turnPath = RoadToolkit.TurnPath(initial, final, turn, vehicleState, relative);// CHANGED
IPath turnPath = new Path(bezierPathSegments, CoordinateMode.AbsoluteProjected);
// make a speed command (set to 2m/s)
SpeedCommand speedCommand = new ScalarSpeedCommand(1);
// make behavior
//return new PathFollowingBehavior(turnPath, speedCommand);
return null;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="waypointID">RndfWaypoint Identification Information</param>
/// <param name="lane">Lane this wypoint belongs to</param>
public RndfWayPoint(RndfWaypointID waypointID, Lane lane)
{
this.waypointID = waypointID;
this.lane = lane;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="waypointID">RndfWaypoint Identification Information</param>
/// <param name="lane">Lane this wypoint belongs to</param>
public RndfWayPoint(RndfWaypointID waypointID, Lane lane)
{
this.waypointID = waypointID;
this.lane = lane;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lanePartitionID">Identification information</param>
/// <param name="lane">Lane this partition is a part of</param>
public LanePartition(LanePartitionID lanePartitionID, Lane lane)
{
this.lanePartitionID = lanePartitionID;
this.lane = lane;
}
