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)); }
private static Polygon GenerateSimplePartitionPolygon(ArbiterLanePartition alp, 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); foreach (ArbiterUserWaypoint auw in alp.UserWaypoints) { alpdefaultPoly.Add(auw.Position); } return(Polygon.GrahamScan(alpdefaultPoly)); }
public static Polygon DefaultLanePolygon(ArbiterLane al) { // fist get the right boundary of the lane LinePath lb = al.LanePath().ShiftLateral(-al.Width / 2.0); LinePath rb = al.LanePath().ShiftLateral(al.Width / 2.0); rb.Reverse(); List <Coordinates> defaultPoly = lb; defaultPoly.AddRange(rb); // start the polygon Polygon poly = new Polygon(defaultPoly); return(poly); }
public static Polygon LanePolygon(ArbiterLane al) { // fist get the right boundary of the lane LinePath lb = al.LanePath().ShiftLateral(-al.Width / 2.0); LinePath rb = al.LanePath().ShiftLateral(al.Width / 2.0); rb.Reverse(); List <Coordinates> defaultPoly = lb; defaultPoly.AddRange(rb); // start the polygon Polygon poly = new Polygon(defaultPoly); // loop through partitions foreach (ArbiterLanePartition alp in al.Partitions) { //if (alp.Initial.PreviousPartition != null && alp.Final.NextPartition != null) //{ // get the good polygon Polygon pPoly = PartitionPolygon(alp); // check not null if (pPoly != null) { poly = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { poly, pPoly })); } //} } // circles for intersections of partitions foreach (ArbiterLanePartition alp in al.Partitions) { if (alp.Final.NextPartition != null) { double interAngle = Math.Abs(FinalIntersectionAngle(alp)); if (interAngle > 15) { Circle connect = new Circle((alp.Lane.Width / 2.0) + (interAngle / 15.0 * 0.5), alp.Final.Position); poly = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { poly, connect.ToPolygon(16) })); } } } // return the polygon return(poly); }
/// <summary> /// Updates the current vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> public void Update(ArbiterLane lane, VehicleState state) { // get the forward path LinePath p = lane.LanePath().Clone(); p.Reverse(); // 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; // get clsoest foreach (VehicleAgent va in vas) { // get position of front Coordinates frontPos = va.ClosestPosition; // gets distance from other vehicle to us along the lane double frontDist = lane.DistanceBetween(frontPos, f); if (frontDist >= 0 && frontDist < minDistance) { minDistance = frontDist; closest = va; } } this.CurrentVehicle = closest; this.currentDistance = minDistance; }
/// <summary> /// Parameters to follow the forward vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters Follow(ArbiterLane lane, VehicleState state) { // travelling parameters TravelingParameters tp = new TravelingParameters(); // ignorable obstacles List <int> ignoreVehicle = new List <int>(); ignoreVehicle.Add(CurrentVehicle.VehicleId); // get control parameters ForwardVehicleTrackingControl fvtc = GetControl(lane, state); // init params tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.Decorators = TurnDecorators.NoDecorators; // flag to ignore forward vehicle bool ignoreForward = false; // reversed lane path LinePath lp = lane.LanePath().Clone(); lp.Reverse(); #region Following Control #region Immediate Stop // need to stop immediately if (fvtc.vFollowing == 0.0) { // don't ignore forward ignoreForward = false; // speed command SpeedCommand sc = new ScalarSpeedCommand(0.0); tp.UsingSpeed = true; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #region Distance Stop // stop at distance else if (fvtc.vFollowing < 0.7 && CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 && fvtc.xSeparation > fvtc.xAbsMin) { // ignore forward vehicle ignoreForward = true; // speed command SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood); tp.UsingSpeed = false; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #region Normal Following // else normal else { // ignore the forward vehicle as we are tracking properly ignoreForward = true; // speed command SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing); tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.UsingSpeed = true; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #endregion // check ignore if (ignoreForward) { List <int> ignorable = new List <int>(); ignorable.Add(this.CurrentVehicle.VehicleId); tp.VehiclesToIgnore = ignorable; } else { tp.VehiclesToIgnore = new List <int>(); } // return parameterization return(tp); }
public void Render(IGraphics g, WorldTransform wt) { LocalLaneModel laneModel = this.laneModel; if (laneModel == null || laneModel.LanePath == null || laneModel.LanePath.Count <= 1) { return; } // generate the left and right bounds LinePath leftBound = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0); LinePath leftBound2 = null; if (laneModel.WidthVariance > 0.01) { leftBound2 = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0 + Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0); } LinePath rightBound = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0); LinePath rightBound2 = null; if (laneModel.WidthVariance > 0.01) { rightBound2 = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0 - Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0); } // generate a polygon of the confidence region of the center line Polygon centerConfLeft = null; Polygon centerConfRight = null; Polygon centerConfFull = null; if (sigma > 0 && laneModel.LaneYVariance != null && laneModel.LaneYVariance.Length == laneModel.LanePath.Count) { float varianceThreshold = 9f * 9f; if (laneModel.LaneYVariance[0] > varianceThreshold) { varianceThreshold = 600; } int numPoints; for (numPoints = 0; numPoints < laneModel.LaneYVariance.Length; numPoints++) { if (laneModel.LaneYVariance[numPoints] > varianceThreshold) { break; } } double[] leftDist = new double[numPoints]; double[] rightDist = new double[numPoints]; double prevDist = 0; for (int i = 0; i < numPoints; i++) { double dist = laneModel.LaneYVariance[i]; if (dist > 0.01) { dist = Math.Sqrt(dist) * sigma; leftDist[i] = dist; rightDist[i] = -dist; prevDist = dist; } else { leftDist[i] = prevDist; rightDist[i] = -prevDist; } } // get the subset of the lane model we're interested in LinePath subset = laneModel.LanePath.SubPath(0, numPoints - 1); LinePath left = subset.ShiftLateral(leftDist); LinePath right = subset.ShiftLateral(rightDist); Coordinates midTop = (left[left.Count - 1] + right[right.Count - 1]) / 2.0; Coordinates midBottom = (left[0] + right[0]) / 2.0; centerConfLeft = new Polygon(numPoints + 2); centerConfLeft.Add(midTop); centerConfLeft.Add(midBottom); centerConfLeft.AddRange(left); centerConfRight = new Polygon(numPoints + 2); centerConfRight.Add(midTop); centerConfRight.Add(midBottom); centerConfRight.AddRange(right); centerConfFull = new Polygon(numPoints * 2); right.Reverse(); centerConfFull.AddRange(right); centerConfFull.AddRange(left); } // draw the shits IPen pen = g.CreatePen(); pen.Width = 1.0f / wt.Scale; pen.Color = color; // first draw the confidence polygon if (centerConfFull != null) { g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfLeft)); g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfRight)); pen.Color = Color.FromArgb(30, color); g.DrawPolygon(pen, Utility.ToPointF(centerConfFull)); } // next draw the center line pen.Color = color; //g.DrawLines(pen, Utility.ToPointF(laneModel.LanePath)); //// next draw the left lane confidence bound //if (leftBound2 != null) { // pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot; // g.DrawLines(pen, Utility.ToPointF(leftBound2)); //} //// draw the right lane confidence bound //if (rightBound2 != null) { // pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot; // g.DrawLines(pen, Utility.ToPointF(rightBound2)); //} // draw the left bound pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Solid; g.DrawLines(pen, Utility.ToPointF(leftBound)); // draw the right bound g.DrawLines(pen, Utility.ToPointF(rightBound)); // draw the model confidence string labelString = laneModel.Probability.ToString("F3"); SizeF stringSize = g.MeasureString(labelString, labelFont); stringSize.Width /= wt.Scale; stringSize.Height /= wt.Scale; RectangleF rect = new RectangleF(Utility.ToPointF(laneModel.LanePath[0]), stringSize); float inflateValue = 4 / wt.Scale; rect.X -= inflateValue; rect.Y -= inflateValue; g.FillRectangle(Color.FromArgb(127, Color.White), rect); g.DrawString(labelString, labelFont, Color.Black, Utility.ToPointF(laneModel.LanePath[0])); prevLeftBound = leftBound; prevRightBound = rightBound; }
public void GenerateInterconnectPolygon(ArbiterInterconnect ai) { List <Coordinates> polyPoints = new List <Coordinates>(); try { // width double width = 3.0; if (ai.InitialGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.TurnDirection == ArbiterTurnDirection.UTurn || ai.TurnDirection == ArbiterTurnDirection.Straight || !(ai.InitialGeneric is ArbiterWaypoint) || !(ai.FinalGeneric is ArbiterWaypoint)) { LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0); LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0); polyPoints.AddRange(lp); polyPoints.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(polyPoints); if (ai.TurnDirection == ArbiterTurnDirection.UTurn) { List <Coordinates> updatedPts = new List <Coordinates>(); LinePath interTmp = ai.InterconnectPath.Clone(); Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position; interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0); interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0); lp = interTmp.ShiftLateral(TahoeParams.VL); rp = interTmp.ShiftLateral(-TahoeParams.VL); updatedPts.AddRange(lp); updatedPts.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(updatedPts); } } else { // polygon points List <Coordinates> interPoints = new List <Coordinates>(); // waypoint ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric; ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric; // left and right path LinePath leftPath = new LinePath(); LinePath rightPath = new LinePath(); // some initial points LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position }); LinePath il = initialPath.ShiftLateral(width / 2.0); LinePath ir = initialPath.ShiftLateral(-width / 2.0); leftPath.Add(il[1]); rightPath.Add(ir[1]); // some final points LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position }); LinePath fl = finalPath.ShiftLateral(width / 2.0); LinePath fr = finalPath.ShiftLateral(-width / 2.0); leftPath.Add(fl[0]); rightPath.Add(fr[0]); // initial and final paths Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position); Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position); // get where the paths intersect and vector to normal path Coordinates c; iPath.Intersect(fPath, out c); Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c; Coordinates center = c + vector.Normalize((vector.Length / 2.0)); // get width expansion Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0); double iRot = -iVec.ArcTan; Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0); fVec = fVec.Rotate(iRot); double fDeg = fVec.ToDegrees(); double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI; double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0; // get inner point (small scale) Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0); // get outer Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0); if (ai.TurnDirection == ArbiterTurnDirection.Right) { rightPath.Insert(1, innerPoint); ai.InnerCoordinates = rightPath; leftPath.Reverse(); leftPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } else { leftPath.Insert(1, innerPoint); ai.InnerCoordinates = leftPath; rightPath.Reverse(); rightPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } } } catch (Exception e) { Console.WriteLine("error generating turn polygon: " + ai.ToString()); ai.TurnPolygon = ai.DefaultPoly(); } }
/// <summary> /// Generate the traveling parameterization for the desired behaivor /// </summary> /// <param name="lane"></param> /// <param name="navStopSpeed"></param> /// <param name="navStopDistance"></param> /// <param name="navStop"></param> /// <param name="navStopType"></param> /// <param name="state"></param> /// <returns></returns> private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state) { // get min dist double distanceCutOff = CoreCommon.OperationalStopDistance; // turn direction default List <BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // create new params TravelingParameters tp = new TravelingParameters(); #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; // get lane path LinePath lp = lane.LanePath().Clone(); lp.Reverse(); #region Distance Cutoff // check if distance is less than cutoff if (navStopDistance < distanceCutOff) { // default behavior tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance); Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List <int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); // stopping so not using speed param usingSpeed = false; IState nextState = CoreCommon.CorePlanningState; m = new Maneuver(b, nextState, decorators, state.Timestamp); } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // get lane ArbiterLane al = lane; // default behavior tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)); Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List <int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false)); // standard behavior is fine for maneuver m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp); } #endregion #endregion #region Parameterize tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = navStopDistance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = navStopSpeed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.VehiclesToIgnore = new List <int>(); // return navigation params return(tp); #endregion }
public static Polygon PartitionPolygon(ArbiterLanePartition alp) { if (alp.Initial.PreviousPartition != null && alp.Final.NextPartition != null && alp.Length < 30.0 && alp.Length > 4.0) { // get partition turn direction ArbiterTurnDirection pTD = PartitionTurnDirection(alp); // check if angles of previous and next are such that not straight through if (pTD != ArbiterTurnDirection.Straight) { // get partition poly ArbiterInterconnect tmpAi = alp.ToInterconnect; tmpAi.TurnDirection = pTD; GenerateInterconnectPolygon(tmpAi); Polygon pPoly = tmpAi.TurnPolygon; // here is default partition polygon LinePath alplb = alp.PartitionPath.ShiftLateral(-alp.Lane.Width / 2.0); LinePath alprb = alp.PartitionPath.ShiftLateral(alp.Lane.Width / 2.0); alprb.Reverse(); List <Coordinates> alpdefaultPoly = alplb; alpdefaultPoly.AddRange(alprb); // get full poly pPoly.AddRange(alpdefaultPoly); pPoly = Polygon.GrahamScan(pPoly); return(pPoly); } } else if (alp.Length >= 30) { Polygon pBase = GenerateSimplePartitionPolygon(alp, alp.PartitionPath, alp.Lane.Width); if (alp.Initial.PreviousPartition != null && Math.Abs(FinalIntersectionAngle(alp.Initial.PreviousPartition)) > 15) { // initial portion Coordinates i1 = alp.Initial.Position - alp.Initial.PreviousPartition.Vector().Normalize(15.0); Coordinates i2 = alp.Initial.Position; Coordinates i3 = i2 + alp.Vector().Normalize(15.0); LinePath il12 = new LinePath(new Coordinates[] { i1, i2 }); LinePath il23 = new LinePath(new Coordinates[] { i2, i3 }); LinePath il13 = new LinePath(new Coordinates[] { i1, i3 }); Coordinates iCC = il13.GetClosestPoint(i2).Location; if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0) { il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0); } else { il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0); } LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC); iCCP = il13.AdvancePoint(iCCP, -10.0); il13 = il13.SubPath(iCCP, 20.0); Polygon iBase = GenerateSimplePolygon(il23, alp.Lane.Width); iBase.Add(il13[1]); Polygon iP = Polygon.GrahamScan(iBase); pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP })); } if (alp.Final.NextPartition != null && Math.Abs(FinalIntersectionAngle(alp)) > 15) { // initial portion Coordinates i1 = alp.Final.Position - alp.Vector().Normalize(15.0); Coordinates i2 = alp.Final.Position; Coordinates i3 = i2 + alp.Final.NextPartition.Vector().Normalize(15.0); LinePath il12 = new LinePath(new Coordinates[] { i1, i2 }); LinePath il23 = new LinePath(new Coordinates[] { i2, i3 }); LinePath il13 = new LinePath(new Coordinates[] { i1, i3 }); Coordinates iCC = il13.GetClosestPoint(i2).Location; if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0) { il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0); } else { il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0); } LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC); iCCP = il13.AdvancePoint(iCCP, -10.0); il13 = il13.SubPath(iCCP, 20.0); Polygon iBase = GenerateSimplePolygon(il12, alp.Lane.Width); iBase.Add(il13[0]); Polygon iP = Polygon.GrahamScan(iBase); pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP })); } return(pBase); } // fall out return(null); }
public void GenerateInterconnectPolygon(ArbiterInterconnect ai) { List<Coordinates> polyPoints = new List<Coordinates>(); try { // width double width = 3.0; if (ai.InitialGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.TurnDirection == ArbiterTurnDirection.UTurn || ai.TurnDirection == ArbiterTurnDirection.Straight || !(ai.InitialGeneric is ArbiterWaypoint) || !(ai.FinalGeneric is ArbiterWaypoint)) { LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0); LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0); polyPoints.AddRange(lp); polyPoints.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(polyPoints); if (ai.TurnDirection == ArbiterTurnDirection.UTurn) { List<Coordinates> updatedPts = new List<Coordinates>(); LinePath interTmp = ai.InterconnectPath.Clone(); Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position; interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0); interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0); lp = interTmp.ShiftLateral(TahoeParams.VL); rp = interTmp.ShiftLateral(-TahoeParams.VL); updatedPts.AddRange(lp); updatedPts.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(updatedPts); } } else { // polygon points List<Coordinates> interPoints = new List<Coordinates>(); // waypoint ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric; ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric; // left and right path LinePath leftPath = new LinePath(); LinePath rightPath = new LinePath(); // some initial points LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position }); LinePath il = initialPath.ShiftLateral(width / 2.0); LinePath ir = initialPath.ShiftLateral(-width / 2.0); leftPath.Add(il[1]); rightPath.Add(ir[1]); // some final points LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position }); LinePath fl = finalPath.ShiftLateral(width / 2.0); LinePath fr = finalPath.ShiftLateral(-width / 2.0); leftPath.Add(fl[0]); rightPath.Add(fr[0]); // initial and final paths Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position); Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position); // get where the paths intersect and vector to normal path Coordinates c; iPath.Intersect(fPath, out c); Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c; Coordinates center = c + vector.Normalize((vector.Length / 2.0)); // get width expansion Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0); double iRot = -iVec.ArcTan; Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0); fVec = fVec.Rotate(iRot); double fDeg = fVec.ToDegrees(); double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI; double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0; // get inner point (small scale) Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0); // get outer Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0); if (ai.TurnDirection == ArbiterTurnDirection.Right) { rightPath.Insert(1, innerPoint); ai.InnerCoordinates = rightPath; leftPath.Reverse(); leftPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } else { leftPath.Insert(1, innerPoint); ai.InnerCoordinates = leftPath; rightPath.Reverse(); rightPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } } } catch (Exception e) { Console.WriteLine("error generating turn polygon: " + ai.ToString()); ai.TurnPolygon = ai.DefaultPoly(); } }