@BrianTee_Admin, The default right or left turn also has some great potential. Below is the code that I used to force the dubins draw point to the right or left. Basically, it looks along a line a set distance either side of the current abline. It looks for an intersection of this “offset” line and the next ABLine in the mix. Not the next lateral shift, but the next reference line. Once cycle is clicked, it builds a dubins from the current position to the intersection. It locks and drives this line. Below is the stolen code of yours that I modified to do this. It is a hack for sure, you can clean it up much better, but is really a nice feature to be able to click the default turn direction regardless of the angle. However, you have to limit the angles or it just becomes a nasty U-turn.
By the way, taking sneak peaks at the new sticky. I really like what you did with the U-turn. Snap the ABLine early rather than latter. NICE!!
Edit:
My son, who does much of the spraying for my dad, his grandfather, asked about the sticky line, so I can leave the field and come back and it keep the same line? He asked with an excitement. With the raven system and with the current flags, we can mark and return, but keep the ABLine stationary while, filling the tank, or other needs, that might be something to consider.
public void GetNextABLine(vec3 pivot, vec3 steer, int numb)
{
double dx, dy;
isValidTurn = false;
//if ((mf.secondsSinceStart - lastSecond) > .01)
//{
// lastSecond = mf.secondsSinceStart;
//move the ABLine over based on the overlap amount set in vehicle
double widthMinusOverlap = mf.tool.toolWidth - mf.tool.toolOverlap;
double diffDelta = (Math.Abs(lineArr[numb].heading - abHeading));
if (diffDelta >= Math.PI) abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
isValidTurn = Math.Abs(diffDelta) >= .42;
//x2-x1
dx = lineArr[numb].ref2.easting - lineArr[numb].ref1.easting;
//z2-z1
dy = lineArr[numb].ref2.northing - lineArr[numb].ref1.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));
double lookdist = (Math.Max(mf.vehicle.minTurningRadius * 1.5, mf.pn.speed * 0.277777 * 2));
vec3 ABLOOK = new vec3(pivot.easting + (Math.Sin(mf.fixHeading) * (lookdist)), pivot.northing + (Math.Cos(mf.fixHeading) * (lookdist)), 0);
double distanceFromRefLine2 = ((dy * ABLOOK.easting) - (dx * ABLOOK.northing) + (lineArr[numb].ref2.easting
* lineArr[numb].ref1.northing) - (lineArr[numb].ref2.northing * lineArr[numb].ref1.easting))
/ Math.Sqrt((dy * dy) + (dx * dx));
double dishowManyPathsAway = Math.Round(distanceFromRefLine2 / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);
//depending which way you are going, the offset can be either side
vec2 point1 = new vec2((Math.Cos(-lineArr[numb].heading) * (widthMinusOverlap * dishowManyPathsAway + (isABSameAsVehicleHeading ? -mf.tool.toolOffset : mf.tool.toolOffset))) + lineArr[numb].ref1.easting,
(Math.Sin(-lineArr[numb].heading) * ((widthMinusOverlap * dishowManyPathsAway) + (isABSameAsVehicleHeading ? -mf.tool.toolOffset : mf.tool.toolOffset))) + lineArr[numb].ref1.northing);
//create the new line extent points for current ABLine based on original heading of AB line
nextABLineP1.easting = point1.easting - (Math.Sin(lineArr[numb].heading) * abLength);
nextABLineP1.northing = point1.northing - (Math.Cos(lineArr[numb].heading) * abLength);
nextABLineP2.easting = point1.easting + (Math.Sin(lineArr[numb].heading) * abLength);
nextABLineP2.northing = point1.northing + (Math.Cos(lineArr[numb].heading) * abLength);
nextABLineP1.heading = lineArr[numb].heading;
nextABLineP2.heading = lineArr[numb].heading;
isNextCurrentSameHeading = Math.PI - Math.Abs(Math.Abs(pivot.heading - abHeading) - Math.PI) < glm.PIBy2;
vec3 A = currentABLineP1;
vec3 B = currentABLineP2;
if (!isNextCurrentSameHeading)
{
A = currentABLineP2;
B = currentABLineP1;
}
vec3 P = nextABLineP1;
B -= A;
P -= A;
int cross = Convert.ToInt32((B.easting * P.northing) -( B.northing * P.easting));
double cosHeading = Math.Cos(-abHeading);
double sinHeading = Math.Sin(-abHeading);
if (isNextCurrentSameHeading)
{
cosHeading = cosHeading * -1;
sinHeading = sinHeading * -1;
}
double stag = lookdist;
if (isRight) stag = -1 * stag;
double A1 = (currentABLineP2.northing + (sinHeading* (stag))) - (currentABLineP1.northing + (sinHeading * (stag)));
double B1 = (currentABLineP1.easting+(cosHeading*stag)) - (currentABLineP2.easting + (cosHeading*stag));
double C1 = (A1 * (currentABLineP1.easting + (cosHeading*stag))) + (B1 * (currentABLineP1.northing + (sinHeading * (stag))));
double A2 = nextABLineP2.northing - nextABLineP1.northing;
double B2 = nextABLineP1.easting - nextABLineP2.easting;
double C2 = (A2 * nextABLineP1.easting) + (B2 * nextABLineP1.northing);
double delta = A1 * B2 - A2 * B1;
if (delta == 0)
{
isValidTurn = false;
return;
}
double x = (B2 * C1 - B1 * C2) / delta;
double y = (A1 * C2 - A2 * C1) / delta;
//isNextCurrentSameHeading = (nextABLineP1.heading - abHeading) < 0;
if (isRight)
{
if (cross > 0) nextPointAB = new vec3(x, y, nextABLineP1.heading);
else
{
double temphead = nextABLineP1.heading + Math.PI;
if (temphead > glm.twoPI) temphead = temphead - glm.twoPI;
nextPointAB = new vec3(x, y, temphead);
}
}
else
{
if (cross < 0) nextPointAB = new vec3(x, y, nextABLineP1.heading);
else
{
double temphead = nextABLineP1.heading + Math.PI;
if (temphead > glm.twoPI) temphead = temphead - glm.twoPI;
nextPointAB = new vec3(x, y, temphead);
}
}
}
