/// <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 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>
/// 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;
}
