Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Change automatic steering to follow "local" nearest point instead of "global" while following a curve #392

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

Change automatic steering to follow "local" nearest point instead of "global" while following a curve #392

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
165 changes: 139 additions & 26 deletions SourceCode/GPS/Classes/CABCurve.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,9 @@ public class CABCurve

public double inty;

// Should we find the global nearest curve point (instead of local) on the next search.
private bool findGlobalNearestCurvePoint = false;

public CABCurve(FormGPS _f)
{
//constructor
Expand Down Expand Up @@ -402,16 +405,16 @@ public void BuildCurveCurrentList(vec3 pivot)
refPoint1 = mf.trk.gArr[idx].ptA;

//cross product
isHeadingSameWay = ((mf.pivotAxlePos.easting - refPoint1.easting) * (mf.steerAxlePos.northing - refPoint1.northing)
isHeadingSameWay = ((mf.pivotAxlePos.easting - refPoint1.easting) * (mf.steerAxlePos.northing - refPoint1.northing)
- (mf.pivotAxlePos.northing - refPoint1.northing) * (mf.steerAxlePos.easting - refPoint1.easting)) < 0;

//how far are we away from the reference line at 90 degrees - 2D cross product and distance
distanceFromRefLine = glm.Distance(mf.pivotAxlePos, refPoint1);

distanceFromRefLine -= (0.5 * widthMinusOverlap);

double RefDist = (distanceFromRefLine
+ (isHeadingSameWay ? mf.tool.offset : -mf.tool.offset)
double RefDist = (distanceFromRefLine
+ (isHeadingSameWay ? mf.tool.offset : -mf.tool.offset)
+ mf.trk.gArr[idx].nudgeDistance) / widthMinusOverlap;

if (RefDist < 0) howManyPathsAway = (int)(RefDist - 0.5);
Expand All @@ -423,7 +426,7 @@ public void BuildCurveCurrentList(vec3 pivot)
//build the current line
curList?.Clear();

double distAway = widthMinusOverlap * howManyPathsAway
double distAway = widthMinusOverlap * howManyPathsAway
+ (isHeadingSameWay ? -mf.tool.offset : mf.tool.offset) - mf.trk.gArr[idx].nudgeDistance;

distAway += (0.5 * widthMinusOverlap);
Expand All @@ -434,7 +437,7 @@ public void BuildCurveCurrentList(vec3 pivot)

//distAway += mf.trk.gArr[idx].nudgeDistance;

vec3 currentPos = new vec3(refPoint1.easting-distAway, refPoint1.northing, 0);
vec3 currentPos = new vec3(refPoint1.easting - distAway, refPoint1.northing, 0);

while (currentPos.heading < glm.twoPI)
{
Expand Down Expand Up @@ -466,6 +469,7 @@ public void BuildCurveCurrentList(vec3 pivot)
}

lastSecond = mf.secondsSinceStart;
findGlobalNearestCurvePoint = true;
}

public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
Expand All @@ -484,6 +488,10 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer)

if (curList.Count > 0)
{
// Update based on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();
bool ReverseHeading = mf.isReverse ? !isHeadingSameWay : isHeadingSameWay;

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
Expand Down Expand Up @@ -513,14 +521,21 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
//close call hit
int cc = 0, dd;

for (int j = 0; j < curList.Count; j += 10)
if (findGlobalNearestCurvePoint)
{
dist = glm.DistanceSquared(pivot, curList[j]);
if (dist < minDistA)
{
minDistA = dist;
cc = j;
}
// When not already following some line, find the globally nearest point

cc = findNearestGlobalCurvePoint(pivot, 10);

findGlobalNearestCurvePoint = false;
}
else
{
// When already "locked" to follow some line, try to find the "local" nearest point
// based on the last one. This prevents jumping between lines close to each other (or crossing lines).
// As this is prone to find a "local minimum", this should only be used when already following some line.

cc = findNearestLocalCurvePoint(pivot, currentLocationIndex, goalPointDistance, ReverseHeading);
}

minDistA = double.MaxValue;
Expand Down Expand Up @@ -556,14 +571,21 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
}
else
{
for (int j = 0; j < curList.Count; j++)
if (findGlobalNearestCurvePoint)
{
dist = glm.DistanceSquared(pivot, curList[j]);
if (dist < minDistA)
{
minDistA = dist;
A = j;
}
// When not already following some line, find the globally nearest point

A = findNearestGlobalCurvePoint(pivot);

findGlobalNearestCurvePoint = false;
}
else
{
// When already "locked" to follow some line, try to find the "local" nearest point
// based on the last one. This prevents jumping between lines close to each other (or crossing lines).
// As this is prone to find a "local minimum", this should only be used when already following some line.

A = findNearestLocalCurvePoint(pivot, currentLocationIndex, goalPointDistance, ReverseHeading);
}

currentLocationIndex = A;
Expand All @@ -579,7 +601,6 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
curList[B].easting, curList[B].northing, pivot.easting, pivot.northing))
goto SegmentFound;

