public void GeneratePathToStartInfield()
{
//go to the startpoint
CDubins dubPathToStart = new CDubins();
CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.2;
////get the dubins path vec3 point coordinates of turn
//dubList.Clear();
//dubList = dubPathToStart.GenerateDubins(endHeadlandPoint, startInfieldPoint, mf.bnd);
//transfer point list to genList class
for (int i = 0; i < dubList.Count; i++)
{
CGenPathPt pt = new CGenPathPt(dubList[i].easting, dubList[i].northing, dubList[i].heading);
if (i < 10)
{
pt.sequence = 1;
}
genList.Add(pt);
}
//made the path so set the flag
isPathToInfieldMade = true;
genListCount = genList.Count;
currentPositonIndex = 0;
isEndOfTheGenLine = false;
}
public void GeneratePathHome()
{
genList.Clear();
currentPositonIndex = 0;
isEndOfTheGenLine = false;
//go to the startpoint
CDubins dubPathToStart = new CDubins();
CDubins.turningRadius = mf.vehicle.minTurningRadius * 2.0;
////get the dubins path vec3 point coordinates of turn
//dubList.Clear();
//dubList = dubPathToStart.GenerateDubins(mf.pivotAxlePos, homePoint, mf.bnd);
//transfer point list to genList class
for (int i = 0; i < dubList.Count; i++)
{
CGenPathPt pt = new CGenPathPt(dubList[i].easting, dubList[i].northing, dubList[i].heading);
genList.Add(pt);
}
genListCount = genList.Count;
currentPositonIndex = 0;
//shut everything off
isDrivingHome = true;
}
private void GetDubinsPath(vec3 goal)
{
CDubins dubPath = new CDubins();
//
pivotPos = mf.pivotAxlePos;
//bump it forward
vec3 pt2 = new vec3
{
easting = pivotPos.easting + (Math.Sin(pivotPos.heading) * 4),
northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 4),
heading = pivotPos.heading
};
//get the dubins path vec3 point coordinates of turn
dubinsList.Clear();
dubinsList = dubPath.GenerateDubins(pt2, goal, mf.turn);
dubinsList.Insert(0, mf.pivotAxlePos);
//transfer point list to recPath class point style
dubList.Clear();
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
dubList.Add(pt);
}
//no longer needed
dubinsList?.Clear();
}
private void MakeDubinsLineFromPivotToFlag()
{
//if (mf.ABLine.isBtnABLineOn)
//{
// mf.ABLine.isBtnABLineOn = false;
// mf.btnABLine.Image = Properties.Resources.ABLineOff;
//}
CDubins.turningRadius = mf.vehicle.minTurningRadius * 3.0;
CDubins dubPath = new CDubins();
// current psition
vec3 steerAxlePosRP = mf.pivotAxlePos;
//bump it back so you can line up to point
vec3 goal = new vec3
{
easting = mf.flagPts[mf.flagNumberPicked - 1].easting - (Math.Sin(mf.flagPts[mf.flagNumberPicked - 1].heading) * 6),
northing = mf.flagPts[mf.flagNumberPicked - 1].northing - (Math.Cos(mf.flagPts[mf.flagNumberPicked - 1].heading) * 6),
heading = mf.flagPts[mf.flagNumberPicked - 1].heading
};
//bump it forward
vec3 pt2 = new vec3
{
easting = steerAxlePosRP.easting + (Math.Sin(steerAxlePosRP.heading) * 6),
northing = steerAxlePosRP.northing + (Math.Cos(steerAxlePosRP.heading) * 6),
heading = steerAxlePosRP.heading
};
}
private void GetDubinsPath(vec3 goal)
{
CDubins dubPath = new CDubins();
//get the dubins path vec3 point coordinates of turn
dubinsList.Clear();
dubinsList = dubPath.GenerateDubins(mf.pivotAxlePos, goal);
//transfer point list to recPath class point style
dubList.Clear();
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
dubList.Add(pt);
}
}
private void GetDubinsPath(vec3 goal)
{
CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.2;
CDubins dubPath = new CDubins();
// current psition
pivotAxlePosRP = mf.pivotAxlePos;
//bump it forward
vec3 pt2 = new vec3
{
easting = pivotAxlePosRP.easting + (Math.Sin(pivotAxlePosRP.heading) * 3),
northing = pivotAxlePosRP.northing + (Math.Cos(pivotAxlePosRP.heading) * 3),
heading = pivotAxlePosRP.heading
};
//get the dubins path vec3 point coordinates of turn
shortestDubinsList.Clear();
shuttleDubinsList.Clear();
shortestDubinsList = dubPath.GenerateDubins(pt2, goal);
//if Dubins returns 0 elements, there is an unavoidable blockage in the way.
if (shortestDubinsList.Count > 0)
{
shortestDubinsList.Insert(0, mf.pivotAxlePos);
//transfer point list to recPath class point style
for (int i = 0; i < shortestDubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 9.0, false);
shuttleDubinsList.Add(pt);
}
return;
}
}
//build the points and path of youturn to be scaled and transformed
public void BuildYouTurnListToRight(bool isTurnRight)
{
isYouTurnShapeDisplayed = true;
CDubins dubYouTurnPath = new CDubins();
CDubins.turningRadius = mf.vehicle.minTurningRadius;
//point on AB line closest to pivot axle point from ABLine PurePursuit
rEastYT = mf.ABLine.rEastAB;
rNorthYT = mf.ABLine.rNorthAB;
isABSameAsFixHeading = mf.ABLine.isABSameAsFixHeading;
//pivotAxlePosYT = mf.pivotAxlePos;
//grab the vehicle widths and offsets
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
double toolOffset = mf.vehicle.toolOffset * 2.0;
double turnOffset = 0;
abHeading = mf.ABLine.abHeading;
//turning right
if (isTurnRight)
{
turnOffset = (widthMinusOverlap + toolOffset);
}
else
{
turnOffset = (widthMinusOverlap - toolOffset);
}
//if using dubins to calculate youturn
if (isUsingDubinsTurn)
{
//if (lastTime == 3)
//{
// lastTime = 5;
// rowSkipsWidth = 5;
//}
//else
//{
// lastTime = 3;
// rowSkipsWidth = 3;
//}
double head = mf.ABLine.abHeading;
//if its straight across it makes 2 loops instead so goal is a little lower then start
if (!isABSameAsFixHeading)
{
head += 3.14;
}
else
{
head -= 0.01;
}
var start = new vec3(rEastYT, rNorthYT, head);
var goal = new vec3();
//also adjust for rowskips
head -= Math.PI;
if (head < 0)
{
head += glm.twoPI;
}
if (isTurnRight)
{
goal.easting = rEastYT - Math.Cos(-head) * turnOffset * rowSkipsWidth;
goal.northing = rNorthYT - Math.Sin(-head) * turnOffset * rowSkipsWidth;
goal.heading = head;
}
else
{
goal.easting = rEastYT + Math.Cos(-head) * turnOffset * rowSkipsWidth;
goal.northing = rNorthYT + Math.Sin(-head) * turnOffset * rowSkipsWidth;
goal.heading = head;
}
ytList = dubYouTurnPath.GenerateDubins(start, goal);
}
//or the patterns
else
{
numShapePoints = youFileList.Count;
vec3[] pt = new vec3[numShapePoints];
//Now put the shape into an array since lists are immutable
for (int i = 0; i < numShapePoints; i++)
{
pt[i].easting = youFileList[i].easting;
pt[i].northing = youFileList[i].northing;
}
//start of path on the origin. Mirror the shape if left turn
if (!isTurnRight)
{
for (int i = 0; i < pt.Length; i++)
{
pt[i].easting *= -1;
}
}
//scaling - Drawing is 10m wide so find ratio of tool width
double scale = turnOffset * 0.1;
for (int i = 0; i < pt.Length; i++)
{
pt[i].easting *= scale * rowSkipsWidth;
pt[i].northing *= scale * rowSkipsWidth;
}
//rotate pattern to match AB Line heading
for (int i = 0; i < pt.Length; i++)
{
double xr, yr;
if (isABSameAsFixHeading)
{
xr = (Math.Cos(-abHeading) * pt[i].easting) - (Math.Sin(-abHeading) * pt[i].northing);
yr = (Math.Sin(-abHeading) * pt[i].easting) + (Math.Cos(-abHeading) * pt[i].northing);
}
else
{
xr = (Math.Cos(-abHeading + Math.PI) * pt[i].easting) - (Math.Sin(-abHeading + Math.PI) * pt[i].northing);
yr = (Math.Sin(-abHeading + Math.PI) * pt[i].easting) + (Math.Cos(-abHeading + Math.PI) * pt[i].northing);
}
pt[i].easting = xr + rEastYT;
pt[i].northing = yr + rNorthYT;
pt[i].heading = Math.Atan2(pt[i].northing, pt[i].easting);
ytList.Add(pt[i]);
}
}
}
public void GenerateHeadlandPath()
{
//first find out which side is inside the boundary
//insideOutside = glm.PIBy2;
//point.easting = mf.bnd.ptList[3].easting - (Math.Sin(insideOutside + mf.bnd.ptList[3].heading) * 2.0);
//point.northing = mf.bnd.ptList[3].northing - (Math.Cos(insideOutside + mf.bnd.ptList[3].heading) * 2.0);
//if (!mf.bnd.IsPointInsideBoundary(point)) insideOutside *= -1.0;
//keep copy of original position/heading
homePoint = mf.pivotAxlePos;
//clear the main list
genList.Clear();
for (int passNumber = 1; passNumber <= numHeadlandPasses; passNumber++)
{
//determine the start point of first headland -
accList.Clear();
BuildOneHeadlandPath((mf.vehicle.toolWidth - mf.vehicle.toolOverlap) * ((double)passNumber - 0.4));
startHeadlandPoint = accList[0];
CDubins dubPath = new CDubins();
CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.2;
////get the dubins path vec3 point coordinates of turn
//dubList.Clear();
//if (passNumber == 1) dubList = dubPath.GenerateDubins(mf.pivotAxlePos, startHeadlandPoint, mf.bnd);
//else dubList = dubPath.GenerateDubins(endHeadlandPoint, startHeadlandPoint, mf.bnd);
//transfer point list to genList class
for (int i = 0; i < dubList.Count; i++)
{
CGenPathPt pt = new CGenPathPt(dubList[i].easting, dubList[i].northing, dubList[i].heading);
if (i < 5)
{
pt.sequence = 1;
}
genList.Add(pt);
}
for (int i = 0; i < accList.Count; i++)
{
CGenPathPt pt = new CGenPathPt(accList[i].easting, accList[i].northing, accList[i].heading);
if (i < 5)
{
pt.sequence = 2;
}
genList.Add(pt);
}
for (int i = 0; i < 5; i++)
{
CGenPathPt pt = new CGenPathPt(accList[i].easting, accList[i].northing, accList[i].heading);
genList.Add(pt);
}
endHeadlandPoint = accList[5];
}
genListCount = genList.Count;
currentPositonIndex = 0;
isEndOfTheGenLine = false;
}
private void GetDubinsPath(vec3 goal)
{
//CDubins dubPath = new CDubins();
////
//pivotPos = mf.pivotAxlePos;
////bump it forward
//vec3 pt2 = new vec3
//{
// easting = pivotPos.easting + (Math.Sin(pivotPos.heading) * 4),
// northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 4),
// heading = pivotPos.heading
//};
////get the dubins path vec3 point coordinates of turn
//dubinsList.Clear();
//dubinsList = dubPath.GenerateDubins(pt2, goal);
//dubinsList.Insert(0, mf.pivotAxlePos);
////transfer point list to recPath class point style
//dubList.Clear();
//for (int i = 0; i < dubinsList.Count; i++)
//{
// CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
// dubList.Add(pt);
//}
////no longer needed
//dubinsList?.Clear();
CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.0;
CDubins dubPath = new CDubins();
// current psition
pivotPos = mf.pivotAxlePos;
//bump it forward
vec3 pt2 = new vec3
{
easting = pivotPos.easting + (Math.Sin(pivotPos.heading) * 3),
northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 3),
heading = pivotPos.heading
};
//get the dubins path vec3 point coordinates of turn
dubinsList.Clear();
dubList.Clear();
dubinsList = dubPath.GenerateDubins(pt2, goal, mf.gf);
//if Dubins returns 0 elements, there is an unavoidable blockage in the way.
if (dubinsList.Count > 0)
{
dubinsList.Insert(0, mf.pivotAxlePos);
//transfer point list to recPath class point style
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 9.0, false);
dubList.Add(pt);
}
return;
}
//find a path from start to goal - diagnostic, but also used later
mazeList = mf.mazeGrid.SearchForPath(pt2, goal);
//you can't get anywhere!
if (mazeList == null)
{
return;
}
//start is navigateable - maybe
int cnt = mazeList.Count;
if (cnt > 0)
{
{
int turnRadius = (int)(3 * mf.vehicle.minTurningRadius);
if (cnt > 2 * turnRadius)
{
mazeList.RemoveRange(0, turnRadius);
cnt = mazeList.Count;
mazeList.RemoveRange(cnt - turnRadius, turnRadius);
}
}
dubinsList = dubPath.GenerateDubins(pt2, mazeList[0], mf.gf);
if (dubinsList.Count > 0)
{
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 10.0, false);
dubList.Add(pt);
}
}
else
{
return; //unable to generate a dubins to the start
}
for (int i = 0; i < mazeList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(mazeList[i].easting, mazeList[i].northing, mazeList[i].heading, 15.0, false);
dubList.Add(pt);
}
dubinsList = dubPath.GenerateDubins(mazeList[mazeList.Count - 1], goal, mf.gf);
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 11.0, false);
dubList.Add(pt);
}
return;
}
}
//build the points and path of youturn to be scaled and transformed
public void BuildYouTurnListToRight(bool isTurnRight, bool isTurnButtonTriggered)
{
isYouTurnShapeDisplayed = true;
CDubins dubYouTurnPath = new CDubins();
CDubins.turningRadius = mf.vehicle.minTurningRadius;
double delta;
//point on AB line closest to pivot axle point from ABLine PurePursuit
if (mf.ABLine.isABLineSet)
{
rEastYT = mf.ABLine.rEastAB;
rNorthYT = mf.ABLine.rNorthAB;
isABSameAsFixHeading = mf.ABLine.isABSameAsVehicleHeading;
abHeading = mf.ABLine.abHeading;
delta = 1;
}
else
{
rEastYT = mf.curve.rEastCu;
rNorthYT = mf.curve.rNorthCu;
isABSameAsFixHeading = mf.curve.isSameWay;
abHeading = mf.curve.refHeading;
delta = mf.curve.deltaOfRefAndAveHeadings;
}
//grab the vehicle widths and offsets
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
double toolOffset = mf.vehicle.toolOffset * 2.0;
double turnOffset = 0;
//turning right
if (isTurnRight)
{
turnOffset = (widthMinusOverlap + toolOffset);
}
else
{
turnOffset = (widthMinusOverlap - toolOffset);
}
//to compensate for AB Curve overlap
turnOffset *= delta;
double turnRadius = turnOffset / Math.Cos(boundaryAngleOffPerpendicular);
//if using dubins to calculate youturn
if (isUsingDubinsTurn)
{
double head = abHeading;
//if its straight across it makes 2 loops instead so goal is a little lower then start
if (!isABSameAsFixHeading)
{
head += 3.14;
}
else
{
head -= 0.01;
}
//move the start forward 2 meters
rEastYT += (Math.Sin(head) * 2);
rNorthYT += (Math.Cos(head) * 2);
var start = new vec3(rEastYT, rNorthYT, head);
var goal = new vec3();
//also adjust for rowskips if Dew loops are set
if (isDew2Set | isDew4Set)
{
if (isDew2Set)
{
skips = dew2Skips[dew2Index];
//if at end of turn, restart sequence
dew2Index++;
if (dew2Index >= dew2Skips.Length)
{
dew2Index = 0;
}
}
if (isDew4Set)
{
skips = dew4Skips[dew4Index];
//if at end of turn, restart sequence
dew4Index++;
if (dew4Index >= dew4Skips.Length)
{
dew4Index = 0;
}
}
}
else
{
skips = rowSkipsWidth;
}
turnRadius *= skips;
turnOffset *= skips;
//move the cross line calc to not include first turn
goal.easting = rEastYT + (Math.Sin(head) * ping);
goal.northing = rNorthYT + (Math.Cos(head) * ping);
//headland angle relative to vehicle heading to head along the boundary left or right
double bndAngle = head - boundaryAngleOffPerpendicular + glm.PIBy2;
//now we go the other way to turn round
head -= Math.PI;
if (head < 0)
{
head += glm.twoPI;
}
if ((mf.vehicle.minTurningRadius * 1.98) < turnOffset)
{
//are we right of boundary
if (boundaryAngleOffPerpendicular > 0)
{
if (isYouTurnRight) //which is actually right now
{
goal.easting += (Math.Sin(bndAngle) * turnRadius);
goal.northing += (Math.Cos(bndAngle) * turnRadius);
double dis = (mf.vehicle.minTurningRadius / Math.Tan(tangencyAngle)); //long
goal.easting += (Math.Sin(head) * dis);
goal.northing += (Math.Cos(head) * dis);
}
else //going left
{
goal.easting -= (Math.Sin(bndAngle) * turnRadius);
goal.northing -= (Math.Cos(bndAngle) * turnRadius);
double dis = (mf.vehicle.minTurningRadius * Math.Tan(tangencyAngle)); //short
goal.easting += (Math.Sin(head) * dis);
goal.northing += (Math.Cos(head) * dis);
}
}
else // going left of boundary
{
if (isYouTurnRight) //pointing to right
{
goal.easting += (Math.Sin(bndAngle) * turnRadius);
goal.northing += (Math.Cos(bndAngle) * turnRadius);
double dis = (mf.vehicle.minTurningRadius * Math.Tan(tangencyAngle)); //short
goal.easting += (Math.Sin(head) * dis);
goal.northing += (Math.Cos(head) * dis);
}
else
{
goal.easting -= (Math.Sin(bndAngle) * turnRadius);
goal.northing -= (Math.Cos(bndAngle) * turnRadius);
double dis = (mf.vehicle.minTurningRadius / Math.Tan(tangencyAngle)); //long
goal.easting += (Math.Sin(head) * dis);
goal.northing += (Math.Cos(head) * dis);
}
}
}
else
{
if (isTurnRight)
{
goal.easting = rEastYT - (Math.Cos(-head) * turnOffset);
goal.northing = rNorthYT - (Math.Sin(-head) * turnOffset);
}
else
{
goal.easting = rEastYT + (Math.Cos(-head) * turnOffset);
goal.northing = rNorthYT + (Math.Sin(-head) * turnOffset);
}
}
goal.heading = head;
if (isTurnButtonTriggered)
{
if (isTurnRight)
{
goal.easting = rEastYT - (Math.Cos(-head) * turnOffset);
goal.northing = rNorthYT - (Math.Sin(-head) * turnOffset);
}
else
{
goal.easting = rEastYT + (Math.Cos(-head) * turnOffset);
goal.northing = rNorthYT + (Math.Sin(-head) * turnOffset);
}
}
//generate the turn points
ytList = dubYouTurnPath.GenerateDubins(start, goal);
}
//or the patterns
else
{
numShapePoints = youFileList.Count;
vec3[] pt = new vec3[numShapePoints];
//Now put the shape into an array since lists are immutable
for (int i = 0; i < numShapePoints; i++)
{
pt[i].easting = youFileList[i].easting;
pt[i].northing = youFileList[i].northing;
}
//start of path on the origin. Mirror the shape if left turn
if (!isTurnRight)
{
for (int i = 0; i < pt.Length; i++)
{
pt[i].easting *= -1;
}
}
//scaling - Drawing is 10m wide so find ratio of tool width
double scale = turnOffset * 0.1;
for (int i = 0; i < pt.Length; i++)
{
pt[i].easting *= scale * rowSkipsWidth;
pt[i].northing *= scale * rowSkipsWidth;
}
//rotate pattern to match AB Line heading
for (int i = 0; i < pt.Length; i++)
{
double xr, yr;
if (isABSameAsFixHeading)
{
xr = (Math.Cos(-abHeading) * pt[i].easting) - (Math.Sin(-abHeading) * pt[i].northing);
yr = (Math.Sin(-abHeading) * pt[i].easting) + (Math.Cos(-abHeading) * pt[i].northing);
}
else
{
xr = (Math.Cos(-abHeading + Math.PI) * pt[i].easting) - (Math.Sin(-abHeading + Math.PI) * pt[i].northing);
yr = (Math.Sin(-abHeading + Math.PI) * pt[i].easting) + (Math.Cos(-abHeading + Math.PI) * pt[i].northing);
}
pt[i].easting = xr + rEastYT;
pt[i].northing = yr + rNorthYT;
pt[i].heading = Math.Atan2(pt[i].northing, pt[i].easting);
ytList.Add(pt[i]);
}
}
}
private void GetDubinsPath(vec3 goal)
{
CDubins.turningRadius = mf.vehicle.minTurningRadius * 2.0;
CDubins dubPath = new CDubins();
// current psition
pivotAxlePosRP = mf.pivotAxlePos;
//bump it forward
vec3 pt2 = new vec3
{
easting = pivotAxlePosRP.easting + (Math.Sin(pivotAxlePosRP.heading) * 3),
northing = pivotAxlePosRP.northing + (Math.Cos(pivotAxlePosRP.heading) * 3),
heading = pivotAxlePosRP.heading
};
//get the dubins path vec3 point coordinates of turn
shortestDubinsList.Clear();
shuttleDubinsList.Clear();
shortestDubinsList = dubPath.GenerateDubins(pt2, goal, mf.gf);
//if Dubins returns 0 elements, there is an unavoidable blockage in the way.
if (shortestDubinsList.Count > 0)
{
shortestDubinsList.Insert(0, mf.pivotAxlePos);
//transfer point list to recPath class point style
for (int i = 0; i < shortestDubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 9.0, false);
shuttleDubinsList.Add(pt);
}
return;
}
//else
//{
// shuttleDubinsList?.Clear();
// return;
//}
//find a path from start to goal - diagnostic, but also used later
mazeList = mf.mazeGrid.SearchForPath(pt2, goal);
//you can't get anywhere!
if (mazeList == null)
{
return;
}
//start is navigateable - maybe
if (mazeList.Count > 0)
{
shortestDubinsList = dubPath.GenerateDubins(pt2, mazeList[0], mf.gf);
for (int i = 0; i < shortestDubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 10.0, false);
shuttleDubinsList.Add(pt);
}
for (int i = 0; i < mazeList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(mazeList[i].easting, mazeList[i].northing, mazeList[i].heading, 15.0, false);
shuttleDubinsList.Add(pt);
}
shortestDubinsList = dubPath.GenerateDubins(mazeList[mazeList.Count - 1], goal, mf.gf);
for (int i = 0; i < shortestDubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 11.0, false);
shuttleDubinsList.Add(pt);
}
return;
}
else
{
}
}
//build the points and path of youturn to be scaled and transformed
public void BuildYouTurnListToRight(bool isTurnRight)
{
isYouTurnShapeDisplayed = true;
CDubins dubYouTurnPath = new CDubins();
CDubins.turningRadius = mf.vehicle.minTurningRadius;
double delta;
//point on AB line closest to pivot axle point from ABLine PurePursuit
if (mf.ABLine.isABLineSet)
{
rEastYT = mf.ABLine.rEastAB;
rNorthYT = mf.ABLine.rNorthAB;
isABSameAsFixHeading = mf.ABLine.isABSameAsFixHeading;
abHeading = mf.ABLine.abHeading;
delta = 1;
}
else
{
rEastYT = mf.curve.rEastCu;
rNorthYT = mf.curve.rNorthCu;
isABSameAsFixHeading = mf.curve.isSameWay;
abHeading = mf.curve.refHeading;
delta = mf.curve.deltaOfRefAndAveHeadings;
}
//grab the vehicle widths and offsets
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
double toolOffset = mf.vehicle.toolOffset * 2.0;
double turnOffset = 0;
//turning right
if (isTurnRight)
{
turnOffset = (widthMinusOverlap + toolOffset);
}
else
{
turnOffset = (widthMinusOverlap - toolOffset);
}
//to compensate for AB Curve overlap
turnOffset *= delta;
//if using dubins to calculate youturn
if (isUsingDubinsTurn)
{
int skips = 0;
double head = abHeading;
//if its straight across it makes 2 loops instead so goal is a little lower then start
if (!isABSameAsFixHeading)
{
head += 3.14;
}
else
{
head -= 0.01;
}
var start = new vec3(rEastYT, rNorthYT, head);
var goal = new vec3();
head -= Math.PI;
if (head < 0)
{
head += glm.twoPI;
}
//also adjust for rowskips if Dew loops are set
if (isDew2Set | isDew4Set)
{
if (isDew2Set)
{
skips = dew2Skips[dew2Index];
//if at end of turn, restart sequence
dew2Index++;
if (dew2Index >= dew2Skips.Length)
{
dew2Index = 0;
}
}
if (isDew4Set)
{
skips = dew4Skips[dew4Index];
//if at end of turn, restart sequence
dew4Index++;
if (dew4Index >= dew4Skips.Length)
{
dew4Index = 0;
}
}
}
else
{
skips = rowSkipsWidth;
}
//calculate the turn
if (isTurnRight)
{
goal.easting = rEastYT - (Math.Cos(-head) * turnOffset * skips);
goal.northing = rNorthYT - (Math.Sin(-head) * turnOffset * skips);
goal.heading = head;
}
else
{
goal.easting = rEastYT + (Math.Cos(-head) * turnOffset * skips);
goal.northing = rNorthYT + (Math.Sin(-head) * turnOffset * skips);
goal.heading = head;
}
//generate the turn points
ytList = dubYouTurnPath.GenerateDubins(start, goal);
}
//or the patterns
else
{
numShapePoints = youFileList.Count;
vec3[] pt = new vec3[numShapePoints];
//Now put the shape into an array since lists are immutable
for (int i = 0; i < numShapePoints; i++)
{
pt[i].easting = youFileList[i].easting;
pt[i].northing = youFileList[i].northing;
}
//start of path on the origin. Mirror the shape if left turn
if (!isTurnRight)
{
for (int i = 0; i < pt.Length; i++)
{
pt[i].easting *= -1;
}
}
//scaling - Drawing is 10m wide so find ratio of tool width
double scale = turnOffset * 0.1;
for (int i = 0; i < pt.Length; i++)
{
pt[i].easting *= scale * rowSkipsWidth;
pt[i].northing *= scale * rowSkipsWidth;
}
//rotate pattern to match AB Line heading
for (int i = 0; i < pt.Length; i++)
{
double xr, yr;
if (isABSameAsFixHeading)
{
xr = (Math.Cos(-abHeading) * pt[i].easting) - (Math.Sin(-abHeading) * pt[i].northing);
yr = (Math.Sin(-abHeading) * pt[i].easting) + (Math.Cos(-abHeading) * pt[i].northing);
}
else
{
xr = (Math.Cos(-abHeading + Math.PI) * pt[i].easting) - (Math.Sin(-abHeading + Math.PI) * pt[i].northing);
yr = (Math.Sin(-abHeading + Math.PI) * pt[i].easting) + (Math.Cos(-abHeading + Math.PI) * pt[i].northing);
}
pt[i].easting = xr + rEastYT;
pt[i].northing = yr + rNorthYT;
pt[i].heading = Math.Atan2(pt[i].northing, pt[i].easting);
ytList.Add(pt[i]);
}
}
}
