//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); } }