public bool FindHeadlandDistance()
{
if (headArr[0].hdLine.Count == 0)
{
leftToolDistance = 99999;
rightToolDistance = 99999;
return(false);
}
else
{
leftToolDistance = 999999;
rightToolDistance = 999999;
double minDistA = 1000000, minDistB = 1000000;
int ptCount = headArr[0].hdLine.Count;
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
double dist = glm.DistanceSquared(mf.section[0].leftPoint, headArr[0].hdLine[t]);
if (dist < minDistA)
{
minDistB = minDistA;
B = A;
minDistA = dist;
A = t;
}
else if (dist < minDistB)
{
minDistB = dist;
B = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (A > B)
{
Q = A; A = B; B = Q;
}
double dx = headArr[0].hdLine[B].easting - headArr[0].hdLine[A].easting;
double dz = headArr[0].hdLine[B].northing - headArr[0].hdLine[A].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return(false);
}
//abHeading = Math.Atan2(dz, dx);
//double abHeading = curList[A].heading;
//how far from current AB Line is fix
leftToolDistance = ((dz * mf.section[0].leftPoint.easting) - (dx * mf.section[0].leftPoint.northing) + (headArr[0].hdLine[B].easting
* headArr[0].hdLine[A].northing) - (headArr[0].hdLine[B].northing * headArr[0].hdLine[A].easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//are we on the right side or not
//bool isOnRightSideCurrentLine = leftToolDistance > 0;
//absolute the distance
//leftToolDistance = Math.Abs(leftToolDistance);
//find the closest 2 points to current fixm for right side
minDistA = 1000000;
minDistB = 1000000;
for (int t = 0; t < ptCount; t++)
{
double dist = glm.DistanceSquared(mf.section[mf.tool.numOfSections - 1].rightPoint, headArr[0].hdLine[t]);
if (dist < minDistA)
{
minDistB = minDistA;
D = C;
minDistA = dist;
C = t;
}
else if (dist < minDistB)
{
minDistB = dist;
D = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (C > D)
{
Q = C; C = D; D = Q;
}
dx = headArr[0].hdLine[D].easting - headArr[0].hdLine[C].easting;
dz = headArr[0].hdLine[D].northing - headArr[0].hdLine[C].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return(false);
}
//abHeading = Math.Atan2(dz, dx);
//double abHeading = curList[C].heading;
//how far from current AB Line is fix
rightToolDistance = ((dz * mf.section[mf.tool.numOfSections - 1].rightPoint.easting) - (dx * mf.section[mf.tool.numOfSections - 1].rightPoint.northing) + (headArr[0].hdLine[D].easting
* headArr[0].hdLine[C].northing) - (headArr[0].hdLine[D].northing * headArr[0].hdLine[C].easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//are we on the right side or not
//bool isOnRightSideCurrentLine = leftToolDistance > 0;
//absolute the distance
//leftToolDistance = Math.Abs(leftToolDistance);
//double scanWidthR = (mf.tool.toolWidth * 0.75);
if (leftToolDistance > 0 && rightToolDistance > 0)
{
isToolInWorkArea = true;
isToolInBothPlaces = isToolInHeadland = false;
}
else if (leftToolDistance < 0 && rightToolDistance < 0)
{
isToolInHeadland = true;
isToolInBothPlaces = isToolInWorkArea = false;
}
else
{
isToolInBothPlaces = true;
isToolInHeadland = isToolInWorkArea = false;
}
vec3 toolFix = mf.toolPos;
double headAB = toolFix.heading;
downL.easting = mf.section[0].leftPoint.easting + (Math.Sin(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarLeftSpeed);
downL.northing = mf.section[0].leftPoint.northing + (Math.Cos(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarLeftSpeed);
downR.easting = mf.section[mf.tool.numOfSections - 1].rightPoint.easting + (Math.Sin(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarRightSpeed);
downR.northing = mf.section[mf.tool.numOfSections - 1].rightPoint.northing + (Math.Cos(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarRightSpeed);
//bool isToolFullyOut =
bool isLookRightIn = IsPointInsideHeadLine(downR);
bool isLookLeftIn = IsPointInsideHeadLine(downL);
if (isLookLeftIn && isLookRightIn)
{
isLookInWorkArea = true;
isLookInBothPlaces = isLookInHealand = false;
}
else if (!isLookLeftIn && !isLookRightIn)
{
isLookInHealand = true;
isLookInBothPlaces = isLookInWorkArea = false;
}
else
{
isLookInBothPlaces = true;
isLookInHealand = isLookInWorkArea = false;
}
//isToolUp = false;
if (!isLookInHealand && isToolUp)
{
isToolUp = false;
}
if (isLookInHealand && isToolInHeadland && !isToolUp)
{
isToolUp = true;
}
if (isToolUp)
{
mf.mc.pgn[mf.mc.azRelayData][mf.mc.rdHydLift] = 1;
}
else
{
mf.mc.pgn[mf.mc.azRelayData][mf.mc.rdHydLift] = 0;
}
return(isToolUp);
}
}
private void btnLoadBoundaryFromGE_Click(object sender, EventArgs e)
{
if (sender is Button button)
{
Selectedreset = true;
string fileAndDirectory;
{
//create the dialog instance
OpenFileDialog ofd = new OpenFileDialog
{
//set the filter to text KML only
Filter = "KML files (*.KML)|*.KML",
//the initial directory, fields, for the open dialog
InitialDirectory = mf.fieldsDirectory + mf.currentFieldDirectory
};
//was a file selected
if (ofd.ShowDialog() == DialogResult.Cancel)
{
return;
}
else
{
fileAndDirectory = ofd.FileName;
}
}
string coordinates = null;
int startIndex;
int i = 0;
using (StreamReader reader = new StreamReader(fileAndDirectory))
{
if (button.Name == "btnLoadMultiBoundaryFromGE")
{
ResetAllBoundary();
}
else
{
i = mf.bnd.boundarySelected;
}
try
{
while (!reader.EndOfStream)
{
//start to read the file
string line = reader.ReadLine();
startIndex = line.IndexOf("<coordinates>");
if (startIndex != -1)
{
while (true)
{
int endIndex = line.IndexOf("</coordinates>");
if (endIndex == -1)
{
//just add the line
if (startIndex == -1)
{
coordinates += line.Substring(0);
}
else
{
coordinates += line.Substring(startIndex + 13);
}
}
else
{
if (startIndex == -1)
{
coordinates += line.Substring(0, endIndex);
}
else
{
coordinates += line.Substring(startIndex + 13, endIndex - (startIndex + 13));
}
break;
}
line = reader.ReadLine();
line = line.Trim();
startIndex = -1;
}
line = coordinates;
char[] delimiterChars = { ' ', '\t', '\r', '\n' };
string[] numberSets = line.Split(delimiterChars);
//at least 3 points
if (numberSets.Length > 2)
{
mf.bnd.bndArr.Add(new CBoundaryLines());
mf.turn.turnArr.Add(new CTurnLines());
foreach (var item in numberSets)
{
string[] fix = item.Split(',');
double.TryParse(fix[0], NumberStyles.Float, CultureInfo.InvariantCulture, out lonK);
double.TryParse(fix[1], NumberStyles.Float, CultureInfo.InvariantCulture, out latK);
mf.pn.ConvertWGS84ToLocal(latK, lonK, out norting, out easting);
//add the point to boundary
vec3 bndPt = new vec3(easting, norting, 0);
mf.bnd.bndArr[i].bndLine.Add(bndPt);
}
//build the boundary, make sure is clockwise for outer counter clockwise for inner
bool isCW = mf.bnd.bndArr[i].CalculateBoundaryArea();
if (mf.bnd.boundarySelected == 0 && isCW)
{
mf.bnd.bndArr[i].ReverseWinding();
}
//inner boundaries
if (mf.bnd.boundarySelected > 0 && !isCW)
{
mf.bnd.bndArr[i].ReverseWinding();
}
mf.bnd.bndArr[i].FixBoundaryLine(i);
mf.bnd.bndArr[i].PreCalcBoundaryEarLines();
mf.bnd.bndArr[i].PreCalcBoundaryLines();
mf.bnd.bndArr[i].isSet = true;
mf.fd.UpdateFieldBoundaryGUIAreas();
mf.btnMakeLinesFromBoundary.Visible = true;
coordinates = "";
i++;
}
else
{
mf.TimedMessageBox(2000, gStr.gsErrorreadingKML, gStr.gsChooseBuildDifferentone);
}
if (button.Name == "btnLoadBoundaryFromGE")
{
break;
}
}
}
mf.FileSaveBoundary();
mf.turn.BuildTurnLines();
mf.btnMakeLinesFromBoundary.Visible = true;
mf.fd.UpdateFieldBoundaryGUIAreas();
UpdateChart();
}
catch (Exception)
{
return;
}
}
}
mf.bnd.isOkToAddPoints = false;
panelMain.Visible = true;
panelChoose.Visible = false;
panelKML.Visible = false;
this.Size = new Size(566, 377);
UpdateChart();
}
public void BuildGeoFenceLines()
{
//update the GUI values for boundaries
//mf.fd.UpdateFieldBoundaryGUIAreas();
if (mf.bnd.bndArr.Count == 0)
{
//mf.TimedMessageBox(1500, " No Boundary ", "No GeoFence Made");
return;
}
//to fill the list of line points
vec3 point = new vec3();
//determine how wide a headland space
double totalHeadWidth = mf.yt.geoFenceDistance;
//outside boundary - count the points from the boundary
geoFenceArr[0].geoFenceLine.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))
{
vec2 tPnt = new vec2(point.easting, point.northing);
geoFenceArr[0].geoFenceLine.Add(tPnt);
}
}
geoFenceArr[0].FixGeoFenceLine(totalHeadWidth, mf.bnd.bndArr[0].bndLine, mf.tool.toolWidth);
geoFenceArr[0].PreCalcTurnLines();
//inside boundaries
for (int j = 1; j < mf.bnd.bndArr.Count; j++)
{
geoFenceArr[j].geoFenceLine.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))
{
vec2 tPnt = new vec2(point.easting, point.northing);
geoFenceArr[j].geoFenceLine.Add(tPnt);
}
}
geoFenceArr[j].FixGeoFenceLine(totalHeadWidth, mf.bnd.bndArr[j].bndLine, mf.tool.toolWidth * 0.5);
geoFenceArr[j].PreCalcTurnLines();
}
//mf.TimedMessageBox(800, "Turn Lines", "Turn limits Created");
}
private void oglSelf_MouseDown(object sender, MouseEventArgs e)
{
btnCancelTouch.Enabled = true;
btnMakeABLine.Enabled = false;
btnMakeCurve.Enabled = false;
isMakingAB = isMakingCurve = false;
Point pt = oglSelf.PointToClient(Cursor.Position);
//Convert to Origin in the center of window, 800 pixels
fixPt.X = pt.X - 350;
fixPt.Y = (700 - pt.Y - 350);
vec3 plotPt = new vec3
{
//convert screen coordinates to field coordinates
easting = fixPt.X * mf.maxFieldDistance / 632.0,
northing = fixPt.Y * mf.maxFieldDistance / 632.0,
heading = 0
};
plotPt.easting += mf.fieldCenterX;
plotPt.northing += mf.fieldCenterY;
pint.easting = plotPt.easting;
pint.northing = plotPt.northing;
if (isA)
{
double minDistA = 1000000, minDistB = 1000000;
start = 99999; end = 99999;
int ptCount = arr.Length;
if (ptCount > 0)
{
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
double dist = ((pint.easting - arr[t].easting) * (pint.easting - arr[t].easting))
+ ((pint.northing - arr[t].northing) * (pint.northing - arr[t].northing));
if (dist < minDistA)
{
minDistB = minDistA;
B = A;
minDistA = dist;
A = t;
}
else if (dist < minDistB)
{
minDistB = dist;
B = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (A > B)
{
E = A; A = B; B = E;
}
start = A;
}
isA = false;
}
else
{
double minDistA = 1000000, minDistB = 1000000;
int ptCount = arr.Length;
if (ptCount > 0)
{
//find the closest 2 points to current point
for (int t = 0; t < ptCount; t++)
{
double dist = ((pint.easting - arr[t].easting) * (pint.easting - arr[t].easting))
+ ((pint.northing - arr[t].northing) * (pint.northing - arr[t].northing));
if (dist < minDistA)
{
minDistB = minDistA;
D = C;
minDistA = dist;
C = t;
}
else if (dist < minDistB)
{
minDistB = dist;
D = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (C > D)
{
E = C; C = D; D = E;
}
}
isA = true;
int A1 = Math.Abs(A - C);
int B1 = Math.Abs(A - D);
int C1 = Math.Abs(B - C);
int D1 = Math.Abs(B - D);
if (A1 <= B1 && A1 <= C1 && A1 <= D1)
{
start = A; end = C;
}
else if (B1 <= A1 && B1 <= C1 && B1 <= D1)
{
start = A; end = D;
}
else if (C1 <= B1 && C1 <= A1 && C1 <= D1)
{
start = B; end = C;
}
else if (D1 <= B1 && D1 <= C1 && D1 <= A1)
{
start = B; end = D;
}
if (start > end)
{
E = start; start = end; end = E;
}
btnMakeABLine.Enabled = true;
btnMakeCurve.Enabled = true;
}
}
private void BtnMakeCurve_Click(object sender, EventArgs e)
{
btnCancelTouch.Enabled = false;
double moveDist = (double)nudDistance.Value * mf.inchOrCm2m;
double distSq = (moveDist) * (moveDist) * 0.999;
mf.curve.refList?.Clear();
vec3 pt3 = new vec3(arr[start]);
for (int i = start; i < end; i++)
{
//calculate the point inside the boundary
pt3.easting = arr[i].easting -
(Math.Sin(glm.PIBy2 + arr[i].heading) * (moveDist));
pt3.northing = arr[i].northing -
(Math.Cos(glm.PIBy2 + arr[i].heading) * (moveDist));
pt3.heading = arr[i].heading;
bool Add = true;
for (int j = start; j < end; j++)
{
double check = glm.DistanceSquared(pt3.northing, pt3.easting,
arr[j].northing, arr[j].easting);
if (check < distSq)
{
Add = false;
break;
}
}
if (Add)
{
if (mf.curve.refList.Count > 0)
{
double dist = ((pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting) * (pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting))
+ ((pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing) * (pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing));
if (dist > 1)
{
mf.curve.refList.Add(pt3);
}
}
else
{
mf.curve.refList.Add(pt3);
}
}
}
int cnt = mf.curve.refList.Count;
if (cnt > 3)
{
//make sure distance isn't too big between points on Turn
for (int i = 0; i < cnt - 1; i++)
{
int j = i + 1;
//if (j == cnt) j = 0;
double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]);
if (distance > 1.6)
{
vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0,
(mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0,
mf.curve.refList[i].heading);
mf.curve.refList.Insert(j, pointB);
cnt = mf.curve.refList.Count;
i = -1;
}
}
//who knows which way it actually goes
mf.curve.CalculateTurnHeadings();
//calculate average heading of line
double x = 0, y = 0;
mf.curve.isCurveSet = true;
foreach (vec3 pt in mf.curve.refList)
{
x += Math.Cos(pt.heading);
y += Math.Sin(pt.heading);
}
x /= mf.curve.refList.Count;
y /= mf.curve.refList.Count;
mf.curve.aveLineHeading = Math.Atan2(y, x);
if (mf.curve.aveLineHeading < 0)
{
mf.curve.aveLineHeading += glm.twoPI;
}
//build the tail extensions
mf.curve.AddFirstLastPoints();
mf.curve.SmoothAB(4);
mf.curve.CalculateTurnHeadings();
mf.curve.isCurveSet = true;
mf.curve.curveArr.Add(new CCurveLines());
mf.curve.numCurveLines = mf.curve.curveArr.Count;
mf.curve.numCurveLineSelected = mf.curve.numCurveLines;
//array number is 1 less since it starts at zero
int idx = mf.curve.curveArr.Count - 1;
//create a name
mf.curve.curveArr[idx].Name = (Math.Round(glm.toDegrees(mf.curve.aveLineHeading), 1)).ToString(CultureInfo.InvariantCulture)
+ "\u00B0" + mf.FindDirection(mf.curve.aveLineHeading) + DateTime.Now.ToString("hh:mm:ss", CultureInfo.InvariantCulture);
mf.curve.curveArr[idx].aveHeading = mf.curve.aveLineHeading;
//write out the Curve Points
foreach (vec3 item in mf.curve.refList)
{
mf.curve.curveArr[idx].curvePts.Add(item);
}
mf.FileSaveCurveLines();
//update the arrays
btnMakeABLine.Enabled = false;
btnMakeCurve.Enabled = false;
isMakingCurve = false;
isMakingAB = false;
start = 99999; end = 99999;
FixLabelsCurve();
}
else
{
mf.curve.isCurveSet = false;
mf.curve.refList?.Clear();
}
btnExit.Focus();
}
public void UpdatePosition()
{
if (isFollowingDubinsToPath)
{
//set a speed of 7 kmh
mf.sim.stepDistance = 7 / 17.86;
pivotAxlePosRP = mf.pivotAxlePos;
FindGoalPointDubinsPath(dubListCount);
PurePursuit();
//check if close to recorded path
double distSqr = glm.DistanceSquared(pivotAxlePosRP.northing, pivotAxlePosRP.easting, recList[0].northing, recList[0].easting);
if (distSqr < 2)
{
isFollowingRecPath = true;
isFollowingDubinsToPath = false;
dubList.Clear();
dubinsList.Clear();
}
}
if (isFollowingRecPath)
{
pivotAxlePosRP = mf.pivotAxlePos;
FindGoalPointRecPath(recListCount);
PurePursuit();
//if end of the line then stop
if (!isEndOfTheRecLine)
{
mf.sim.stepDistance = recList[C].speed / 17.86;
north = recList[C].northing;
//section control - only if different click the button
bool autoBtn = (mf.autoBtnState == FormGPS.btnStates.Auto);
trig = autoBtn;
if (autoBtn != recList[C].autoBtnState)
{
mf.btnSectionOffAutoOn.PerformClick();
}
}
else
{
//create the dubins path based on start and goal to start of recorded path
GetDubinsPath(homePos);
dubListCount = dubList.Count;
//its too small
if (dubListCount < 5)
{
StopDrivingRecordedPath();
}
//set all the flags
isFollowingDubinsHome = true;
isFollowingRecPath = false;
isFollowingDubinsToPath = false;
isEndOfTheRecLine = false;
}
}
if (isFollowingDubinsHome)
{
mf.sim.stepDistance = 7 / 17.86;
pivotAxlePosRP = mf.pivotAxlePos;
FindGoalPointDubinsPath(dubListCount);
PurePursuit();
//check if close to home position
double distSqr = glm.DistanceSquared(pivotAxlePosRP.easting, pivotAxlePosRP.northing, homePos.easting, homePos.northing);
if (distSqr < 3)
{
StopDrivingRecordedPath();
}
}
//if paused, set the sim to 0
if (isPausedDrivingRecordedPath)
{
mf.sim.stepDistance = 0 / 17.86;
}
}
public vec3(vec3 v)
{
easting = v.easting;
northing = v.northing;
heading = v.heading;
}
public void CreateBndTramRef()
{
//count the points from the boundary
int ptCount = mf.bnd.bndArr[0].bndLine.Count;
outArr?.Clear();
//outside point
vec3 pt3 = new vec3();
double distSq = ((tramWidth * 0.5) - halfWheelTrack) * ((tramWidth * 0.5) - halfWheelTrack) * 0.97;
bool fail = false;
//make the boundary tram outer array
for (int i = 0; i < ptCount; i++)
{
//calculate the point inside the boundary
pt3.easting = mf.bnd.bndArr[0].bndLine[i].easting -
(Math.Sin(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * (tramWidth * 0.5 - halfWheelTrack));
pt3.northing = mf.bnd.bndArr[0].bndLine[i].northing -
(Math.Cos(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * (tramWidth * 0.5 - halfWheelTrack));
for (int j = 0; j < ptCount; j++)
{
double check = glm.DistanceSquared(pt3.northing, pt3.easting,
mf.bnd.bndArr[0].bndLine[j].northing, mf.bnd.bndArr[0].bndLine[j].easting);
if (check < distSq)
{
fail = true;
break;
}
}
if (!fail)
{
pt3.heading = mf.bnd.bndArr[0].bndLine[i].heading;
outArr.Add(pt3);
}
fail = false;
}
int cnt = outArr.Count;
if (cnt < 6)
{
return;
}
const double spacing = 2.0;
double distance;
for (int i = 0; i < cnt - 1; i++)
{
distance = glm.Distance(outArr[i], outArr[i + 1]);
if (distance < spacing)
{
outArr.RemoveAt(i + 1);
cnt = outArr.Count;
i--;
}
}
}
public static double Distance(vec3 first, vec2 second)
{
return(Math.Sqrt(
Math.Pow(first.easting - second.easting, 2)
+ Math.Pow(first.northing - second.northing, 2)));
}
public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
{
int ptCount = refList.Count;
int ptCnt = ptCount - 1;
if (ptCount < 5)
{
return;
}
boxA.easting = pivot.easting - (Math.Sin(aveLineHeading + glm.PIBy2) * 2000);
boxA.northing = pivot.northing - (Math.Cos(aveLineHeading + glm.PIBy2) * 2000);
boxB.easting = pivot.easting + (Math.Sin(aveLineHeading + glm.PIBy2) * 2000);
boxB.northing = pivot.northing + (Math.Cos(aveLineHeading + glm.PIBy2) * 2000);
boxC.easting = boxB.easting + (Math.Sin(aveLineHeading) * 1.0);
boxC.northing = boxB.northing + (Math.Cos(aveLineHeading) * 1.0);
boxD.easting = boxA.easting + (Math.Sin(aveLineHeading) * 1.0);
boxD.northing = boxA.northing + (Math.Cos(aveLineHeading) * 1.0);
boxA.easting -= (Math.Sin(aveLineHeading) * 1.0);
boxA.northing -= (Math.Cos(aveLineHeading) * 1.0);
boxB.easting -= (Math.Sin(aveLineHeading) * 1.0);
boxB.northing -= (Math.Cos(aveLineHeading) * 1.0);
//determine if point are in frustum box
for (int s = 0; s < ptCnt; s++)
{
if ((((boxB.easting - boxA.easting) * (refList[s].northing - boxA.northing))
- ((boxB.northing - boxA.northing) * (refList[s].easting - boxA.easting))) < 0)
{
continue;
}
if ((((boxD.easting - boxC.easting) * (refList[s].northing - boxC.northing))
- ((boxD.northing - boxC.northing) * (refList[s].easting - boxC.easting))) < 0)
{
continue;
}
closestRefIndex = s;
break;
}
double dist = ((pivot.easting - refList[closestRefIndex].easting) * (pivot.easting - refList[closestRefIndex].easting))
+ ((pivot.northing - refList[closestRefIndex].northing) * (pivot.northing - refList[closestRefIndex].northing));
//determine closest point
double minDistance = Math.Sqrt(dist);
//grab the heading at the closest point
refHeading = refList[closestRefIndex].heading;
//which side of the patch are we on is next
//calculate endpoints of reference line based on closest point
refPoint1.easting = refList[closestRefIndex].easting - (Math.Sin(refHeading) * 50.0);
refPoint1.northing = refList[closestRefIndex].northing - (Math.Cos(refHeading) * 50.0);
refPoint2.easting = refList[closestRefIndex].easting + (Math.Sin(refHeading) * 50.0);
refPoint2.northing = refList[closestRefIndex].northing + (Math.Cos(refHeading) * 50.0);
//x2-x1
double dx = refPoint2.easting - refPoint1.easting;
//z2-z1
double dz = refPoint2.northing - refPoint1.northing;
//how far are we away from the reference line at 90 degrees - 2D cross product and distance
double distanceFromRefLine = ((dz * pivot.easting) - (dx * pivot.northing) + (refPoint2.easting
* refPoint1.northing) - (refPoint2.northing * refPoint1.easting));
// / Math.Sqrt((dz * dz) + (dx * dx));
//are we going same direction as stripList was created?
isSameWay = Math.PI - Math.Abs(Math.Abs(pivot.heading - refHeading) - Math.PI) < glm.PIBy2;
deltaOfRefAndAveHeadings = Math.PI - Math.Abs(Math.Abs(aveLineHeading - refHeading) - Math.PI);
deltaOfRefAndAveHeadings = Math.Cos(deltaOfRefAndAveHeadings);
//add or subtract pi by 2 depending on which side of ref line
double piSide;
//sign of distance determines which side of line we are on
if (distanceFromRefLine > 0)
{
piSide = glm.PIBy2;
}
else
{
piSide = -glm.PIBy2;
}
double widthMinusOverlap;
//move the ABLine over based on the overlap amount set in vehicle
if (mf.tool.toolOffset != 0)
{
widthMinusOverlap = mf.tool.toolWidth / 2 - mf.tool.toolOverlap;
}
else
{
widthMinusOverlap = mf.tool.toolWidth - mf.tool.toolOverlap;
}
howManyPathsAway = Math.Round(minDistance / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);
curveNumber = howManyPathsAway;
if (distanceFromRefLine < 0)
{
curveNumber = -curveNumber;
}
//double toolOffset = mf.tool.toolOffset;
//build the current line
curList?.Clear();
for (int i = 0; i < ptCount; i++)
{
var point = new vec3(
refList[i].easting + (Math.Sin(piSide + aveLineHeading) * ((widthMinusOverlap * howManyPathsAway))),
refList[i].northing + (Math.Cos(piSide + aveLineHeading) * ((widthMinusOverlap * howManyPathsAway))),
refList[i].heading);
curList.Add(point);
}
double minDistA = 1000000, minDistB = 1000000;
ptCount = curList.Count;
if (ptCount > 0)
{
if (mf.isStanleyUsed)
{
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
dist = ((steer.easting - curList[t].easting) * (steer.easting - curList[t].easting))
+ ((steer.northing - curList[t].northing) * (steer.northing - curList[t].northing));
if (dist < minDistA)
{
minDistB = minDistA;
B = A;
minDistA = dist;
A = t;
}
else if (dist < minDistB)
{
minDistB = dist;
B = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (A > B)
{
C = A; A = B; B = C;
}
currentLocationIndex = A;
//get the distance from currently active AB line
dx = curList[B].easting - curList[A].easting;
dz = curList[B].northing - curList[A].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return;
}
//abHeading = Math.Atan2(dz, dx);
double abHeading = curList[A].heading;
//how far from current AB Line is fix
distanceFromCurrentLine = ((dz * steer.easting) - (dx * steer.northing) + (curList[B].easting
* curList[A].northing) - (curList[B].northing * curList[A].easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLine > 0;
//absolute the distance
distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);
//Subtract the two headings, if > 1.57 its going the opposite heading as refAB
double abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
if (abFixHeadingDelta >= Math.PI)
{
abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
}
isABSameAsVehicleHeading = abFixHeadingDelta < glm.PIBy2;
// calc point on ABLine closest to current position
double U = (((steer.easting - curList[A].easting) * dx)
+ ((steer.northing - curList[A].northing) * dz))
/ ((dx * dx) + (dz * dz));
rEastCu = curList[A].easting + (U * dx);
rNorthCu = curList[A].northing + (U * dz);
//distance is negative if on left, positive if on right
if (isABSameAsVehicleHeading)
{
if (!isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
abFixHeadingDelta = (steer.heading - abHeading);
}
//opposite way so right is left
else
{
if (isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
abFixHeadingDelta = (steer.heading - abHeading + Math.PI);
}
//Fix the circular error
if (abFixHeadingDelta > Math.PI)
{
abFixHeadingDelta -= Math.PI;
}
else if (abFixHeadingDelta < Math.PI)
{
abFixHeadingDelta += Math.PI;
}
if (abFixHeadingDelta > glm.PIBy2)
{
abFixHeadingDelta -= Math.PI;
}
else if (abFixHeadingDelta < -glm.PIBy2)
{
abFixHeadingDelta += Math.PI;
}
abFixHeadingDelta *= mf.vehicle.stanleyHeadingErrorGain;
if (abFixHeadingDelta > 0.74)
{
abFixHeadingDelta = 0.74;
}
if (abFixHeadingDelta < -0.74)
{
abFixHeadingDelta = -0.74;
}
steerAngleCu = Math.Atan((distanceFromCurrentLine * mf.vehicle.stanleyGain) / ((mf.pn.speed * 0.277777) + 1));
if (steerAngleCu > 0.74)
{
steerAngleCu = 0.74;
}
if (steerAngleCu < -0.74)
{
steerAngleCu = -0.74;
}
steerAngleCu = glm.toDegrees((steerAngleCu + abFixHeadingDelta) * -1.0);
if (steerAngleCu < -mf.vehicle.maxSteerAngle)
{
steerAngleCu = -mf.vehicle.maxSteerAngle;
}
if (steerAngleCu > mf.vehicle.maxSteerAngle)
{
steerAngleCu = mf.vehicle.maxSteerAngle;
}
//Convert to millimeters
distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);
}
else
{
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
dist = ((pivot.easting - curList[t].easting) * (pivot.easting - curList[t].easting))
+ ((pivot.northing - curList[t].northing) * (pivot.northing - curList[t].northing));
if (dist < minDistA)
{
minDistB = minDistA;
B = A;
minDistA = dist;
A = t;
}
else if (dist < minDistB)
{
minDistB = dist;
B = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (A > B)
{
C = A; A = B; B = C;
}
currentLocationIndex = A;
//get the distance from currently active AB line
dx = curList[B].easting - curList[A].easting;
dz = curList[B].northing - curList[A].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return;
}
//abHeading = Math.Atan2(dz, dx);
//how far from current AB Line is fix
distanceFromCurrentLine = ((dz * pivot.easting) - (dx * pivot.northing) + (curList[B].easting
* curList[A].northing) - (curList[B].northing * curList[A].easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLine > 0;
//absolute the distance
distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivot.easting - curList[A].easting) * dx)
+ ((pivot.northing - curList[A].northing) * dz))
/ ((dx * dx) + (dz * dz));
rEastCu = curList[A].easting + (U * dx);
rNorthCu = curList[A].northing + (U * dz);
//double minx, maxx, miny, maxy;
//minx = Math.Min(curList[A].northing, curList[B].northing);
//maxx = Math.Max(curList[A].northing, curList[B].northing);
//miny = Math.Min(curList[A].easting, curList[B].easting);
//maxy = Math.Max(curList[A].easting, curList[B].easting);
//isValid = rNorthCu >= minx && rNorthCu <= maxx && (rEastCu >= miny && rEastCu <= maxy);
//if (!isValid)
//{
// //invalid distance so tell AS module
// distanceFromCurrentLine = 32000;
// mf.guidanceLineDistanceOff = 32000;
// return;
//}
//used for accumulating distance to find goal point
double distSoFar;
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLine);
mf.lookaheadActual = goalPointDistance;
// used for calculating the length squared of next segment.
double tempDist = 0.0;
if (!isSameWay)
{
//counting down
isABSameAsVehicleHeading = false;
distSoFar = glm.Distance(curList[A], rEastCu, rNorthCu);
//Is this segment long enough to contain the full lookahead distance?
if (distSoFar > goalPointDistance)
{
//treat current segment like an AB Line
goalPointCu.easting = rEastCu - (Math.Sin(curList[A].heading) * goalPointDistance);
goalPointCu.northing = rNorthCu - (Math.Cos(curList[A].heading) * goalPointDistance);
}
//multiple segments required
else
{
//cycle thru segments and keep adding lengths. check if start and break if so.
while (A > 0)
{
B--; A--;
tempDist = glm.Distance(curList[B], curList[A]);
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
break; //tempDist contains the full length of next segment
}
else
{
distSoFar += tempDist;
}
}
double t = (goalPointDistance - distSoFar); // the remainder to yet travel
t /= tempDist;
goalPointCu.easting = (((1 - t) * curList[B].easting) + (t * curList[A].easting));
goalPointCu.northing = (((1 - t) * curList[B].northing) + (t * curList[A].northing));
}
}
else
{
//counting up
isABSameAsVehicleHeading = true;
distSoFar = glm.Distance(curList[B], rEastCu, rNorthCu);
//Is this segment long enough to contain the full lookahead distance?
if (distSoFar > goalPointDistance)
{
//treat current segment like an AB Line
goalPointCu.easting = rEastCu + (Math.Sin(curList[A].heading) * goalPointDistance);
goalPointCu.northing = rNorthCu + (Math.Cos(curList[A].heading) * goalPointDistance);
}
//multiple segments required
else
{
//cycle thru segments and keep adding lengths. check if end and break if so.
// ReSharper disable once LoopVariableIsNeverChangedInsideLoop
while (B < ptCount - 1)
{
B++; A++;
tempDist = glm.Distance(curList[B], curList[A]);
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
//A--; B--;
break; //tempDist contains the full length of next segment
}
distSoFar += tempDist;
}
//xt = (((1 - t) * x0 + t * x1)
//yt = ((1 - t) * y0 + t * y1))
double t = (goalPointDistance - distSoFar); // the remainder to yet travel
t /= tempDist;
goalPointCu.easting = (((1 - t) * curList[A].easting) + (t * curList[B].easting));
goalPointCu.northing = (((1 - t) * curList[A].northing) + (t * curList[B].northing));
}
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceSquared = glm.DistanceSquared(goalPointCu.northing, goalPointCu.easting, pivot.northing, pivot.easting);
//calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
double localHeading = glm.twoPI - mf.fixHeading;
ppRadiusCu = goalPointDistanceSquared / (2 * (((goalPointCu.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCu.northing - pivot.northing) * Math.Sin(localHeading))));
steerAngleCu = glm.toDegrees(Math.Atan(2 * (((goalPointCu.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointCu.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));
if (steerAngleCu < -mf.vehicle.maxSteerAngle)
{
steerAngleCu = -mf.vehicle.maxSteerAngle;
}
if (steerAngleCu > mf.vehicle.maxSteerAngle)
{
steerAngleCu = mf.vehicle.maxSteerAngle;
}
if (ppRadiusCu < -500)
{
ppRadiusCu = -500;
}
if (ppRadiusCu > 500)
{
ppRadiusCu = 500;
}
radiusPointCu.easting = pivot.easting + (ppRadiusCu * Math.Cos(localHeading));
radiusPointCu.northing = pivot.northing + (ppRadiusCu * Math.Sin(localHeading));
//angular velocity in rads/sec = 2PI * m/sec * radians/meters
double angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleCu))) / mf.vehicle.wheelbase;
//clamp the steering angle to not exceed safe angular velocity
if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
{
steerAngleCu = glm.toDegrees(steerAngleCu > 0 ?
(Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))
: (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
}
//Convert to centimeters
distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);
//distance is negative if on left, positive if on right
//if you're going the opposite direction left is right and right is left
//double temp;
if (isABSameAsVehicleHeading)
{
//temp = (abHeading);
if (!isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
//opposite way so right is left
else
{
//temp = (abHeading - Math.PI);
//if (temp < 0) temp = (temp + glm.twoPI);
//temp = glm.toDegrees(temp);
if (isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
}
mf.guidanceLineDistanceOff = mf.distanceDisplay = (Int16)distanceFromCurrentLine;
mf.guidanceLineSteerAngle = (Int16)(steerAngleCu * 100);
if (mf.yt.isYouTurnTriggered)
{
//do the pure pursuit from youTurn
mf.yt.DistanceFromYouTurnLine();
mf.seq.DoSequenceEvent();
//now substitute what it thinks are AB line values with auto turn values
steerAngleCu = mf.yt.steerAngleYT;
distanceFromCurrentLine = mf.yt.distanceFromCurrentLine;
goalPointCu = mf.yt.goalPointYT;
radiusPointCu.easting = mf.yt.radiusPointYT.easting;
radiusPointCu.northing = mf.yt.radiusPointYT.northing;
ppRadiusCu = mf.yt.ppRadiusYT;
}
}
else
{
//invalid distance so tell AS module
distanceFromCurrentLine = 32000;
mf.guidanceLineDistanceOff = 32000;
}
}
public void GetCurrentABLine(vec3 pivot)
{
//move the ABLine over based on the overlap amount set in vehicle
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
//x2-x1
double dx = refABLineP2.easting - refABLineP1.easting;
//z2-z1
double dy = refABLineP2.northing - refABLineP1.northing;
//how far are we away from the reference line at 90 degrees
distanceFromRefLine = ((dy * pivot.easting) - (dx * pivot.northing) + (refABLineP2.easting
* refABLineP1.northing) - (refABLineP2.northing * refABLineP1.easting))
/ Math.Sqrt((dy * dy) + (dx * dx));
//sign of distance determines which side of line we are on
if (distanceFromRefLine > 0)
{
refLineSide = 1;
}
else
{
refLineSide = -1;
}
//absolute the distance
distanceFromRefLine = Math.Abs(distanceFromRefLine);
//Which ABLine is the vehicle on, negative is left and positive is right side
howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);
//generate that pass number as signed integer
passNumber = Convert.ToInt32(refLineSide * howManyPathsAway);
//calculate the new point that is number of implement widths over
double toolOffset = mf.vehicle.toolOffset;
vec2 point1;
//depending which way you are going, the offset can be either side
if (isABSameAsVehicleHeading)
{
point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.easting,
(Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.northing);
}
else
{
point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.easting,
(Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.northing);
}
//create the new line extent points for current ABLine based on original heading of AB line
currentABLineP1.easting = point1.easting - (Math.Sin(abHeading) * 40000.0);
currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 40000.0);
currentABLineP2.easting = point1.easting + (Math.Sin(abHeading) * 40000.0);
currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 40000.0);
//get the distance from currently active AB line
//x2-x1
dx = currentABLineP2.easting - currentABLineP1.easting;
//z2-z1
dy = currentABLineP2.northing - currentABLineP1.northing;
//save a copy of dx,dy in youTurn
mf.yt.dxAB = dx; mf.yt.dyAB = dy;
//how far from current AB Line is fix
distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
* currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
/ Math.Sqrt((dy * dy) + (dx * dx));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLine > 0;
// filter for distance
if (Properties.Settings.Default.is_xte)
{
distancefilter = ((Properties.Settings.Default.xtefilter * distancefilter) + distanceFromCurrentLine) / (Properties.Settings.Default.xtefilter + 1);
distanceFromCurrentLine = distancefilter;
}
//absolute the distance
distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);
//double goalPointDistance = mf.vehicle.goalPointLookAhead;
//if (distanceFromCurrentLine < mf.vehicle.goalPointDistanceFromLine )
// goalPointDistance -= goalPointDistance * (mf.vehicle.goalPointDistanceFromLine - distanceFromCurrentLine);
//if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum) goalPointDistance = mf.vehicle.goalPointLookAheadMinimum;
if (Properties.Settings.Default.isortner)
{
goalPointDistance = (mf.pn.speed - distanceFromCurrentLine * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead; // goalPointLookAhead should be 10-20
if (distanceFromCurrentLine > 0.4)
{
goalPointDistance = (mf.pn.speed - 0.4 * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead;;
goalPointDistance += ((distanceFromCurrentLine - 0.4) * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead;
if (goalPointDistance > mf.pn.speed * speedmaxlahead)
{
goalPointDistance = mf.pn.speed * speedmaxlahead;
}
}
if (goalPointDistance < mf.pn.speed * speedminlahead)
{
goalPointDistance = mf.pn.speed * speedminlahead;
}
}
else if (Properties.Settings.Default.isspeed)
{
goalPointDistance = mf.pn.speed * speedmaxlahead;
if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum)
{
goalPointDistance = mf.vehicle.goalPointLookAheadMinimum;
}
}
else if (Properties.Settings.Default.isschelter)
{
//!!!!!how far should goal point be away
//!!!!!double goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777);
//!!!!!if (goalPointDistance < mf.vehicle.minLookAheadDistance) goalPointDistance = mf.vehicle.minLookAheadDistance;
//!!!!!Versuch: how far should goal point be away
if (distanceFromCurrentLine < 0.3)
{
goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) - (distanceFromCurrentLine * 12.5 * (mf.pn.speed / 10));
}
if (distanceFromCurrentLine >= 0.3 & distanceFromCurrentLine < 0.6)
{
goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) - (((0.3 - (distanceFromCurrentLine - 0.3)) * 12.5 * (mf.pn.speed / 10)));
}
if (distanceFromCurrentLine >= 0.6 & distanceFromCurrentLine < 1)
{
goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777);
}
if (distanceFromCurrentLine > 1 & distanceFromCurrentLine < 2.5)
{
goalPointDistance = ((mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) * (((distanceFromCurrentLine - 1) / 10) + 1));
}
if (distanceFromCurrentLine >= 2.5)
{
goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) * 1.15;
}
//minimum of 3.0 meters look ahead
if (goalPointDistance < speedminlahead)
{
goalPointDistance = speedminlahead;
}
}
else
{ //how far should goal point be away - speed * seconds * kmph -> m/s then limit min value
goalPointDistance = mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.27777777;
if (distanceFromCurrentLine < 1.0)
{
goalPointDistance += distanceFromCurrentLine * goalPointDistance * mf.vehicle.goalPointDistanceMultiplier;
}
else
{
goalPointDistance += goalPointDistance * mf.vehicle.goalPointDistanceMultiplier;
}
if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum)
{
goalPointDistance = mf.vehicle.goalPointLookAheadMinimum;
}
}
mf.test1 = goalPointDistance;
//Subtract the two headings, if > 1.57 its going the opposite heading as refAB
abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
if (abFixHeadingDelta >= Math.PI)
{
abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
}
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivot.easting - currentABLineP1.easting) * dx)
+ ((pivot.northing - currentABLineP1.northing) * dy))
/ ((dx * dx) + (dy * dy));
//point on AB line closest to pivot axle point
rEastAB = currentABLineP1.easting + (U * dx);
rNorthAB = currentABLineP1.northing + (U * dy);
//how far should goal point be away - speed * seconds * kmph -> m/s + min value
//double goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777);
//if (goalPointDistance < mf.vehicle.minLookAheadDistance) goalPointDistance = mf.vehicle.minLookAheadDistance;
if (abFixHeadingDelta >= glm.PIBy2)
{
isABSameAsVehicleHeading = false;
goalPointAB.easting = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
}
else
{
isABSameAsVehicleHeading = true;
goalPointAB.easting = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceDSquared
= glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);
//calculate the the new x in local coordinates and steering angle degrees based on wheelbase
double localHeading = glm.twoPI - mf.fixHeading;
ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));
//make sure pp doesn't generate a radius smaller then turn radius
//if (ppRadiusAB > 0)
//{
// if (ppRadiusAB < mf.vehicle.minTurningRadius * 0.95) ppRadiusAB = mf.vehicle.minTurningRadius * 0.95;
//}
//else if (ppRadiusAB > -mf.vehicle.minTurningRadius * 0.95)
//{
// ppRadiusAB = -mf.vehicle.minTurningRadius * 0.95;
//}
steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
/ goalPointDistanceDSquared));
if (steerAngleAB < -mf.vehicle.maxSteerAngle)
{
steerAngleAB = -mf.vehicle.maxSteerAngle;
}
if (steerAngleAB > mf.vehicle.maxSteerAngle)
{
steerAngleAB = mf.vehicle.maxSteerAngle;
}
//limit circle size for display purpose
if (ppRadiusAB < -500)
{
ppRadiusAB = -500;
}
if (ppRadiusAB > 500)
{
ppRadiusAB = 500;
}
radiusPointAB.easting = pivot.easting + (ppRadiusAB * Math.Cos(localHeading));
radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading));
//Convert to millimeters
distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);
//angular velocity in rads/sec = 2PI * m/sec * radians/meters
angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase;
//clamp the steering angle to not exceed safe angular velocity
if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
{
steerAngleAB = glm.toDegrees(steerAngleAB > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity)
/ (glm.twoPI * mf.pn.speed * 0.277777)))
: (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
}
//distance is negative if on left, positive if on right
if (isABSameAsVehicleHeading)
{
if (!isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
//opposite way so right is left
else
{
if (isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
mf.guidanceLineSteerAngle = (Int16)(steerAngleAB * 100);
if (mf.yt.isYouTurnShapeDisplayed)
{
//do the pure pursuit from youTurn
mf.yt.DistanceFromYouTurnLine();
//now substitute what it thinks are AB line values with auto turn values
steerAngleAB = mf.yt.steerAngleYT;
distanceFromCurrentLine = mf.yt.distanceFromCurrentLine;
goalPointAB = mf.yt.goalPointYT;
radiusPointAB.easting = mf.yt.radiusPointYT.easting;
radiusPointAB.northing = mf.yt.radiusPointYT.northing;
ppRadiusAB = mf.yt.ppRadiusYT;
}
}
//every time a new fix, a new patch point from last point to this point
//only need prev point on the first points of triangle strip that makes a box (2 triangles)
public void AddMappingPoint()
{
//add two triangles for next step.
//left side
vec3 point = new vec3(leftPoint.easting, leftPoint.northing, 0);
//add the point to List
triangleList.Add(point);
//Right side
vec3 point2 = new vec3(rightPoint.easting, rightPoint.northing, 0);
//add the point to the list
triangleList.Add(point2);
//count the triangle pairs
numTriangles++;
//quick count
int c = triangleList.Count - 1;
//when closing a job the triangle patches all are emptied but the section delay keeps going.
//Prevented by quick check. 4 points plus colour
if (c >= 5)
{
//calculate area of these 2 new triangles - AbsoluteValue of (Ax(By-Cy) + Bx(Cy-Ay) + Cx(Ay-By)/2)
{
double temp = (triangleList[c].easting * (triangleList[c - 1].northing - triangleList[c - 2].northing))
+ (triangleList[c - 1].easting * (triangleList[c - 2].northing - triangleList[c].northing))
+ (triangleList[c - 2].easting * (triangleList[c].northing - triangleList[c - 1].northing));
temp = Math.Abs(temp / 2.0);
mf.fd.workedAreaTotal += temp;
mf.fd.workedAreaTotalUser += temp;
//temp = 0;
temp = (triangleList[c - 1].easting * (triangleList[c - 2].northing - triangleList[c - 3].northing))
+ (triangleList[c - 2].easting * (triangleList[c - 3].northing - triangleList[c - 1].northing))
+ (triangleList[c - 3].easting * (triangleList[c - 1].northing - triangleList[c - 2].northing));
temp = Math.Abs(temp / 2.0);
mf.fd.workedAreaTotal += temp;
mf.fd.workedAreaTotalUser += temp;
}
}
if (numTriangles > 36)
{
numTriangles = 0;
//save the cutoff patch to be saved later
mf.patchSaveList.Add(triangleList);
triangleList = new List <vec3>();
patchList.Add(triangleList);
//Add Patch colour
vec3 colur = new vec3(mf.sectionColorDay.R, mf.sectionColorDay.G, mf.sectionColorDay.B);
triangleList.Add(colur);
//add the points to List, yes its more points, but breaks up patches for culling
triangleList.Add(point);
triangleList.Add(point2);
}
}
//determine closest point on left side
public void BuildContourGuidanceLine(vec3 pivot)
{
double toolWid = mf.vehicle.toolWidth;
double sinH = Math.Sin(pivot.heading) * 2.0 * toolWid;
double cosH = Math.Cos(pivot.heading) * 2.0 * toolWid;
double sin2HL = 0;
double cos2HL = 0;
double sin2HR = 0;
double cos2HR = 0;
if (mf.vehicle.toolOffset < 0)
{
//sticks out more left
sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2)));
cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2)));
sin2HR = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset)));
cos2HR = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset)));
}
else
{
//sticks out more right
sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset)));
cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset)));
sin2HR = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2)));
cos2HR = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2)));
}
//narrow equipment needs bigger bounding box.
if (mf.vehicle.toolWidth < 6)
{
sinH = Math.Sin(pivot.heading) * 4 * toolWid;
cosH = Math.Cos(pivot.heading) * 4 * toolWid;
}
double sin3H = Math.Sin(pivot.heading + glm.PIBy2) * 0.5;
double cos3H = Math.Cos(pivot.heading + glm.PIBy2) * 0.5;
//build a frustum box ahead of fix to find adjacent paths and points
//left
boxA.easting = pivot.easting - sin2HL;
boxA.northing = pivot.northing - cos2HL;
boxA.easting -= (sinH * 0.25); //bottom left outside
boxA.northing -= (cosH * 0.25);
boxD.easting = boxA.easting + sinH; //top left outside
boxD.northing = boxA.northing + cosH;
boxE.easting = pivot.easting - sin3H; // inside bottom
boxE.northing = pivot.northing - cos3H;
boxG.easting = boxE.easting + (sinH * 0.3); //inside top
boxG.northing = boxE.northing + (cosH * 0.3);
//right
boxB.easting = pivot.easting + sin2HR;
boxB.northing = pivot.northing + cos2HR;
boxB.easting -= (sinH * 0.25);
boxB.northing -= (cosH * 0.25);
boxC.easting = boxB.easting + sinH;
boxC.northing = boxB.northing + cosH;
boxF.easting = pivot.easting + sin3H;
boxF.northing = pivot.northing + cos3H;
boxH.easting = boxF.easting + (sinH * 0.3); //inside top
boxH.northing = boxF.northing + (cosH * 0.3);
conList.Clear();
ctList.Clear();
int ptCount;
//check if no strips yet, return
int stripCount = stripList.Count;
if (stripCount == 0) return;
cvec pointC = new cvec();
if (isRightPriority)
{
//determine if points are in right side frustum box
for (int s = 0; s < stripCount; s++)
{
ptCount = stripList[s].Count;
for (int p = 0; p < ptCount; p++)
{
//FHCBF
if ((((boxH.easting - boxC.easting) * (stripList[s][p].northing - boxC.northing))
- ((boxH.northing - boxC.northing) * (stripList[s][p].easting - boxC.easting))) < 0) { continue; }
if ((((boxC.easting - boxB.easting) * (stripList[s][p].northing - boxB.northing))
- ((boxC.northing - boxB.northing) * (stripList[s][p].easting - boxB.easting))) < 0) { continue; }
if ((((boxB.easting - boxF.easting) * (stripList[s][p].northing - boxF.northing))
- ((boxB.northing - boxF.northing) * (stripList[s][p].easting - boxF.easting))) < 0) { continue; }
if ((((boxF.easting - boxH.easting) * (stripList[s][p].northing - boxH.northing))
- ((boxF.northing - boxH.northing) * (stripList[s][p].easting - boxH.easting))) < 0) { continue; }
//in the box so is it parallelish or perpedicularish to current heading
ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI);
if (ref2 < 1.2 || ref2 > 1.9)
{
//it's in the box and parallelish so add to list
pointC.x = stripList[s][p].easting;
pointC.z = stripList[s][p].northing;
pointC.h = stripList[s][p].heading;
pointC.strip = s;
pointC.pt = p;
conList.Add(pointC);
}
}
}
if (conList.Count == 0)
{
//determine if points are in frustum box
for (int s = 0; s < stripCount; s++)
{
ptCount = stripList[s].Count;
for (int p = 0; p < ptCount; p++)
{
//EADGE
if ((((boxG.easting - boxE.easting) * (stripList[s][p].northing - boxE.northing))
- ((boxG.northing - boxE.northing) * (stripList[s][p].easting - boxE.easting))) < 0) { continue; }
if ((((boxE.easting - boxA.easting) * (stripList[s][p].northing - boxA.northing))
- ((boxE.northing - boxA.northing) * (stripList[s][p].easting - boxA.easting))) < 0) { continue; }
if ((((boxA.easting - boxD.easting) * (stripList[s][p].northing - boxD.northing))
- ((boxA.northing - boxD.northing) * (stripList[s][p].easting - boxD.easting))) < 0) { continue; }
if ((((boxD.easting - boxG.easting) * (stripList[s][p].northing - boxG.northing))
- ((boxD.northing - boxG.northing) * (stripList[s][p].easting - boxG.easting))) < 0) { continue; }
//in the box so is it parallelish or perpedicularish to current heading
ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI);
if (ref2 < 1.2 || ref2 > 1.9)
{
//it's in the box and parallelish so add to list
pointC.x = stripList[s][p].easting;
pointC.z = stripList[s][p].northing;
pointC.h = stripList[s][p].heading;
pointC.strip = s;
pointC.pt = p;
conList.Add(pointC);
}
}
}
}
}
else
{
for (int s = 0; s < stripCount; s++)
{
ptCount = stripList[s].Count;
for (int p = 0; p < ptCount; p++)
{
//EADGE
if ((((boxG.easting - boxE.easting) * (stripList[s][p].northing - boxE.northing))
- ((boxG.northing - boxE.northing) * (stripList[s][p].easting - boxE.easting))) < 0) { continue; }
if ((((boxE.easting - boxA.easting) * (stripList[s][p].northing - boxA.northing))
- ((boxE.northing - boxA.northing) * (stripList[s][p].easting - boxA.easting))) < 0) { continue; }
if ((((boxA.easting - boxD.easting) * (stripList[s][p].northing - boxD.northing))
- ((boxA.northing - boxD.northing) * (stripList[s][p].easting - boxD.easting))) < 0) { continue; }
if ((((boxD.easting - boxG.easting) * (stripList[s][p].northing - boxG.northing))
- ((boxD.northing - boxG.northing) * (stripList[s][p].easting - boxG.easting))) < 0) { continue; }
//in the box so is it parallelish or perpedicularish to current heading
ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI);
if (ref2 < 1.2 || ref2 > 1.9)
{
//it's in the box and parallelish so add to list
pointC.x = stripList[s][p].easting;
pointC.z = stripList[s][p].northing;
pointC.h = stripList[s][p].heading;
pointC.strip = s;
pointC.pt = p;
conList.Add(pointC);
}
}
}
if (conList.Count == 0)
{
//determine if points are in frustum box
for (int s = 0; s < stripCount; s++)
{
ptCount = stripList[s].Count;
for (int p = 0; p < ptCount; p++)
{
if ((((boxH.easting - boxC.easting) * (stripList[s][p].northing - boxC.northing))
- ((boxH.northing - boxC.northing) * (stripList[s][p].easting - boxC.easting))) < 0) { continue; }
if ((((boxC.easting - boxB.easting) * (stripList[s][p].northing - boxB.northing))
- ((boxC.northing - boxB.northing) * (stripList[s][p].easting - boxB.easting))) < 0) { continue; }
if ((((boxB.easting - boxF.easting) * (stripList[s][p].northing - boxF.northing))
- ((boxB.northing - boxF.northing) * (stripList[s][p].easting - boxF.easting))) < 0) { continue; }
if ((((boxF.easting - boxH.easting) * (stripList[s][p].northing - boxH.northing))
- ((boxF.northing - boxH.northing) * (stripList[s][p].easting - boxH.easting))) < 0) { continue; }
//in the box so is it parallelish or perpedicularish to current heading
ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI);
if (ref2 < 1.2 || ref2 > 1.9)
{
//it's in the box and parallelish so add to list
pointC.x = stripList[s][p].easting;
pointC.z = stripList[s][p].northing;
pointC.h = stripList[s][p].heading;
pointC.strip = s;
pointC.pt = p;
conList.Add(pointC);
}
}
}
}
}
//no points in the box, exit
ptCount = conList.Count;
if (ptCount == 0)
{
distanceFromCurrentLine = 9999;
distanceFromCurrentLine = 32000;
mf.guidanceLineDistanceOff = 32000;
return;
}
//determine closest point
minDistance = 99999;
for (int i = 0; i < ptCount; i++)
{
double dist = ((pivot.easting - conList[i].x) * (pivot.easting - conList[i].x))
+ ((pivot.northing - conList[i].z) * (pivot.northing - conList[i].z));
if (minDistance >= dist)
{
minDistance = dist;
closestRefPoint = i;
}
}
//now we have closest point, the distance squared from it, and which patch and point its from
int strip = conList[closestRefPoint].strip;
int pt = conList[closestRefPoint].pt;
refX = stripList[strip][pt].easting;
refZ = stripList[strip][pt].northing;
refHeading = stripList[strip][pt].heading;
//are we going same direction as stripList was created?
bool isSameWay = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - refHeading) - Math.PI) < 1.4;
//which side of the patch are we on is next
//calculate endpoints of reference line based on closest point
refPoint1.easting = refX - (Math.Sin(refHeading) * 50.0);
refPoint1.northing = refZ - (Math.Cos(refHeading) * 50.0);
refPoint2.easting = refX + (Math.Sin(refHeading) * 50.0);
refPoint2.northing = refZ + (Math.Cos(refHeading) * 50.0);
//x2-x1
double dx = refPoint2.easting - refPoint1.easting;
//z2-z1
double dz = refPoint2.northing - refPoint1.northing;
//how far are we away from the reference line at 90 degrees - 2D cross product and distance
distanceFromRefLine = ((dz * mf.pn.fix.easting) - (dx * mf.pn.fix.northing) + (refPoint2.easting
* refPoint1.northing) - (refPoint2.northing * refPoint1.easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//add or subtract pi by 2 depending on which side of ref line
double piSide;
//sign of distance determines which side of line we are on
if (distanceFromRefLine > 0) piSide = glm.PIBy2;
else piSide = -glm.PIBy2;
//offset calcs
double toolOffset = mf.vehicle.toolOffset;
if (isSameWay)
{
toolOffset = 0.0;
}
else
{
if (distanceFromRefLine > 0) toolOffset *= 2.0;
else toolOffset *= -2.0;
}
//move the Guidance Line over based on the overlap, width, and offset amount set in vehicle
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap + toolOffset;
//absolute the distance
distanceFromRefLine = Math.Abs(distanceFromRefLine);
//make the new guidance line list called guideList
ptCount = stripList[strip].Count - 1;
int start, stop;
start = pt - 35; if (start < 0) start = 0;
stop = pt + 35; if (stop > ptCount) stop = ptCount + 1;
double distSq = widthMinusOverlap * widthMinusOverlap * 0.875;
bool fail = false;
for (int i = start; i < stop; i++)
{
//var point = new vec3(
// stripList[strip][i].easting + (Math.Sin(piSide + stripList[strip][i].heading) * widthMinusOverlap),
// stripList[strip][i].northing + (Math.Cos(piSide + stripList[strip][i].heading) * widthMinusOverlap),
// stripList[strip][i].heading);
//ctList.Add(point);
var point = new vec3(
stripList[strip][i].easting + (Math.Sin(piSide + stripList[strip][i].heading) * widthMinusOverlap),
stripList[strip][i].northing + (Math.Cos(piSide + stripList[strip][i].heading) * widthMinusOverlap),
stripList[strip][i].heading);
//ctList.Add(point);
//make sure its not closer then 1 eq width
for (int j = start; j < stop; j++)
{
double check = glm.DistanceSquared(point.northing, point.easting, stripList[strip][j].northing, stripList[strip][j].easting);
if (check < distSq)
{
fail = true;
break;
}
}
if (!fail) ctList.Add(point);
fail = false;
}
//if no boundaries, just return.
if (!mf.bnd.bndArr[0].isSet) return;
int ctCount = ctList.Count;
if (ctCount < 6) return;
const double spacing = 0.8;
double distance;
for (int i = 0; i < ctCount - 1; i++)
{
distance = glm.Distance(ctList[i], ctList[i + 1]);
if (distance < spacing)
{
ctList.RemoveAt(i + 1);
ctCount = ctList.Count;
i--;
}
}
{
//count the reference list of original curve
int cnt = ctList.Count;
//just go back if not very long
if (cnt < 10) return;
//the temp array
vec3[] arr = new vec3[cnt];
//how many samples
const int smPts = 2;
//read the points before and after the setpoint
for (int s = 0; s < smPts; s++)
{
arr[s].easting = ctList[s].easting;
arr[s].northing = ctList[s].northing;
arr[s].heading = ctList[s].heading;
}
for (int s = cnt - smPts; s < cnt; s++)
{
arr[s].easting = ctList[s].easting;
arr[s].northing = ctList[s].northing;
arr[s].heading = ctList[s].heading;
}
//average them - center weighted average
for (int i = smPts; i < cnt - smPts; i++)
{
for (int j = -smPts; j < smPts; j++)
{
arr[i].easting += ctList[j + i].easting;
arr[i].northing += ctList[j + i].northing;
}
arr[i].easting /= (smPts * 2);
arr[i].northing /= (smPts * 2);
arr[i].heading = ctList[i].heading;
}
//make a list to draw
ctList?.Clear();
for (int i = 0; i < cnt; i++)
{
ctList.Add(arr[i]);
}
}
}
public void FixBoundaryLine(int bndNum, double spacing)
{
//count the points from the boundary
int ptCount = bndLine.Count;
//boundary point spacing based on eq width
spacing *= 0.25;
if (spacing < 1)
{
spacing = 1;
}
if (spacing > 3)
{
spacing = 3;
}
//first find out which side is inside the boundary
double oneSide = glm.PIBy2;
vec3 point = new vec3(bndLine[3].easting - (Math.Sin(oneSide + bndLine[3].heading) * 2.0),
bndLine[3].northing - (Math.Cos(oneSide + bndLine[3].heading) * 2.0), 0.0);
//make sure boundaries are wound correctly
if (bndNum == 0)
{
//outside an outer boundary means its wound clockwise
if (!IsPointInsideBoundary(point))
{
ReverseWinding();
}
}
else
{
//inside an inner boundary means its wound clockwise
if (IsPointInsideBoundary(point))
{
ReverseWinding();
}
spacing *= 0.66;
}
//make sure distance isn't too small between points on headland
int bndCount = bndLine.Count;
double distance;
for (int i = 0; i < bndCount - 1; i++)
{
distance = glm.Distance(bndLine[i], bndLine[i + 1]);
if (distance < spacing)
{
bndLine.RemoveAt(i + 1);
bndCount = bndLine.Count;
i--;
}
}
//make sure distance isn't too big between points on boundary
bndCount = bndLine.Count;
spacing *= 1.33;
for (int i = 0; i < bndCount; i++)
{
int j = i + 1;
if (j == bndCount)
{
j = 0;
}
distance = glm.Distance(bndLine[i], bndLine[j]);
if (distance > spacing)
{
CBndPt pointB = new CBndPt((bndLine[i].easting + bndLine[j].easting) / 2.0,
(bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading);
bndLine.Insert(j, pointB);
bndCount = bndLine.Count;
i--;
}
}
//make sure distance isn't too big between points on boundary
bndCount = bndLine.Count;
spacing *= 1.33;
for (int i = 0; i < bndCount; i++)
{
int j = i + 1;
if (j == bndCount)
{
j = 0;
}
distance = glm.Distance(bndLine[i], bndLine[j]);
if (distance > spacing)
{
CBndPt pointB = new CBndPt((bndLine[i].easting + bndLine[j].easting) / 2.0,
(bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading);
bndLine.Insert(j, pointB);
bndCount = bndLine.Count;
i--;
}
}
//make sure headings are correct for calculated points
CalculateBoundaryHeadings();
}
public void FindClosestTurnPoint(bool isYouTurnRight, vec3 fromPt, double headAB)
{
//initial scan is straight ahead of pivot point of vehicle to find the right turnLine/boundary
vec3 pt = new vec3();
vec3 rayPt = new vec3();
int closestTurnNum = 99;
double cosHead = Math.Cos(headAB);
double sinHead = Math.Sin(headAB);
for (int b = 1; b < mf.maxCrossFieldLength; b += 2)
{
pt.easting = fromPt.easting + (sinHead * b);
pt.northing = fromPt.northing + (cosHead * b);
int idx = IsPointInsideTurnArea(pt);
if (idx != 0)
{
closestTurnNum = idx;
rayPt.easting = pt.easting;
rayPt.northing = pt.northing;
break;
}
}
if (closestTurnNum < 0)
{
closestTurnNum = 0;
}
//second scan is straight ahead of outside of tool based on turn direction
double scanWidthL, scanWidthR;
if (isYouTurnRight) //its actually left
{
scanWidthL = -(mf.tool.toolWidth * 0.25) - (mf.tool.toolWidth * 0.5);
scanWidthR = (mf.tool.toolWidth * 0.25) - (mf.tool.toolWidth * 0.5);
}
else
{
scanWidthL = -(mf.tool.toolWidth * 0.25) + (mf.tool.toolWidth * 0.5);
scanWidthR = (mf.tool.toolWidth * 0.25) + (mf.tool.toolWidth * 0.5);
}
//isYouTurnRight actuall means turning left - Painful, but it switches later
boxA.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthL);
boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthL);
boxB.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthR);
boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthR);
boxC.easting = boxB.easting + (Math.Sin(headAB) * boxLength);
boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength);
boxD.easting = boxA.easting + (Math.Sin(headAB) * boxLength);
boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength);
//determine if point is inside bounding box of the 1 turn chosen above
turnClosestList.Clear();
vec4 inBox;
int ptCount = bndList[closestTurnNum].turnLine.Count;
for (int p = 0; p < ptCount; p++)
{
if ((((boxB.easting - boxA.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxA.northing))
- ((boxB.northing - boxA.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxA.easting))) < 0)
{
continue;
}
if ((((boxD.easting - boxC.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxC.northing))
- ((boxD.northing - boxC.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxC.easting))) < 0)
{
continue;
}
if ((((boxC.easting - boxB.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxB.northing))
- ((boxC.northing - boxB.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxB.easting))) < 0)
{
continue;
}
if ((((boxA.easting - boxD.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxD.northing))
- ((boxA.northing - boxD.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxD.easting))) < 0)
{
continue;
}
//it's in the box, so add to list
inBox.easting = bndList[closestTurnNum].turnLine[p].easting;
inBox.northing = bndList[closestTurnNum].turnLine[p].northing;
inBox.heading = bndList[closestTurnNum].turnLine[p].heading;
inBox.index = closestTurnNum;
//which turn/headland is it from
turnClosestList.Add(inBox);
}
if (turnClosestList.Count == 0)
{
if (isYouTurnRight) //its actually left
{
scanWidthL = -(mf.tool.toolWidth * 0.5);
scanWidthR = 0;
}
else
{
scanWidthL = 0;
scanWidthR = (mf.tool.toolWidth * 0.5);
}
//isYouTurnRight actuall means turning left - Painful, but it switches later
boxA.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthL);
boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthL);
boxB.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthR);
boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthR);
boxC.easting = boxB.easting + (Math.Sin(headAB) * boxLength);
boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength);
boxD.easting = boxA.easting + (Math.Sin(headAB) * boxLength);
boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength);
//determine if point is inside bounding box of the 1 turn chosen above
turnClosestList.Clear();
ptCount = bndList[closestTurnNum].turnLine.Count;
for (int p = 0; p < ptCount; p++)
{
if ((((boxB.easting - boxA.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxA.northing))
- ((boxB.northing - boxA.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxA.easting))) < 0)
{
continue;
}
if ((((boxD.easting - boxC.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxC.northing))
- ((boxD.northing - boxC.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxC.easting))) < 0)
{
continue;
}
if ((((boxC.easting - boxB.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxB.northing))
- ((boxC.northing - boxB.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxB.easting))) < 0)
{
continue;
}
if ((((boxA.easting - boxD.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxD.northing))
- ((boxA.northing - boxD.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxD.easting))) < 0)
{
continue;
}
//it's in the box, so add to list
inBox.easting = bndList[closestTurnNum].turnLine[p].easting;
inBox.northing = bndList[closestTurnNum].turnLine[p].northing;
inBox.heading = bndList[closestTurnNum].turnLine[p].heading;
inBox.index = closestTurnNum;
//which turn/headland is it from
turnClosestList.Add(inBox);
}
}
//which of the points is closest
//closestTurnPt.easting = -20000; closestTurnPt.northing = -20000;
ptCount = turnClosestList.Count;
if (ptCount != 0)
{
double totalDist = 0.75 * Math.Sqrt(((fromPt.easting - rayPt.easting) * (fromPt.easting - rayPt.easting))
+ ((fromPt.northing - rayPt.northing) * (fromPt.northing - rayPt.northing)));
//determine closest point
double minDistance = 9999999;
for (int i = 0; i < ptCount; i++)
{
double dist = Math.Sqrt(((fromPt.easting - turnClosestList[i].easting) * (fromPt.easting - turnClosestList[i].easting))
+ ((fromPt.northing - turnClosestList[i].northing) * (fromPt.northing - turnClosestList[i].northing)));
//double distRay = ((rayPt.easting - turnClosestList[i].easting) * (rayPt.easting - turnClosestList[i].easting))
// + ((rayPt.northing - turnClosestList[i].northing) * (rayPt.northing - turnClosestList[i].northing));
if (minDistance >= dist && dist > totalDist)
{
minDistance = dist;
closestTurnPt.easting = turnClosestList[i].easting;
closestTurnPt.northing = turnClosestList[i].northing;
closestTurnPt.heading = turnClosestList[i].heading;
}
}
if (closestTurnPt.heading < 0)
{
closestTurnPt.heading += glm.twoPI;
}
}
}
public static double Distance(vec3 first, double east, double north)
{
return(Math.Sqrt(
Math.Pow(first.easting - east, 2)
+ Math.Pow(first.northing - north, 2)));
}
public void FindClosestHeadlandPoint(vec3 fromPt, double headAB, int bndNum)
{
//heading is based on ABLine, average Curve, and going same direction as AB or not
//Draw a bounding box to determine if points are in it
if (mf.yt.isYouTurnTriggered || mf.yt.isEnteringDriveThru || mf.yt.isExitingDriveThru)
{
boxA.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * -scanWidth); //subtract if positive
boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * -scanWidth);
boxB.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidth);
boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidth);
boxC.easting = boxB.easting + (Math.Sin(headAB) * boxLength);
boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength);
boxD.easting = boxA.easting + (Math.Sin(headAB) * boxLength);
boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength);
boxA.easting -= (Math.Sin(headAB) * boxLength);
boxA.northing -= (Math.Cos(headAB) * boxLength);
boxB.easting -= (Math.Sin(headAB) * boxLength);
boxB.northing -= (Math.Cos(headAB) * boxLength);
}
else
{
boxA.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * -scanWidth);
boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * -scanWidth);
boxB.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidth);
boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidth);
boxC.easting = boxB.easting + (Math.Sin(headAB) * 2000.0);
boxC.northing = boxB.northing + (Math.Cos(headAB) * 2000.0);
boxD.easting = boxA.easting + (Math.Sin(headAB) * 2000.0);
boxD.northing = boxA.northing + (Math.Cos(headAB) * 2000.0);
}
//determine if point is inside bounding box
int ptCount;
hdClosestList.Clear();
vec3 inBox;
ptCount = mf.hlArr[bndNum].hlLine.Count;
for (int p = 0; p < ptCount; p++)
{
if ((((boxB.easting - boxA.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxA.northing))
- ((boxB.northing - boxA.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxA.easting))) < 0)
{
continue;
}
if ((((boxD.easting - boxC.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxC.northing))
- ((boxD.northing - boxC.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxC.easting))) < 0)
{
continue;
}
if ((((boxC.easting - boxB.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxB.northing))
- ((boxC.northing - boxB.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxB.easting))) < 0)
{
continue;
}
if ((((boxA.easting - boxD.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxD.northing))
- ((boxA.northing - boxD.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxD.easting))) < 0)
{
continue;
}
//it's in the box, so add to list
inBox = mf.hlArr[bndNum].hlLine[p];
//which boundary/headland is it from
hdClosestList.Add(inBox);
}
//which of the points is closest
closestHeadlandPt.easting = -20000; closestHeadlandPt.northing = -20000;
ptCount = hdClosestList.Count;
if (ptCount != 0)
{
//determine closest point
double minDistance = 9999999;
for (int i = 0; i < ptCount; i++)
{
double dist = ((fromPt.easting - hdClosestList[i].easting) * (fromPt.easting - hdClosestList[i].easting))
+ ((fromPt.northing - hdClosestList[i].northing) * (fromPt.northing - hdClosestList[i].northing));
if (minDistance >= dist)
{
minDistance = dist;
closestHeadlandPt.easting = hdClosestList[i].easting;
closestHeadlandPt.northing = hdClosestList[i].northing;
closestHeadlandPt.heading = hdClosestList[i].heading;
}
}
if (closestHeadlandPt.heading < 0)
{
closestHeadlandPt.heading += glm.twoPI;
}
}
}
public static double DistanceSquared(vec3 first, vec3 second)
{
return(
Math.Pow(first.easting - second.easting, 2)
+ Math.Pow(first.northing - second.northing, 2));
}
public void BuildHeadland(double widthSet, int inBndNum)
{
//count the points from the boundary
int ptCount = mf.bndArr[inBndNum].bndLine.Count;
//first find out which side is inside the boundary
oneSide = glm.PIBy2;
vec3 point = new vec3(mf.bndArr[inBndNum].bndLine[3].easting - (Math.Sin(oneSide + mf.bndArr[inBndNum].bndLine[3].heading) * 2.0),
mf.bndArr[inBndNum].bndLine[3].northing - (Math.Cos(oneSide + mf.bndArr[inBndNum].bndLine[3].heading) * 2.0), 0.0);
if (inBndNum == 0 && !mf.bndArr[inBndNum].IsPointInsideBoundary(point))
{
oneSide *= -1.0;
}
if (inBndNum != 0 && mf.bndArr[inBndNum].IsPointInsideBoundary(point))
{
oneSide *= -1.0;
}
//clear the headland point list
hlLine.Clear();
//determine how wide a headland space
double totalHeadWidth = (mf.vehicle.toolWidth - mf.vehicle.toolOverlap) * widthSet;
for (int i = 0; i < ptCount; i++)
{
//calculate the point inside the boundary
point.easting = mf.bndArr[inBndNum].bndLine[i].easting - (Math.Sin(oneSide + mf.bndArr[inBndNum].bndLine[i].heading) * totalHeadWidth);
point.northing = mf.bndArr[inBndNum].bndLine[i].northing - (Math.Cos(oneSide + mf.bndArr[inBndNum].bndLine[i].heading) * totalHeadWidth);
point.heading = mf.bndArr[inBndNum].bndLine[i].heading;
//only add if inside actual field boundary, outer boundary is 0 and opposite
if (inBndNum == 0 && mf.bndArr[inBndNum].IsPointInsideBoundary(point))
{
hlLine.Add(point);
}
if (inBndNum != 0 && !mf.bndArr[inBndNum].IsPointInsideBoundary(point))
{
hlLine.Add(point);
}
}
int headCount = hlLine.Count;
//remove the points too close to boundary
for (int i = 0; i < ptCount; i++)
{
for (int j = 0; j < headCount; j++)
{
//make sure distance between headland and boundary is not less then width
distance = glm.Distance(mf.bndArr[inBndNum].bndLine[i], hlLine[j]);
if (distance < (totalHeadWidth * 0.98))
{
hlLine.RemoveAt(j);
headCount = hlLine.Count;
j = 0;
}
}
}
//fix the gaps and overlaps
FixHeadlandList();
//pre calculate all the constants and multiples
PreCalcHeadlandLines();
}
public void FixTurnLine(double totalHeadWidth, List <vec3> curBnd, double spacing)
{
//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.99))
{
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.tool.toolWidth * 0.25;
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--;
}
}
//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))
{
vec3 pointB = new vec3((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--;
}
}
//make sure headings are correct for calculated points
if (turnLine.Count > 0)
{
CalculateTurnHeadings();
}
//int cnt = turnLine.Count;
//vec3[] arr = new vec3[cnt];
//turnLine.CopyTo(arr);
//turnLine.Clear();
//double delta = 0;
//for (int i = 0; i < arr.Length; i++)
//{
// if (i == 0)
// {
// turnLine.Add(new vec3(arr[i].easting, arr[i].northing, arr[i].heading));
// continue;
// }
// delta += (arr[i - 1].heading - arr[i].heading);
// if (Math.Abs(delta) > 0.1)
// {
// vec3 pt = new vec3(arr[i].easting, arr[i].northing, arr[i].heading);
// turnLine.Add(pt);
// delta = 0;
// }
//}
}
public void GetCurrentABLine(vec3 pivot)
{
//move the ABLine over based on the overlap amount set in vehicle
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
//x2-x1
double dx = refABLineP2.easting - refABLineP1.easting;
//z2-z1
double dy = refABLineP2.northing - refABLineP1.northing;
//how far are we away from the reference line at 90 degrees
distanceFromRefLine = ((dy * pivot.easting) - (dx * pivot.northing) + (refABLineP2.easting
* refABLineP1.northing) - (refABLineP2.northing * refABLineP1.easting))
/ Math.Sqrt((dy * dy) + (dx * dx));
//sign of distance determines which side of line we are on
if (distanceFromRefLine > 0)
{
refLineSide = 1;
}
else
{
refLineSide = -1;
}
//absolute the distance
distanceFromRefLine = Math.Abs(distanceFromRefLine);
//Which ABLine is the vehicle on, negative is left and positive is right side
howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);
//generate that pass number as signed integer
passNumber = Convert.ToInt32(refLineSide * howManyPathsAway);
//calculate the new point that is number of implement widths over
double toolOffset = mf.vehicle.toolOffset;
vec2 point1;
//depending which way you are going, the offset can be either side
if (isABSameAsVehicleHeading)
{
point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.easting,
(Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.northing);
}
else
{
point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.easting,
(Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.northing);
}
//create the new line extent points for current ABLine based on original heading of AB line
currentABLineP1.easting = point1.easting - (Math.Sin(abHeading) * 40000.0);
currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 40000.0);
currentABLineP2.easting = point1.easting + (Math.Sin(abHeading) * 40000.0);
currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 40000.0);
//get the distance from currently active AB line
//x2-x1
dx = currentABLineP2.easting - currentABLineP1.easting;
//z2-z1
dy = currentABLineP2.northing - currentABLineP1.northing;
//save a copy of dx,dy in youTurn
mf.yt.dxAB = dx; mf.yt.dyAB = dy;
//how far from current AB Line is fix
distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
* currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
/ Math.Sqrt((dy * dy) + (dx * dx));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLine > 0;
//absolute the distance
distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLine);
mf.lookaheadActual = goalPointDistance;
//Subtract the two headings, if > 1.57 its going the opposite heading as refAB
abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
if (abFixHeadingDelta >= Math.PI)
{
abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
}
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivot.easting - currentABLineP1.easting) * dx)
+ ((pivot.northing - currentABLineP1.northing) * dy))
/ ((dx * dx) + (dy * dy));
//point on AB line closest to pivot axle point
rEastAB = currentABLineP1.easting + (U * dx);
rNorthAB = currentABLineP1.northing + (U * dy);
if (abFixHeadingDelta >= glm.PIBy2)
{
isABSameAsVehicleHeading = false;
goalPointAB.easting = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
}
else
{
isABSameAsVehicleHeading = true;
goalPointAB.easting = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceDSquared
= glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);
//calculate the the new x in local coordinates and steering angle degrees based on wheelbase
double localHeading = glm.twoPI - mf.fixHeading;
ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));
steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
/ goalPointDistanceDSquared));
if (steerAngleAB < -mf.vehicle.maxSteerAngle)
{
steerAngleAB = -mf.vehicle.maxSteerAngle;
}
if (steerAngleAB > mf.vehicle.maxSteerAngle)
{
steerAngleAB = mf.vehicle.maxSteerAngle;
}
//limit circle size for display purpose
if (ppRadiusAB < -500)
{
ppRadiusAB = -500;
}
if (ppRadiusAB > 500)
{
ppRadiusAB = 500;
}
radiusPointAB.easting = pivot.easting + (ppRadiusAB * Math.Cos(localHeading));
radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading));
//Convert to millimeters
distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);
//angular velocity in rads/sec = 2PI * m/sec * radians/meters
angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase;
//clamp the steering angle to not exceed safe angular velocity
if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
{
steerAngleAB = glm.toDegrees(steerAngleAB > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity)
/ (glm.twoPI * mf.pn.speed * 0.277777)))
: (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
}
//distance is negative if on left, positive if on right
if (isABSameAsVehicleHeading)
{
if (!isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
//opposite way so right is left
else
{
if (isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
mf.guidanceLineSteerAngle = (Int16)(steerAngleAB * 100);
if (mf.yt.isYouTurnTriggered)
{
//do the pure pursuit from youTurn
mf.yt.DistanceFromYouTurnLine();
mf.seq.DoSequenceEvent();
//now substitute what it thinks are AB line values with auto turn values
steerAngleAB = mf.yt.steerAngleYT;
distanceFromCurrentLine = mf.yt.distanceFromCurrentLine;
goalPointAB = mf.yt.goalPointYT;
radiusPointAB.easting = mf.yt.radiusPointYT.easting;
radiusPointAB.northing = mf.yt.radiusPointYT.northing;
ppRadiusAB = mf.yt.ppRadiusYT;
}
}
//Add current position to stripList
public void AddPoint(vec3 pivot)
{
vec3 point = new vec3(pivot.easting, pivot.northing, pivot.heading);
ptList.Add(point);
}
private void btnMakeBoundaryCurve_Click(object sender, EventArgs e)
{ //count the points from the boundary
int ptCount = mf.bnd.bndList[0].fenceLine.Count;
mf.curve.refList?.Clear();
//outside point
vec3 pt3 = new vec3();
double moveDist = (double)nudDistance.Value * mf.inchOrCm2m;
double distSq = (moveDist) * (moveDist) * 0.999;
//make the boundary tram outer array
for (int i = 0; i < ptCount; i++)
{
//calculate the point inside the boundary
pt3.easting = mf.bnd.bndList[0].fenceLine[i].easting -
(Math.Sin(glm.PIBy2 + mf.bnd.bndList[0].fenceLine[i].heading) * (moveDist));
pt3.northing = mf.bnd.bndList[0].fenceLine[i].northing -
(Math.Cos(glm.PIBy2 + mf.bnd.bndList[0].fenceLine[i].heading) * (moveDist));
pt3.heading = mf.bnd.bndList[0].fenceLine[i].heading;
bool Add = true;
for (int j = 0; j < ptCount; j++)
{
double check = glm.DistanceSquared(pt3.northing, pt3.easting,
mf.bnd.bndList[0].fenceLine[j].northing, mf.bnd.bndList[0].fenceLine[j].easting);
if (check < distSq)
{
Add = false;
break;
}
}
if (Add)
{
if (mf.curve.refList.Count > 0)
{
double dist = ((pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting) * (pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting))
+ ((pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing) * (pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing));
if (dist > 1)
{
mf.curve.refList.Add(pt3);
}
}
else
{
mf.curve.refList.Add(pt3);
}
}
}
btnCancelTouch.Enabled = false;
int cnt = mf.curve.refList.Count;
if (cnt > 3)
{
//make sure distance isn't too big between points on Turn
for (int i = 0; i < cnt - 1; i++)
{
int j = i + 1;
//if (j == cnt) j = 0;
double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]);
if (distance > 1.2)
{
vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0,
(mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0,
mf.curve.refList[i].heading);
mf.curve.refList.Insert(j, pointB);
cnt = mf.curve.refList.Count;
i = -1;
}
}
//who knows which way it actually goes
mf.curve.CalculateTurnHeadings();
mf.curve.isCurveSet = true;
mf.curve.aveLineHeading = 0;
//mf.curve.SmoothAB(4);
//mf.curve.CalculateTurnHeadings();
mf.curve.isCurveSet = true;
//double offset = ((double)nudDistance.Value) / 200.0;
mf.curve.curveArr.Add(new CCurveLines());
mf.curve.numCurveLines = mf.curve.curveArr.Count;
mf.curve.numCurveLineSelected = mf.curve.numCurveLines;
//array number is 1 less since it starts at zero
int idx = mf.curve.curveArr.Count - 1;
//create a name
mf.curve.curveArr[idx].Name = "Boundary Curve";
mf.curve.curveArr[idx].aveHeading = mf.curve.aveLineHeading;
//write out the Curve Points
foreach (vec3 item in mf.curve.refList)
{
mf.curve.curveArr[idx].curvePts.Add(item);
}
mf.FileSaveCurveLines();
//update the arrays
btnMakeABLine.Enabled = false;
btnMakeCurve.Enabled = false;
isMakingCurve = false;
isMakingAB = false;
start = 99999; end = 99999;
FixLabelsCurve();
}
else
{
mf.curve.isCurveSet = false;
mf.curve.refList?.Clear();
}
btnExit.Focus();
}
//determine closest point on left side
public void BuildContourGuidanceLine(vec3 pivot)
{
//2 triangles EAD and CBF
double sinH = Math.Sin(pivot.heading) * 1.5 * mf.vehicle.toolWidth;
double cosH = Math.Cos(pivot.heading) * 1.5 * mf.vehicle.toolWidth;
double sin2H = Math.Sin(pivot.heading + glm.PIBy2) * 1.5 * mf.vehicle.toolWidth;
double cos2H = Math.Cos(pivot.heading + glm.PIBy2) * 1.5 * mf.vehicle.toolWidth;
double sin3H = Math.Sin(pivot.heading + glm.PIBy2) * 0.5;
double cos3H = Math.Cos(pivot.heading + glm.PIBy2) * 0.5;
//build a frustum box ahead of fix to find adjacent paths and points
boxA.easting = pivot.easting - (sin2H);
boxA.northing = pivot.northing - (cos2H);
boxA.easting -= (sinH * 0.5);
boxA.northing -= (cosH * 0.5);
boxB.easting = pivot.easting + (sin2H);
boxB.northing = pivot.northing + (cos2H);
boxB.easting -= (sinH * 0.5);
boxB.northing -= (cosH * 0.5);
boxC.easting = boxB.easting + (sinH);
boxC.northing = boxB.northing + (cosH);
boxD.easting = boxA.easting + (sinH);
boxD.northing = boxA.northing + (cosH);
boxE.easting = pivot.easting - (sin3H);
boxE.northing = pivot.northing - (cos3H);
boxF.easting = pivot.easting + (sin3H);
boxF.northing = pivot.northing + (cos3H);
conList.Clear();
ctList.Clear();
int ptCount;
//check if no strips yet, return
int stripCount = stripList.Count;
if (stripCount == 0) return;
cvec pointC = new cvec();
//determine if points are in right side frustum box
for (int s = 0; s < stripCount; s++)
{
ptCount = stripList[s].Count;
for (int p = 0; p < ptCount; p++)
{
if ((((boxF.easting - boxC.easting) * (stripList[s][p].northing - boxC.northing))
- ((boxF.northing - boxC.northing) * (stripList[s][p].easting - boxC.easting))) < 0) { continue; }
if ((((boxC.easting - boxB.easting) * (stripList[s][p].northing - boxB.northing))
- ((boxC.northing - boxB.northing) * (stripList[s][p].easting - boxB.easting))) < 0) { continue; }
if ((((boxB.easting - boxF.easting) * (stripList[s][p].northing - boxF.northing))
- ((boxB.northing - boxF.northing) * (stripList[s][p].easting - boxF.easting))) < 0) { continue; }
//in the box so is it parallelish or perpedicularish to current heading
ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI);
if (ref2 < 1.2 || ref2 > 1.9)
{
//it's in the box and parallelish so add to list
pointC.x = stripList[s][p].easting;
pointC.z = stripList[s][p].northing;
pointC.h = stripList[s][p].heading;
pointC.strip = s;
pointC.pt = p;
conList.Add(pointC);
}
}
}
if (conList.Count == 0)
{
//determine if points are in frustum box
for (int s = 0; s < stripCount; s++)
{
ptCount = stripList[s].Count;
for (int p = 0; p < ptCount; p++)
{
if ((((boxE.easting - boxA.easting) * (stripList[s][p].northing - boxA.northing))
- ((boxE.northing - boxA.northing) * (stripList[s][p].easting - boxA.easting))) < 0) { continue; }
if ((((boxD.easting - boxE.easting) * (stripList[s][p].northing - boxE.northing))
- ((boxD.northing - boxE.northing) * (stripList[s][p].easting - boxE.easting))) < 0) { continue; }
if ((((boxA.easting - boxD.easting) * (stripList[s][p].northing - boxD.northing))
- ((boxA.northing - boxD.northing) * (stripList[s][p].easting - boxD.easting))) < 0) { continue; }
//in the box so is it parallelish or perpedicularish to current heading
ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI);
if (ref2 < 1.2 || ref2 > 1.9)
{
//it's in the box and parallelish so add to list
pointC.x = stripList[s][p].easting;
pointC.z = stripList[s][p].northing;
pointC.h = stripList[s][p].heading;
pointC.strip = s;
pointC.pt = p;
conList.Add(pointC);
}
}
}
}
//no points in the box, exit
ptCount = conList.Count;
if (ptCount == 0)
{
distanceFromCurrentLine = 9999;
distanceFromCurrentLine = 32000;
mf.guidanceLineDistanceOff = 32000;
return;
}
//determine closest point
minDistance = 99999;
for (int i = 0; i < ptCount; i++)
{
double dist = ((pivot.easting - conList[i].x) * (pivot.easting - conList[i].x))
+ ((pivot.northing - conList[i].z) * (pivot.northing - conList[i].z));
if (minDistance >= dist)
{
minDistance = dist;
closestRefPoint = i;
}
}
//now we have closest point, the distance squared from it, and which patch and point its from
int strip = conList[closestRefPoint].strip;
int pt = conList[closestRefPoint].pt;
refX = stripList[strip][pt].easting;
refZ = stripList[strip][pt].northing;
refHeading = stripList[strip][pt].heading;
//are we going same direction as stripList was created?
bool isSameWay = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - refHeading) - Math.PI) < 1.4;
//which side of the patch are we on is next
//calculate endpoints of reference line based on closest point
refPoint1.easting = refX - (Math.Sin(refHeading) * 50.0);
refPoint1.northing = refZ - (Math.Cos(refHeading) * 50.0);
refPoint2.easting = refX + (Math.Sin(refHeading) * 50.0);
refPoint2.northing = refZ + (Math.Cos(refHeading) * 50.0);
//x2-x1
double dx = refPoint2.easting - refPoint1.easting;
//z2-z1
double dz = refPoint2.northing - refPoint1.northing;
//how far are we away from the reference line at 90 degrees - 2D cross product and distance
distanceFromRefLine = ((dz * mf.pn.fix.easting) - (dx * mf.pn.fix.northing) + (refPoint2.easting
* refPoint1.northing) - (refPoint2.northing * refPoint1.easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//add or subtract pi by 2 depending on which side of ref line
double piSide;
//sign of distance determines which side of line we are on
if (distanceFromRefLine > 0) piSide = glm.PIBy2;
else piSide = -glm.PIBy2;
//offset calcs
double toolOffset = mf.vehicle.toolOffset;
if (isSameWay)
{
toolOffset = 0.0;
}
else
{
if (distanceFromRefLine > 0) toolOffset *= 2.0;
else toolOffset *= -2.0;
}
//move the Guidance Line over based on the overlap, width, and offset amount set in vehicle
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap + toolOffset;
//absolute the distance
distanceFromRefLine = Math.Abs(distanceFromRefLine);
//make the new guidance line list called guideList
ptCount = stripList[strip].Count - 1;
int start, stop;
if (isSameWay)
{
start = pt - 35; if (start < 0) start = 0;
stop = pt + 40; if (stop > ptCount) stop = ptCount + 1;
}
else
{
start = pt - 40; if (start < 0) start = 0;
stop = pt + 35; if (stop > ptCount) stop = ptCount + 1;
}
double distSq = widthMinusOverlap * widthMinusOverlap * 0.98;
bool fail = false;
for (int i = start; i < stop; i++)
{
var point = new vec3(
stripList[strip][i].easting + (Math.Sin(piSide + stripList[strip][i].heading) * widthMinusOverlap),
stripList[strip][i].northing + (Math.Cos(piSide + stripList[strip][i].heading) * widthMinusOverlap),
stripList[strip][i].heading);
//ctList.Add(point);
//make sure its not closer then 1 eq width
for (int j = start; j < stop; j++)
{
double check = glm.DistanceSquared(point.northing, point.easting, stripList[strip][j].northing, stripList[strip][j].easting);
if (check < distSq)
{
fail = true;
break;
}
}
if (!fail) ctList.Add(point);
fail = false;
}
}
private void btnLoadBoundaryFromGE_Click(object sender, EventArgs e)
{
if (sender is Button button)
{
Selectedreset = true;
btnLoadBoundaryFromGE.Enabled = false;
btnDelete.Enabled = false;
string fileAndDirectory;
{
//create the dialog instance
OpenFileDialog ofd = new OpenFileDialog
{
//set the filter to text KML only
Filter = "KML files (*.KML)|*.KML",
//the initial directory, fields, for the open dialog
InitialDirectory = mf.fieldsDirectory + mf.currentFieldDirectory
};
//was a file selected
if (ofd.ShowDialog() == DialogResult.Cancel)
{
return;
}
else
{
fileAndDirectory = ofd.FileName;
}
}
//start to read the file
string line = null;
string coordinates = null;
int startIndex;
int i = 0;
using (System.IO.StreamReader reader = new System.IO.StreamReader(fileAndDirectory))
{
if (button.Name == "btnLoadMultiBoundaryFromGE")
{
ResetAllBoundary();
}
else
{
i = mf.bnd.boundarySelected;
}
try
{
while (!reader.EndOfStream)
{
line = reader.ReadLine();
startIndex = line.IndexOf("<coordinates>");
if (startIndex != -1)
{
while (true)
{
int endIndex = line.IndexOf("</coordinates>");
if (endIndex == -1)
{
//just add the line
if (startIndex == -1)
{
coordinates += line.Substring(0);
}
else
{
coordinates += line.Substring(startIndex + 13);
}
}
else
{
if (startIndex == -1)
{
coordinates += line.Substring(0, endIndex);
}
else
{
coordinates += line.Substring(startIndex + 13, endIndex - (startIndex + 13));
}
break;
}
line = reader.ReadLine();
line = line.Trim();
startIndex = -1;
}
line = coordinates;
char[] delimiterChars = { ' ', '\t', '\r', '\n' };
string[] numberSets = line.Split(delimiterChars);
//at least 3 points
if (numberSets.Length > 2)
{
mf.bnd.bndArr.Add(new CBoundaryLines());
mf.turn.turnArr.Add(new CTurnLines());
mf.gf.geoFenceArr.Add(new CGeoFenceLines());
foreach (var item in numberSets)
{
string[] fix = item.Split(',');
double.TryParse(fix[0], NumberStyles.Float, CultureInfo.InvariantCulture, out lonK);
double.TryParse(fix[1], NumberStyles.Float, CultureInfo.InvariantCulture, out latK);
double[] xy = mf.pn.DecDeg2UTM(latK, lonK);
//match new fix to current position
easting = xy[0] - mf.pn.utmEast;
northing = xy[1] - mf.pn.utmNorth;
double east = easting;
double nort = northing;
//fix the azimuth error
easting = (Math.Cos(-mf.pn.convergenceAngle) * east) - (Math.Sin(-mf.pn.convergenceAngle) * nort);
northing = (Math.Sin(-mf.pn.convergenceAngle) * east) + (Math.Cos(-mf.pn.convergenceAngle) * nort);
//add the point to boundary
vec3 bndPt = new vec3(easting, northing, 0);
mf.bnd.bndArr[i].bndLine.Add(bndPt);
}
//fix the points if there are gaps bigger then
mf.bnd.bndArr[i].CalculateBoundaryHeadings();
mf.bnd.bndArr[i].PreCalcBoundaryLines();
mf.bnd.bndArr[i].FixBoundaryLine(i, mf.tool.toolWidth);
//boundary area, pre calcs etc
mf.bnd.bndArr[i].CalculateBoundaryArea();
mf.bnd.bndArr[i].PreCalcBoundaryLines();
mf.bnd.bndArr[i].isSet = true;
//if (i == 0) mf.bnd.bndArr[i].isOwnField = true;
//else mf.bnd.bndArr[i].isOwnField = false;
coordinates = "";
i++;
}
else
{
mf.TimedMessageBox(2000, gStr.gsErrorreadingKML, gStr.gsChooseBuildDifferentone);
}
if (button.Name == "btnLoadBoundaryFromGE")
{
break;
}
}
}
mf.FileSaveBoundary();
mf.fd.UpdateFieldBoundaryGUIAreas();
UpdateChart();
}
catch (Exception)
{
return;
}
}
}
}
//determine distance from contour guidance line
public void DistanceFromContourLine(vec3 pivot)
{
isValid = false;
double minDistA = 1000000, minDistB = 1000000;
int ptCount = ctList.Count;
//distanceFromCurrentLine = 9999;
if (ptCount > 0)
{
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
double dist = ((pivot.easting - ctList[t].easting) * (pivot.easting - ctList[t].easting))
+ ((pivot.northing - ctList[t].northing) * (pivot.northing - ctList[t].northing));
if (dist < minDistA)
{
minDistB = minDistA;
B = A;
minDistA = dist;
A = t;
}
else if (dist < minDistB)
{
minDistB = dist;
B = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (A > B) { C = A; A = B; B = C; }
//get the distance from currently active AB line
//x2-x1
double dx = ctList[B].easting - ctList[A].easting;
//z2-z1
double dy = ctList[B].northing - ctList[A].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dy) < Double.Epsilon) return;
//abHeading = Math.Atan2(dz, dx);
abHeading = ctList[A].heading;
//how far from current AB Line is fix
distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (ctList[B].easting
* ctList[A].northing) - (ctList[B].northing * ctList[A].easting))
/ Math.Sqrt((dy * dy) + (dx * dx));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLine > 0;
//absolute the distance
distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivot.easting - ctList[A].easting) * (dx)) + ((pivot.northing - ctList[A].northing) * (dy)))
/ ((dx * dx) + (dy * dy));
rEastCT = ctList[A].easting + (U * (dx));
rNorthCT = ctList[A].northing + (U * (dy));
////determine if the point is between 2 points initially determined
//double minx, maxx, miny, maxy;
//minx = Math.Min(ctList[A].northing, ctList[B].northing);
//maxx = Math.Max(ctList[A].northing, ctList[B].northing);
//miny = Math.Min(ctList[A].easting, ctList[B].easting);
//maxy = Math.Max(ctList[A].easting, ctList[B].easting);
//isValid = (rNorthCT >= minx && rNorthCT <= maxx) && (rEastCT >= miny && rEastCT <= maxy);
//if (!isValid)
//{
// //invalid distance so tell AS module
// distanceFromCurrentLine = 32000;
// mf.guidanceLineDistanceOff = 32000;
// return;
//}
//Subtract the two headings, if > 1.57 its going the opposite heading as refAB
abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
if (abFixHeadingDelta >= Math.PI) abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
//used for accumulating distance to find goal point
double distSoFar;
//how far should goal point be away - speed * seconds * kmph -> m/s + min value
double goalPointDistance = mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.27777777;
//minimum of 4.0 meters look ahead
if (goalPointDistance < 4.0) goalPointDistance = 4.0;
// used for calculating the length squared of next segment.
double tempDist = 0.0;
if (abFixHeadingDelta >= glm.PIBy2)
{
//counting down
isABSameAsFixHeading = false;
distSoFar = glm.Distance(ctList[A], rEastCT, rNorthCT);
//Is this segment long enough to contain the full lookahead distance?
if (distSoFar > goalPointDistance)
{
//treat current segment like an AB Line
goalPointCT.easting = rEastCT - (Math.Sin(ctList[A].heading) * goalPointDistance);
goalPointCT.northing = rNorthCT - (Math.Cos(ctList[A].heading) * goalPointDistance);
}
//multiple segments required
else
{
//cycle thru segments and keep adding lengths. check if start and break if so.
while (A > 0)
{
B--; A--;
tempDist = glm.Distance(ctList[B], ctList[A]);
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
//A++; B++;
break; //tempDist contains the full length of next segment
}
else
{
distSoFar += tempDist;
}
}
double t = (goalPointDistance - distSoFar); // the remainder to yet travel
t /= tempDist;
goalPointCT.easting = (((1 - t) * ctList[B].easting) + (t * ctList[A].easting));
goalPointCT.northing = (((1 - t) * ctList[B].northing) + (t * ctList[A].northing));
}
}
else
{
//counting up
isABSameAsFixHeading = true;
distSoFar = glm.Distance(ctList[B], rEastCT, rNorthCT);
//Is this segment long enough to contain the full lookahead distance?
if (distSoFar > goalPointDistance)
{
//treat current segment like an AB Line
goalPointCT.easting = rEastCT + (Math.Sin(ctList[A].heading) * goalPointDistance);
goalPointCT.northing = rNorthCT + (Math.Cos(ctList[A].heading) * goalPointDistance);
}
//multiple segments required
else
{
//cycle thru segments and keep adding lengths. check if end and break if so.
// ReSharper disable once LoopVariableIsNeverChangedInsideLoop
while (B < ptCount - 1)
{
B++; A++;
tempDist = glm.Distance(ctList[B], ctList[A]);
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
//A--; B--;
break; //tempDist contains the full length of next segment
}
distSoFar += tempDist;
}
//xt = (((1 - t) * x0 + t * x1)
//yt = ((1 - t) * y0 + t * y1))
double t = (goalPointDistance - distSoFar); // the remainder to yet travel
t /= tempDist;
goalPointCT.easting = (((1 - t) * ctList[A].easting) + (t * ctList[B].easting));
goalPointCT.northing = (((1 - t) * ctList[A].northing) + (t * ctList[B].northing));
}
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceSquared = glm.DistanceSquared(goalPointCT.northing, goalPointCT.easting, pivot.northing, pivot.easting);
//calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
double localHeading = glm.twoPI - mf.fixHeading;
ppRadiusCT = goalPointDistanceSquared / (2 * (((goalPointCT.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCT.northing - pivot.northing) * Math.Sin(localHeading))));
steerAngleCT = glm.toDegrees(Math.Atan(2 * (((goalPointCT.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointCT.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));
if (steerAngleCT < -mf.vehicle.maxSteerAngle) steerAngleCT = -mf.vehicle.maxSteerAngle;
if (steerAngleCT > mf.vehicle.maxSteerAngle) steerAngleCT = mf.vehicle.maxSteerAngle;
if (ppRadiusCT < -500) ppRadiusCT = -500;
if (ppRadiusCT > 500) ppRadiusCT = 500;
radiusPointCT.easting = pivot.easting + (ppRadiusCT * Math.Cos(localHeading));
radiusPointCT.northing = pivot.northing + (ppRadiusCT * Math.Sin(localHeading));
//angular velocity in rads/sec = 2PI * m/sec * radians/meters
double angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleCT))) / mf.vehicle.wheelbase;
//clamp the steering angle to not exceed safe angular velocity
if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
{
steerAngleCT = glm.toDegrees(steerAngleCT > 0 ?
(Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))
: (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
}
//Convert to centimeters
distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);
//distance is negative if on left, positive if on right
//if you're going the opposite direction left is right and right is left
//double temp;
if (isABSameAsFixHeading)
{
if (!isOnRightSideCurrentLine) distanceFromCurrentLine *= -1.0;
}
//opposite way so right is left
else if (isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
//fill in the autosteer variables
mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
mf.guidanceLineSteerAngle = (Int16)(steerAngleCT * 100);
}
else
{
//invalid distance so tell AS module
distanceFromCurrentLine = 32000;
mf.guidanceLineDistanceOff = 32000;
}
}
public void FixBoundaryLine(int bndNum )
{
double spacing;
//boundary point spacing based on eq width
//close if less then 30 ha, 60ha, more then 60
if (area < 200000) spacing = 1.1;
else if (area < 400000) spacing = 2.2;
else spacing = 3.3;
if (bndNum > 0) spacing *= 0.5;
int bndCount = bndLine.Count;
double distance;
//make sure distance isn't too big between points on boundary
for (int i = 0; i < bndCount; i++)
{
int j = i + 1;
if (j == bndCount) j = 0;
distance = glm.Distance(bndLine[i], bndLine[j]);
if (distance > spacing*1.5)
{
vec3 pointB = new vec3((bndLine[i].easting + bndLine[j].easting) / 2.0,
(bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading);
bndLine.Insert(j, pointB);
bndCount = bndLine.Count;
i--;
}
}
//make sure distance isn't too big between points on boundary
bndCount = bndLine.Count;
for (int i = 0; i < bndCount; i++)
{
int j = i + 1;
if (j == bndCount) j = 0;
distance = glm.Distance(bndLine[i], bndLine[j]);
if (distance > spacing*1.6)
{
vec3 pointB = new vec3((bndLine[i].easting + bndLine[j].easting) / 2.0,
(bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading);
bndLine.Insert(j, pointB);
bndCount = bndLine.Count;
i--;
}
}
//make sure distance isn't too small between points on headland
spacing *= 1.2;
bndCount = bndLine.Count;
for (int i = 0; i < bndCount - 1; i++)
{
distance = glm.Distance(bndLine[i], bndLine[i + 1]);
if (distance < spacing)
{
bndLine.RemoveAt(i + 1);
bndCount = bndLine.Count;
i--;
}
}
//make sure headings are correct for calculated points
CalculateBoundaryHeadings();
double delta = 0;
bndLineEar?.Clear();
for (int i = 0; i < bndLine.Count; i++)
{
if (i == 0)
{
bndLineEar.Add(new vec2(bndLine[i].easting, bndLine[i].northing));
continue;
}
delta += (bndLine[i - 1].heading - bndLine[i].heading);
if (Math.Abs(delta) > 0.01)
{
bndLineEar.Add(new vec2(bndLine[i].easting, bndLine[i].northing));
delta = 0;
}
}
}
public void GetCurrentABLine(vec3 pivot, vec3 steer)
{
double dx, dy;
//build new current ref line if required
if (!isABValid || ((mf.secondsSinceStart - lastSecond) > 0.66 && (!mf.isAutoSteerBtnOn || mf.mc.steerSwitchHigh)))
{
BuildCurrentABLineList(pivot);
}
//Check uturn first
if (mf.yt.isYouTurnTriggered && mf.yt.DistanceFromYouTurnLine())//do the pure pursuit from youTurn
{
//now substitute what it thinks are AB line values with auto turn values
steerAngleAB = mf.yt.steerAngleYT;
distanceFromCurrentLinePivot = mf.yt.distanceFromCurrentLine;
goalPointAB = mf.yt.goalPointYT;
radiusPointAB.easting = mf.yt.radiusPointYT.easting;
radiusPointAB.northing = mf.yt.radiusPointYT.northing;
ppRadiusAB = mf.yt.ppRadiusYT;
}
//Stanley
else if (mf.isStanleyUsed)
{
mf.gyd.StanleyGuidanceABLine(currentABLineP1, currentABLineP2, pivot, steer);
}
//Pure Pursuit
else
{
//get the distance from currently active AB line
//x2-x1
dx = currentABLineP2.easting - currentABLineP1.easting;
//z2-z1
dy = currentABLineP2.northing - currentABLineP1.northing;
//save a copy of dx,dy in youTurn
mf.yt.dxAB = dx; mf.yt.dyAB = dy;
//how far from current AB Line is fix
distanceFromCurrentLinePivot = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
* currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
/ Math.Sqrt((dy * dy) + (dx * dx));
//integral slider is set to 0
if (mf.vehicle.purePursuitIntegralGain != 0 && !mf.isReverse)
{
pivotDistanceError = distanceFromCurrentLinePivot * 0.2 + pivotDistanceError * 0.8;
if (counter2++ > 4)
{
pivotDerivative = pivotDistanceError - pivotDistanceErrorLast;
pivotDistanceErrorLast = pivotDistanceError;
counter2 = 0;
pivotDerivative *= 2;
//limit the derivative
//if (pivotDerivative > 0.03) pivotDerivative = 0.03;
//if (pivotDerivative < -0.03) pivotDerivative = -0.03;
//if (Math.Abs(pivotDerivative) < 0.01) pivotDerivative = 0;
}
//pivotErrorTotal = pivotDistanceError + pivotDerivative;
if (mf.isAutoSteerBtnOn &&
Math.Abs(pivotDerivative) < (0.1) &&
mf.avgSpeed > 2.5 &&
!mf.yt.isYouTurnTriggered)
//&& Math.Abs(pivotDistanceError) < 0.2)
{
//if over the line heading wrong way, rapidly decrease integral
if ((inty < 0 && distanceFromCurrentLinePivot < 0) || (inty > 0 && distanceFromCurrentLinePivot > 0))
{
inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.04;
}
else
{
if (Math.Abs(distanceFromCurrentLinePivot) > 0.02)
{
inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.02;
if (inty > 0.2)
{
inty = 0.2;
}
else if (inty < -0.2)
{
inty = -0.2;
}
}
}
}
else
{
inty *= 0.95;
}
}
else
{
inty = 0;
}
//Subtract the two headings, if > 1.57 its going the opposite heading as refAB
abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
if (abFixHeadingDelta >= Math.PI)
{
abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
}
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivot.easting - currentABLineP1.easting) * dx)
+ ((pivot.northing - currentABLineP1.northing) * dy))
/ ((dx * dx) + (dy * dy));
//point on AB line closest to pivot axle point
rEastAB = currentABLineP1.easting + (U * dx);
rNorthAB = currentABLineP1.northing + (U * dy);
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();
if (mf.isReverse ? isHeadingSameWay : !isHeadingSameWay)
{
goalPointAB.easting = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
}
else
{
goalPointAB.easting = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceDSquared
= glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);
//calculate the the new x in local coordinates and steering angle degrees based on wheelbase
double localHeading;
if (isHeadingSameWay)
{
localHeading = glm.twoPI - mf.fixHeading + inty;
}
else
{
localHeading = glm.twoPI - mf.fixHeading - inty;
}
ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));
steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
/ goalPointDistanceDSquared));
if (mf.ahrs.imuRoll != 88888)
{
steerAngleAB += mf.ahrs.imuRoll * -mf.gyd.sideHillCompFactor;
}
if (steerAngleAB < -mf.vehicle.maxSteerAngle)
{
steerAngleAB = -mf.vehicle.maxSteerAngle;
}
if (steerAngleAB > mf.vehicle.maxSteerAngle)
{
steerAngleAB = mf.vehicle.maxSteerAngle;
}
//limit circle size for display purpose
if (ppRadiusAB < -500)
{
ppRadiusAB = -500;
}
if (ppRadiusAB > 500)
{
ppRadiusAB = 500;
}
radiusPointAB.easting = pivot.easting + (ppRadiusAB * Math.Cos(localHeading));
radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading));
if (mf.isAngVelGuidance)
{
//angular velocity in rads/sec = 2PI * m/sec * radians/meters
mf.setAngVel = 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase;
mf.setAngVel = glm.toDegrees(mf.setAngVel) * 100;
//clamp the steering angle to not exceed safe angular velocity
if (Math.Abs(mf.setAngVel) > 1000)
{
//mf.setAngVel = mf.setAngVel < 0 ? -mf.vehicle.maxAngularVelocity : mf.vehicle.maxAngularVelocity;
mf.setAngVel = mf.setAngVel < 0 ? -1000 : 1000;
}
}
//distance is negative if on left, positive if on right
if (!isHeadingSameWay)
{
distanceFromCurrentLinePivot *= -1.0;
}
//Convert to millimeters
mf.guidanceLineDistanceOff = (short)Math.Round(distanceFromCurrentLinePivot * 1000.0, MidpointRounding.AwayFromZero);
mf.guidanceLineSteerAngle = (short)(steerAngleAB * 100);
}
}
public void FindPointsDriveAround(vec3 fromPt, double headAB, ref vec3 start, ref vec3 stop)
{
//initial scan is straight ahead of pivot point of vehicle to find the right turnLine/boundary
vec3 pt = new vec3();
bool isFound = false;
int closestTurnNum = 99;
int closestTurnIndex = 99999;
int mazeDim = mf.mazeGrid.mazeColXDim * mf.mazeGrid.mazeRowYDim;
double cosHead = Math.Cos(headAB);
double sinHead = Math.Sin(headAB);
for (int b = 1; b < mf.maxCrossFieldLength; b += 2)
{
pt.easting = fromPt.easting + (sinHead * b);
pt.northing = fromPt.northing + (cosHead * b);
if (mf.turn.turnArr[0].IsPointInTurnWorkArea(pt))
{
for (int t = 1; t < mf.bnd.bndArr.Count; t++)
{
if (!mf.bnd.bndArr[t].isSet || mf.bnd.bndArr[t].isDriveThru)
{
continue;
}
if (mf.bnd.bndArr[t].isDriveAround)
{
if (mf.gf.geoFenceArr[t].IsPointInGeoFenceArea(pt))
{
isFound = true;
closestTurnNum = t;
closestTurnIndex = b;
start.easting = fromPt.easting + (sinHead * b);
start.northing = fromPt.northing + (cosHead * b);
start.heading = headAB;
break;
}
}
else
{
//its a uturn obstacle
if (mf.turn.turnArr[t].IsPointInTurnWorkArea(pt))
{
start.easting = 88888;
return;
}
}
}
}
else
{
isFound = true;
start.easting = 88888;
closestTurnNum = 0;
return;
}
if (isFound)
{
break;
}
}
isFound = false;
for (int b = closestTurnIndex + 200; b > closestTurnIndex; b--)
{
pt.easting = fromPt.easting + (sinHead * b);
pt.northing = fromPt.northing + (cosHead * b);
if (mf.gf.geoFenceArr[closestTurnNum].IsPointInGeoFenceArea(pt))
{
isFound = true;
stop.easting = fromPt.easting + (sinHead * b);
stop.northing = fromPt.northing + (cosHead * b);
stop.heading = headAB;
}
if (isFound)
{
break;
}
}
for (int i = 0; i < 30; i++)
{
start.easting -= sinHead;
start.northing -= cosHead;
start.heading = headAB;
int iStart = (int)((((int)((start.northing - mf.minFieldY) / mf.mazeGrid.mazeScale)) * mf.mazeGrid.mazeColXDim)
+ (int)((start.easting - mf.minFieldX) / mf.mazeGrid.mazeScale));
if (iStart >= mazeDim)
{
isFound = true;
start.easting = 88888;
closestTurnNum = 0;
return;
}
if (mf.mazeGrid.mazeArr[iStart] == 0)
{
break;
}
}
for (int i = 0; i < 30; i++)
{
stop.easting += sinHead;
stop.northing += cosHead;
stop.heading = headAB;
int iStop = (int)((((int)((stop.northing - mf.minFieldY) / mf.mazeGrid.mazeScale)) * mf.mazeGrid.mazeColXDim)
+ (int)((stop.easting - mf.minFieldX) / mf.mazeGrid.mazeScale));
if (iStop >= mazeDim)
{
isFound = true;
start.easting = 88888;
closestTurnNum = 0;
return;
}
if (mf.mazeGrid.mazeArr[iStop] == 0)
{
break;
}
}
}
private void btnBPoint_Click(object sender, System.EventArgs e)
{
mf.curve.aveLineHeading = 0;
mf.curve.isOkToAddPoints = false;
btnBPoint.Enabled = false;
btnAPoint.Enabled = false;
btnPausePlay.Enabled = false;
btnCancel.Enabled = false;
lvLines.Enabled = false;
int cnt = mf.curve.refList.Count;
if (cnt > 3)
{
//make sure distance isn't too big between points on Turn
for (int i = 0; i < cnt - 1; i++)
{
int j = i + 1;
//if (j == cnt) j = 0;
double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]);
if (distance > 1.2)
{
vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0,
(mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0,
mf.curve.refList[i].heading);
mf.curve.refList.Insert(j, pointB);
cnt = mf.curve.refList.Count;
i = -1;
}
}
//calculate average heading of line
double x = 0, y = 0;
mf.curve.isCurveSet = true;
foreach (var pt in mf.curve.refList)
{
x += Math.Cos(pt.heading);
y += Math.Sin(pt.heading);
}
x /= mf.curve.refList.Count;
y /= mf.curve.refList.Count;
mf.curve.aveLineHeading = Math.Atan2(y, x);
if (mf.curve.aveLineHeading < 0)
{
mf.curve.aveLineHeading += glm.twoPI;
}
//build the tail extensions
mf.curve.AddFirstLastPoints();
SmoothAB(4);
mf.curve.CalculateTurnHeadings();
mf.curve.isCurveSet = true;
mf.EnableYouTurnButtons();
//mf.FileSaveCurveLine();
lblCurveExists.Text = gStr.gsCurveSet;
ShowSavedPanel(true);
btnAddAndGo.Enabled = true;
btnAddToFile.Enabled = true;
btnAPoint.Enabled = false;
btnBPoint.Enabled = false;
btnPausePlay.Enabled = false;
textBox1.BackColor = Color.LightGreen;
textBox1.Enabled = true;
textBox1.Text = (Math.Round(glm.toDegrees(mf.curve.aveLineHeading), 1)).ToString(CultureInfo.InvariantCulture)
+ "\u00B0" + mf.FindDirection(mf.curve.aveLineHeading)
+ DateTime.Now.ToString("hh:mm:ss", CultureInfo.InvariantCulture);
}
else
{
mf.curve.isCurveSet = false;
mf.curve.refList?.Clear();
lblCurveExists.Text = " > Off <";
ShowSavedPanel(true);
btnNewCurve.Enabled = true;
btnCancel.Enabled = true;
lvLines.Enabled = true;
}
lvLines.SelectedItems.Clear();
}