A = currentLocationIndex;
//step back one
if (A == 0)
{
Expand Down Expand Up @@ -666,11 +687,6 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
rNorthCu = curList[A].northing + (U * dz);
manualUturnHeading = curList[A].heading;

//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();

bool ReverseHeading = mf.isReverse ? !isHeadingSameWay : isHeadingSameWay;

int count = ReverseHeading ? 1 : -1;
vec3 start = new vec3(rEastCu, rNorthCu, 0);
double distSoFar = 0;
Expand Down Expand Up @@ -879,7 +895,7 @@ public void DrawCurve()
GL.Color3(0.95f, 0.2f, 0.95f);

GL.Begin(PrimitiveType.LineStrip);
GL.Vertex3(mf.trk.gArr[mf.trk.idx].ptA.easting, mf.trk.gArr[mf.trk.idx].ptA.northing, 0) ;
GL.Vertex3(mf.trk.gArr[mf.trk.idx].ptA.easting, mf.trk.gArr[mf.trk.idx].ptA.northing, 0);
for (int h = 0; h < curList.Count; h++) GL.Vertex3(curList[h].easting, curList[h].northing, 0);
GL.End();

Expand Down Expand Up @@ -1250,5 +1266,102 @@ public void ResetCurveLine()
mf.trk.idx = -1;
}

// Searches for the nearest "global" curve point to the refPoint by checking all points of the curve.
// Parameter "increment" added here to give possibility to make a "sparser" search (to speed it up?)
// Return: index to the nearest point
private int findNearestGlobalCurvePoint(vec3 refPoint, int increment = 1)
{
double minDist = double.MaxValue;
int minDistIndex = 0;

for (int i = 0; i < curList.Count; i += increment)
{
double dist = glm.DistanceSquared(refPoint, curList[i]);
if (dist < minDist)
{
minDist = dist;
minDistIndex = i;
}
}
return minDistIndex;
}

// Searches for the nearest "local" curve point to the refPoint by traversing forward and backward on the curve
// startIndex means the starting point (index to curList) of the search.
// Return: index to the nearest (local) point
private int findNearestLocalCurvePoint(vec3 refPoint, int startIndex, double minSearchDistance, bool reverseSearchDirection)
{
double minDist = glm.DistanceSquared(refPoint, curList[startIndex]);
int minDistIndex = startIndex;

int directionMultiplier = reverseSearchDirection ? 1 : -1;
double distSoFar = 0;
vec3 start = curList[startIndex];

// Check all points' distances from the pivot inside the "look ahead"-distance and find the nearest
int offset = 1;

while (offset < curList.Count)
{
int pointIndex = (startIndex + (offset * directionMultiplier) + curList.Count) % curList.Count; // Wrap around
double dist = glm.DistanceSquared(refPoint, curList[pointIndex]);

if (dist < minDist)
{
minDist = dist;
minDistIndex = pointIndex;
}

distSoFar += glm.Distance(start, curList[pointIndex]);
start = curList[pointIndex];

offset++;

if (distSoFar > minSearchDistance)
{
break;
}
}

// Continue traversing until the distance starts growing
while (offset < curList.Count)
{
int pointIndex = (startIndex + (offset * directionMultiplier) + curList.Count) % curList.Count; // Wrap around
double dist = glm.DistanceSquared(refPoint, curList[pointIndex]);
if (dist < minDist)
{
// Getting closer
minDist = dist;
minDistIndex = pointIndex;
}
else
{
// Getting farther, no point to continue
break;
}
offset++;
}

// Traverse from the start point also into another direction to be sure we choose the minimum local distance.
// (This is also needed due to the way AB-curve is handled (the search may start one off from the last known nearest point)).
for (offset = 1; offset < curList.Count; offset++)
{
int pointIndex = (startIndex + (offset * (-directionMultiplier)) + curList.Count) % curList.Count; // Wrap around
double dist = glm.DistanceSquared(refPoint, curList[pointIndex]);
if (dist < minDist)
{
// Getting closer
minDist = dist;
minDistIndex = pointIndex;
}
else
{
// Getting farther, no point to continue
break;
}
}

return minDistIndex;
}
}
}