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)));
}
