diff --git a/SourceCode/GPS/Classes/CGuidance.cs b/SourceCode/GPS/Classes/CGuidance.cs index 9080baea8..c01eceb0f 100644 --- a/SourceCode/GPS/Classes/CGuidance.cs +++ b/SourceCode/GPS/Classes/CGuidance.cs @@ -119,7 +119,7 @@ private void DoSteerAngleCalc() /// public void StanleyGuidanceABLine(vec3 curPtA, vec3 curPtB, vec3 pivot, vec3 steer) { - //get the steer distance from currently active AB segment /////////// steer //////////// + //get the pivot distance from currently active AB segment /////////// Pivot //////////// double dx = curPtB.easting - curPtA.easting; double dy = curPtB.northing - curPtA.northing; if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dy) < Double.Epsilon) return; @@ -128,7 +128,7 @@ public void StanleyGuidanceABLine(vec3 curPtA, vec3 curPtB, vec3 pivot, vec3 ste mf.yt.dxAB = dx; mf.yt.dyAB = dy; //how far from current AB Line is fix - distanceFromCurrentLinePivot = ((dy * steer.easting) - (dx * steer.northing) + (curPtB.easting + distanceFromCurrentLinePivot = ((dy * pivot.easting) - (dx * pivot.northing) + (curPtB.easting * curPtA.northing) - (curPtB.northing * curPtA.easting)) / Math.Sqrt((dy * dy) + (dx * dx)); @@ -136,8 +136,8 @@ public void StanleyGuidanceABLine(vec3 curPtA, vec3 curPtB, vec3 pivot, vec3 ste distanceFromCurrentLinePivot *= -1.0; mf.ABLine.distanceFromCurrentLinePivot = distanceFromCurrentLinePivot; - double U = (((steer.easting - curPtA.easting) * dx) - + ((steer.northing - curPtA.northing) * dy)) + double U = (((pivot.easting - curPtA.easting) * dx) + + ((pivot.northing - curPtA.northing) * dy)) / ((dx * dx) + (dy * dy)); rEastPivot = curPtA.easting + (U * dx);