/// <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);
}
public IPath Wp2CubicPath(IPath path, List <Polygon> obstacles, double nothing)
{
PointPath ret = new PointPath();
CubicBezier[] bez = SmoothingSpline.BuildCardinalSpline(PathUtils.IPathToPoints(path).ToArray(), null, null, 1.0);
foreach (CubicBezier b in bez)
{
ret.Add(new BezierPathSegment(b, null, false));
}
return(ret);
}
/// <summary>
/// Generates a C2 spline from a set of points and input derivatives
/// </summary>
/// <param name="initialDirection"></param>
/// <param name="intial"></param>
/// <param name="final"></param>
/// <param name="finalDirection"></param>
/// <returns></returns>
public static List <CubicBezier> SplineC2FromSegmentAndDerivatives(List <Coordinates> coordinates, Coordinates d0, Coordinates dn)
{
// final list of beziers
List <CubicBezier> spline = new List <CubicBezier>();
// generate spline
CubicBezier[] bez = SmoothingSpline.BuildC2Spline(coordinates.ToArray(), d0, dn, 0.5);
// loop through individual beziers
foreach (CubicBezier cb in bez)
{
// add to final spline
spline.Add(cb);
}
// return final list of beziers
return(spline);
}
public LinePath SplineInterpolate(double desiredSpacing)
{
CubicBezier[] beziers = SmoothingSpline.BuildCatmullRomSpline(this.ToArray(), null, null);
LinePath newPath = new LinePath();
// insert the first point
newPath.Add(this[0]);
for (int i = 0; i < beziers.Length; i++)
{
double splineLength = beziers[i].ArcLength;
int numT = (int)(splineLength / desiredSpacing);
double tspacing = 1.0 / numT;
double t;
for (t = tspacing; t < 1; t += tspacing)
{
newPath.Add(beziers[i].Bt(t));
}
newPath.Add(this[i + 1]);
}
return(newPath);
}
/// <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);
}
