public void CalculateTurnHeadings() { //to calc heading based on next and previous points to give an average heading. int cnt = turnLine.Count; CTurnPt[] arr = new CTurnPt[cnt]; cnt--; turnLine.CopyTo(arr); turnLine.Clear(); //first point needs last, first, second points CTurnPt pt3 = arr[0]; pt3.heading = Math.Atan2(arr[1].easting - arr[cnt].easting, arr[1].northing - arr[cnt].northing); if (pt3.heading < 0) { pt3.heading += glm.twoPI; } turnLine.Add(pt3); //middle points for (int i = 1; i < cnt; i++) { pt3 = arr[i]; pt3.heading = Math.Atan2(arr[i + 1].easting - arr[i - 1].easting, arr[i + 1].northing - arr[i - 1].northing); if (pt3.heading < 0) { pt3.heading += glm.twoPI; } turnLine.Add(pt3); } //last and first point pt3 = arr[cnt]; pt3.heading = Math.Atan2(arr[0].easting - arr[cnt - 1].easting, arr[0].northing - arr[cnt - 1].northing); if (pt3.heading < 0) { pt3.heading += glm.twoPI; } turnLine.Add(pt3); }
public void BuildTurnLines() { //uodate the GUI values for boundaries mf.fd.UpdateFieldBoundaryGUIAreas(); if (!mf.bnd.bndArr[0].isSet) { mf.TimedMessageBox(1500, " Error", "No Boundaries Made"); return; } //to fill the list of line points vec3 point = new vec3(); //determine how wide a headland space double totalHeadWidth = mf.yt.triggerDistanceOffset; //outside boundary - count the points from the boundary turnArr[0].turnLine.Clear(); int ptCount = mf.bnd.bndArr[0].bndLine.Count; for (int i = ptCount - 1; i >= 0; i--) { //calculate the point inside the boundary point.easting = mf.bnd.bndArr[0].bndLine[i].easting + (-Math.Sin(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * totalHeadWidth); point.northing = mf.bnd.bndArr[0].bndLine[i].northing + (-Math.Cos(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * totalHeadWidth); point.heading = mf.bnd.bndArr[0].bndLine[i].heading; if (point.heading < -glm.twoPI) { point.heading += glm.twoPI; } //only add if inside actual field boundary if (mf.bnd.bndArr[0].IsPointInsideBoundary(point)) { CTurnPt tPnt = new CTurnPt(point.easting, point.northing, point.heading); turnArr[0].turnLine.Add(tPnt); } } turnArr[0].FixTurnLine(totalHeadWidth, mf.bnd.bndArr[0].bndLine); turnArr[0].PreCalcTurnLines(); //inside boundaries for (int j = 1; j < FormGPS.MAXBOUNDARIES; j++) { turnArr[j].turnLine.Clear(); if (!mf.bnd.bndArr[j].isSet || mf.bnd.bndArr[j].isDriveThru) { continue; } ptCount = mf.bnd.bndArr[j].bndLine.Count; for (int i = ptCount - 1; i >= 0; i--) { //calculate the point outside the boundary point.easting = mf.bnd.bndArr[j].bndLine[i].easting + (-Math.Sin(glm.PIBy2 + mf.bnd.bndArr[j].bndLine[i].heading) * totalHeadWidth); point.northing = mf.bnd.bndArr[j].bndLine[i].northing + (-Math.Cos(glm.PIBy2 + mf.bnd.bndArr[j].bndLine[i].heading) * totalHeadWidth); point.heading = mf.bnd.bndArr[j].bndLine[i].heading; if (point.heading < -glm.twoPI) { point.heading += glm.twoPI; } //only add if outside actual field boundary if (!mf.bnd.bndArr[j].IsPointInsideBoundary(point)) { CTurnPt tPnt = new CTurnPt(point.easting, point.northing, point.heading); turnArr[j].turnLine.Add(tPnt); } } turnArr[j].FixTurnLine(totalHeadWidth, mf.bnd.bndArr[j].bndLine); turnArr[j].PreCalcTurnLines(); } mf.TimedMessageBox(800, "Turn Lines", "Turn limits Created"); }
public void FixTurnLine(double totalHeadWidth, List <CBndPt> curBnd) { //count the points from the boundary int lineCount = turnLine.Count; double distance = 0; //int headCount = mf.bndArr[inTurnNum].bndLine.Count; int bndCount = curBnd.Count; //remove the points too close to boundary for (int i = 0; i < bndCount; i++) { for (int j = 0; j < lineCount; j++) { //make sure distance between headland and boundary is not less then width distance = glm.Distance(curBnd[i], turnLine[j]); if (distance < (totalHeadWidth * 0.96)) { turnLine.RemoveAt(j); lineCount = turnLine.Count; j = -1; } } } //make sure distance isn't too small between points on turnLine bndCount = turnLine.Count; //double spacing = mf.vehicle.toolWidth * 0.25; const double spacing = 2; for (int i = 0; i < bndCount - 1; i++) { distance = glm.Distance(turnLine[i], turnLine[i + 1]); if (distance < spacing) { turnLine.RemoveAt(i + 1); bndCount = turnLine.Count; i = -1; } } //make sure distance isn't too big between points on Turn bndCount = turnLine.Count; for (int i = 0; i < bndCount; i++) { int j = i + 1; if (j == bndCount) { j = 0; } distance = glm.Distance(turnLine[i], turnLine[j]); if (distance > (spacing * 1.25)) { CTurnPt pointB = new CTurnPt((turnLine[i].easting + turnLine[j].easting) / 2.0, (turnLine[i].northing + turnLine[j].northing) / 2.0, turnLine[i].heading); turnLine.Insert(j, pointB); bndCount = turnLine.Count; i = -1; } } //make sure headings are correct for calculated points CalculateTurnHeadings(); }
public static double Distance(CTurnPt first, CTurnPt second) { return(Math.Sqrt( Math.Pow(first.easting - second.easting, 2) + Math.Pow(first.northing - second.northing, 2))); }