//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();
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++)
{
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);
}
}
}
}
}
else
{
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);
}
}
}
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 ((((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);
}
}
}
}
}
//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 - 45; if (start < 0) start = 0;
stop = pt + 45; 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);
//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;
}
}
//determine closest point on applied
public void BuildContourGuidanceLine(double eastFix, double northFix)
{
//build a frustum box ahead of fix to find adjacent paths and points
//double startX = eastFix + Math.Sin(mf.fixHeading)* 0;
//double startY = northFix + Math.Cos(mf.fixHeading) * 0;
boxA.x = eastFix - Math.Sin(mf.fixHeading + glm.PIBy2) * 1.5 * mf.vehicle.toolWidth;
boxA.z = northFix - Math.Cos(mf.fixHeading + glm.PIBy2) * 1.5 * mf.vehicle.toolWidth;
boxB.x = eastFix + Math.Sin(mf.fixHeading + glm.PIBy2) * 1.5 * mf.vehicle.toolWidth;
boxB.z = northFix + Math.Cos(mf.fixHeading + glm.PIBy2) * 1.5 * mf.vehicle.toolWidth;
boxC.x = boxB.x + Math.Sin(mf.fixHeading) * 13.0;
boxC.z = boxB.z + Math.Cos(mf.fixHeading) * 13.0;
boxD.x = boxA.x + Math.Sin(mf.fixHeading) * 13.0;
boxD.z = boxA.z + Math.Cos(mf.fixHeading) * 13.0;
conList.Clear();
guideList.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 frustum box
for (int s = 0; s < stripCount; s++)
{
ptCount = stripList[s].Count;
for (int p = 0; p < ptCount; p++)
{
if (((boxB.x - boxA.x) * (stripList[s][p].z - boxA.z) -
(boxB.z - boxA.z) * (stripList[s][p].x - boxA.x)) < 0) continue;
if (((boxD.x - boxC.x) * (stripList[s][p].z - boxC.z) -
(boxD.z - boxC.z) * (stripList[s][p].x - boxC.x)) < 0) continue;
if (((boxC.x - boxB.x) * (stripList[s][p].z - boxB.z) -
(boxC.z - boxB.z) * (stripList[s][p].x - boxB.x)) < 0) continue;
if (((boxA.x - boxD.x) * (stripList[s][p].z - boxD.z) -
(boxA.z - boxD.z) * (stripList[s][p].x - boxD.x)) < 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].h) - Math.PI);
if (ref2 < 1.1 || ref2 > 2.04)
{
//it's in the box and parallelish so add to list
pointC.x = stripList[s][p].x;
pointC.z = stripList[s][p].z;
pointC.h = stripList[s][p].h;
pointC.strip = s;
pointC.pt = p;
conList.Add(pointC);
}
}
}
//determine closest point
ptCount = conList.Count;
if (ptCount == 0)
{
distanceFromCurrentLine = 0;
return;
}
minDistance = 99999;
for (int i = 0; i < ptCount; i++)
{
double dist = (eastFix - conList[i].x) * (eastFix - conList[i].x) +
(northFix - conList[i].z) * (northFix - 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].x;
refZ = stripList[strip][pt].z;
refHeading = stripList[strip][pt].h;
//which side of the patch are we on is next
//calculate endpoints of reference line based on closest point
refPoint1.x = refX - Math.Sin(refHeading) * 50.0;
refPoint1.z = refZ - Math.Cos(refHeading) * 50.0;
refPoint2.x = refX + Math.Sin(refHeading) * 50.0;
refPoint2.z = refZ + Math.Cos(refHeading) * 50.0;
//move the Guidance Line over based on the overlap amount set in vehicle
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
//x2-x1
double dx = refPoint2.x - refPoint1.x;
//z2-z1
double dz = refPoint2.z - refPoint1.z;
//how far are we away from the reference line at 90 degrees - 2D cross product and distance
distanceFromRefLine = (dz * mf.fixPosX - dx * mf.fixPosZ + refPoint2.x *
refPoint1.z - refPoint2.z * refPoint1.x) /
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;
//absolute the distance
distanceFromRefLine = Math.Abs(distanceFromRefLine);
//are we going same direction as stripList was created?
bool isSameWay = false;
if (Math.PI - Math.Abs(Math.Abs(mf.fixHeading - refHeading) - Math.PI) < 1.1) isSameWay = true;
//make the new guidance line list called guideList
ptCount = stripList[strip].Count-1;
int start, stop;
vec3 point;
if (isSameWay)
{
start = pt - 5;
if (start < 0) start = 0;
stop = pt + 20;
if (stop > ptCount) stop = ptCount+1;
}
else
{
start = pt - 20;
if (start < 0) start = 0;
stop = pt + 5;
if (stop > ptCount) stop = ptCount+1;
}
for (int i = start; i < stop; i++)
{
point = new vec3(
stripList[strip][i].x + Math.Sin(piSide + stripList[strip][i].h) * widthMinusOverlap,
stripList[strip][i].h,
stripList[strip][i].z + Math.Cos(piSide + stripList[strip][i].h) * widthMinusOverlap);
guideList.Add(point);
}
}
//determine closest point on left side
public void BuildContourGuidanceLine(vec3 pivot)
{
double toolWid = mf.tool.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.tool.toolOffset < 0)
{
//sticks out more left
sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.tool.toolOffset * 2)));
cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.tool.toolOffset * 2)));
sin2HR = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.tool.toolOffset)));
cos2HR = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.tool.toolOffset)));
}
else
{
//sticks out more right
sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.tool.toolOffset)));
cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.tool.toolOffset)));
sin2HR = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.tool.toolOffset * 2)));
cos2HR = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.tool.toolOffset * 2)));
}
//narrow equipment needs bigger bounding box.
if (mf.tool.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.tool.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.tool.toolWidth - mf.tool.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.95;
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;
}
int ctCount = ctList.Count;
if (ctCount < 6) return;
const double spacing = 1.0;
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]);
// }
//}
}
//determine closest point on applied
public void BuildContourGuidanceLine(double eastFix, double northFix)
{
//build a frustum box ahead of fix to find adjacent paths and points
//double startX = eastFix + Math.Sin(mf.fixHeading)* 0;
//double startY = northFix + Math.Cos(mf.fixHeading) * 0;
boxA.easting = eastFix - (Math.Sin(mf.fixHeading + glm.PIBy2) * 2.0 * mf.vehicle.toolWidth);
boxA.northing = northFix - (Math.Cos(mf.fixHeading + glm.PIBy2) * 2.0 * mf.vehicle.toolWidth);
boxB.easting = eastFix + (Math.Sin(mf.fixHeading + glm.PIBy2) * 2.0 * mf.vehicle.toolWidth);
boxB.northing = northFix + (Math.Cos(mf.fixHeading + glm.PIBy2) * 2.0 * mf.vehicle.toolWidth);
boxC.easting = boxB.easting + (Math.Sin(mf.fixHeading) * 13.0);
boxC.northing = boxB.northing + (Math.Cos(mf.fixHeading) * 13.0);
boxD.easting = boxA.easting + (Math.Sin(mf.fixHeading) * 13.0);
boxD.northing = boxA.northing + (Math.Cos(mf.fixHeading) * 13.0);
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 frustum box
for (int s = 0; s < stripCount; s++)
{
ptCount = stripList[s].Count;
for (int p = 0; p < ptCount; p++)
{
if ((((boxB.easting - boxA.easting) * (stripList[s][p].northing - boxA.northing))
- ((boxB.northing - boxA.northing) * (stripList[s][p].easting - boxA.easting))) < 0) { continue; }
if ((((boxD.easting - boxC.easting) * (stripList[s][p].northing - boxC.northing))
- ((boxD.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 ((((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;
return;
}
//determine closest point
minDistance = 99999;
for (int i = 0; i < ptCount; i++)
{
double dist = ((eastFix - conList[i].x) * (eastFix - conList[i].x))
+ ((northFix - conList[i].z) * (northFix - 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 - 25; if (start < 0) start = 0;
stop = pt + 60; if (stop > ptCount) stop = ptCount+1;
}
else
{
start = pt - 60; if (start < 0) start = 0;
stop = pt + 25; if (stop > ptCount) stop = ptCount+1;
}
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);
}
}
