public LinePath LinearizeCenterLine(LinearizationOptions options)
{
LinePath transformedPath = centerlinePath;
if (options.Timestamp.IsValid) {
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp);
transformedPath = centerlinePath.Transform(relTransform);
}
LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance);
LinePath subPath = new LinePath(); ;
if (options.EndDistance > options.StartDistance) {
subPath = transformedPath.SubPath(startPoint, options.EndDistance-options.StartDistance);
}
if (subPath.Count < 2) {
subPath.Clear();
Coordinates startPt = startPoint.Location;
subPath.Add(startPt);
subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector*Math.Max(options.EndDistance-options.StartDistance, 0.1)+startPt);
}
return subPath;
}
public void findPath(Vector3 endPos, bool tilesOnly = true)
{
Vector3 startPos = Map.map.mapToWorldPoint(reservedPos[0], reservedPos[1]);
currentPath = AStar.findPath(Map.map, startPos, endPos, target, distToTarget, tilesOnly);
lastEndPos = Map.map.worldToMapPoint(endPos);
}
public void Initialize(Behavior b)
{
SimpleStayInLaneBehavior sb = (SimpleStayInLaneBehavior)b;
// store the base path
basePath = ConvertPath(sb.BasePath);
// get the lower, upper bound
leftBound = FindBoundary(sb.LaneWidth/2);
rightBound = FindBoundary(-sb.LaneWidth/2);
// convert everything to be vehicle relative
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();
pathTime = absTransform.Timestamp;
basePath = basePath.Transform(absTransform);
leftBound = leftBound.Transform(absTransform);
rightBound = rightBound.Transform(absTransform);
// send the left and right bounds
Services.UIService.PushRelativePath(leftBound, pathTime, "left bound");
Services.UIService.PushRelativePath(rightBound, pathTime, "right bound");
maxSpeed = sb.MaxSpeed;
obstacleManager = new ObstacleManager(); // hik
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="safetyZoneEnd"></param>
/// <param name="safetyZoneBegin"></param>
public ArbiterSafetyZone(ArbiterLane lane, LinePath.PointOnPath safetyZoneEnd, LinePath.PointOnPath safetyZoneBegin)
{
this.safetyZoneBegin = safetyZoneBegin;
this.safetyZoneEnd = safetyZoneEnd;
this.lane = lane;
this.GenerateSafetyZone();
}
public PathLaneModel(CarTimestamp timestamp, LinePath path, double width)
{
this.timestamp = timestamp;
this.path = path;
this.width = width;
this.laneID = "not specified";
}
public CombinedLaneModel(LinePath centerlinePath, LinePath leftBound, LinePath rightBound, double nominalWidth, CarTimestamp timestamp)
{
this.centerlinePath = centerlinePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.timestamp = timestamp;
this.nominalWidth = nominalWidth;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zoneId"></param>
/// <param name="zonePerimeter"></param>
/// <param name="stayOutside"></param>
/// <param name="speedCommand"></param>
/// <param name="recommendedPath"></param>
/// <param name="endingLeftBound"></param>
/// <param name="endingRightBound"></param>
public ZoneTravelingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand,
LinePath recommendedPath, LinePath endingLeftBound, LinePath endingRightBound)
: base(zoneId, zonePerimeter, stayOutside, speedCommand)
{
this.RecommendedPath = recommendedPath;
this.EndingLeftBound = endingLeftBound;
this.EndingRightBound = endingRightBound;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
public ArbiterUserPartition(ArbiterUserPartitionId partitionId, IConnectAreaWaypoints partition, IGenericWaypoint initial, IGenericWaypoint final)
{
this.PartitionId = partitionId;
this.InitialGeneric = initial;
this.FinalGeneric = final;
this.Length = this.InitialGeneric.Position.DistanceTo(this.FinalGeneric.Position);
this.Partition = partition;
this.UserPartitionPath = new LinePath(new Coordinates[] { initial.Position, final.Position });
}
/// <summary>
/// Sets the path of the supra lane
/// </summary>
public void SetDefaultLanePath()
{
// new path
this.supraPath = new LinePath();
// get initial waypoints
this.supraPath = this.Initial.LanePath(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric);
this.supraPath.AddRange(this.Final.LanePath((ArbiterWaypoint)this.Interconnect.FinalGeneric, this.Final.WaypointList[this.Final.WaypointList.Count - 1]));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zoneId"></param>
/// <param name="zonePerimeter"></param>
/// <param name="stayOutside"></param>
/// <param name="speedCommand"></param>
/// <param name="parkingSpotPath"></param>
/// <param name="spotLeftBound"></param>
/// <param name="spotRightBound"></param>
/// <param name="spotId"></param>
/// <param name="extraDistance"></param>
/// <param name="endingPolygon"></param>
/// <param name="orientations"></param>
public ZoneParkingPullOutBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand,
LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId,
LinePath reversePath, LinePath finalLeftBound, LinePath finalRightBound)
: base(zoneId, zonePerimeter, stayOutside, speedCommand, parkingSpotPath, spotLeftBound, spotRightBound, spotId, 0.0)
{
this.RecommendedPullOutPath = reversePath;
this.EndingLeftBound = finalLeftBound;
this.EndingRightBound = finalRightBound;
}
public UTurnBehavior(Polygon boundary, LinePath endingPath, ArbiterLaneId endingLane, SpeedCommand endingSpeedCommand, List <Polygon> stayOutPolygons)
{
this.boundary = boundary;
this.endingPath = endingPath;
this.endingLane = endingLane;
this.endingSpeedCommand = endingSpeedCommand;
this.stayOutPolygons = stayOutPolygons;
this.stopOnEndingPath = true;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
public ArbiterUserPartition(ArbiterUserPartitionId partitionId, IConnectAreaWaypoints partition, IGenericWaypoint initial, IGenericWaypoint final)
{
this.PartitionId = partitionId;
this.InitialGeneric = initial;
this.FinalGeneric = final;
this.Length = this.InitialGeneric.Position.DistanceTo(this.FinalGeneric.Position);
this.Partition = partition;
this.UserPartitionPath = new LinePath(new Coordinates[] { initial.Position, final.Position });
}
protected override void SetUpCannon()
{
var start = transform.position + transform.up / 2;
var end = start + transform.forward * 50f;
path = new LinePath(count, start, end);
path.EvaluateWaypoints();
InvokeRepeating("Shoot", firerate, firerate);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zoneId"></param>
/// <param name="zonePerimeter"></param>
/// <param name="stayOutside"></param>
/// <param name="speedCommand"></param>
/// <param name="parkingSpotPath"></param>
/// <param name="spotLeftBound"></param>
/// <param name="spotRightBound"></param>
/// <param name="spotId"></param>
/// <param name="extraDistance"></param>
/// <param name="endingPolygon"></param>
/// <param name="orientations"></param>
public ZoneParkingPullOutBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand,
LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId,
LinePath reversePath, LinePath finalLeftBound, LinePath finalRightBound)
: base(zoneId, zonePerimeter, stayOutside, speedCommand, parkingSpotPath, spotLeftBound, spotRightBound, spotId, 0.0)
{
this.RecommendedPullOutPath = reversePath;
this.EndingLeftBound = finalLeftBound;
this.EndingRightBound = finalRightBound;
}
private void HandleBehavior(ZoneTravelingBehavior b)
{
base.HandleBaseBehavior(b);
this.recommendedPath = b.RecommendedPath;
Services.UIService.PushLineList(b.RecommendedPath, b.TimeStamp, "original path1", false);
Services.UIService.PushLineList(null, b.TimeStamp, "original path2", false);
}
void FixedUpdate()
{
LinePath lp = tileComponent.FindLinePathClosest(transform.position, target.position);
movement.steering = followPath.GetSteering(lp);
lp.Draw();
tileComponent.SetTile(transform.position);
}
/// <summary>
/// Updates the current vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
public void Update(IFQMPlanable lane, VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath();
// get our position
Coordinates f = state.Front;
// get all vehicles associated with those components
List <VehicleAgent> vas = new List <VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
{
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// set the vehicles to ignore
this.VehiclesToIgnore = new List <int>();
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
// check relatively inside
if (lane.RelativelyInside(frontPos))
{
// distance to vehicle
double frontDist = lane.DistanceBetween(f, frontPos);
// check forward of us
if (frontDist > 0)
{
// add to ignorable
this.VehiclesToIgnore.Add(va.VehicleId);
// check for closest
if (frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
private bool CheckGeneralShape(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel centerLaneModel)
{
// bad newz bears
if (centerLaneModel.LanePath == null || centerLaneModel.LanePath.Count < 2)
{
return(false);
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = centerLaneModel.LanePath.Transform(relTransform);
// get the zero point of the lane model path
LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;
// get the first waypoint on the RNDF path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
Coordinates firstWaypoint = rndfPath[rndfZeroPoint.Index + 1];
// get the projection of the first waypoint onto the lane path
LinePath.PointOnPath laneFirstWaypoint = laneModelPath.GetClosestPoint(firstWaypoint);
// get the offset vector
Coordinates offsetVector = laneFirstWaypoint.Location - firstWaypoint;
// start iterating through the waypoints forward and project them onto the rndf path
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// get the waypoint
Coordinates waypoint = rndfPath[i];
// adjust by the first waypoint offset
waypoint += offsetVector;
// project onto the lane model
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check the distance from the zero point on the lane model
if (laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint) > road_model_max_dist)
{
break;
}
// check the devation from the rndf
double deviation = waypoint.DistanceTo(laneWaypoint.Location);
// if the deviation is over some threshold, then we reject the model
if (deviation > road_deviation_reject_threshold)
{
return(false);
}
}
// we got this far, so this stuff is OK
return(true);
}
/// <summary>
/// Update grid search path
/// </summary>
/// <param name="obstaclePath">Path that avoids obstacles</param>
private void UpdateGridSearchPath(LinePath obstaclePath)
{
StartWatch(); // start stopwatch
float gridStartValue = 1;
float gridExtValue = -1;
List <GridCoordinates> gridExtendLocs;
// retrieve A star search path
List <GridCoordinates> searchPath;
gridCost.GetPath(out searchPath);
// extend start of path to grid border, if necessary
ExtendPoints(obstaclePath[1], obstaclePath[0], out gridExtendLocs);
gridSearchPath.SetValues(gridExtendLocs, gridExtValue);
// extend end of path to grid border, if necessary
int lastIndex = obstaclePath.Count - 1;
ExtendPoints(obstaclePath[lastIndex - 1], obstaclePath[lastIndex], out gridExtendLocs);
gridSearchPath.SetValues(gridExtendLocs, gridExtValue);
// set grid search path points
gridSearchPath.SetValues(searchPath, gridStartValue);
// generate a line path with a 7.5 m extension off the end of the path
//List<Coordinates> pathPoints = new List<Coordinates>();
//pathPoints.Add(obstaclePath[obstaclePath.Count-1]);
//pathPoints.Add(obstaclePath[obstaclePath.Count-1] + obstaclePath.EndSegment.Vector.Normalize(1.5*TahoeParams.VL));
//FindPoints(pathPoints, out gridExtendLocs);
//gridSearchPath.SetValues(gridExtendLocs, gridStartValue);
// set up left region for grid search path using floodfill
int offsetX = searchPath[1].X - searchPath[0].X;
int offsetY = searchPath[1].Y - searchPath[0].Y;
GridCoordinates leftStartLoc = new GridCoordinates(searchPath[0].X - offsetY,
searchPath[0].Y + offsetX);
gridSearchPath.FloodFill(leftStartLoc, gridStartValue + 1, 0);
// set up right region for grid search path
// do nothing, remains 0
// display time to process grid search path
gridSearchPathTime = StopWatch("ObstacleManager - GridSearchPath - ");
// write grid data to file (for debugging)
if (writeToFileFlag == true)
{
StartWatch();
gridSearchPath.WriteGridToFile("GridSearchPath.txt");
StopWatch("ObstacleManager - GridSearchPath File Write - ");
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zoneId"></param>
/// <param name="zonePerimeter"></param>
/// <param name="stayOutside"></param>
/// <param name="speedCommand"></param>
/// <param name="parkingSpotPath"></param>
/// <param name="spotLeftBound"></param>
/// <param name="spotRightBound"></param>
/// <param name="spotId"></param>
/// <param name="extraDistance"></param>
public ZoneParkingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand,
LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId, double extraDistance)
: base(zoneId, zonePerimeter, stayOutside, speedCommand)
{
this.ParkingSpotPath = parkingSpotPath;
this.SpotLeftBound = spotLeftBound;
this.SpotRightBound = spotRightBound;
this.ParkingSpotId = spotId;
this.ExtraDistance = extraDistance;
}
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath,
LinePath leftBound, LinePath rightBound,
double maxSpeed, double?endingHeading, bool endingOffsetBound,
CarTimestamp curTimestamp, bool doAvoidance,
SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
string commandLabel, ref bool cancelled,
ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double?approachSpeed)
{
SmoothAndTrack(basePath, useTargetPath, new LinePath[] { leftBound }, new LinePath[] { rightBound }, maxSpeed, endingHeading, endingOffsetBound, curTimestamp, doAvoidance, speedCommand, behaviorTimestamp, commandLabel, ref cancelled, ref previousSmoothedPath, ref previousSmoothedPathTimestamp, ref approachSpeed);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zoneId"></param>
/// <param name="zonePerimeter"></param>
/// <param name="stayOutside"></param>
/// <param name="speedCommand"></param>
/// <param name="parkingSpotPath"></param>
/// <param name="spotLeftBound"></param>
/// <param name="spotRightBound"></param>
/// <param name="spotId"></param>
/// <param name="extraDistance"></param>
public ZoneParkingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand,
LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId, double extraDistance)
: base(zoneId, zonePerimeter, stayOutside, speedCommand)
{
this.ParkingSpotPath = parkingSpotPath;
this.SpotLeftBound = spotLeftBound;
this.SpotRightBound = spotRightBound;
this.ParkingSpotId = spotId;
this.ExtraDistance = extraDistance;
}
public static LinePath FindBoundary(double offset, LinePath path)
{
// create a list of shift line segments
List<LineSegment> segs = new List<LineSegment>();
foreach (LineSegment ls in path.GetSegmentEnumerator()) {
segs.Add(ls.ShiftLateral(offset));
}
// find the intersection points between all of the segments
LinePath boundPoints = new LinePath();
// add the first point
boundPoints.Add(segs[0].P0);
// loop through the stuff
for (int i = 0; i < segs.Count-1; i++) {
// find the intersection
Line l0 = (Line)segs[i];
Line l1 = (Line)segs[i+1];
Coordinates pt;
if (l0.Intersect(l1, out pt)) {
// figure out the angle of the intersection
double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector));
double angle2 = Math.Sin(l0.UnitVector.Cross(l1.UnitVector));
// determine if it's a left or right turn
bool leftTurn = angle2 > 0;
double f = 2.5;
if ((leftTurn && offset > 0) || (!leftTurn && offset < 0)) {
// left turn and looking for left bound or right turn and looking for right bound
f = 0.5;
}
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angle/(Math.PI/2.0),1.25)*f + 1;
// get the line formed by pt and the corresponding path point
Coordinates boundLine = pt - path[i+1];
boundPoints.Add(widthFactor*Math.Abs(offset)*boundLine.Normalize() + path[i+1]);
}
else {
boundPoints.Add(segs[i].P1);
}
}
// add the last point
boundPoints.Add(segs[segs.Count-1].P1);
return boundPoints;
}
private void ProcessReverse()
{
double planningDistance = GetPlanningDistance();
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
Console.WriteLine("cur rndf path count: " + curRndfPath.Count + ", " + curRndfPath.PathLength);
Console.WriteLine("cur rndf path zero point valid: " + curRndfPath.ZeroPoint.Valid + ", loc: " + curRndfPath.ZeroPoint.Location + ", index: " + curRndfPath.ZeroPoint.Index);
Console.WriteLine("planning dist: " + planningDistance + ", stop dist: " + GetSpeedCommandStopDistance());
// get the path in reverse
double dist = -(planningDistance + TahoeParams.RL + 2);
LinePath targetPath = curRndfPath.SubPath(curRndfPath.ZeroPoint, ref dist);
if (dist < 0)
{
targetPath.Add(curRndfPath[0] - curRndfPath.GetSegment(0).Vector.Normalize(-dist));
}
Console.WriteLine("target path count: " + targetPath.Count + ", " + targetPath.PathLength);
Console.WriteLine("target path zero point valud: " + targetPath.ZeroPoint.Valid);
// generate a box by shifting the path
LinePath leftBound = targetPath.ShiftLateral(rndfPathWidth / 2.0);
LinePath rightBound = targetPath.ShiftLateral(-rndfPathWidth / 2.0);
double leftStartDist, rightStartDist;
GetLaneBoundStartDists(targetPath, rndfPathWidth, out leftStartDist, out rightStartDist);
leftBound = leftBound.RemoveBefore(leftBound.AdvancePoint(leftBound.StartPoint, leftStartDist));
rightBound = rightBound.RemoveBefore(rightBound.AdvancePoint(rightBound.StartPoint, rightStartDist));
AddTargetPath(targetPath, 0.0025);
AddLeftBound(leftBound, false);
AddRightBound(rightBound, false);
avoidanceBasePath = targetPath;
double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance);
smootherBasePath = targetPath.SubPath(targetPath.StartPoint, targetDist);
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
settings.endingPositionFixed = true;
settings.endingPositionMax = rndfPathWidth / 2.0;
settings.endingPositionMin = -rndfPathWidth / 2.0;
settings.Options.reverse = true;
Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
SmoothAndTrack(commandLabel, true);
}
public void ReformPath()
{
List <Coordinates> coords = new List <Coordinates>();
coords.Add(this.Partitions[0].Initial.Position);
foreach (ArbiterLanePartition alp in this.Partitions)
{
coords.AddRange(alp.NotInitialPathCoords());
}
this.laneLinePath = new LinePath(coords);
}
// Update is called once per frame
void Update()
{
if (playerControlled)
{
playerControlledUpdate();
}
else if (patrolPath != null)
{
currentPath = patrolPath;
}
}
public void GetLaneChangeModels(LinePath startingRndfPath, double startingRndfPathWidth, int startingNumLanesLeft, int startingNumLanesRight,
LinePath endingRndfPath, double endingRndfPathWidth, bool changeLeft, CarTimestamp rndfPathTimestamp,
out ILaneModel startingLaneModel, out ILaneModel endingLaneModel)
{
LocalRoadModel localRoadModel = this.localRoadModel;
// get the selected lane for the starting path
int startingSelectedLane = SelectLane(localRoadModel, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp, startingNumLanesLeft, startingNumLanesRight);
// calculate the number of lanes left and right for the ending lane
int endingNumLanesLeft = startingNumLanesLeft;
int endingNumLanesRight = startingNumLanesRight;
if (changeLeft)
{
endingNumLanesLeft--;
endingNumLanesRight++;
}
else
{
endingNumLanesLeft++;
endingNumLanesRight--;
}
// get the selected lane for the ending path
int endingSelectedLane = SelectLane(localRoadModel, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp, endingNumLanesLeft, endingNumLanesRight);
// check if either is invalid or the difference does not line up with the change direction
int deltaExpected = changeLeft ? 1 : -1; // starting - ending
if (startingSelectedLane < 0 || endingSelectedLane < 0 || (startingSelectedLane - endingSelectedLane) != deltaExpected)
{
// this is some invalid stuff
// we won't use either of them since we're not sure which one is truly valid
startingLaneModel = GetLaneModel(localRoadModel, -1, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
endingLaneModel = GetLaneModel(localRoadModel, -1, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);
// mark that we did reject
didRejectLast = true;
}
else
{
// looks like the lane model selection was valid
startingLaneModel = GetLaneModel(localRoadModel, startingSelectedLane, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
endingLaneModel = GetLaneModel(localRoadModel, endingSelectedLane, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);
// mark that we did not reject
didRejectLast = false;
}
// figure out what we want to send to the ui
// for now, just send starting lane model
SendLaneModelToUI(startingLaneModel, rndfPathTimestamp);
}
private static Polyline readLineString(Stream stream, WKBByteOrder byteOrder)
{
LinePath path = new LinePath();
path.Vertices = readCoordsList(stream, byteOrder);
Polyline pl = new Polyline();
pl.Paths.Add(path);
return(pl);
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void ZoneTurnInfo(ArbiterInterconnect ai, ArbiterPerimeterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
//Coordinates centerVec = entry.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center - entry.Position;
Coordinates centerVec = ai.InterconnectPath[1] - ai.InterconnectPath[0];
centerVec = centerVec.Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, entry.Position + centerVec });
leftBound = finalPath.ShiftLateral(TahoeParams.T * 2.0);
rightBound = finalPath.ShiftLateral(-TahoeParams.T * 2.0);
}
private void OnEnable()
{
targetScript = (LinePathComponent)target;
linePath = targetScript.Path;
settings = targetScript.EditorOnlyToolSettings;
if (!Safety())
{
SceneView.currentDrawingSceneView.ShowNotification(new GUIContent(PathEditorUtility.EditorUnavailable));
return;
}
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.ignorableObstacles = new List<int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List<int>();
this.decorators = new List<BehaviorDecorator>();
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.ignorableObstacles = new List <int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List <int>();
this.decorators = new List <BehaviorDecorator>();
}
public bool DoesIntersect(LinePath path)
{
foreach (LineSegment ls in path.GetSegmentEnumerator())
{
if (DoesIntersect(ls))
{
return(true);
}
}
return(false);
}
private static Polygon GenerateSimplePolygon(LinePath path, double width)
{
// here is default partition polygon
LinePath alplb = path.ShiftLateral(-width / 2.0);
LinePath alprb = path.ShiftLateral(width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
return(new Polygon(alpdefaultPoly));
}
/// <summary>
/// Find minimum and maximum Xs and Ys for given path
/// </summary>
/// <param name="path">Path to get limits from</param>
/// <param name="minX">Minimum X of path</param>
/// <param name="maxX">Maximum X of path</param>
/// <param name="minY">Minimum Y of path</param>
/// <param name="maxY">Maximum Y of path</param>
private void FindMinMax(LinePath path, ref double minX, ref double maxX, ref double minY, ref double maxY)
{
int totalPoints = path.Count;
for (int i = 0; i < totalPoints; i++)
{
minX = Math.Min(minX, path[i].X);
minY = Math.Min(minY, path[i].Y);
maxX = Math.Max(maxX, path[i].X);
maxY = Math.Max(maxY, path[i].Y);
}
}
public void ReformPath()
{
List <Coordinates> coords = new List <Coordinates>();
foreach (ArbiterUserPartition aup in this.UserPartitions)
{
coords.Add(aup.InitialGeneric.Position);
}
coords.Add(this.UserPartitions[this.UserPartitions.Count - 1].FinalGeneric.Position);
this.PartitionPath = new LinePath(coords);
this.SetDefaultSparsePolygon();
}
private LinePath ConvertPath(UrbanChallenge.Common.Path.Path p)
{
LinePath lineList = new LinePath();
lineList.Add(p[0].Start);
for (int i = 0; i < p.Count; i++)
{
lineList.Add(p[i].End);
}
return(lineList);
}
public Vector2 getSteering(LinePath path)
{
Vector2 targetPosition;
// If the path has only one node then just go to that position;
if (path.Length == 1)
{
targetPosition = path[0];
}
// Else find the closest spot on the path to the character and go to that instead.
else
{
/* Find the final destination of the character on this path */
Vector2 finalDestination = (pathDirection > 0) ? path[path.Length - 1] : path[0];
/* If we are close enough to the final destination then either stop moving or reverse if
* the character is set to loop on paths */
if (Vector2.Distance(transform.position, finalDestination) < stopRadius)
{
if (pathLoop)
{
pathDirection *= -1;
}
else
{
rb.velocity = Vector2.zero;
return(Vector2.zero);
}
}
/* Get the param for the closest position point on the path given the character's position */
float param = path.getParam(transform.position);
/* Move down the path */
param += pathDirection * pathOffset;
/* Make sure we don't move past the beginning or end of the path */
if (param < 0)
{
param = 0;
}
else if (param > path.maxDist)
{
param = path.maxDist;
}
/* Set the target position */
targetPosition = path.getPosition(param);
}
return(steeringUtils.arrive(targetPosition));
}
/// <summary>
/// Transforms coordinates of the line path.
/// </summary>
/// <param name="linePath">Line path to transform</param>
/// <param name="transform">The transformation to apply</param>
/// <returns>The transformed line path</returns>
public static LinePath TransformLinePath(LinePath linePath, IMathTransform transform)
{
List <double[]> points = new List <double[]>();
for (int i = 0; i < linePath.Vertices.Count; i++)
{
points.Add(new double[2] {
linePath.Vertices[i].X, linePath.Vertices[i].Y
});
}
return(new LinePath(transform.TransformList(points)));
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, Polygon intersectionPolygon, IEnumerable <int> ignorableObstacles, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.intersectionPolygon = intersectionPolygon;
this.ignorableObstacles = ignorableObstacles != null ? new List <int>(ignorableObstacles) : new List <int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List <int>();
this.decorators = new List <BehaviorDecorator>();
}
public Polygon GetForwardPolygon(double distanceForward, double halfWidth)
{
LinePath lp1 = new LinePath(new Coordinates[] { this.Front, this.Front + this.Heading.Normalize(distanceForward) });
LinePath lpL = lp1.ShiftLateral(halfWidth);
LinePath lpR = lp1.ShiftLateral(-halfWidth);
List <Coordinates> polyCoors = new List <Coordinates>();
polyCoors.AddRange(lpL);
polyCoors.AddRange(lpR);
Polygon p = Polygon.GrahamScan(polyCoors);
return(p);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, Polygon intersectionPolygon, IEnumerable<int> ignorableObstacles, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.intersectionPolygon = intersectionPolygon;
this.ignorableObstacles = ignorableObstacles != null ? new List<int>(ignorableObstacles) : new List<int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List<int>();
this.decorators = new List<BehaviorDecorator>();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
public void GetLaneChangeModels(LinePath startingRndfPath, double startingRndfPathWidth, int startingNumLanesLeft, int startingNumLanesRight,
LinePath endingRndfPath, double endingRndfPathWidth, bool changeLeft, CarTimestamp rndfPathTimestamp,
out ILaneModel startingLaneModel, out ILaneModel endingLaneModel)
{
LocalRoadModel localRoadModel = this.localRoadModel;
// get the selected lane for the starting path
int startingSelectedLane = SelectLane(localRoadModel, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp, startingNumLanesLeft, startingNumLanesRight);
// calculate the number of lanes left and right for the ending lane
int endingNumLanesLeft = startingNumLanesLeft;
int endingNumLanesRight = startingNumLanesRight;
if (changeLeft) {
endingNumLanesLeft--;
endingNumLanesRight++;
}
else {
endingNumLanesLeft++;
endingNumLanesRight--;
}
// get the selected lane for the ending path
int endingSelectedLane = SelectLane(localRoadModel, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp, endingNumLanesLeft, endingNumLanesRight);
// check if either is invalid or the difference does not line up with the change direction
int deltaExpected = changeLeft ? 1 : -1; // starting - ending
if (startingSelectedLane < 0 || endingSelectedLane < 0 || (startingSelectedLane - endingSelectedLane) != deltaExpected) {
// this is some invalid stuff
// we won't use either of them since we're not sure which one is truly valid
startingLaneModel = GetLaneModel(localRoadModel, -1, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
endingLaneModel = GetLaneModel(localRoadModel, -1, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);
// mark that we did reject
didRejectLast = true;
}
else {
// looks like the lane model selection was valid
startingLaneModel = GetLaneModel(localRoadModel, startingSelectedLane, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
endingLaneModel = GetLaneModel(localRoadModel, endingSelectedLane, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);
// mark that we did not reject
didRejectLast = false;
}
// figure out what we want to send to the ui
// for now, just send starting lane model
SendLaneModelToUI(startingLaneModel, rndfPathTimestamp);
}
public StayInLaneBehavior(ArbiterLaneId targetLane, SpeedCommand speedCommand, IEnumerable<int> ignorableObstacles, LinePath backupPath, double laneWidth, int numLanesLeft, int numLanesRight)
{
this.targetLane = targetLane;
this.speedCommand = speedCommand;
if (ignorableObstacles != null) {
this.ignorableObstacles = new List<int>(ignorableObstacles);
}
else {
this.ignorableObstacles = new List<int>();
}
this.backupPath = backupPath;
this.laneWidth = laneWidth;
this.numLanesLeft = numLanesLeft;
this.numLanesRight = numLanesRight;
}
public static LinePath GetEdgeLine(SideRoadEdge edge)
{
if (edge == null || !edge.isValid || Math.Abs(edge.curbDistance) < TahoeParams.T/2.0 + 0.1) {
return null;
}
LinePath ret = new LinePath();
Coordinates pt1 = new Coordinates(2, 0).Rotate(edge.curbHeading) + new Coordinates(0, edge.curbDistance);
Coordinates pt2 = new Coordinates(10 + TahoeParams.FL, 0).Rotate(edge.curbHeading) + new Coordinates(0, edge.curbDistance);
ret.Add(pt1);
ret.Add(pt2);
return ret;
}
public PathLaneModel(CarTimestamp timestamp, PathRoadModel.LaneEstimate lane)
{
this.path = lane.path;
int dotIndex = lane.paritionID.LastIndexOf('.');
if (dotIndex == -1) {
this.laneID = lane.paritionID + ".not a lane";
}
else {
this.laneID = lane.paritionID.Substring(0, dotIndex);
}
this.width = lane.width;
if (this.width < TahoeParams.T + 2) {
this.width = TahoeParams.T + 2;
}
this.timestamp = timestamp;
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void TurnInfo(ArbiterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
if (entry.NextPartition != null)
{
double distance = entry.NextPartition.Length;
// get left bound
rightBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).RotateM90());
// get right bound
leftBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).Rotate90());
ArbiterWaypoint current = entry.NextPartition.Final;
while (current.NextPartition != null && distance < 50)
{
distance += current.NextPartition.Length;
LineList rtTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).RotateM90());
rightBound.Add(rtTemp[rtTemp.Count - 1]);
LineList ltTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).Rotate90());
leftBound.Add(ltTemp[ltTemp.Count - 1]);
current = current.NextPartition.Final;
}
finalPath = entry.Lane.LanePath(entry, 50.0);
}
else
{
Coordinates final = entry.Position + entry.PreviousPartition.Vector().Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, final });
LinePath lB = finalPath.ShiftLateral(entry.Lane.Width / 2.0);
LinePath rB = finalPath.ShiftLateral(-entry.Lane.Width / 2.0);
leftBound = new LineList(lB);
rightBound = new LineList(rB);
}
}
public static void CalculateMaxPathSpeed(LinePath path, LinePath.PointOnPath startPoint, LinePath.PointOnPath endPoint, ref double maxSpeed)
{
// get the angles
List<Pair<int, double>> angles = path.GetIntersectionAngles(startPoint.Index, endPoint.Index);
foreach (Pair<int, double> angleValue in angles) {
// calculate the desired speed to take the point
double dist = path.DistanceBetween(startPoint, path.GetPointOnPath(angleValue.Left));
// calculate the desired speed at the intersection
double desiredSpeed = 1.5/(angleValue.Right/(Math.PI/2));
// limit the desired speed to 2.5
desiredSpeed = Math.Max(2.5, desiredSpeed);
// calculate the speed we would be allowed to go right now
double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*target_decel*dist);
// check if the desired max speed is lower
if (desiredMaxSpeed < maxSpeed) {
maxSpeed = desiredMaxSpeed;
}
}
}
public LocalLaneModel(LinePath lanePath, double[] laneYVariance, double width, double widthVariance, double probability)
{
// do some checking to make sure stuff is value
if (probability > 0) {
if (lanePath == null) {
throw new ArgumentNullException("lanePath");
}
if (laneYVariance == null) {
throw new ArgumentNullException("laneYVariance");
}
if (laneYVariance.Length != lanePath.Count) {
throw new ArgumentException("Number of lane points and length of lane point variance must match");
}
}
this.lanePath = lanePath;
this.laneYVariance = laneYVariance;
this.width = width;
this.widthVariance = widthVariance;
this.probability = probability;
}
/*public SupraLaneBehavior(
ArbiterLaneId startingLaneId, LinePath startingLanePath, double startingLaneWidth, int startingNumLanesLeft, int startingNumLanesRight,
ArbiterLaneId endingLaneId, LinePath endingLanePath, double endingLaneWidth, int endingNumLanesLeft, int endingNumLanesRight,
SpeedCommand speedCommand, List<int> ignorableObstacles) {
this.startingLaneId = startingLaneId;
this.startingLanePath = startingLanePath;
this.startingLaneWidth = startingLaneWidth;
this.startingNumLanesLeft = startingNumLanesLeft;
this.startingNumLanesRight = startingNumLanesRight;
this.endingLaneId = endingLaneId;
this.endingLanePath = endingLanePath;
this.endingLaneWidth = endingLaneWidth;
this.endingNumLanesLeft = endingNumLanesLeft;
this.endingNumLanesRight = endingNumLanesRight;
this.speedCommand = speedCommand;
this.ignorableObstacles = ignorableObstacles;
}*/
public SupraLaneBehavior(
ArbiterLaneId startingLaneId, LinePath startingLanePath, double startingLaneWidth, int startingNumLanesLeft, int startingNumLanesRight,
ArbiterLaneId endingLaneId, LinePath endingLanePath, double endingLaneWidth, int endingNumLanesLeft, int endingNumLanesRight,
SpeedCommand speedCommand, List<int> ignorableObstacles, Polygon intersectionPolygon)
{
this.startingLaneId = startingLaneId;
this.startingLanePath = startingLanePath;
this.startingLaneWidth = startingLaneWidth;
this.startingNumLanesLeft = startingNumLanesLeft;
this.startingNumLanesRight = startingNumLanesRight;
this.endingLaneId = endingLaneId;
this.endingLanePath = endingLanePath;
this.endingLaneWidth = endingLaneWidth;
this.endingNumLanesLeft = endingNumLanesLeft;
this.endingNumLanesRight = endingNumLanesRight;
this.intersectionPolygon = intersectionPolygon;
this.speedCommand = speedCommand;
this.ignorableObstacles = ignorableObstacles;
}
public ChangeLaneBehavior(ArbiterLaneId startLane, ArbiterLaneId targetLane, bool changeLeft, double maxDist, SpeedCommand speedCommand,
IEnumerable<int> ignorableObstacles, LinePath backupStartLanePath, LinePath backupTargetLanePath, double startWidth, double targetWidth,
int startingNumLanesLeft, int startingNumLanesRight)
{
this.startLane = startLane;
this.targetLane = targetLane;
this.maxDist = maxDist;
this.speedCommand = speedCommand;
this.changeLeft = changeLeft;
this.backupStartLanePath = backupStartLanePath;
this.backupTargetLanePath = backupTargetLanePath;
this.startWidth = startWidth;
this.targetWidth = targetWidth;
this.startingNumLanesLeft = startingNumLanesLeft;
this.startingNumLanesRight = startingNumLanesRight;
if (ignorableObstacles != null) {
this.ignorableObstacles = new List<int>(ignorableObstacles);
}
else {
this.ignorableObstacles = new List<int>();
}
}
public Vector2 getSteering(LinePath path, bool pathLoop, out Vector2 targetPosition)
{
// If the path has only one node then just go to that position;
if (path.Length == 1) {
targetPosition = path[0];
}
// Else find the closest spot on the path to the character and go to that instead.
else {
/* Find the final destination of the character on this path */
Vector2 finalDestination = (pathDirection > 0) ? path[path.Length-1] : path[0];
/* If we are close enough to the final destination then either stop moving or reverse if
* the character is set to loop on paths */
if( Vector2.Distance(transform.position, finalDestination) < stopRadius ) {
if(pathLoop) {
pathDirection *= -1;
} else {
targetPosition = finalDestination;
rb.velocity = Vector2.zero;
return Vector2.zero;
}
}
/* Get the param for the closest position point on the path given the character's position */
float param = path.getParam(transform.position);
/* Move down the path */
param += pathDirection * pathOffset;
/* Set the target position */
targetPosition = path.getPosition(param);
}
return steeringUtils.arrive(targetPosition);
}
public static bool TestBlockage(IList<Polygon> obstaclePolygons, LinePath leftBound, LinePath rightBound, double expandDist, double trackWidth)
{
try
{
Circle robotCircle = new Circle(expandDist, Vector2.Zero);
Polygon robotPoly = robotCircle.ToPolygon(24);
List<BlockageData> obstacles = new List<BlockageData>(obstaclePolygons.Count);
foreach (Polygon poly in obstaclePolygons)
{
Polygon convolvedPoly = null;
try
{
convolvedPoly = Polygon.ConvexMinkowskiConvolution(robotPoly, poly);
}
catch (Exception)
{
// minkowski convolution failed, just expand that shit with the gaheezy inflate method
convolvedPoly = poly.Inflate(expandDist);
}
// add the entry to the obstacle collection
BlockageData data = new BlockageData(convolvedPoly);
obstacles.Add(data);
}
// shrink in the lanes by a half robot width
leftBound = leftBound.ShiftLateral(-trackWidth / 2.0);
rightBound = rightBound.ShiftLateral(-trackWidth / 2.0);
Queue<BlockageData> testQueue = new Queue<BlockageData>();
foreach (BlockageData obs in obstacles)
{
if (obs.convolvedPolygon.DoesIntersect(leftBound))
{
// check if this hits the right bound
if (obs.convolvedPolygon.DoesIntersect(rightBound))
{
// this extends across the entire lane, the segment is blocked
return true;
}
else
{
testQueue.Enqueue(obs);
obs.searchMark = true;
}
}
}
while (testQueue.Count > 0)
{
BlockageData obs = testQueue.Dequeue();
foreach (BlockageData neighbor in obstacles)
{
if (!neighbor.searchMark && Polygon.TestConvexIntersection(obs.convolvedPolygon, neighbor.convolvedPolygon))
{
if (neighbor.convolvedPolygon.DoesIntersect(rightBound))
return true;
testQueue.Enqueue(neighbor);
neighbor.searchMark = true;
}
}
}
return false;
}
catch (Exception) { }
return false;
}
private void OnEnable()
{
targetScript = (LinePathComponent)target;
linePath = targetScript.Path;
settings = targetScript.EditorOnlyToolSettings;
if (!Safety())
{
SceneView.currentDrawingSceneView.ShowNotification(new GUIContent(PathEditorUtility.EditorUnavailable));
return;
}
}
private static double getPathLength(List<ICoordinate> path)
{
LinePath tempPart = new LinePath();
tempPart.Vertices = path;
return tempPart.Length();
}
private FollowingTitle getFollowingTitle(Graphics g, LinePath part, double length, string label, TitleStyle titleStyle, BoundingRectangle viewBox, double scaleFactor)
{
StringFormat format = StringFormat.GenericTypographic;
SizeF sizeF;
PointF zeroPoint = new PointF(0, 0);
using (Font f = titleStyle.GetFont())
{
sizeF = g.MeasureString(label, f, zeroPoint, format);
// label length must be less than the length of the line
if (sizeF.Width / scaleFactor < length)
{
LinePath tempPart = new LinePath();
foreach (ICoordinate p in part.Vertices)
tempPart.Vertices.Add(p);
int vertexNumber = 0;
ICoordinate centerPoint = getDistantPoint(tempPart.Vertices, length / 2, out vertexNumber);
// if the point of the proposed mid-label misses the display area of the map, the inscription does not appear
if (!viewBox.ContainsPoint(centerPoint))
return null;
// simplify the line
tempPart.Weed(sizeF.Height / scaleFactor / 2);
//get the center point of the simplified line
centerPoint = getDistantPoint(tempPart.Vertices, length / 2, out vertexNumber);
List<double> leftPointsRotationDeltas = new List<double>();
List<double> rightPointsRotationDeltas = new List<double>();
// coordinates of points on the left of the middle of the inscription
IList<ICoordinate> leftPoints =
getLeftPoints(tempPart.Vertices,
centerPoint,
sizeF.Width / 2 / scaleFactor,
vertexNumber,
sizeF.Height / 2 / scaleFactor,
leftPointsRotationDeltas);
// coordinates of the points to the right of the middle of the inscription
IList<ICoordinate> rightPoints =
getRightPoints(tempPart.Vertices,
centerPoint,
sizeF.Width / 2 / scaleFactor,
vertexNumber,
sizeF.Height / 2 / scaleFactor,
rightPointsRotationDeltas);
//coordinates of the vertices of the broken line, which will be located along the inscription
List<ICoordinate> points = leftPoints.ToList();
points.AddRange(rightPoints);
// shifts of the inscriptions associated with break lines
List<double> rotationDeltas = leftPointsRotationDeltas;
rotationDeltas.AddRange(rightPointsRotationDeltas);
for (int i = 0; i < points.Count; i++)
points[i] = PlanimetryEnvironment.NewCoordinate((points[i].X - viewBox.MinX) * scaleFactor,
(viewBox.MaxY - points[i].Y) * scaleFactor);
for (int i = 0; i < rotationDeltas.Count; i++)
rotationDeltas[i] = rotationDeltas[i] * scaleFactor;
//determine the direction of following labels (direct or reverse)
double forwardWeight = 0;
double backwardWeight = 0;
for (int i = 1; i < points.Count; i++)
{
Segment s = new Segment(PlanimetryEnvironment.NewCoordinate(points[i].X, points[i].Y),
PlanimetryEnvironment.NewCoordinate(points[i - 1].X, points[i - 1].Y));
int quadNumber = pointQuadrantNumber(PlanimetryEnvironment.NewCoordinate(s.V1.X - s.V2.X, s.V1.Y - s.V2.Y));
if (quadNumber == 1 || quadNumber == 4)
forwardWeight += s.Length();
else
backwardWeight += s.Length();
}
if (backwardWeight > forwardWeight)
{
points.Reverse();
rotationDeltas.Reverse();
}
// inscriptions along the route should not be a large number of points
if (label.Length > points.Count - 2)
{
List<int> subStringLengths = new List<int>();
List<double> deltas = new List<double>();
LinePath p1 = new LinePath(points.ToArray());
double l = p1.Length();
// partition of the inscription on the straight parts, the calculation of displacement
int startIndex = 0;
for (int i = 1; i < points.Count; i++)
{
double currentDistance = PlanimetryAlgorithms.Distance(points[i - 1], points[i]);
if (deltas.Count > 0)
if (deltas[deltas.Count - 1] < currentDistance)
currentDistance -= deltas[deltas.Count - 1];
//subtract the offset associated with line breaks
currentDistance -= rotationDeltas[i - 1];
if (i < rotationDeltas.Count)
currentDistance -= rotationDeltas[i];
int currentLength = (int)(currentDistance / l * label.Length);
if (startIndex + currentLength > label.Length)
currentLength = label.Length - startIndex;
subStringLengths.Add(currentLength > 0 ? currentLength : 0);
string subString;
if (subStringLengths[i - 1] > 0)
subString = label.Substring(startIndex, subStringLengths[i - 1]);
else
subString = string.Empty;
float width1, width2, width3;
width1 = width2 = width3 = g.MeasureString(subString, f, zeroPoint, format).Width;
if (!string.IsNullOrEmpty(subString))
{
if (subStringLengths[i - 1] > 1)
{
width2 = g.MeasureString(label.Substring(startIndex, subStringLengths[i - 1] - 1), f, zeroPoint, format).Width;
if (Math.Abs(width2 - currentDistance) < Math.Abs(width1 - currentDistance))
{
subStringLengths[i - 1] = subStringLengths[i - 1] - 1;
width1 = width2;
}
}
if (label.Length > subStringLengths[i - 1] + startIndex)
{
width3 = g.MeasureString(label.Substring(startIndex, subStringLengths[i - 1] + 1), f, zeroPoint, format).Width;
if (Math.Abs(width3 - currentDistance) < Math.Abs(width1 - currentDistance) &&
Math.Abs(width3 - currentDistance) < sizeF.Width / label.Length / 6)
{
subStringLengths[i - 1] = subStringLengths[i - 1] + 1;
width1 = width3;
}
}
}
deltas.Add(0.5 * (width1 - currentDistance));
if (currentLength > 0)
startIndex += currentLength;
}
int sum = 0;
int maxZeroLengths = 0;
int zeroLengthsCount = 0;
foreach (int k in subStringLengths)
{
if (k <= 0)
{
zeroLengthsCount++;
if (maxZeroLengths < zeroLengthsCount)
maxZeroLengths = zeroLengthsCount;
}
else
zeroLengthsCount = 0;
sum += k;
}
if (maxZeroLengths > 1)
return null;
int lastIndex = subStringLengths.Count() - 1;
if (lastIndex >= 0)
{
subStringLengths[lastIndex] += label.Length - sum;
if (subStringLengths[lastIndex] < 0)
{
subStringLengths[lastIndex - 1] -= subStringLengths[lastIndex];
subStringLengths[lastIndex] = 0;
}
}
FollowingTitle followingTitle = new FollowingTitle();
startIndex = 0;
double? previousAngle = null;
for (int i = 0; i < subStringLengths.Count(); i++)
{
if (subStringLengths[i] <= 0)
continue;
if (startIndex + subStringLengths[i] > label.Length)
return null;
SizeF size;
size = g.MeasureString(label.Substring(startIndex, subStringLengths[i]), f, zeroPoint, format);
int x0 = (i == 0 ? 0 : (int)Math.Round(deltas[i - 1] + rotationDeltas[i - 1]));
PointF[] v = new PointF[4];
v[0].X = x0;
v[0].Y = -size.Height / 2;
v[1].X = size.Width + x0;
v[1].Y = -size.Height / 2;
v[2].X = size.Width + x0;
v[2].Y = size.Height / 2;
v[3].X = x0;
v[3].Y = size.Height / 2;
float angle = (float)(180 / Math.PI * getAngle(PlanimetryEnvironment.NewCoordinate(Math.Abs(points[i].X * 2), points[i].Y),
points[i],
points[i + 1],
false));
if (previousAngle != null)
{
double angleDelta = Math.Abs(angle - previousAngle.Value);
if (angleDelta > 45 && 360 - angleDelta > 45)
return null;
}
previousAngle = angle;
g.TranslateTransform((float)points[i].X, (float)points[i].Y);
g.RotateTransform(-(angle % 360));
g.Transform.TransformPoints(v);
for (int j = 0; j < 4; j++)
{
v[j].X = (float)(v[j].X / scaleFactor + viewBox.MinX);
v[j].Y = (float)(viewBox.MaxY - v[j].Y / scaleFactor);
}
FollowingTitleElement element =
new FollowingTitleElement(new PointF((float)points[i].X, (float)points[i].Y),
-(angle % 360),
new PointF(x0, -size.Height / 2),
label.Substring(startIndex, subStringLengths[i]),
v[0], v[1], v[2], v[3]);
startIndex += subStringLengths[i];
followingTitle.AddElement(element);
g.ResetTransform();
}
return followingTitle.EnvelopePolygon == null ? null : followingTitle;
}
}
}
return null;
}
private Contour getPathContour(LinePath path)
{
Contour c = new Contour();
IList<ICoordinate> pv = path.Vertices;
if (pv.Count > 1)
{
// начало линии
addCap(_lineStartCap,
c.Vertices,
CapLocation.Start,
pv[0].X, pv[0].Y,
pv[1].X, pv[1].Y);
int cnt = path.Vertices.Count;
for (int i = 0; i < cnt - 2; i++)
addJoin(_lineJoin,
c.Vertices,
pv[i].X, pv[i].Y,
pv[i + 1].X, pv[i + 1].Y,
pv[i + 2].X, pv[i + 2].Y);
// окончание линии
addCap(_lineEndCap,
c.Vertices,
CapLocation.End,
pv[cnt - 2].X, pv[cnt - 2].Y,
pv[cnt - 1].X, pv[cnt - 1].Y);
// соединения линий "по правую сторону"
for (int i = cnt - 1; i > 1; i--)
addJoin(_lineJoin,
c.Vertices,
pv[i].X, pv[i].Y,
pv[i - 1].X, pv[i - 1].Y,
pv[i - 2].X, pv[i - 2].Y);
}
return c;
}
/// <summary>
/// Преобразует ломаную линию в полигон.
/// </summary>
/// <param name="path">Ломаная линия</param>
/// <returns>Контуры полигона</returns>
public IList<Contour> GetPolygon(LinePath path)
{
List<Contour> contours = new List<Contour>();
contours.Add(getPathContour(path));
return contours;
}
