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>
/// Check for all waypoints who have exit interconnects that overlaps input and no stop
/// </summary>
/// <param name="exits"></param>
/// <param name="ai"></param>
/// <returns></returns>
private List<IntersectionInvolved> nonStopOverlaps(IEnumerable<ITraversableWaypoint> exits, ArbiterInterconnect ai)
{
// list of exits that have an interconnect which overlaps the interconnect input
List<IntersectionInvolved> nonStopOverlapWaypoints = new List<IntersectionInvolved>();
// get line of the interconnect
Line aiLine = new Line(ai.InitialGeneric.Position, ai.FinalGeneric.Position);
// loop over all exits
foreach (ITraversableWaypoint exit in exits)
{
// make sure not our exit and the exit is not a stop and if exit and other are both waypoints then ways not the same
if (!exit.Equals(ai.InitialGeneric) && !exit.IsStop &&
((!(ai.InitialGeneric is ArbiterWaypoint) || !(exit is ArbiterWaypoint))
|| !((ArbiterWaypoint)ai.InitialGeneric).Lane.Way.Equals(((ArbiterWaypoint)exit).Lane.Way)))
{
// get all interconnects of the exit
foreach (ArbiterInterconnect tmp in exit.Exits)
{
// check relative priority that these are equal or lesser priority
if (ai.ComparePriority(tmp) != -1)
{
// simple check if the interconnect's final is same as input final
if (tmp.FinalGeneric.Equals(ai.FinalGeneric))
{
// check not already added
if (!nonStopOverlapWaypoints.Contains(new IntersectionInvolved(((ITraversableWaypoint)tmp.FinalGeneric).VehicleArea)))
{
// add exit
nonStopOverlapWaypoints.Add(new IntersectionInvolved(exit, exit.VehicleArea, tmp.TurnDirection));
}
}
// otherwise check overlap of interconnects
else
{
// get line of tmp interconnect
Line tmpLine = new Line(tmp.InitialGeneric.Position, tmp.FinalGeneric.Position);
// position of cross
Coordinates intersectionPoint;
// check intersection
bool intersects = aiLine.Intersect(tmpLine, out intersectionPoint) && ai.IsInside(intersectionPoint);
if (intersects)
{
// check not already added
if (!nonStopOverlapWaypoints.Contains(new IntersectionInvolved(((ITraversableWaypoint)tmp.FinalGeneric).VehicleArea)))
{
// add exit
nonStopOverlapWaypoints.Add(new IntersectionInvolved(exit, exit.VehicleArea, tmp.TurnDirection));
}
}
}
}
}
}
}
return nonStopOverlapWaypoints;
}
protected void GetIntersectionPullPath(LinePath startingPath, LinePath endingPath, Polygon intersectionPolygon, bool addStartingPoint, bool addEndingPoint, LinePath targetPath, ref double pullWeight)
{
double angle = Math.Acos(startingPath.EndSegment.UnitVector.Dot(endingPath.GetSegment(0).UnitVector));
// get the centroid of the intersection
Coordinates centroid;
// check if the angle is great than an threshold
if (angle > 10*Math.PI/180.0) {
// intersect the two lines formed by the starting and ending lanes
Line startingLaneLine = new Line(startingPath[startingPath.Count-2], startingPath[startingPath.Count-1]);
Line endingLaneLine = new Line(endingPath[1], endingPath[0]);
// intersect them stuff and see if the point of intersection is between the two lines
Coordinates K;
if (!startingLaneLine.Intersect(endingLaneLine, out centroid, out K) || K.X <= 0 || K.Y <= 0)
return;
}
else {
// if there is no intersection polygon, there isn't much we can do
if (intersectionPolygon == null || intersectionPolygon.Count < 3) {
return;
}
centroid = intersectionPolygon.GetCentroid();
}
// calculate the pull weighting dependent on angle of intersection
// angle 0 -> 0 weighting
// angle 45 -> 0.00025 weighting
// angle 90 -> 0.001 weighting
pullWeight = Math.Pow(angle/(Math.PI/2), 2)*0.001;
// get the relative transform from the behavior timestamp to the current timestamp
RelativeTransform transform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
centroid = transform.TransformPoint(centroid);
if (addStartingPoint) {
targetPath.Add(startingPath.EndPoint.Location);
}
// add the line from exit -> centroid (assuming that exit is already in the target path)
targetPath.Add(centroid);
if (addEndingPoint) {
// add the line from centroid -> entrance
targetPath.Add(endingPath[0]);
}
Services.UIService.PushLineList(targetPath, curTimestamp, "intersection path", true);
Services.Dataset.ItemAs<double>("intersection weight").Add(pullWeight, curTimestamp);
}