private void UpdatePolygon(RobotPose p)
{
Matrix3 transMat = Matrix3.Translation(p.x, p.y) * Matrix3.Rotation(p.yaw - Math.PI / 2.0);
bodyPlygn.Clear();
bodyPlygn.AddRange(transMat.TransformPoints(origPlygn));
}
public static Polygon CreateConvexHull(this Polygons polygons)
{
Polygon allPoints = new Polygon();
foreach (Polygon polygon in polygons)
{
allPoints.AddRange(polygon);
}
return allPoints.CreateConvexHull();
}
public static Polygon CreateConvexHull(Polygons polygons)
{
Polygon allPoints = new Polygon();
foreach (Polygon polygon in polygons)
{
allPoints.AddRange(polygon);
}
return(CreateConvexHull(allPoints));
}
public bool CreatePathInsideBoundary(IntPoint startPointIn, IntPoint endPointIn, Polygon pathThatIsInside, bool optimizePath = true, int layerIndex = -1)
{
if (IsSimpleConvex)
{
return(true);
}
var goodPath = CalculatePath(startPointIn, endPointIn, pathThatIsInside, layerIndex);
if (goodPath)
{
if (optimizePath)
{
OptimizePathPoints(pathThatIsInside);
}
if (pathThatIsInside.Count == 0)
{
if ((startPointIn - endPointIn).Length() > InsetAmount * 3)
{
pathThatIsInside.Add(startPointIn);
pathThatIsInside.Add(endPointIn);
}
else
{
return(true);
}
}
CutIntoSmallSegments(pathThatIsInside);
MovePointsInsideIfPossible(startPointIn, endPointIn, pathThatIsInside);
var cleanPath = pathThatIsInside.CleanClosedPolygon(InsetAmount / 2);
pathThatIsInside.Clear();
pathThatIsInside.AddRange(cleanPath);
// remove any segment that goes to one point and then back to same point (a -> b -> a)
RemoveUTurnSegments(startPointIn, endPointIn, pathThatIsInside);
}
//Remove0LengthSegments(startPointIn, endPointIn, pathThatIsInside);
if (saveBadPathToDisk)
{
AllPathSegmentsAreInsideOutlines(pathThatIsInside, startPointIn, endPointIn, true, layerIndex);
}
CalculatedPath?.Invoke(this, pathThatIsInside, startPointIn, endPointIn);
return(goodPath);
}
public Polygon ProjectRectangle(Rectangle rectangle)
{
Polygon polygon = new Polygon();
// get corners counter clockwise starting on the top left and project them to create the polygon:
polygon.AddRange(new Point[] {
new Point(rectangle.X, rectangle.Y), // top left
new Point(rectangle.X, rectangle.Y + rectangle.Height), // bottom left
new Point(rectangle.X + rectangle.Width, rectangle.Y + rectangle.Height), // bottom right
new Point(rectangle.X + rectangle.Width, rectangle.Y) // top right
}.Select(ProjectPoint));
return(polygon);
}
public void UpdatePose(double x, double y, double theta)
{
this.x = (float)x;
this.y = (float)y;
this.heading = (float)theta;
// Update the polygon that represents the body
Matrix3 transMat = Matrix3.Translation(x, y) * Matrix3.Rotation(heading - Math.PI / 2.0);
//transMat.TransformPointsInPlace(origPlygn);
bodyPlygn.Clear();
bodyPlygn.AddRange(transMat.TransformPoints(origPlygn));
}
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;
}
static void Main(string[] args)
{
#region 枚举与switch case
//Color c = (Color)(new Random()).Next(0, 3);
//switch (c)
//{
// case Color.Red:
// Console.WriteLine("The color is red");
// break;
// case Color.Green:
// Console.WriteLine("The color is green");
// break;
// case Color.Blue:
// Console.WriteLine("The color is blue");
// break;
// default:
// Console.WriteLine("The color is unknown.");
// break;
//}
#endregion
#region 值类型与引用类型测试
//Type t = typeof(DateTime);
//Type t = typeof(string);
//Type t = typeof(IShape);
//Type t = typeof(Action);
//Type t = typeof(int[]);
//if (t.IsValueType)
//{
// Console.WriteLine($"{t.Name} is ValueType");
//}
//else
// Console.WriteLine($"{t.Name} is ReferenceType");
#endregion
#region 排序算法：冒泡排序与快速排序
/*
* //int[] arr = new int[] { 9, 3, 1, 4, 2, 7, 8, 6, 5, -100, 0, -30, -88 };
* //List<int> list = new List<int>(arr);
*
* Random rd = new Random();
* int n = 10000000;
* List<int> list = new List<int>(n);
* for (int i = 0; i < n; i++)
* {
* list.Add(rd.Next(1, n * 100));
* }
*
* //Console.WriteLine($"排序前的数据：");
* //Console.WriteLine(string.Join(',', arr));
*
* //DateTime dt1 = DateTime.Now;
* //Sort.BubbleSort(arr);
* //DateTime dt2 = DateTime.Now;
*
* //Console.WriteLine($"排序后的数据：");
* //Console.WriteLine(string.Join(',', arr));
*
* //Console.WriteLine($"排序前的时间：{dt1}");
* //Console.WriteLine($"排序后的时间：{dt2}");
*
* Stopwatch st1 = new Stopwatch(); //计时器用来计算算法要用多久时间
* st1.Start();
* //Sort.BubbleSort(list);
* Sort.QuickSort(list);
* st1.Stop();
* Console.WriteLine("运行时间：" + st1.Elapsed);
* //Console.WriteLine(string.Join(',', list));
*/
#endregion
#region SPoint类测试
//SPoint p1 = new SPoint();
//Console.WriteLine(p1);
//SPoint p2 = new SPoint("2", 200, 200, 200);
//Console.WriteLine(p2);
//double d = p1.Distance(p2);
//double d1 = p2.Distance(p1);
//Console.WriteLine($"p1->p2的距离为{d}");
//SPoint p3 = new SPoint(300, 300, 300);
//Console.WriteLine(p3);
//SPoint p4 = new SPoint(400, 400);
//p4.Y = -500;
//Console.WriteLine(p4);
//SPoint p5 = p3;
//p5.Y = -500;
//Console.WriteLine(p5);
//double d4 = SPoint.Distance(p3, p4);
//Circle c1 = new Circle(10);
//Console.WriteLine(c1);
//c1.R = 20;
//Console.WriteLine(c1);
//double rad = ZXY.SurMath.DMStoRad(1.2030);
////0.0234165007975906
//Console.WriteLine($"1 20 30 -> Rad:{rad}");
#endregion
#region Event测试代码
var pub = new Publisher();
var sub1 = new Subscriber("sub1", pub);
var sub2 = new Subscriber("sub2", pub);
// Call the method that raises the event.
pub.DoSomething();
#endregion
#region 继承与多态测试
List <Shape> shapes = new List <Shape>();
shapes.Add(new SPoint());
shapes.Add(new Circle(10));
Polyline pl = new Polyline();
pl.Add(new SPoint(1, 0));
pl.Add(new SPoint(2, 0));
pl.Add(new SPoint(3, 0));
SPoint[] pts = new SPoint[] { new SPoint(4, 0), new SPoint(5, 0), new SPoint(6, 0) };
pl.AddRange(pts);
shapes.Add(pl);
Polyline pl2 = new Polyline();//长度21.494848364936182
pl2.Add(new SPoint(-1, 0));
pl2.Add(new SPoint(2, 3));
pl2.Add(new SPoint(4, 2));
pts = new SPoint[] { new SPoint(4, 4), new SPoint(6, 8), new SPoint(-2, 5) };
pl2.AddRange(pts);
shapes.Add(pl2);
Polygon pg = new Polygon();
pg.Add(new SPoint(1, 0));
pg.Add(new SPoint(2, 0));
pg.Add(new SPoint(3, 0));
pts = new SPoint[] { new SPoint(4, 0), new SPoint(5, 0), new SPoint(6, 0) };
pg.AddRange(pts);
shapes.Add(pg);
Polygon pg2 = new Polygon(); //面积28， 长度26.593867878528968
pg2.AddRange(new SPoint[] {
new SPoint(-1, 0),
new SPoint(2, 3),
new SPoint(4, 2),
new SPoint(4, 4),
new SPoint(6, 8),
new SPoint(-2, 5)
});
shapes.Add(pg2);
//foreach (var item in shapes)
//{
// item.Drawing();
//}
DrawAllShapes(shapes);//此处用一个一般化的函数代替上面的循环
#endregion
}
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();
}
}
public void WriteQueuedGCode(long layerThickness_um)
{
GCodePathConfig lastConfig = null;
int extruderIndex = gcodeExport.GetExtruderIndex();
for (int pathIndex = 0; pathIndex < paths.Count; pathIndex++)
{
var path = paths[pathIndex];
if (extruderIndex != path.ExtruderIndex)
{
extruderIndex = path.ExtruderIndex;
gcodeExport.SwitchExtruder(extruderIndex);
}
else if (path.Retract != RetractType.None)
{
double timeOfMove = 0;
if (path.Config.LineWidthUM == 0)
{
var lengthToStart = (gcodeExport.GetPosition() - path.Polygon[0]).Length();
var lengthOfMove = lengthToStart + path.Polygon.PolygonLength();
timeOfMove = lengthOfMove / 1000.0 / path.Speed;
}
gcodeExport.WriteRetraction(timeOfMove, path.Retract == RetractType.Force);
}
if (lastConfig != path.Config && path.Config != travelConfig)
{
gcodeExport.WriteComment("TYPE:{0}".FormatWith(path.Config.GCodeComment));
lastConfig = path.Config;
}
if (path.FanPercent != -1)
{
gcodeExport.WriteFanCommand(path.FanPercent);
}
if (path.Polygon.Count == 1 &&
path.Config != travelConfig &&
(gcodeExport.GetPositionXY() - path.Polygon[0]).ShorterThen(path.Config.LineWidthUM * 2))
{
//Check for lots of small moves and combine them into one large line
IntPoint nextPosition = path.Polygon[0];
int i = pathIndex + 1;
while (i < paths.Count && paths[i].Polygon.Count == 1 && (nextPosition - paths[i].Polygon[0]).ShorterThen(path.Config.LineWidthUM * 2))
{
nextPosition = paths[i].Polygon[0];
i++;
}
if (paths[i - 1].Config == travelConfig)
{
i--;
}
if (i > pathIndex + 2)
{
nextPosition = gcodeExport.GetPosition();
for (int x = pathIndex; x < i - 1; x += 2)
{
long oldLen = (nextPosition - paths[x].Polygon[0]).Length();
IntPoint newPoint = (paths[x].Polygon[0] + paths[x + 1].Polygon[0]) / 2;
long newLen = (gcodeExport.GetPosition() - newPoint).Length();
if (newLen > 0)
{
gcodeExport.WriteMove(newPoint, path.Speed, (int)(path.Config.LineWidthUM * oldLen / newLen));
}
nextPosition = paths[x + 1].Polygon[0];
}
long lineWidth_um = path.Config.LineWidthUM;
if (paths[i - 1].Polygon[0].Width != 0)
{
lineWidth_um = paths[i - 1].Polygon[0].Width;
}
gcodeExport.WriteMove(paths[i - 1].Polygon[0], path.Speed, lineWidth_um);
pathIndex = i - 1;
continue;
}
}
bool spiralize = path.Config.Spiralize;
if (spiralize)
{
//Check if we are the last spiralize path in the list, if not, do not spiralize.
for (int m = pathIndex + 1; m < paths.Count; m++)
{
if (paths[m].Config.Spiralize)
{
spiralize = false;
}
}
}
if (spiralize) // if we are still in spiralize mode
{
//If we need to spiralize then raise the head slowly by 1 layer as this path progresses.
double totalLength = 0;
long z = gcodeExport.GetPositionZ();
IntPoint currentPosition = gcodeExport.GetPositionXY();
for (int pointIndex = 0; pointIndex < path.Polygon.Count; pointIndex++)
{
IntPoint nextPosition = path.Polygon[pointIndex];
totalLength += (currentPosition - nextPosition).LengthMm();
currentPosition = nextPosition;
}
double length = 0.0;
currentPosition = gcodeExport.GetPositionXY();
for (int i = 0; i < path.Polygon.Count; i++)
{
IntPoint nextPosition = path.Polygon[i];
length += (currentPosition - nextPosition).LengthMm();
currentPosition = nextPosition;
IntPoint nextExtrusion = path.Polygon[i];
nextExtrusion.Z = (int)(z + layerThickness_um * length / totalLength + .5);
gcodeExport.WriteMove(nextExtrusion, path.Speed, path.Config.LineWidthUM);
}
}
else
{
var loopStart = gcodeExport.GetPosition();
int pointCount = path.Polygon.Count;
bool outerPerimeter = path.Config.GCodeComment == "WALL-OUTER";
bool innerPerimeter = path.Config.GCodeComment == "WALL-INNER";
bool perimeter = outerPerimeter || innerPerimeter;
bool completeLoop = (pointCount > 0 && path.Polygon[pointCount - 1] == loopStart);
bool trimmed = perimeter && completeLoop && perimeterStartEndOverlapRatio < 1;
// This is test code to remove double drawn small perimeter lines.
if (trimmed)
{
long targetDistance = (long)(path.Config.LineWidthUM * (1 - perimeterStartEndOverlapRatio));
path = TrimGCodePathEnd(path, targetDistance);
// update the point count after trimming
pointCount = path.Polygon.Count;
}
for (int i = 0; i < pointCount; i++)
{
long lineWidth_um = path.Config.LineWidthUM;
if (path.Polygon[i].Width != 0)
{
lineWidth_um = path.Polygon[i].Width;
}
gcodeExport.WriteMove(path.Polygon[i], path.Speed, lineWidth_um);
}
if (trimmed)
{
// go back to the start of the loop
gcodeExport.WriteMove(loopStart, path.Speed, 0);
var length = path.Polygon.PolygonLength(false);
if (outerPerimeter &&
config.CoastAtEndDistance_um > 0 &&
length > config.CoastAtEndDistance_um)
{
//gcodeExport.WriteRetraction
var wipePoly = new Polygon(new IntPoint[] { loopStart });
wipePoly.AddRange(path.Polygon);
// then drive down it just a bit more to make sure we have a clean overlap
var extraMove = wipePoly.CutToLength(config.CoastAtEndDistance_um);
for (int i = 0; i < extraMove.Count; i++)
{
gcodeExport.WriteMove(extraMove[i], path.Speed, 0);
}
}
}
}
}
gcodeExport.UpdateLayerPrintTime();
}
public void MakePolygons(OptimizedVolume optomizedMesh, ConfigConstants.REPAIR_OUTLINES outlineRepairTypes)
{
for (int startingSegmentIndex = 0; startingSegmentIndex < segmentList.Count; startingSegmentIndex++)
{
if (segmentList[startingSegmentIndex].hasBeenAddedToPolygon)
{
continue;
}
Polygon poly = new Polygon();
// We start by adding the start, as we will add ends from now on.
IntPoint polygonStartPosition = segmentList[startingSegmentIndex].start;
poly.Add(polygonStartPosition);
int segmentIndexBeingAdded = startingSegmentIndex;
bool canClose;
bool lastAddWasAStart = true;
while (true)
{
canClose = false;
segmentList[segmentIndexBeingAdded].hasBeenAddedToPolygon = true;
IntPoint addedSegmentEndPoint = segmentList[segmentIndexBeingAdded].end;
if (!lastAddWasAStart)
{
// the last added point was an end so add the start as the text end point
addedSegmentEndPoint = segmentList[segmentIndexBeingAdded].start;
}
poly.Add(addedSegmentEndPoint);
int nextSegmentToCheckIndex = -1;
OptimizedFace face = optomizedMesh.facesTriangle[segmentList[segmentIndexBeingAdded].faceIndex];
for (int connectedFaceIndex = 0; connectedFaceIndex < 3; connectedFaceIndex++)
{
int testFaceIndex = face.touchingFaces[connectedFaceIndex];
if (testFaceIndex > -1)
{
// If the connected face has an edge that is in the segment list
if (faceTo2DSegmentIndex.ContainsKey(testFaceIndex))
{
int touchingSegmentIndex = faceTo2DSegmentIndex[testFaceIndex];
IntPoint foundSegmentStart = segmentList[touchingSegmentIndex].start;
if (addedSegmentEndPoint == foundSegmentStart)
{
// if we have looped back around to where we started
if (addedSegmentEndPoint == polygonStartPosition)
{
canClose = true;
}
// If this segment has already been added
if (segmentList[touchingSegmentIndex].hasBeenAddedToPolygon)
{
continue;
}
nextSegmentToCheckIndex = touchingSegmentIndex;
lastAddWasAStart = true;
}
else // let's check if the other side of this segment can hook up (the normal is facing the wrong way)
{
IntPoint foundSegmentEnd = segmentList[touchingSegmentIndex].end;
if (addedSegmentEndPoint == foundSegmentEnd)
{
// if we have looped back around to where we started
if (addedSegmentEndPoint == polygonStartPosition)
{
canClose = true;
}
// If this segment has already been added
if (segmentList[touchingSegmentIndex].hasBeenAddedToPolygon)
{
continue;
}
nextSegmentToCheckIndex = touchingSegmentIndex;
lastAddWasAStart = false;
}
}
}
}
}
if (nextSegmentToCheckIndex == -1)
{
break;
}
segmentIndexBeingAdded = nextSegmentToCheckIndex;
}
if (canClose)
{
polygonList.Add(poly);
}
else
{
openPolygonList.Add(poly);
}
}
// Link up all the missing ends, closing up the smallest gaps first. This is an inefficient implementation which can run in O(n*n*n) time.
while (true)
{
long bestScore = 10000 * 10000;
int bestA = -1;
int bestB = -1;
bool reversed = false;
for (int polygonAIndex = 0; polygonAIndex < openPolygonList.Count; polygonAIndex++)
{
if (openPolygonList[polygonAIndex].Count < 1)
{
continue;
}
for (int polygonBIndex = 0; polygonBIndex < openPolygonList.Count; polygonBIndex++)
{
if (openPolygonList[polygonBIndex].Count < 1)
{
continue;
}
IntPoint diff1 = openPolygonList[polygonAIndex][openPolygonList[polygonAIndex].Count - 1] - openPolygonList[polygonBIndex][0];
long distSquared1 = (diff1).LengthSquared();
if (distSquared1 < bestScore)
{
bestScore = distSquared1;
bestA = polygonAIndex;
bestB = polygonBIndex;
reversed = false;
if (bestScore == 0)
{
// found a perfect match stop looking
break;
}
}
if (polygonAIndex != polygonBIndex)
{
IntPoint diff2 = openPolygonList[polygonAIndex][openPolygonList[polygonAIndex].Count - 1] - openPolygonList[polygonBIndex][openPolygonList[polygonBIndex].Count - 1];
long distSquared2 = (diff2).LengthSquared();
if (distSquared2 < bestScore)
{
bestScore = distSquared2;
bestA = polygonAIndex;
bestB = polygonBIndex;
reversed = true;
if (bestScore == 0)
{
// found a perfect match stop looking
break;
}
}
}
}
if (bestScore == 0)
{
// found a perfect match stop looking
break;
}
}
if (bestScore >= 10000 * 10000)
{
break;
}
if (bestA == bestB)
{
polygonList.Add(new Polygon(openPolygonList[bestA]));
openPolygonList[bestA].Clear();
}
else
{
if (reversed)
{
if (openPolygonList[bestA].PolygonLength() > openPolygonList[bestB].PolygonLength())
{
if (openPolygonList[bestA].PolygonLength() > openPolygonList[bestB].PolygonLength())
{
openPolygonList[bestA].AddRange(openPolygonList[bestB]);
openPolygonList[bestB].Clear();
}
else
{
openPolygonList[bestB].AddRange(openPolygonList[bestA]);
openPolygonList[bestA].Clear();
}
}
else
{
openPolygonList[bestB].AddRange(openPolygonList[bestA]);
openPolygonList[bestB].Clear();
}
}
else
{
openPolygonList[bestA].AddRange(openPolygonList[bestB]);
openPolygonList[bestB].Clear();
}
}
}
if ((outlineRepairTypes & ConfigConstants.REPAIR_OUTLINES.EXTENSIVE_STITCHING) == ConfigConstants.REPAIR_OUTLINES.EXTENSIVE_STITCHING)
{
//For extensive stitching find 2 open polygons that are touching 2 closed polygons.
// Then find the sortest path over this polygon that can be used to connect the open polygons,
// And generate a path over this shortest bit to link up the 2 open polygons.
// (If these 2 open polygons are the same polygon, then the final result is a closed polyon)
while (true)
{
int bestA = -1;
int bestB = -1;
GapCloserResult bestResult = new GapCloserResult();
bestResult.len = long.MaxValue;
bestResult.polygonIndex = -1;
bestResult.pointIndexA = -1;
bestResult.pointIndexB = -1;
for (int i = 0; i < openPolygonList.Count; i++)
{
if (openPolygonList[i].Count < 1)
{
continue;
}
{
GapCloserResult res = FindPolygonGapCloser(openPolygonList[i][0], openPolygonList[i][openPolygonList[i].Count - 1]);
if (res.len > 0 && res.len < bestResult.len)
{
bestA = i;
bestB = i;
bestResult = res;
}
}
for (int j = 0; j < openPolygonList.Count; j++)
{
if (openPolygonList[j].Count < 1 || i == j)
{
continue;
}
GapCloserResult res = FindPolygonGapCloser(openPolygonList[i][0], openPolygonList[j][openPolygonList[j].Count - 1]);
if (res.len > 0 && res.len < bestResult.len)
{
bestA = i;
bestB = j;
bestResult = res;
}
}
}
if (bestResult.len < long.MaxValue)
{
if (bestA == bestB)
{
if (bestResult.pointIndexA == bestResult.pointIndexB)
{
polygonList.Add(new Polygon(openPolygonList[bestA]));
openPolygonList[bestA].Clear();
}
else if (bestResult.AtoB)
{
Polygon poly = new Polygon();
polygonList.Add(poly);
for (int j = bestResult.pointIndexA; j != bestResult.pointIndexB; j = (j + 1) % polygonList[bestResult.polygonIndex].Count)
{
poly.Add(polygonList[bestResult.polygonIndex][j]);
}
poly.AddRange(openPolygonList[bestA]);
openPolygonList[bestA].Clear();
}
else
{
int n = polygonList.Count;
polygonList.Add(new Polygon(openPolygonList[bestA]));
for (int j = bestResult.pointIndexB; j != bestResult.pointIndexA; j = (j + 1) % polygonList[bestResult.polygonIndex].Count)
{
polygonList[n].Add(polygonList[bestResult.polygonIndex][j]);
}
openPolygonList[bestA].Clear();
}
}
else
{
if (bestResult.pointIndexA == bestResult.pointIndexB)
{
openPolygonList[bestB].AddRange(openPolygonList[bestA]);
openPolygonList[bestA].Clear();
}
else if (bestResult.AtoB)
{
Polygon poly = new Polygon();
for (int n = bestResult.pointIndexA; n != bestResult.pointIndexB; n = (n + 1) % polygonList[bestResult.polygonIndex].Count)
{
poly.Add(polygonList[bestResult.polygonIndex][n]);
}
for (int n = poly.Count - 1; (int)(n) >= 0; n--)
{
openPolygonList[bestB].Add(poly[n]);
}
for (int n = 0; n < openPolygonList[bestA].Count; n++)
{
openPolygonList[bestB].Add(openPolygonList[bestA][n]);
}
openPolygonList[bestA].Clear();
}
else
{
for (int n = bestResult.pointIndexB; n != bestResult.pointIndexA; n = (n + 1) % polygonList[bestResult.polygonIndex].Count)
{
openPolygonList[bestB].Add(polygonList[bestResult.polygonIndex][n]);
}
for (int n = openPolygonList[bestA].Count - 1; n >= 0; n--)
{
openPolygonList[bestB].Add(openPolygonList[bestA][n]);
}
openPolygonList[bestA].Clear();
}
}
}
else
{
break;
}
}
}
if ((outlineRepairTypes & ConfigConstants.REPAIR_OUTLINES.KEEP_OPEN) == ConfigConstants.REPAIR_OUTLINES.KEEP_OPEN)
{
for (int n = 0; n < openPolygonList.Count; n++)
{
if (openPolygonList[n].Count > 0)
{
polygonList.Add(new Polygon(openPolygonList[n]));
}
}
}
//Remove all the tiny polygons, or polygons that are not closed. As they do not contribute to the actual print.
int snapDistance = 1000;
for (int polygonIndex = 0; polygonIndex < polygonList.Count; polygonIndex++)
{
int length = 0;
for (int intPointIndex = 1; intPointIndex < polygonList[polygonIndex].Count; intPointIndex++)
{
length += (polygonList[polygonIndex][intPointIndex] - polygonList[polygonIndex][intPointIndex - 1]).vSize();
if (length > snapDistance)
{
break;
}
}
if (length < snapDistance)
{
polygonList.RemoveAt(polygonIndex);
polygonIndex--;
}
}
//Finally optimize all the polygons. Every point removed saves time in the long run.
double minimumDistanceToCreateNewPosition = 1.415;
polygonList = Clipper.CleanPolygons(polygonList, minimumDistanceToCreateNewPosition);
}
private void UpdateIntersectionPolygon(ArbiterIntersection aInt)
{
// get previous polygon
Polygon interPoly = new Polygon();
// add all turn points
foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
{
interPoly.AddRange(ai.TurnPolygon);
}
// wrap it to get intersection polygon
interPoly = Polygon.GrahamScan(interPoly);
// get outer edges of poly
List<LinePath> polyEdges = new List<LinePath>();
for (int i = 0; i < interPoly.Count; i++)
{
Coordinates init = interPoly[i];
Coordinates fin = i == interPoly.Count - 1 ? interPoly[0] : interPoly[i + 1];
polyEdges.Add(new LinePath(new Coordinates[] { init, fin }));
}
// get all edges of all the turns
List<LinePath> other = new List<LinePath>();
foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
{
for (int i = 0; i < ai.TurnPolygon.Count; i++)
{
Coordinates init = ai.TurnPolygon[i];
Coordinates fin = i == ai.TurnPolygon.Count - 1 ? ai.TurnPolygon[0] : ai.TurnPolygon[i + 1];
other.Add(new LinePath(new Coordinates[] { init, fin }));
}
}
// test points
List<Coordinates> testPoints = new List<Coordinates>();
// path segs of inner turns
List<LinePath> innerTurnPaths = new List<LinePath>();
// check all inner points against all turn edges
foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
{
// check for inner point
if (ai.InnerCoordinates.Count == 3)
{
// inner point of the turn on the inside
testPoints.Add(ai.InnerCoordinates[1]);
// add to paths
innerTurnPaths.Add(new LinePath(new Coordinates[] { ai.InnerCoordinates[0], ai.InnerCoordinates[1] }));
innerTurnPaths.Add(new LinePath(new Coordinates[] { ai.InnerCoordinates[1], ai.InnerCoordinates[2] }));
}
}
// list of used segments
List<LinePath> closed = new List<LinePath>();
// check all other intersections of turn polygon edges
foreach (LinePath seg in innerTurnPaths)
{
foreach (LinePath otherEdge in other)
{
if (!seg.Equals(otherEdge) && !closed.Contains(otherEdge))
{
double x1 = seg[0].X;
double y1 = seg[0].Y;
double x2 = seg[1].X;
double y2 = seg[1].Y;
double x3 = otherEdge[0].X;
double y3 = otherEdge[0].Y;
double x4 = otherEdge[1].X;
double y4 = otherEdge[1].Y;
// get if inside both
double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
if (0.05 < ua && ua < 0.95 && 0.5 < ub && ub < 0.95)
{
double x = x1 + ua * (x2 - x1);
double y = y1 + ua * (y2 - y1);
testPoints.Add(new Coordinates(x, y));
}
}
}
closed.Add(seg);
}
// loop through test points
foreach(Coordinates inner in testPoints)
{
// list of replacements
List<LinePath> toReplace = new List<LinePath>();
// loop through outer
foreach (LinePath edge in polyEdges)
{
// flag to replace
bool replace = false;
// make sure this goes to a valid edge section
LinePath.PointOnPath closest = edge.GetClosestPoint(inner);
if (!closest.Equals(edge.StartPoint) && !closest.Equals(edge.EndPoint) &&
!(closest.Location.DistanceTo(edge.StartPoint.Location) < 0.5) &&
!(closest.Location.DistanceTo(edge.EndPoint.Location) < 0.5))
{
// create seg (extend a bit)
Coordinates expansion = closest.Location - inner;
LinePath seg = new LinePath(new Coordinates[] { inner, closest.Location + expansion.Normalize(1.0) });
// set flag
replace = true;
// loop through other edges
foreach (LinePath otherEdge in other)
{
double x1 = seg[0].X;
double y1 = seg[0].Y;
double x2 = seg[1].X;
double y2 = seg[1].Y;
double x3 = otherEdge[0].X;
double y3 = otherEdge[0].Y;
double x4 = otherEdge[1].X;
double y4 = otherEdge[1].Y;
// get if inside both
double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
if (0.01 < ua && ua < 0.99 && 0 < ub && ub < 1)
{
replace = false;
}
}
}
// check if should replace
if (replace)
{
// add analyzed to adjacent
toReplace.Add(edge);
}
}
// loop through replacements
foreach (LinePath ll in toReplace)
{
LinePath[] tmpArrayPoly = new LinePath[polyEdges.Count];
polyEdges.CopyTo(tmpArrayPoly);
List<LinePath> tmpPoly = new List<LinePath>(tmpArrayPoly);
// get index of edge
int index = tmpPoly.IndexOf(ll);
// remove
tmpPoly.RemoveAt(index);
// add correctly to outer
tmpPoly.Insert(index, new LinePath(new Coordinates[] { ll[0], inner }));
tmpPoly.Insert(index + 1, new LinePath(new Coordinates[] { inner, ll[1] }));
// poly
Polygon temp = new Polygon();
foreach(LinePath lpTemp in tmpPoly)
temp.Add(lpTemp[1]);
temp.Inflate(0.5);
// make sure none of original outside
bool ok = true;
foreach (LinePath lp in other)
{
if (!temp.IsInside(lp[1]) && !temp.Contains(lp[1]))
ok = false;
}
// set if created ok
if (ok)
polyEdges = tmpPoly;
}
}
// create final
List<Coordinates> finalPoly = new List<Coordinates>();
foreach (LinePath outerEdge in polyEdges)
finalPoly.Add(outerEdge[1]);
interPoly = new Polygon(finalPoly);
aInt.IntersectionPolygon = interPoly;
aInt.Center = interPoly.GetCentroid();
}
public void MakePolygons(ConfigConstants.REPAIR_OUTLINES outlineRepairTypes)
{
if (false) // you can use this output segments for debugging
{
using (StreamWriter stream = File.AppendText("segments.txt"))
{
stream.WriteLine(DumpSegmentListToString(SegmentList));
}
}
CreateFastIndexLookup();
for (int startingSegmentIndex = 0; startingSegmentIndex < SegmentList.Count; startingSegmentIndex++)
{
if (SegmentList[startingSegmentIndex].hasBeenAddedToPolygon)
{
continue;
}
Polygon poly = new Polygon();
// We start by adding the start, as we will add ends from now on.
IntPoint polygonStartPosition = SegmentList[startingSegmentIndex].start;
poly.Add(polygonStartPosition);
int segmentIndexBeingAdded = startingSegmentIndex;
bool canClose;
while (true)
{
canClose = false;
SegmentList[segmentIndexBeingAdded].hasBeenAddedToPolygon = true;
IntPoint addedSegmentEndPoint = SegmentList[segmentIndexBeingAdded].end;
poly.Add(addedSegmentEndPoint);
segmentIndexBeingAdded = GetTouchingSegmentIndex(addedSegmentEndPoint);
if (segmentIndexBeingAdded == -1)
{
break;
}
else
{
IntPoint foundSegmentStart = SegmentList[segmentIndexBeingAdded].start;
if (addedSegmentEndPoint == foundSegmentStart)
{
// if we have looped back around to where we started
if (addedSegmentEndPoint == polygonStartPosition)
{
canClose = true;
}
}
}
}
if (canClose)
{
PolygonList.Add(poly);
}
else
{
openPolygonList.Add(poly);
}
}
// Link up all the missing ends, closing up the smallest gaps first. This is an inefficient implementation which can run in O(n*n*n) time.
while (true)
{
long bestScore = 10000 * 10000;
int bestA = -1;
int bestB = -1;
bool reversed = false;
for (int polygonAIndex = 0; polygonAIndex < openPolygonList.Count; polygonAIndex++)
{
if (openPolygonList[polygonAIndex].Count < 1)
{
continue;
}
for (int polygonBIndex = 0; polygonBIndex < openPolygonList.Count; polygonBIndex++)
{
if (openPolygonList[polygonBIndex].Count < 1)
{
continue;
}
IntPoint diff1 = openPolygonList[polygonAIndex][openPolygonList[polygonAIndex].Count - 1] - openPolygonList[polygonBIndex][0];
long distSquared1 = (diff1).LengthSquared();
if (distSquared1 < bestScore)
{
bestScore = distSquared1;
bestA = polygonAIndex;
bestB = polygonBIndex;
reversed = false;
if (bestScore == 0)
{
// found a perfect match stop looking
break;
}
}
if (polygonAIndex != polygonBIndex)
{
IntPoint diff2 = openPolygonList[polygonAIndex][openPolygonList[polygonAIndex].Count - 1] - openPolygonList[polygonBIndex][openPolygonList[polygonBIndex].Count - 1];
long distSquared2 = (diff2).LengthSquared();
if (distSquared2 < bestScore)
{
bestScore = distSquared2;
bestA = polygonAIndex;
bestB = polygonBIndex;
reversed = true;
if (bestScore == 0)
{
// found a perfect match stop looking
break;
}
}
}
}
if (bestScore == 0)
{
// found a perfect match stop looking
break;
}
}
if (bestScore >= 10000 * 10000)
{
break;
}
if (bestA == bestB) // This loop connects to itself, close the polygon.
{
PolygonList.Add(new Polygon(openPolygonList[bestA]));
openPolygonList[bestA].Clear(); // B is cleared as it is A
}
else
{
if (reversed)
{
if (openPolygonList[bestA].PolygonLength() > openPolygonList[bestB].PolygonLength())
{
openPolygonList[bestA].AddRange(openPolygonList[bestB]);
openPolygonList[bestB].Clear();
}
else
{
openPolygonList[bestB].AddRange(openPolygonList[bestA]);
openPolygonList[bestA].Clear();
}
}
else
{
openPolygonList[bestA].AddRange(openPolygonList[bestB]);
openPolygonList[bestB].Clear();
}
}
}
if ((outlineRepairTypes & ConfigConstants.REPAIR_OUTLINES.EXTENSIVE_STITCHING) == ConfigConstants.REPAIR_OUTLINES.EXTENSIVE_STITCHING)
{
//For extensive stitching find 2 open polygons that are touching 2 closed polygons.
// Then find the sortest path over this polygon that can be used to connect the open polygons,
// And generate a path over this shortest bit to link up the 2 open polygons.
// (If these 2 open polygons are the same polygon, then the final result is a closed polyon)
while (true)
{
int bestA = -1;
int bestB = -1;
GapCloserResult bestResult = new GapCloserResult();
bestResult.len = long.MaxValue;
bestResult.polygonIndex = -1;
bestResult.pointIndexA = -1;
bestResult.pointIndexB = -1;
for (int i = 0; i < openPolygonList.Count; i++)
{
if (openPolygonList[i].Count < 1)
{
continue;
}
{
GapCloserResult res = FindPolygonGapCloser(openPolygonList[i][0], openPolygonList[i][openPolygonList[i].Count - 1]);
if (res.len > 0 && res.len < bestResult.len)
{
bestA = i;
bestB = i;
bestResult = res;
}
}
for (int j = 0; j < openPolygonList.Count; j++)
{
if (openPolygonList[j].Count < 1 || i == j)
{
continue;
}
GapCloserResult res = FindPolygonGapCloser(openPolygonList[i][0], openPolygonList[j][openPolygonList[j].Count - 1]);
if (res.len > 0 && res.len < bestResult.len)
{
bestA = i;
bestB = j;
bestResult = res;
}
}
}
if (bestResult.len < long.MaxValue)
{
if (bestA == bestB)
{
if (bestResult.pointIndexA == bestResult.pointIndexB)
{
PolygonList.Add(new Polygon(openPolygonList[bestA]));
openPolygonList[bestA].Clear();
}
else if (bestResult.AtoB)
{
Polygon poly = new Polygon();
PolygonList.Add(poly);
for (int j = bestResult.pointIndexA; j != bestResult.pointIndexB; j = (j + 1) % PolygonList[bestResult.polygonIndex].Count)
{
poly.Add(PolygonList[bestResult.polygonIndex][j]);
}
poly.AddRange(openPolygonList[bestA]);
openPolygonList[bestA].Clear();
}
else
{
int n = PolygonList.Count;
PolygonList.Add(new Polygon(openPolygonList[bestA]));
for (int j = bestResult.pointIndexB; j != bestResult.pointIndexA; j = (j + 1) % PolygonList[bestResult.polygonIndex].Count)
{
PolygonList[n].Add(PolygonList[bestResult.polygonIndex][j]);
}
openPolygonList[bestA].Clear();
}
}
else
{
if (bestResult.pointIndexA == bestResult.pointIndexB)
{
openPolygonList[bestB].AddRange(openPolygonList[bestA]);
openPolygonList[bestA].Clear();
}
else if (bestResult.AtoB)
{
Polygon poly = new Polygon();
for (int n = bestResult.pointIndexA; n != bestResult.pointIndexB; n = (n + 1) % PolygonList[bestResult.polygonIndex].Count)
{
poly.Add(PolygonList[bestResult.polygonIndex][n]);
}
for (int n = poly.Count - 1; (int)(n) >= 0; n--)
{
openPolygonList[bestB].Add(poly[n]);
}
for (int n = 0; n < openPolygonList[bestA].Count; n++)
{
openPolygonList[bestB].Add(openPolygonList[bestA][n]);
}
openPolygonList[bestA].Clear();
}
else
{
for (int n = bestResult.pointIndexB; n != bestResult.pointIndexA; n = (n + 1) % PolygonList[bestResult.polygonIndex].Count)
{
openPolygonList[bestB].Add(PolygonList[bestResult.polygonIndex][n]);
}
for (int n = openPolygonList[bestA].Count - 1; n >= 0; n--)
{
openPolygonList[bestB].Add(openPolygonList[bestA][n]);
}
openPolygonList[bestA].Clear();
}
}
}
else
{
break;
}
}
}
if ((outlineRepairTypes & ConfigConstants.REPAIR_OUTLINES.KEEP_OPEN) == ConfigConstants.REPAIR_OUTLINES.KEEP_OPEN)
{
for (int n = 0; n < openPolygonList.Count; n++)
{
if (openPolygonList[n].Count > 0)
{
PolygonList.Add(new Polygon(openPolygonList[n]));
}
}
}
//Remove all the tiny polygons, or polygons that are not closed. As they do not contribute to the actual print.
int minimumPerimeter = 1000;
for (int polygonIndex = 0; polygonIndex < PolygonList.Count; polygonIndex++)
{
long perimeterLength = 0;
for (int intPointIndex = 1; intPointIndex < PolygonList[polygonIndex].Count; intPointIndex++)
{
perimeterLength += (PolygonList[polygonIndex][intPointIndex] - PolygonList[polygonIndex][intPointIndex - 1]).Length();
if (perimeterLength > minimumPerimeter)
{
break;
}
}
if (perimeterLength < minimumPerimeter)
{
PolygonList.RemoveAt(polygonIndex);
polygonIndex--;
}
}
//Finally optimize all the polygons. Every point removed saves time in the long run.
double minimumDistanceToCreateNewPosition = 10;
PolygonList = Clipper.CleanPolygons(PolygonList, minimumDistanceToCreateNewPosition);
}
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 Polygon ProjectRectangle(Rectangle rectangle)
{
Polygon polygon = new Polygon();
// get corners counter clockwise starting on the top left and project them to create the polygon:
polygon.AddRange(new Point[] {
new Point(rectangle.X, rectangle.Y), // top left
new Point(rectangle.X, rectangle.Y + rectangle.Height), // bottom left
new Point(rectangle.X + rectangle.Width, rectangle.Y + rectangle.Height), // bottom right
new Point(rectangle.X + rectangle.Width, rectangle.Y) // top right
}.Select(ProjectPoint));
return polygon;
}
// Find the convex hull of a set of (x,y) points.
// returns a polygon which form the convex hull.
// Could be:
// - empty (if input is empty, or if there is an internal error)
// - single point (if all input points are identical)
// - 2 points (if >=2 input points, but all collinear)
// - 3 or more points forming a convex polygon in CCW order.
//
// Returns 0 (or -1 if an internal
// error occurs, in which case the result will be empty).
// No case has been found in testing which causes an internal error. It's
// possible that this could occur if 64-bit multplies and adds overflow.
public static Polygon CreateConvexHull(Polygon inPolygon)
{
Polygon points = new Polygon(inPolygon);
int count = points.Count;
if (count <= 2)
{
if (count == 2 &&
points[0].X == points[1].X &&
points[1].Y == points[1].Y)
{
// remove one point if two are the same
points.RemoveAt(1);
}
return(points);
}
// step 1: sort the values in order from min y to max y.
// (and increasing X where Y are equal)
points.Sort(new IntPointSorterYX());
long minY = points[0].Y;
long maxY = points[count - 1].Y;
// (2) make a pass, find the min and max x.
long minX = points[0].X;
long maxX = minX;
for (int i = 1; i < count; i++)
{
minX = Math.Min(minX, points[i].X);
maxX = Math.Max(maxX, points[i].X);
}
long upperLeftX;
long upperRightX;
long bottomLeftY;
long bottomRightY;
long bottomLeftX;
long bottomRightX;
int leftCount;
int rightCount;
int indexOfLowestPoint;
// OK next step is to find out if there's more than one identical
// 'y' value at the end of the list. Don't forget to
// consider the case where all of these are identical...
{
int sameYFromEndCount = 1;
int lastValidIndex = count - 1;
upperLeftX = upperRightX = points[lastValidIndex].X;
while (sameYFromEndCount < count && points[lastValidIndex - sameYFromEndCount].Y == maxY)
{
sameYFromEndCount++;
}
// if more than one, they will be sorted by increasing X.
// Delete any in the middle.
//
if (sameYFromEndCount >= 2)
{
int deleteCount = sameYFromEndCount - 2; // always delete this many...
upperRightX = points[count - 1].X;
upperLeftX = points[count - sameYFromEndCount].X;
if (upperLeftX == upperRightX)
{
deleteCount++; // delete one more if all the same...
}
if (deleteCount > 0)
{
points[count - 1 - deleteCount] = points[count - 1];
count -= deleteCount;
}
}
}
// We may now have only 1 or 2 points.
if (count <= 2)
{
points = new Polygon();
return(points);
}
// same thing at the bottoom
{
int sameYFromStartCount = 1;
bottomLeftX = bottomRightX = points[0].X;
while (sameYFromStartCount < count && points[sameYFromStartCount].Y == minY)
{
sameYFromStartCount++;
}
// if more than one, They will be sorted left to right. Delete any in the middle.
indexOfLowestPoint = 0;
if (sameYFromStartCount >= 2)
{
int deleteCount = sameYFromStartCount - 2; // always delete this many...
bottomLeftX = points[0].X;
bottomRightX = points[sameYFromStartCount - 1].X;
if (bottomLeftX == bottomRightX)
{
deleteCount++; // delete one more if all the same...
}
else
{
indexOfLowestPoint = 1; // otherwise we start with 1,
}
if (deleteCount > 0)
{
for (int i = 0; i < count - (deleteCount + 1); i++)
{
points[1 + i] = points[deleteCount + 1 + i];
count -= deleteCount;
}
}
}
// OK, we now have the 'UL/UR' points and the 'LL/LR' points.
// Make a left-side list and a right-side list, each
// of capacity 'num'.
// note that 'pleft' is in reverse order...
// pright grows up from 0, and pleft down from 'num+1',
// in the same array - they can't overlap.
Polygon temp_array = new Polygon(points);
temp_array.Add(new IntPoint());
temp_array.Add(new IntPoint());
int pleftIndex = count + 1;
int prightIndex = 0;
// set up left and right
temp_array[pleftIndex] = points[0];
leftCount = 1;
temp_array[prightIndex] = points[indexOfLowestPoint];
rightCount = 1;
bottomLeftY = bottomRightY = minY;
for (int ipos = indexOfLowestPoint + 1; ipos < count; ipos++)
{
IntPoint pointToCheck = temp_array[ipos]; // get new point.
// left side test:
// is the new point is strictly to the left of a line from (bottomLeftX, BottomLeftY) to ( upperLeftX, maxy )?
if (convex3(upperLeftX, maxY, pointToCheck.X, pointToCheck.Y, bottomLeftX, bottomLeftY))
{
// if so, append to the left side list, but first peel off any existing
// points there which would be on or inside the new line.
while (leftCount > 1 && !convex3(pointToCheck, temp_array[pleftIndex - (leftCount - 1)], temp_array[pleftIndex - (leftCount - 2)]))
{
--leftCount;
}
temp_array[pleftIndex - leftCount] = pointToCheck;
leftCount++;
bottomLeftX = pointToCheck.X;
bottomLeftY = pointToCheck.Y;
}
else if (convex3(bottomRightX, bottomRightY, pointToCheck.X, pointToCheck.Y, upperRightX, maxY)) // right side test is the new point strictly to the right of a line from (URx, maxy) to ( DRx, DRy )?
{
// if so, append to the right side list, but first peel off any existing
// points there which would be on or inside the new line.
//
while (rightCount > 1 && !convex3(temp_array[prightIndex + rightCount - 2], temp_array[prightIndex + rightCount - 1], pointToCheck))
{
--rightCount;
}
temp_array[prightIndex + rightCount] = pointToCheck;
rightCount++;
bottomRightX = pointToCheck.X;
bottomRightY = pointToCheck.Y;
}
// if neither of the above are true we throw out the point.
}
// now add the 'maxy' points to the lists (it will have failed the insert test)
temp_array[pleftIndex - leftCount] = new IntPoint(upperLeftX, maxY);
++leftCount;
if (upperRightX > upperLeftX)
{
temp_array[prightIndex + rightCount] = new IntPoint(upperRightX, maxY);
++rightCount;
}
// now copy the lists to the output area.
//
// if both lists have the same lower point, delete one now.
// (pleft could be empty as a result!)
if (indexOfLowestPoint == 0)
{
--pleftIndex;
--leftCount;
}
// this condition should be true now...
if (!(leftCount + rightCount <= count))
{
// failure... return the original concave list
return(inPolygon);
}
// now just pack the pright and pleft lists into the output.
count = leftCount + rightCount;
for (int i = 0; i < rightCount; i++)
{
points[i] = temp_array[i + prightIndex];
}
if (leftCount > 0)
{
for (int i = 0; i < leftCount; i++)
{
points[i + rightCount] = temp_array[i + pleftIndex - (leftCount - 1)];
}
}
if (points.Count > count)
{
Polygon newList = new Polygon();
newList.AddRange(points.GetRange(0, count));
points = newList;
}
return(points);
}
}
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();
}
}