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