예제 #1
0
        public bool FindHeadlandDistance()
        {
            if (headArr[0].hdLine.Count == 0)
            {
                leftToolDistance  = 99999;
                rightToolDistance = 99999;
                return(false);
            }
            else
            {
                leftToolDistance  = 999999;
                rightToolDistance = 999999;

                double minDistA = 1000000, minDistB = 1000000;

                int ptCount = headArr[0].hdLine.Count;

                //find the closest 2 points to current fix
                for (int t = 0; t < ptCount; t++)
                {
                    double dist = glm.DistanceSquared(mf.section[0].leftPoint, headArr[0].hdLine[t]);
                    if (dist < minDistA)
                    {
                        minDistB = minDistA;
                        B        = A;
                        minDistA = dist;
                        A        = t;
                    }
                    else if (dist < minDistB)
                    {
                        minDistB = dist;
                        B        = t;
                    }
                }

                //just need to make sure the points continue ascending or heading switches all over the place
                if (A > B)
                {
                    Q = A; A = B; B = Q;
                }

                double dx = headArr[0].hdLine[B].easting - headArr[0].hdLine[A].easting;
                double dz = headArr[0].hdLine[B].northing - headArr[0].hdLine[A].northing;

                if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
                {
                    return(false);
                }

                //abHeading = Math.Atan2(dz, dx);
                //double abHeading = curList[A].heading;

                //how far from current AB Line is fix
                leftToolDistance = ((dz * mf.section[0].leftPoint.easting) - (dx * mf.section[0].leftPoint.northing) + (headArr[0].hdLine[B].easting
                                                                                                                        * headArr[0].hdLine[A].northing) - (headArr[0].hdLine[B].northing * headArr[0].hdLine[A].easting))
                                   / Math.Sqrt((dz * dz) + (dx * dx));

                //are we on the right side or not
                //bool isOnRightSideCurrentLine = leftToolDistance > 0;
                //absolute the distance
                //leftToolDistance = Math.Abs(leftToolDistance);

                //find the closest 2 points to current fixm for right side
                minDistA = 1000000;
                minDistB = 1000000;

                for (int t = 0; t < ptCount; t++)
                {
                    double dist = glm.DistanceSquared(mf.section[mf.tool.numOfSections - 1].rightPoint, headArr[0].hdLine[t]);
                    if (dist < minDistA)
                    {
                        minDistB = minDistA;
                        D        = C;
                        minDistA = dist;
                        C        = t;
                    }
                    else if (dist < minDistB)
                    {
                        minDistB = dist;
                        D        = t;
                    }
                }

                //just need to make sure the points continue ascending or heading switches all over the place
                if (C > D)
                {
                    Q = C; C = D; D = Q;
                }

                dx = headArr[0].hdLine[D].easting - headArr[0].hdLine[C].easting;
                dz = headArr[0].hdLine[D].northing - headArr[0].hdLine[C].northing;

                if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
                {
                    return(false);
                }

                //abHeading = Math.Atan2(dz, dx);
                //double abHeading = curList[C].heading;

                //how far from current AB Line is fix
                rightToolDistance = ((dz * mf.section[mf.tool.numOfSections - 1].rightPoint.easting) - (dx * mf.section[mf.tool.numOfSections - 1].rightPoint.northing) + (headArr[0].hdLine[D].easting
                                                                                                                                                                           * headArr[0].hdLine[C].northing) - (headArr[0].hdLine[D].northing * headArr[0].hdLine[C].easting))
                                    / Math.Sqrt((dz * dz) + (dx * dx));

                //are we on the right side or not
                //bool isOnRightSideCurrentLine = leftToolDistance > 0;

                //absolute the distance
                //leftToolDistance = Math.Abs(leftToolDistance);
                //double scanWidthR = (mf.tool.toolWidth * 0.75);

                if (leftToolDistance > 0 && rightToolDistance > 0)
                {
                    isToolInWorkArea   = true;
                    isToolInBothPlaces = isToolInHeadland = false;
                }
                else if (leftToolDistance < 0 && rightToolDistance < 0)
                {
                    isToolInHeadland   = true;
                    isToolInBothPlaces = isToolInWorkArea = false;
                }
                else
                {
                    isToolInBothPlaces = true;
                    isToolInHeadland   = isToolInWorkArea = false;
                }

                vec3   toolFix = mf.toolPos;
                double headAB  = toolFix.heading;

                downL.easting  = mf.section[0].leftPoint.easting + (Math.Sin(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarLeftSpeed);
                downL.northing = mf.section[0].leftPoint.northing + (Math.Cos(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarLeftSpeed);

                downR.easting  = mf.section[mf.tool.numOfSections - 1].rightPoint.easting + (Math.Sin(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarRightSpeed);
                downR.northing = mf.section[mf.tool.numOfSections - 1].rightPoint.northing + (Math.Cos(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarRightSpeed);

                //bool isToolFullyOut =
                bool isLookRightIn = IsPointInsideHeadLine(downR);
                bool isLookLeftIn  = IsPointInsideHeadLine(downL);


                if (isLookLeftIn && isLookRightIn)
                {
                    isLookInWorkArea   = true;
                    isLookInBothPlaces = isLookInHealand = false;
                }
                else if (!isLookLeftIn && !isLookRightIn)
                {
                    isLookInHealand    = true;
                    isLookInBothPlaces = isLookInWorkArea = false;
                }
                else
                {
                    isLookInBothPlaces = true;
                    isLookInHealand    = isLookInWorkArea = false;
                }

                //isToolUp = false;

                if (!isLookInHealand && isToolUp)
                {
                    isToolUp = false;
                }
                if (isLookInHealand && isToolInHeadland && !isToolUp)
                {
                    isToolUp = true;
                }

                if (isToolUp)
                {
                    mf.mc.pgn[mf.mc.azRelayData][mf.mc.rdHydLift] = 1;
                }
                else
                {
                    mf.mc.pgn[mf.mc.azRelayData][mf.mc.rdHydLift] = 0;
                }

                return(isToolUp);
            }
        }
예제 #2
0
        private void btnLoadBoundaryFromGE_Click(object sender, EventArgs e)
        {
            if (sender is Button button)
            {
                Selectedreset = true;

                string fileAndDirectory;
                {
                    //create the dialog instance
                    OpenFileDialog ofd = new OpenFileDialog
                    {
                        //set the filter to text KML only
                        Filter = "KML files (*.KML)|*.KML",

                        //the initial directory, fields, for the open dialog
                        InitialDirectory = mf.fieldsDirectory + mf.currentFieldDirectory
                    };

                    //was a file selected
                    if (ofd.ShowDialog() == DialogResult.Cancel)
                    {
                        return;
                    }
                    else
                    {
                        fileAndDirectory = ofd.FileName;
                    }
                }

                string coordinates = null;
                int    startIndex;
                int    i = 0;

                using (StreamReader reader = new StreamReader(fileAndDirectory))
                {
                    if (button.Name == "btnLoadMultiBoundaryFromGE")
                    {
                        ResetAllBoundary();
                    }
                    else
                    {
                        i = mf.bnd.boundarySelected;
                    }

                    try
                    {
                        while (!reader.EndOfStream)
                        {
                            //start to read the file
                            string line = reader.ReadLine();

                            startIndex = line.IndexOf("<coordinates>");

                            if (startIndex != -1)
                            {
                                while (true)
                                {
                                    int endIndex = line.IndexOf("</coordinates>");

                                    if (endIndex == -1)
                                    {
                                        //just add the line
                                        if (startIndex == -1)
                                        {
                                            coordinates += line.Substring(0);
                                        }
                                        else
                                        {
                                            coordinates += line.Substring(startIndex + 13);
                                        }
                                    }
                                    else
                                    {
                                        if (startIndex == -1)
                                        {
                                            coordinates += line.Substring(0, endIndex);
                                        }
                                        else
                                        {
                                            coordinates += line.Substring(startIndex + 13, endIndex - (startIndex + 13));
                                        }
                                        break;
                                    }
                                    line       = reader.ReadLine();
                                    line       = line.Trim();
                                    startIndex = -1;
                                }

                                line = coordinates;
                                char[]   delimiterChars = { ' ', '\t', '\r', '\n' };
                                string[] numberSets     = line.Split(delimiterChars);

                                //at least 3 points
                                if (numberSets.Length > 2)
                                {
                                    mf.bnd.bndArr.Add(new CBoundaryLines());
                                    mf.turn.turnArr.Add(new CTurnLines());

                                    foreach (var item in numberSets)
                                    {
                                        string[] fix = item.Split(',');
                                        double.TryParse(fix[0], NumberStyles.Float, CultureInfo.InvariantCulture, out lonK);
                                        double.TryParse(fix[1], NumberStyles.Float, CultureInfo.InvariantCulture, out latK);

                                        mf.pn.ConvertWGS84ToLocal(latK, lonK, out norting, out easting);

                                        //add the point to boundary
                                        vec3 bndPt = new vec3(easting, norting, 0);
                                        mf.bnd.bndArr[i].bndLine.Add(bndPt);
                                    }

                                    //build the boundary, make sure is clockwise for outer counter clockwise for inner
                                    bool isCW = mf.bnd.bndArr[i].CalculateBoundaryArea();
                                    if (mf.bnd.boundarySelected == 0 && isCW)
                                    {
                                        mf.bnd.bndArr[i].ReverseWinding();
                                    }

                                    //inner boundaries
                                    if (mf.bnd.boundarySelected > 0 && !isCW)
                                    {
                                        mf.bnd.bndArr[i].ReverseWinding();
                                    }

                                    mf.bnd.bndArr[i].FixBoundaryLine(i);
                                    mf.bnd.bndArr[i].PreCalcBoundaryEarLines();
                                    mf.bnd.bndArr[i].PreCalcBoundaryLines();
                                    mf.bnd.bndArr[i].isSet = true;
                                    mf.fd.UpdateFieldBoundaryGUIAreas();

                                    mf.btnMakeLinesFromBoundary.Visible = true;


                                    coordinates = "";
                                    i++;
                                }
                                else
                                {
                                    mf.TimedMessageBox(2000, gStr.gsErrorreadingKML, gStr.gsChooseBuildDifferentone);
                                }
                                if (button.Name == "btnLoadBoundaryFromGE")
                                {
                                    break;
                                }
                            }
                        }
                        mf.FileSaveBoundary();
                        mf.turn.BuildTurnLines();
                        mf.btnMakeLinesFromBoundary.Visible = true;
                        mf.fd.UpdateFieldBoundaryGUIAreas();
                        UpdateChart();
                    }
                    catch (Exception)
                    {
                        return;
                    }
                }
            }
            mf.bnd.isOkToAddPoints = false;

            panelMain.Visible   = true;
            panelChoose.Visible = false;
            panelKML.Visible    = false;

            this.Size = new Size(566, 377);

            UpdateChart();
        }
예제 #3
0
        public void BuildGeoFenceLines()
        {
            //update the GUI values for boundaries
            //mf.fd.UpdateFieldBoundaryGUIAreas();

            if (mf.bnd.bndArr.Count == 0)
            {
                //mf.TimedMessageBox(1500, " No Boundary ", "No GeoFence Made");
                return;
            }

            //to fill the list of line points
            vec3 point = new vec3();

            //determine how wide a headland space
            double totalHeadWidth = mf.yt.geoFenceDistance;

            //outside boundary - count the points from the boundary
            geoFenceArr[0].geoFenceLine.Clear();
            int ptCount = mf.bnd.bndArr[0].bndLine.Count;

            for (int i = ptCount - 1; i >= 0; i--)
            {
                //calculate the point inside the boundary
                point.easting  = mf.bnd.bndArr[0].bndLine[i].easting + (-Math.Sin(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * totalHeadWidth);
                point.northing = mf.bnd.bndArr[0].bndLine[i].northing + (-Math.Cos(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * totalHeadWidth);
                point.heading  = mf.bnd.bndArr[0].bndLine[i].heading;
                if (point.heading < -glm.twoPI)
                {
                    point.heading += glm.twoPI;
                }

                //only add if inside actual field boundary
                if (mf.bnd.bndArr[0].IsPointInsideBoundary(point))
                {
                    vec2 tPnt = new vec2(point.easting, point.northing);
                    geoFenceArr[0].geoFenceLine.Add(tPnt);
                }
            }
            geoFenceArr[0].FixGeoFenceLine(totalHeadWidth, mf.bnd.bndArr[0].bndLine, mf.tool.toolWidth);
            geoFenceArr[0].PreCalcTurnLines();

            //inside boundaries
            for (int j = 1; j < mf.bnd.bndArr.Count; j++)
            {
                geoFenceArr[j].geoFenceLine.Clear();
                if (!mf.bnd.bndArr[j].isSet || mf.bnd.bndArr[j].isDriveThru)
                {
                    continue;
                }

                ptCount = mf.bnd.bndArr[j].bndLine.Count;

                for (int i = ptCount - 1; i >= 0; i--)
                {
                    //calculate the point outside the boundary
                    point.easting  = mf.bnd.bndArr[j].bndLine[i].easting + (-Math.Sin(glm.PIBy2 + mf.bnd.bndArr[j].bndLine[i].heading) * totalHeadWidth);
                    point.northing = mf.bnd.bndArr[j].bndLine[i].northing + (-Math.Cos(glm.PIBy2 + mf.bnd.bndArr[j].bndLine[i].heading) * totalHeadWidth);
                    point.heading  = mf.bnd.bndArr[j].bndLine[i].heading;
                    if (point.heading < -glm.twoPI)
                    {
                        point.heading += glm.twoPI;
                    }

                    //only add if outside actual field boundary
                    if (!mf.bnd.bndArr[j].IsPointInsideBoundary(point))
                    {
                        vec2 tPnt = new vec2(point.easting, point.northing);
                        geoFenceArr[j].geoFenceLine.Add(tPnt);
                    }
                }
                geoFenceArr[j].FixGeoFenceLine(totalHeadWidth, mf.bnd.bndArr[j].bndLine, mf.tool.toolWidth * 0.5);
                geoFenceArr[j].PreCalcTurnLines();
            }

            //mf.TimedMessageBox(800, "Turn Lines", "Turn limits Created");
        }
예제 #4
0
        private void oglSelf_MouseDown(object sender, MouseEventArgs e)
        {
            btnCancelTouch.Enabled = true;

            btnMakeABLine.Enabled = false;
            btnMakeCurve.Enabled  = false;
            isMakingAB            = isMakingCurve = false;

            Point pt = oglSelf.PointToClient(Cursor.Position);

            //Convert to Origin in the center of window, 800 pixels
            fixPt.X = pt.X - 350;
            fixPt.Y = (700 - pt.Y - 350);
            vec3 plotPt = new vec3
            {
                //convert screen coordinates to field coordinates
                easting  = fixPt.X * mf.maxFieldDistance / 632.0,
                northing = fixPt.Y * mf.maxFieldDistance / 632.0,
                heading  = 0
            };

            plotPt.easting  += mf.fieldCenterX;
            plotPt.northing += mf.fieldCenterY;

            pint.easting  = plotPt.easting;
            pint.northing = plotPt.northing;

            if (isA)
            {
                double minDistA = 1000000, minDistB = 1000000;
                start = 99999; end = 99999;

                int ptCount = arr.Length;

                if (ptCount > 0)
                {
                    //find the closest 2 points to current fix
                    for (int t = 0; t < ptCount; t++)
                    {
                        double dist = ((pint.easting - arr[t].easting) * (pint.easting - arr[t].easting))
                                      + ((pint.northing - arr[t].northing) * (pint.northing - arr[t].northing));
                        if (dist < minDistA)
                        {
                            minDistB = minDistA;
                            B        = A;
                            minDistA = dist;
                            A        = t;
                        }
                        else if (dist < minDistB)
                        {
                            minDistB = dist;
                            B        = t;
                        }
                    }

                    //just need to make sure the points continue ascending or heading switches all over the place
                    if (A > B)
                    {
                        E = A; A = B; B = E;
                    }

                    start = A;
                }

                isA = false;
            }
            else
            {
                double minDistA = 1000000, minDistB = 1000000;

                int ptCount = arr.Length;

                if (ptCount > 0)
                {
                    //find the closest 2 points to current point
                    for (int t = 0; t < ptCount; t++)
                    {
                        double dist = ((pint.easting - arr[t].easting) * (pint.easting - arr[t].easting))
                                      + ((pint.northing - arr[t].northing) * (pint.northing - arr[t].northing));
                        if (dist < minDistA)
                        {
                            minDistB = minDistA;
                            D        = C;
                            minDistA = dist;
                            C        = t;
                        }
                        else if (dist < minDistB)
                        {
                            minDistB = dist;
                            D        = t;
                        }
                    }

                    //just need to make sure the points continue ascending or heading switches all over the place
                    if (C > D)
                    {
                        E = C; C = D; D = E;
                    }
                }

                isA = true;

                int A1 = Math.Abs(A - C);
                int B1 = Math.Abs(A - D);
                int C1 = Math.Abs(B - C);
                int D1 = Math.Abs(B - D);

                if (A1 <= B1 && A1 <= C1 && A1 <= D1)
                {
                    start = A; end = C;
                }
                else if (B1 <= A1 && B1 <= C1 && B1 <= D1)
                {
                    start = A; end = D;
                }
                else if (C1 <= B1 && C1 <= A1 && C1 <= D1)
                {
                    start = B; end = C;
                }
                else if (D1 <= B1 && D1 <= C1 && D1 <= A1)
                {
                    start = B; end = D;
                }

                if (start > end)
                {
                    E = start; start = end; end = E;
                }

                btnMakeABLine.Enabled = true;
                btnMakeCurve.Enabled  = true;
            }
        }
예제 #5
0
        private void BtnMakeCurve_Click(object sender, EventArgs e)
        {
            btnCancelTouch.Enabled = false;

            double moveDist = (double)nudDistance.Value * mf.inchOrCm2m;
            double distSq   = (moveDist) * (moveDist) * 0.999;

            mf.curve.refList?.Clear();
            vec3 pt3 = new vec3(arr[start]);

            for (int i = start; i < end; i++)
            {
                //calculate the point inside the boundary
                pt3.easting = arr[i].easting -
                              (Math.Sin(glm.PIBy2 + arr[i].heading) * (moveDist));

                pt3.northing = arr[i].northing -
                               (Math.Cos(glm.PIBy2 + arr[i].heading) * (moveDist));

                pt3.heading = arr[i].heading;

                bool Add = true;

                for (int j = start; j < end; j++)
                {
                    double check = glm.DistanceSquared(pt3.northing, pt3.easting,
                                                       arr[j].northing, arr[j].easting);
                    if (check < distSq)
                    {
                        Add = false;
                        break;
                    }
                }

                if (Add)
                {
                    if (mf.curve.refList.Count > 0)
                    {
                        double dist = ((pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting) * (pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting))
                                      + ((pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing) * (pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing));
                        if (dist > 1)
                        {
                            mf.curve.refList.Add(pt3);
                        }
                    }
                    else
                    {
                        mf.curve.refList.Add(pt3);
                    }
                }
            }

            int cnt = mf.curve.refList.Count;

            if (cnt > 3)
            {
                //make sure distance isn't too big between points on Turn
                for (int i = 0; i < cnt - 1; i++)
                {
                    int j = i + 1;
                    //if (j == cnt) j = 0;
                    double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]);
                    if (distance > 1.6)
                    {
                        vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0,
                                               (mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0,
                                               mf.curve.refList[i].heading);

                        mf.curve.refList.Insert(j, pointB);
                        cnt = mf.curve.refList.Count;
                        i   = -1;
                    }
                }

                //who knows which way it actually goes
                mf.curve.CalculateTurnHeadings();

                //calculate average heading of line
                double x = 0, y = 0;
                mf.curve.isCurveSet = true;

                foreach (vec3 pt in mf.curve.refList)
                {
                    x += Math.Cos(pt.heading);
                    y += Math.Sin(pt.heading);
                }
                x /= mf.curve.refList.Count;
                y /= mf.curve.refList.Count;
                mf.curve.aveLineHeading = Math.Atan2(y, x);
                if (mf.curve.aveLineHeading < 0)
                {
                    mf.curve.aveLineHeading += glm.twoPI;
                }

                //build the tail extensions
                mf.curve.AddFirstLastPoints();
                mf.curve.SmoothAB(4);
                mf.curve.CalculateTurnHeadings();

                mf.curve.isCurveSet = true;

                mf.curve.curveArr.Add(new CCurveLines());
                mf.curve.numCurveLines        = mf.curve.curveArr.Count;
                mf.curve.numCurveLineSelected = mf.curve.numCurveLines;

                //array number is 1 less since it starts at zero
                int idx = mf.curve.curveArr.Count - 1;

                //create a name
                mf.curve.curveArr[idx].Name = (Math.Round(glm.toDegrees(mf.curve.aveLineHeading), 1)).ToString(CultureInfo.InvariantCulture)
                                              + "\u00B0" + mf.FindDirection(mf.curve.aveLineHeading) + DateTime.Now.ToString("hh:mm:ss", CultureInfo.InvariantCulture);

                mf.curve.curveArr[idx].aveHeading = mf.curve.aveLineHeading;

                //write out the Curve Points
                foreach (vec3 item in mf.curve.refList)
                {
                    mf.curve.curveArr[idx].curvePts.Add(item);
                }

                mf.FileSaveCurveLines();

                //update the arrays
                btnMakeABLine.Enabled = false;
                btnMakeCurve.Enabled  = false;
                isMakingCurve         = false;
                isMakingAB            = false;
                start = 99999; end = 99999;

                FixLabelsCurve();
            }
            else
            {
                mf.curve.isCurveSet = false;
                mf.curve.refList?.Clear();
            }
            btnExit.Focus();
        }
예제 #6
0
        public void UpdatePosition()
        {
            if (isFollowingDubinsToPath)
            {
                //set a speed of 7 kmh
                mf.sim.stepDistance = 7 / 17.86;

                pivotAxlePosRP = mf.pivotAxlePos;

                FindGoalPointDubinsPath(dubListCount);
                PurePursuit();

                //check if close to recorded path
                double distSqr = glm.DistanceSquared(pivotAxlePosRP.northing, pivotAxlePosRP.easting, recList[0].northing, recList[0].easting);
                if (distSqr < 2)
                {
                    isFollowingRecPath      = true;
                    isFollowingDubinsToPath = false;
                    dubList.Clear();
                    dubinsList.Clear();
                }
            }

            if (isFollowingRecPath)
            {
                pivotAxlePosRP = mf.pivotAxlePos;

                FindGoalPointRecPath(recListCount);
                PurePursuit();

                //if end of the line then stop
                if (!isEndOfTheRecLine)
                {
                    mf.sim.stepDistance = recList[C].speed / 17.86;
                    north = recList[C].northing;

                    //section control - only if different click the button
                    bool autoBtn = (mf.autoBtnState == FormGPS.btnStates.Auto);
                    trig = autoBtn;
                    if (autoBtn != recList[C].autoBtnState)
                    {
                        mf.btnSectionOffAutoOn.PerformClick();
                    }
                }
                else
                {
                    //create the dubins path based on start and goal to start of recorded path
                    GetDubinsPath(homePos);
                    dubListCount = dubList.Count;

                    //its too small
                    if (dubListCount < 5)
                    {
                        StopDrivingRecordedPath();
                    }

                    //set all the flags
                    isFollowingDubinsHome   = true;
                    isFollowingRecPath      = false;
                    isFollowingDubinsToPath = false;
                    isEndOfTheRecLine       = false;
                }
            }

            if (isFollowingDubinsHome)
            {
                mf.sim.stepDistance = 7 / 17.86;
                pivotAxlePosRP      = mf.pivotAxlePos;

                FindGoalPointDubinsPath(dubListCount);
                PurePursuit();

                //check if close to home position
                double distSqr = glm.DistanceSquared(pivotAxlePosRP.easting, pivotAxlePosRP.northing, homePos.easting, homePos.northing);
                if (distSqr < 3)
                {
                    StopDrivingRecordedPath();
                }
            }

            //if paused, set the sim to 0
            if (isPausedDrivingRecordedPath)
            {
                mf.sim.stepDistance = 0 / 17.86;
            }
        }
예제 #7
0
 public vec3(vec3 v)
 {
     easting  = v.easting;
     northing = v.northing;
     heading  = v.heading;
 }
예제 #8
0
        public void CreateBndTramRef()
        {
            //count the points from the boundary
            int ptCount = mf.bnd.bndArr[0].bndLine.Count;

            outArr?.Clear();

            //outside point
            vec3 pt3 = new vec3();

            double distSq = ((tramWidth * 0.5) - halfWheelTrack) * ((tramWidth * 0.5) - halfWheelTrack) * 0.97;
            bool   fail   = false;

            //make the boundary tram outer array
            for (int i = 0; i < ptCount; i++)
            {
                //calculate the point inside the boundary
                pt3.easting = mf.bnd.bndArr[0].bndLine[i].easting -
                              (Math.Sin(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * (tramWidth * 0.5 - halfWheelTrack));

                pt3.northing = mf.bnd.bndArr[0].bndLine[i].northing -
                               (Math.Cos(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * (tramWidth * 0.5 - halfWheelTrack));

                for (int j = 0; j < ptCount; j++)
                {
                    double check = glm.DistanceSquared(pt3.northing, pt3.easting,
                                                       mf.bnd.bndArr[0].bndLine[j].northing, mf.bnd.bndArr[0].bndLine[j].easting);
                    if (check < distSq)
                    {
                        fail = true;
                        break;
                    }
                }

                if (!fail)
                {
                    pt3.heading = mf.bnd.bndArr[0].bndLine[i].heading;
                    outArr.Add(pt3);
                }
                fail = false;
            }

            int cnt = outArr.Count;

            if (cnt < 6)
            {
                return;
            }

            const double spacing = 2.0;
            double       distance;

            for (int i = 0; i < cnt - 1; i++)
            {
                distance = glm.Distance(outArr[i], outArr[i + 1]);
                if (distance < spacing)
                {
                    outArr.RemoveAt(i + 1);
                    cnt = outArr.Count;
                    i--;
                }
            }
        }
예제 #9
0
파일: CGLM.cs 프로젝트: BenjaminK87/v4202
 public static double Distance(vec3 first, vec2 second)
 {
     return(Math.Sqrt(
                Math.Pow(first.easting - second.easting, 2)
                + Math.Pow(first.northing - second.northing, 2)));
 }
예제 #10
0
        public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
        {
            int ptCount = refList.Count;
            int ptCnt   = ptCount - 1;

            if (ptCount < 5)
            {
                return;
            }

            boxA.easting  = pivot.easting - (Math.Sin(aveLineHeading + glm.PIBy2) * 2000);
            boxA.northing = pivot.northing - (Math.Cos(aveLineHeading + glm.PIBy2) * 2000);

            boxB.easting  = pivot.easting + (Math.Sin(aveLineHeading + glm.PIBy2) * 2000);
            boxB.northing = pivot.northing + (Math.Cos(aveLineHeading + glm.PIBy2) * 2000);

            boxC.easting  = boxB.easting + (Math.Sin(aveLineHeading) * 1.0);
            boxC.northing = boxB.northing + (Math.Cos(aveLineHeading) * 1.0);

            boxD.easting  = boxA.easting + (Math.Sin(aveLineHeading) * 1.0);
            boxD.northing = boxA.northing + (Math.Cos(aveLineHeading) * 1.0);

            boxA.easting  -= (Math.Sin(aveLineHeading) * 1.0);
            boxA.northing -= (Math.Cos(aveLineHeading) * 1.0);

            boxB.easting  -= (Math.Sin(aveLineHeading) * 1.0);
            boxB.northing -= (Math.Cos(aveLineHeading) * 1.0);

            //determine if point are in frustum box
            for (int s = 0; s < ptCnt; s++)
            {
                if ((((boxB.easting - boxA.easting) * (refList[s].northing - boxA.northing))
                     - ((boxB.northing - boxA.northing) * (refList[s].easting - boxA.easting))) < 0)
                {
                    continue;
                }

                if ((((boxD.easting - boxC.easting) * (refList[s].northing - boxC.northing))
                     - ((boxD.northing - boxC.northing) * (refList[s].easting - boxC.easting))) < 0)
                {
                    continue;
                }

                closestRefIndex = s;
                break;
            }

            double dist = ((pivot.easting - refList[closestRefIndex].easting) * (pivot.easting - refList[closestRefIndex].easting))
                          + ((pivot.northing - refList[closestRefIndex].northing) * (pivot.northing - refList[closestRefIndex].northing));

            //determine closest point
            double minDistance = Math.Sqrt(dist);

            //grab the heading at the closest point
            refHeading = refList[closestRefIndex].heading;

            //which side of the patch are we on is next
            //calculate endpoints of reference line based on closest point
            refPoint1.easting  = refList[closestRefIndex].easting - (Math.Sin(refHeading) * 50.0);
            refPoint1.northing = refList[closestRefIndex].northing - (Math.Cos(refHeading) * 50.0);

            refPoint2.easting  = refList[closestRefIndex].easting + (Math.Sin(refHeading) * 50.0);
            refPoint2.northing = refList[closestRefIndex].northing + (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
            double distanceFromRefLine = ((dz * pivot.easting) - (dx * pivot.northing) + (refPoint2.easting
                                                                                          * refPoint1.northing) - (refPoint2.northing * refPoint1.easting));

            //   / Math.Sqrt((dz * dz) + (dx * dx));
            //are we going same direction as stripList was created?
            isSameWay = Math.PI - Math.Abs(Math.Abs(pivot.heading - refHeading) - Math.PI) < glm.PIBy2;
            deltaOfRefAndAveHeadings = Math.PI - Math.Abs(Math.Abs(aveLineHeading - refHeading) - Math.PI);
            deltaOfRefAndAveHeadings = Math.Cos(deltaOfRefAndAveHeadings);

            //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;
            }
            double widthMinusOverlap;

            //move the ABLine over based on the overlap amount set in vehicle
            if (mf.tool.toolOffset != 0)
            {
                widthMinusOverlap = mf.tool.toolWidth / 2 - mf.tool.toolOverlap;
            }
            else
            {
                widthMinusOverlap = mf.tool.toolWidth - mf.tool.toolOverlap;
            }


            howManyPathsAway = Math.Round(minDistance / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);

            curveNumber = howManyPathsAway;
            if (distanceFromRefLine < 0)
            {
                curveNumber = -curveNumber;
            }

            //double toolOffset = mf.tool.toolOffset;

            //build the current line
            curList?.Clear();
            for (int i = 0; i < ptCount; i++)
            {
                var point = new vec3(
                    refList[i].easting + (Math.Sin(piSide + aveLineHeading) * ((widthMinusOverlap * howManyPathsAway))),
                    refList[i].northing + (Math.Cos(piSide + aveLineHeading) * ((widthMinusOverlap * howManyPathsAway))),
                    refList[i].heading);
                curList.Add(point);
            }

            double minDistA = 1000000, minDistB = 1000000;

            ptCount = curList.Count;

            if (ptCount > 0)
            {
                if (mf.isStanleyUsed)
                {
                    //find the closest 2 points to current fix
                    for (int t = 0; t < ptCount; t++)
                    {
                        dist = ((steer.easting - curList[t].easting) * (steer.easting - curList[t].easting))
                               + ((steer.northing - curList[t].northing) * (steer.northing - curList[t].northing));
                        if (dist < minDistA)
                        {
                            minDistB = minDistA;
                            B        = A;
                            minDistA = dist;
                            A        = t;
                        }
                        else if (dist < minDistB)
                        {
                            minDistB = dist;
                            B        = t;
                        }
                    }

                    //just need to make sure the points continue ascending or heading switches all over the place
                    if (A > B)
                    {
                        C = A; A = B; B = C;
                    }

                    currentLocationIndex = A;

                    //get the distance from currently active AB line
                    dx = curList[B].easting - curList[A].easting;
                    dz = curList[B].northing - curList[A].northing;

                    if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
                    {
                        return;
                    }

                    //abHeading = Math.Atan2(dz, dx);
                    double abHeading = curList[A].heading;

                    //how far from current AB Line is fix
                    distanceFromCurrentLine = ((dz * steer.easting) - (dx * steer.northing) + (curList[B].easting
                                                                                               * curList[A].northing) - (curList[B].northing * curList[A].easting))
                                              / Math.Sqrt((dz * dz) + (dx * dx));

                    //are we on the right side or not
                    isOnRightSideCurrentLine = distanceFromCurrentLine > 0;

                    //absolute the distance
                    distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);

                    //Subtract the two headings, if > 1.57 its going the opposite heading as refAB
                    double abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
                    if (abFixHeadingDelta >= Math.PI)
                    {
                        abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
                    }
                    isABSameAsVehicleHeading = abFixHeadingDelta < glm.PIBy2;

                    // calc point on ABLine closest to current position
                    double U = (((steer.easting - curList[A].easting) * dx)
                                + ((steer.northing - curList[A].northing) * dz))
                               / ((dx * dx) + (dz * dz));

                    rEastCu  = curList[A].easting + (U * dx);
                    rNorthCu = curList[A].northing + (U * dz);

                    //distance is negative if on left, positive if on right
                    if (isABSameAsVehicleHeading)
                    {
                        if (!isOnRightSideCurrentLine)
                        {
                            distanceFromCurrentLine *= -1.0;
                        }
                        abFixHeadingDelta = (steer.heading - abHeading);
                    }

                    //opposite way so right is left
                    else
                    {
                        if (isOnRightSideCurrentLine)
                        {
                            distanceFromCurrentLine *= -1.0;
                        }
                        abFixHeadingDelta = (steer.heading - abHeading + Math.PI);
                    }

                    //Fix the circular error
                    if (abFixHeadingDelta > Math.PI)
                    {
                        abFixHeadingDelta -= Math.PI;
                    }
                    else if (abFixHeadingDelta < Math.PI)
                    {
                        abFixHeadingDelta += Math.PI;
                    }

                    if (abFixHeadingDelta > glm.PIBy2)
                    {
                        abFixHeadingDelta -= Math.PI;
                    }
                    else if (abFixHeadingDelta < -glm.PIBy2)
                    {
                        abFixHeadingDelta += Math.PI;
                    }

                    abFixHeadingDelta *= mf.vehicle.stanleyHeadingErrorGain;
                    if (abFixHeadingDelta > 0.74)
                    {
                        abFixHeadingDelta = 0.74;
                    }
                    if (abFixHeadingDelta < -0.74)
                    {
                        abFixHeadingDelta = -0.74;
                    }

                    steerAngleCu = Math.Atan((distanceFromCurrentLine * mf.vehicle.stanleyGain) / ((mf.pn.speed * 0.277777) + 1));

                    if (steerAngleCu > 0.74)
                    {
                        steerAngleCu = 0.74;
                    }
                    if (steerAngleCu < -0.74)
                    {
                        steerAngleCu = -0.74;
                    }

                    steerAngleCu = glm.toDegrees((steerAngleCu + abFixHeadingDelta) * -1.0);

                    if (steerAngleCu < -mf.vehicle.maxSteerAngle)
                    {
                        steerAngleCu = -mf.vehicle.maxSteerAngle;
                    }
                    if (steerAngleCu > mf.vehicle.maxSteerAngle)
                    {
                        steerAngleCu = mf.vehicle.maxSteerAngle;
                    }

                    //Convert to millimeters
                    distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);
                }
                else
                {
                    //find the closest 2 points to current fix
                    for (int t = 0; t < ptCount; t++)
                    {
                        dist = ((pivot.easting - curList[t].easting) * (pivot.easting - curList[t].easting))
                               + ((pivot.northing - curList[t].northing) * (pivot.northing - curList[t].northing));
                        if (dist < minDistA)
                        {
                            minDistB = minDistA;
                            B        = A;
                            minDistA = dist;
                            A        = t;
                        }
                        else if (dist < minDistB)
                        {
                            minDistB = dist;
                            B        = t;
                        }
                    }

                    //just need to make sure the points continue ascending or heading switches all over the place
                    if (A > B)
                    {
                        C = A; A = B; B = C;
                    }

                    currentLocationIndex = A;

                    //get the distance from currently active AB line
                    dx = curList[B].easting - curList[A].easting;
                    dz = curList[B].northing - curList[A].northing;

                    if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
                    {
                        return;
                    }

                    //abHeading = Math.Atan2(dz, dx);

                    //how far from current AB Line is fix
                    distanceFromCurrentLine = ((dz * pivot.easting) - (dx * pivot.northing) + (curList[B].easting
                                                                                               * curList[A].northing) - (curList[B].northing * curList[A].easting))
                                              / Math.Sqrt((dz * dz) + (dx * dx));

                    //are we on the right side or not
                    isOnRightSideCurrentLine = distanceFromCurrentLine > 0;

                    //absolute the distance
                    distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);

                    // ** Pure pursuit ** - calc point on ABLine closest to current position
                    double U = (((pivot.easting - curList[A].easting) * dx)
                                + ((pivot.northing - curList[A].northing) * dz))
                               / ((dx * dx) + (dz * dz));

                    rEastCu  = curList[A].easting + (U * dx);
                    rNorthCu = curList[A].northing + (U * dz);

                    //double minx, maxx, miny, maxy;

                    //minx = Math.Min(curList[A].northing, curList[B].northing);
                    //maxx = Math.Max(curList[A].northing, curList[B].northing);

                    //miny = Math.Min(curList[A].easting, curList[B].easting);
                    //maxy = Math.Max(curList[A].easting, curList[B].easting);

                    //isValid = rNorthCu >= minx && rNorthCu <= maxx && (rEastCu >= miny && rEastCu <= maxy);

                    //if (!isValid)
                    //{
                    //    //invalid distance so tell AS module
                    //    distanceFromCurrentLine = 32000;
                    //    mf.guidanceLineDistanceOff = 32000;
                    //    return;
                    //}

                    //used for accumulating distance to find goal point
                    double distSoFar;

                    //update base on autosteer settings and distance from line
                    double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLine);
                    mf.lookaheadActual = goalPointDistance;

                    // used for calculating the length squared of next segment.
                    double tempDist = 0.0;

                    if (!isSameWay)
                    {
                        //counting down
                        isABSameAsVehicleHeading = false;
                        distSoFar = glm.Distance(curList[A], rEastCu, rNorthCu);
                        //Is this segment long enough to contain the full lookahead distance?
                        if (distSoFar > goalPointDistance)
                        {
                            //treat current segment like an AB Line
                            goalPointCu.easting  = rEastCu - (Math.Sin(curList[A].heading) * goalPointDistance);
                            goalPointCu.northing = rNorthCu - (Math.Cos(curList[A].heading) * goalPointDistance);
                        }

                        //multiple segments required
                        else
                        {
                            //cycle thru segments and keep adding lengths. check if start and break if so.
                            while (A > 0)
                            {
                                B--; A--;
                                tempDist = glm.Distance(curList[B], curList[A]);

                                //will we go too far?
                                if ((tempDist + distSoFar) > goalPointDistance)
                                {
                                    break;                                             //tempDist contains the full length of next segment
                                }
                                else
                                {
                                    distSoFar += tempDist;
                                }
                            }

                            double t = (goalPointDistance - distSoFar); // the remainder to yet travel
                            t /= tempDist;

                            goalPointCu.easting  = (((1 - t) * curList[B].easting) + (t * curList[A].easting));
                            goalPointCu.northing = (((1 - t) * curList[B].northing) + (t * curList[A].northing));
                        }
                    }
                    else
                    {
                        //counting up
                        isABSameAsVehicleHeading = true;
                        distSoFar = glm.Distance(curList[B], rEastCu, rNorthCu);

                        //Is this segment long enough to contain the full lookahead distance?
                        if (distSoFar > goalPointDistance)
                        {
                            //treat current segment like an AB Line
                            goalPointCu.easting  = rEastCu + (Math.Sin(curList[A].heading) * goalPointDistance);
                            goalPointCu.northing = rNorthCu + (Math.Cos(curList[A].heading) * goalPointDistance);
                        }

                        //multiple segments required
                        else
                        {
                            //cycle thru segments and keep adding lengths. check if end and break if so.
                            // ReSharper disable once LoopVariableIsNeverChangedInsideLoop
                            while (B < ptCount - 1)
                            {
                                B++; A++;
                                tempDist = glm.Distance(curList[B], curList[A]);

                                //will we go too far?
                                if ((tempDist + distSoFar) > goalPointDistance)
                                {
                                    //A--; B--;
                                    break; //tempDist contains the full length of next segment
                                }

                                distSoFar += tempDist;
                            }

                            //xt = (((1 - t) * x0 + t * x1)
                            //yt = ((1 - t) * y0 + t * y1))

                            double t = (goalPointDistance - distSoFar); // the remainder to yet travel
                            t /= tempDist;

                            goalPointCu.easting  = (((1 - t) * curList[A].easting) + (t * curList[B].easting));
                            goalPointCu.northing = (((1 - t) * curList[A].northing) + (t * curList[B].northing));
                        }
                    }

                    //calc "D" the distance from pivot axle to lookahead point
                    double goalPointDistanceSquared = glm.DistanceSquared(goalPointCu.northing, goalPointCu.easting, pivot.northing, pivot.easting);

                    //calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
                    double localHeading = glm.twoPI - mf.fixHeading;
                    ppRadiusCu = goalPointDistanceSquared / (2 * (((goalPointCu.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCu.northing - pivot.northing) * Math.Sin(localHeading))));

                    steerAngleCu = glm.toDegrees(Math.Atan(2 * (((goalPointCu.easting - pivot.easting) * Math.Cos(localHeading))
                                                                + ((goalPointCu.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));

                    if (steerAngleCu < -mf.vehicle.maxSteerAngle)
                    {
                        steerAngleCu = -mf.vehicle.maxSteerAngle;
                    }
                    if (steerAngleCu > mf.vehicle.maxSteerAngle)
                    {
                        steerAngleCu = mf.vehicle.maxSteerAngle;
                    }

                    if (ppRadiusCu < -500)
                    {
                        ppRadiusCu = -500;
                    }
                    if (ppRadiusCu > 500)
                    {
                        ppRadiusCu = 500;
                    }

                    radiusPointCu.easting  = pivot.easting + (ppRadiusCu * Math.Cos(localHeading));
                    radiusPointCu.northing = pivot.northing + (ppRadiusCu * Math.Sin(localHeading));

                    //angular velocity in rads/sec  = 2PI * m/sec * radians/meters
                    double angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleCu))) / mf.vehicle.wheelbase;

                    //clamp the steering angle to not exceed safe angular velocity
                    if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
                    {
                        steerAngleCu = glm.toDegrees(steerAngleCu > 0 ?
                                                     (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))
                            : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
                    }
                    //Convert to centimeters
                    distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);

                    //distance is negative if on left, positive if on right
                    //if you're going the opposite direction left is right and right is left
                    //double temp;
                    if (isABSameAsVehicleHeading)
                    {
                        //temp = (abHeading);
                        if (!isOnRightSideCurrentLine)
                        {
                            distanceFromCurrentLine *= -1.0;
                        }
                    }

                    //opposite way so right is left
                    else
                    {
                        //temp = (abHeading - Math.PI);
                        //if (temp < 0) temp = (temp + glm.twoPI);
                        //temp = glm.toDegrees(temp);
                        if (isOnRightSideCurrentLine)
                        {
                            distanceFromCurrentLine *= -1.0;
                        }
                    }
                }

                mf.guidanceLineDistanceOff = mf.distanceDisplay = (Int16)distanceFromCurrentLine;
                mf.guidanceLineSteerAngle  = (Int16)(steerAngleCu * 100);

                if (mf.yt.isYouTurnTriggered)
                {
                    //do the pure pursuit from youTurn
                    mf.yt.DistanceFromYouTurnLine();
                    mf.seq.DoSequenceEvent();

                    //now substitute what it thinks are AB line values with auto turn values
                    steerAngleCu            = mf.yt.steerAngleYT;
                    distanceFromCurrentLine = mf.yt.distanceFromCurrentLine;

                    goalPointCu            = mf.yt.goalPointYT;
                    radiusPointCu.easting  = mf.yt.radiusPointYT.easting;
                    radiusPointCu.northing = mf.yt.radiusPointYT.northing;
                    ppRadiusCu             = mf.yt.ppRadiusYT;
                }
            }
            else
            {
                //invalid distance so tell AS module
                distanceFromCurrentLine    = 32000;
                mf.guidanceLineDistanceOff = 32000;
            }
        }
예제 #11
0
파일: CABLine.cs 프로젝트: aortner/AgraBot
        public void GetCurrentABLine(vec3 pivot)
        {
            //move the ABLine over based on the overlap amount set in vehicle
            double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;

            //x2-x1
            double dx = refABLineP2.easting - refABLineP1.easting;
            //z2-z1
            double dy = refABLineP2.northing - refABLineP1.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));

            //sign of distance determines which side of line we are on
            if (distanceFromRefLine > 0)
            {
                refLineSide = 1;
            }
            else
            {
                refLineSide = -1;
            }

            //absolute the distance
            distanceFromRefLine = Math.Abs(distanceFromRefLine);

            //Which ABLine is the vehicle on, negative is left and positive is right side
            howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);

            //generate that pass number as signed integer
            passNumber = Convert.ToInt32(refLineSide * howManyPathsAway);

            //calculate the new point that is number of implement widths over
            double toolOffset = mf.vehicle.toolOffset;
            vec2   point1;

            //depending which way you are going, the offset can be either side
            if (isABSameAsVehicleHeading)
            {
                point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.easting,
                                  (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.northing);
            }
            else
            {
                point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.easting,
                                  (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.northing);
            }

            //create the new line extent points for current ABLine based on original heading of AB line
            currentABLineP1.easting  = point1.easting - (Math.Sin(abHeading) * 40000.0);
            currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 40000.0);

            currentABLineP2.easting  = point1.easting + (Math.Sin(abHeading) * 40000.0);
            currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 40000.0);

            //get the distance from currently active AB line
            //x2-x1
            dx = currentABLineP2.easting - currentABLineP1.easting;
            //z2-z1
            dy = currentABLineP2.northing - currentABLineP1.northing;

            //save a copy of dx,dy in youTurn
            mf.yt.dxAB = dx; mf.yt.dyAB = dy;

            //how far from current AB Line is fix
            distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
                                                                                       * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
                                      / Math.Sqrt((dy * dy) + (dx * dx));

            //are we on the right side or not
            isOnRightSideCurrentLine = distanceFromCurrentLine > 0;


            // filter for distance
            if (Properties.Settings.Default.is_xte)
            {
                distancefilter = ((Properties.Settings.Default.xtefilter * distancefilter) + distanceFromCurrentLine) / (Properties.Settings.Default.xtefilter + 1);

                distanceFromCurrentLine = distancefilter;
            }

            //absolute the distance
            distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);

            //double goalPointDistance = mf.vehicle.goalPointLookAhead;
            //if (distanceFromCurrentLine < mf.vehicle.goalPointDistanceFromLine )
            //    goalPointDistance -= goalPointDistance * (mf.vehicle.goalPointDistanceFromLine - distanceFromCurrentLine);
            //if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum) goalPointDistance = mf.vehicle.goalPointLookAheadMinimum;



            if (Properties.Settings.Default.isortner)
            {
                goalPointDistance = (mf.pn.speed - distanceFromCurrentLine * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead; // goalPointLookAhead should be 10-20

                if (distanceFromCurrentLine > 0.4)
                {
                    goalPointDistance  = (mf.pn.speed - 0.4 * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead;;
                    goalPointDistance += ((distanceFromCurrentLine - 0.4) * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead;

                    if (goalPointDistance > mf.pn.speed * speedmaxlahead)
                    {
                        goalPointDistance = mf.pn.speed * speedmaxlahead;
                    }
                }

                if (goalPointDistance < mf.pn.speed * speedminlahead)
                {
                    goalPointDistance = mf.pn.speed * speedminlahead;
                }
            }

            else if (Properties.Settings.Default.isspeed)
            {
                goalPointDistance = mf.pn.speed * speedmaxlahead;
                if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum)
                {
                    goalPointDistance = mf.vehicle.goalPointLookAheadMinimum;
                }
            }

            else if (Properties.Settings.Default.isschelter)
            {
                //!!!!!how far should goal point be away
                //!!!!!double goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777);
                //!!!!!if (goalPointDistance < mf.vehicle.minLookAheadDistance) goalPointDistance = mf.vehicle.minLookAheadDistance;


                //!!!!!Versuch: how far should goal point be away

                if (distanceFromCurrentLine < 0.3)
                {
                    goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) - (distanceFromCurrentLine * 12.5 * (mf.pn.speed / 10));
                }

                if (distanceFromCurrentLine >= 0.3 & distanceFromCurrentLine < 0.6)
                {
                    goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) - (((0.3 - (distanceFromCurrentLine - 0.3)) * 12.5 * (mf.pn.speed / 10)));
                }

                if (distanceFromCurrentLine >= 0.6 & distanceFromCurrentLine < 1)
                {
                    goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777);
                }

                if (distanceFromCurrentLine > 1 & distanceFromCurrentLine < 2.5)
                {
                    goalPointDistance = ((mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) * (((distanceFromCurrentLine - 1) / 10) + 1));
                }

                if (distanceFromCurrentLine >= 2.5)
                {
                    goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) * 1.15;
                }



                //minimum of 3.0 meters look ahead
                if (goalPointDistance < speedminlahead)
                {
                    goalPointDistance = speedminlahead;
                }
            }
            else
            {              //how far should goal point be away  - speed * seconds * kmph -> m/s then limit min value
                goalPointDistance = mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.27777777;

                if (distanceFromCurrentLine < 1.0)
                {
                    goalPointDistance += distanceFromCurrentLine * goalPointDistance * mf.vehicle.goalPointDistanceMultiplier;
                }
                else
                {
                    goalPointDistance += goalPointDistance * mf.vehicle.goalPointDistanceMultiplier;
                }

                if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum)
                {
                    goalPointDistance = mf.vehicle.goalPointLookAheadMinimum;
                }
            }



            mf.test1 = goalPointDistance;

            //Subtract the two headings, if > 1.57 its going the opposite heading as refAB
            abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
            if (abFixHeadingDelta >= Math.PI)
            {
                abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
            }

            // ** Pure pursuit ** - calc point on ABLine closest to current position
            double U = (((pivot.easting - currentABLineP1.easting) * dx)
                        + ((pivot.northing - currentABLineP1.northing) * dy))
                       / ((dx * dx) + (dy * dy));

            //point on AB line closest to pivot axle point
            rEastAB  = currentABLineP1.easting + (U * dx);
            rNorthAB = currentABLineP1.northing + (U * dy);

            //how far should goal point be away  - speed * seconds * kmph -> m/s + min value
            //double goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777);
            //if (goalPointDistance < mf.vehicle.minLookAheadDistance) goalPointDistance = mf.vehicle.minLookAheadDistance;

            if (abFixHeadingDelta >= glm.PIBy2)
            {
                isABSameAsVehicleHeading = false;
                goalPointAB.easting      = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
                goalPointAB.northing     = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
            }
            else
            {
                isABSameAsVehicleHeading = true;
                goalPointAB.easting      = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
                goalPointAB.northing     = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
            }

            //calc "D" the distance from pivot axle to lookahead point
            double goalPointDistanceDSquared
                = glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);

            //calculate the the new x in local coordinates and steering angle degrees based on wheelbase
            double localHeading = glm.twoPI - mf.fixHeading;

            ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                           + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));

            //make sure pp doesn't generate a radius smaller then turn radius
            //if (ppRadiusAB > 0)
            //{
            //    if (ppRadiusAB < mf.vehicle.minTurningRadius * 0.95) ppRadiusAB = mf.vehicle.minTurningRadius * 0.95;
            //}
            //else if (ppRadiusAB > -mf.vehicle.minTurningRadius * 0.95)
            //{
            //    ppRadiusAB = -mf.vehicle.minTurningRadius * 0.95;
            //}

            steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                        + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
                                                   / goalPointDistanceDSquared));
            if (steerAngleAB < -mf.vehicle.maxSteerAngle)
            {
                steerAngleAB = -mf.vehicle.maxSteerAngle;
            }
            if (steerAngleAB > mf.vehicle.maxSteerAngle)
            {
                steerAngleAB = mf.vehicle.maxSteerAngle;
            }

            //limit circle size for display purpose
            if (ppRadiusAB < -500)
            {
                ppRadiusAB = -500;
            }
            if (ppRadiusAB > 500)
            {
                ppRadiusAB = 500;
            }

            radiusPointAB.easting  = pivot.easting + (ppRadiusAB * Math.Cos(localHeading));
            radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading));

            //Convert to millimeters
            distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);

            //angular velocity in rads/sec  = 2PI * m/sec * radians/meters
            angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase;

            //clamp the steering angle to not exceed safe angular velocity
            if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
            {
                steerAngleAB = glm.toDegrees(steerAngleAB > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity)
                                                                           / (glm.twoPI * mf.pn.speed * 0.277777)))
                    : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
            }

            //distance is negative if on left, positive if on right
            if (isABSameAsVehicleHeading)
            {
                if (!isOnRightSideCurrentLine)
                {
                    distanceFromCurrentLine *= -1.0;
                }
            }

            //opposite way so right is left
            else
            {
                if (isOnRightSideCurrentLine)
                {
                    distanceFromCurrentLine *= -1.0;
                }
            }



            mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
            mf.guidanceLineSteerAngle  = (Int16)(steerAngleAB * 100);


            if (mf.yt.isYouTurnShapeDisplayed)
            {
                //do the pure pursuit from youTurn
                mf.yt.DistanceFromYouTurnLine();

                //now substitute what it thinks are AB line values with auto turn values
                steerAngleAB            = mf.yt.steerAngleYT;
                distanceFromCurrentLine = mf.yt.distanceFromCurrentLine;

                goalPointAB            = mf.yt.goalPointYT;
                radiusPointAB.easting  = mf.yt.radiusPointYT.easting;
                radiusPointAB.northing = mf.yt.radiusPointYT.northing;
                ppRadiusAB             = mf.yt.ppRadiusYT;
            }
        }
예제 #12
0
        //every time a new fix, a new patch point from last point to this point
        //only need prev point on the first points of triangle strip that makes a box (2 triangles)

        public void AddMappingPoint()
        {
            //add two triangles for next step.
            //left side
            vec3 point = new vec3(leftPoint.easting, leftPoint.northing, 0);

            //add the point to List
            triangleList.Add(point);

            //Right side
            vec3 point2 = new vec3(rightPoint.easting, rightPoint.northing, 0);

            //add the point to the list
            triangleList.Add(point2);

            //count the triangle pairs
            numTriangles++;

            //quick count
            int c = triangleList.Count - 1;

            //when closing a job the triangle patches all are emptied but the section delay keeps going.
            //Prevented by quick check. 4 points plus colour
            if (c >= 5)
            {
                //calculate area of these 2 new triangles - AbsoluteValue of (Ax(By-Cy) + Bx(Cy-Ay) + Cx(Ay-By)/2)
                {
                    double temp = (triangleList[c].easting * (triangleList[c - 1].northing - triangleList[c - 2].northing))
                                  + (triangleList[c - 1].easting * (triangleList[c - 2].northing - triangleList[c].northing))
                                  + (triangleList[c - 2].easting * (triangleList[c].northing - triangleList[c - 1].northing));

                    temp = Math.Abs(temp / 2.0);
                    mf.fd.workedAreaTotal     += temp;
                    mf.fd.workedAreaTotalUser += temp;

                    //temp = 0;
                    temp = (triangleList[c - 1].easting * (triangleList[c - 2].northing - triangleList[c - 3].northing))
                           + (triangleList[c - 2].easting * (triangleList[c - 3].northing - triangleList[c - 1].northing))
                           + (triangleList[c - 3].easting * (triangleList[c - 1].northing - triangleList[c - 2].northing));

                    temp = Math.Abs(temp / 2.0);
                    mf.fd.workedAreaTotal     += temp;
                    mf.fd.workedAreaTotalUser += temp;
                }
            }

            if (numTriangles > 36)
            {
                numTriangles = 0;

                //save the cutoff patch to be saved later
                mf.patchSaveList.Add(triangleList);

                triangleList = new List <vec3>();
                patchList.Add(triangleList);

                //Add Patch colour
                vec3 colur = new vec3(mf.sectionColorDay.R, mf.sectionColorDay.G, mf.sectionColorDay.B);
                triangleList.Add(colur);

                //add the points to List, yes its more points, but breaks up patches for culling
                triangleList.Add(point);
                triangleList.Add(point2);
            }
        }
예제 #13
0
        //determine closest point on left side
        public void BuildContourGuidanceLine(vec3 pivot)
        {
            double toolWid = mf.vehicle.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.vehicle.toolOffset < 0)
            {
                //sticks out more left
                sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2)));
                cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2)));

                sin2HR = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset)));
                cos2HR = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset)));
            }
            else
            {
                //sticks out more right
                sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset)));
                cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset)));

                sin2HR = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2)));
                cos2HR = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2)));
            }

            //narrow equipment needs bigger bounding box.
            if (mf.vehicle.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.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 - 35; if (start < 0) start = 0;
            stop = pt + 35; if (stop > ptCount) stop = ptCount + 1;

            double distSq = widthMinusOverlap * widthMinusOverlap * 0.875;
            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;
            }

            //if no boundaries, just return.
            if (!mf.bnd.bndArr[0].isSet) return;

            int ctCount = ctList.Count;
            if (ctCount < 6) return;

            const double spacing = 0.8;
            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]);
                }
            }
        }
예제 #14
0
        public void FixBoundaryLine(int bndNum, double spacing)
        {
            //count the points from the boundary
            int ptCount = bndLine.Count;

            //boundary point spacing based on eq width
            spacing *= 0.25;

            if (spacing < 1)
            {
                spacing = 1;
            }
            if (spacing > 3)
            {
                spacing = 3;
            }

            //first find out which side is inside the boundary
            double oneSide = glm.PIBy2;
            vec3   point   = new vec3(bndLine[3].easting - (Math.Sin(oneSide + bndLine[3].heading) * 2.0),
                                      bndLine[3].northing - (Math.Cos(oneSide + bndLine[3].heading) * 2.0), 0.0);

            //make sure boundaries are wound correctly
            if (bndNum == 0)
            {
                //outside an outer boundary means its wound clockwise
                if (!IsPointInsideBoundary(point))
                {
                    ReverseWinding();
                }
            }
            else
            {
                //inside an inner boundary means its wound clockwise
                if (IsPointInsideBoundary(point))
                {
                    ReverseWinding();
                }
                spacing *= 0.66;
            }

            //make sure distance isn't too small between points on headland
            int    bndCount = bndLine.Count;
            double distance;

            for (int i = 0; i < bndCount - 1; i++)
            {
                distance = glm.Distance(bndLine[i], bndLine[i + 1]);
                if (distance < spacing)
                {
                    bndLine.RemoveAt(i + 1);
                    bndCount = bndLine.Count;
                    i--;
                }
            }

            //make sure distance isn't too big between points on boundary
            bndCount = bndLine.Count;
            spacing *= 1.33;

            for (int i = 0; i < bndCount; i++)
            {
                int j = i + 1;

                if (j == bndCount)
                {
                    j = 0;
                }
                distance = glm.Distance(bndLine[i], bndLine[j]);
                if (distance > spacing)
                {
                    CBndPt pointB = new CBndPt((bndLine[i].easting + bndLine[j].easting) / 2.0,
                                               (bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading);

                    bndLine.Insert(j, pointB);
                    bndCount = bndLine.Count;
                    i--;
                }
            }

            //make sure distance isn't too big between points on boundary
            bndCount = bndLine.Count;
            spacing *= 1.33;

            for (int i = 0; i < bndCount; i++)
            {
                int j = i + 1;

                if (j == bndCount)
                {
                    j = 0;
                }
                distance = glm.Distance(bndLine[i], bndLine[j]);
                if (distance > spacing)
                {
                    CBndPt pointB = new CBndPt((bndLine[i].easting + bndLine[j].easting) / 2.0,
                                               (bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading);

                    bndLine.Insert(j, pointB);
                    bndCount = bndLine.Count;
                    i--;
                }
            }

            //make sure headings are correct for calculated points
            CalculateBoundaryHeadings();
        }
예제 #15
0
        public void FindClosestTurnPoint(bool isYouTurnRight, vec3 fromPt, double headAB)
        {
            //initial scan is straight ahead of pivot point of vehicle to find the right turnLine/boundary
            vec3 pt    = new vec3();
            vec3 rayPt = new vec3();

            int closestTurnNum = 99;

            double cosHead = Math.Cos(headAB);
            double sinHead = Math.Sin(headAB);

            for (int b = 1; b < mf.maxCrossFieldLength; b += 2)
            {
                pt.easting  = fromPt.easting + (sinHead * b);
                pt.northing = fromPt.northing + (cosHead * b);

                int idx = IsPointInsideTurnArea(pt);
                if (idx != 0)
                {
                    closestTurnNum = idx;
                    rayPt.easting  = pt.easting;
                    rayPt.northing = pt.northing;
                    break;
                }
            }
            if (closestTurnNum < 0)
            {
                closestTurnNum = 0;
            }

            //second scan is straight ahead of outside of tool based on turn direction
            double scanWidthL, scanWidthR;

            if (isYouTurnRight) //its actually left
            {
                scanWidthL = -(mf.tool.toolWidth * 0.25) - (mf.tool.toolWidth * 0.5);
                scanWidthR = (mf.tool.toolWidth * 0.25) - (mf.tool.toolWidth * 0.5);
            }
            else
            {
                scanWidthL = -(mf.tool.toolWidth * 0.25) + (mf.tool.toolWidth * 0.5);
                scanWidthR = (mf.tool.toolWidth * 0.25) + (mf.tool.toolWidth * 0.5);
            }

            //isYouTurnRight actuall means turning left - Painful, but it switches later
            boxA.easting  = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthL);
            boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthL);

            boxB.easting  = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthR);
            boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthR);

            boxC.easting  = boxB.easting + (Math.Sin(headAB) * boxLength);
            boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength);

            boxD.easting  = boxA.easting + (Math.Sin(headAB) * boxLength);
            boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength);

            //determine if point is inside bounding box of the 1 turn chosen above
            turnClosestList.Clear();

            vec4 inBox;

            int ptCount = bndList[closestTurnNum].turnLine.Count;

            for (int p = 0; p < ptCount; p++)
            {
                if ((((boxB.easting - boxA.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxA.northing))
                     - ((boxB.northing - boxA.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxA.easting))) < 0)
                {
                    continue;
                }

                if ((((boxD.easting - boxC.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxC.northing))
                     - ((boxD.northing - boxC.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxC.easting))) < 0)
                {
                    continue;
                }

                if ((((boxC.easting - boxB.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxB.northing))
                     - ((boxC.northing - boxB.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxB.easting))) < 0)
                {
                    continue;
                }

                if ((((boxA.easting - boxD.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxD.northing))
                     - ((boxA.northing - boxD.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxD.easting))) < 0)
                {
                    continue;
                }

                //it's in the box, so add to list
                inBox.easting  = bndList[closestTurnNum].turnLine[p].easting;
                inBox.northing = bndList[closestTurnNum].turnLine[p].northing;
                inBox.heading  = bndList[closestTurnNum].turnLine[p].heading;
                inBox.index    = closestTurnNum;

                //which turn/headland is it from
                turnClosestList.Add(inBox);
            }

            if (turnClosestList.Count == 0)
            {
                if (isYouTurnRight) //its actually left
                {
                    scanWidthL = -(mf.tool.toolWidth * 0.5);
                    scanWidthR = 0;
                }
                else
                {
                    scanWidthL = 0;
                    scanWidthR = (mf.tool.toolWidth * 0.5);
                }

                //isYouTurnRight actuall means turning left - Painful, but it switches later
                boxA.easting  = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthL);
                boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthL);

                boxB.easting  = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthR);
                boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthR);

                boxC.easting  = boxB.easting + (Math.Sin(headAB) * boxLength);
                boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength);

                boxD.easting  = boxA.easting + (Math.Sin(headAB) * boxLength);
                boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength);

                //determine if point is inside bounding box of the 1 turn chosen above
                turnClosestList.Clear();

                ptCount = bndList[closestTurnNum].turnLine.Count;

                for (int p = 0; p < ptCount; p++)
                {
                    if ((((boxB.easting - boxA.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxA.northing))
                         - ((boxB.northing - boxA.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxA.easting))) < 0)
                    {
                        continue;
                    }

                    if ((((boxD.easting - boxC.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxC.northing))
                         - ((boxD.northing - boxC.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxC.easting))) < 0)
                    {
                        continue;
                    }

                    if ((((boxC.easting - boxB.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxB.northing))
                         - ((boxC.northing - boxB.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxB.easting))) < 0)
                    {
                        continue;
                    }

                    if ((((boxA.easting - boxD.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxD.northing))
                         - ((boxA.northing - boxD.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxD.easting))) < 0)
                    {
                        continue;
                    }

                    //it's in the box, so add to list
                    inBox.easting  = bndList[closestTurnNum].turnLine[p].easting;
                    inBox.northing = bndList[closestTurnNum].turnLine[p].northing;
                    inBox.heading  = bndList[closestTurnNum].turnLine[p].heading;
                    inBox.index    = closestTurnNum;

                    //which turn/headland is it from
                    turnClosestList.Add(inBox);
                }
            }
            //which of the points is closest
            //closestTurnPt.easting = -20000; closestTurnPt.northing = -20000;
            ptCount = turnClosestList.Count;
            if (ptCount != 0)
            {
                double totalDist = 0.75 * Math.Sqrt(((fromPt.easting - rayPt.easting) * (fromPt.easting - rayPt.easting))
                                                    + ((fromPt.northing - rayPt.northing) * (fromPt.northing - rayPt.northing)));

                //determine closest point
                double minDistance = 9999999;
                for (int i = 0; i < ptCount; i++)
                {
                    double dist = Math.Sqrt(((fromPt.easting - turnClosestList[i].easting) * (fromPt.easting - turnClosestList[i].easting))
                                            + ((fromPt.northing - turnClosestList[i].northing) * (fromPt.northing - turnClosestList[i].northing)));

                    //double distRay = ((rayPt.easting - turnClosestList[i].easting) * (rayPt.easting - turnClosestList[i].easting))
                    //                + ((rayPt.northing - turnClosestList[i].northing) * (rayPt.northing - turnClosestList[i].northing));

                    if (minDistance >= dist && dist > totalDist)
                    {
                        minDistance            = dist;
                        closestTurnPt.easting  = turnClosestList[i].easting;
                        closestTurnPt.northing = turnClosestList[i].northing;
                        closestTurnPt.heading  = turnClosestList[i].heading;
                    }
                }
                if (closestTurnPt.heading < 0)
                {
                    closestTurnPt.heading += glm.twoPI;
                }
            }
        }
예제 #16
0
파일: CGLM.cs 프로젝트: BenjaminK87/v4202
 public static double Distance(vec3 first, double east, double north)
 {
     return(Math.Sqrt(
                Math.Pow(first.easting - east, 2)
                + Math.Pow(first.northing - north, 2)));
 }
예제 #17
0
        public void FindClosestHeadlandPoint(vec3 fromPt, double headAB, int bndNum)
        {
            //heading is based on ABLine, average Curve, and going same direction as AB or not
            //Draw a bounding box to determine if points are in it

            if (mf.yt.isYouTurnTriggered || mf.yt.isEnteringDriveThru || mf.yt.isExitingDriveThru)
            {
                boxA.easting  = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * -scanWidth); //subtract if positive
                boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * -scanWidth);

                boxB.easting  = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidth);
                boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidth);

                boxC.easting  = boxB.easting + (Math.Sin(headAB) * boxLength);
                boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength);

                boxD.easting  = boxA.easting + (Math.Sin(headAB) * boxLength);
                boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength);

                boxA.easting  -= (Math.Sin(headAB) * boxLength);
                boxA.northing -= (Math.Cos(headAB) * boxLength);

                boxB.easting  -= (Math.Sin(headAB) * boxLength);
                boxB.northing -= (Math.Cos(headAB) * boxLength);
            }
            else
            {
                boxA.easting  = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * -scanWidth);
                boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * -scanWidth);

                boxB.easting  = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidth);
                boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidth);

                boxC.easting  = boxB.easting + (Math.Sin(headAB) * 2000.0);
                boxC.northing = boxB.northing + (Math.Cos(headAB) * 2000.0);

                boxD.easting  = boxA.easting + (Math.Sin(headAB) * 2000.0);
                boxD.northing = boxA.northing + (Math.Cos(headAB) * 2000.0);
            }

            //determine if point is inside bounding box
            int ptCount;

            hdClosestList.Clear();
            vec3 inBox;

            ptCount = mf.hlArr[bndNum].hlLine.Count;
            for (int p = 0; p < ptCount; p++)
            {
                if ((((boxB.easting - boxA.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxA.northing))
                     - ((boxB.northing - boxA.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxA.easting))) < 0)
                {
                    continue;
                }

                if ((((boxD.easting - boxC.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxC.northing))
                     - ((boxD.northing - boxC.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxC.easting))) < 0)
                {
                    continue;
                }

                if ((((boxC.easting - boxB.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxB.northing))
                     - ((boxC.northing - boxB.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxB.easting))) < 0)
                {
                    continue;
                }

                if ((((boxA.easting - boxD.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxD.northing))
                     - ((boxA.northing - boxD.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxD.easting))) < 0)
                {
                    continue;
                }

                //it's in the box, so add to list
                inBox = mf.hlArr[bndNum].hlLine[p];

                //which boundary/headland is it from
                hdClosestList.Add(inBox);
            }

            //which of the points is closest
            closestHeadlandPt.easting = -20000; closestHeadlandPt.northing = -20000;
            ptCount = hdClosestList.Count;
            if (ptCount != 0)
            {
                //determine closest point
                double minDistance = 9999999;
                for (int i = 0; i < ptCount; i++)
                {
                    double dist = ((fromPt.easting - hdClosestList[i].easting) * (fromPt.easting - hdClosestList[i].easting))
                                  + ((fromPt.northing - hdClosestList[i].northing) * (fromPt.northing - hdClosestList[i].northing));
                    if (minDistance >= dist)
                    {
                        minDistance = dist;
                        closestHeadlandPt.easting  = hdClosestList[i].easting;
                        closestHeadlandPt.northing = hdClosestList[i].northing;
                        closestHeadlandPt.heading  = hdClosestList[i].heading;
                    }
                }
                if (closestHeadlandPt.heading < 0)
                {
                    closestHeadlandPt.heading += glm.twoPI;
                }
            }
        }
예제 #18
0
파일: CGLM.cs 프로젝트: BenjaminK87/v4202
 public static double DistanceSquared(vec3 first, vec3 second)
 {
     return(
         Math.Pow(first.easting - second.easting, 2)
         + Math.Pow(first.northing - second.northing, 2));
 }
예제 #19
0
        public void BuildHeadland(double widthSet, int inBndNum)
        {
            //count the points from the boundary
            int ptCount = mf.bndArr[inBndNum].bndLine.Count;

            //first find out which side is inside the boundary
            oneSide = glm.PIBy2;
            vec3 point = new vec3(mf.bndArr[inBndNum].bndLine[3].easting - (Math.Sin(oneSide + mf.bndArr[inBndNum].bndLine[3].heading) * 2.0),
                                  mf.bndArr[inBndNum].bndLine[3].northing - (Math.Cos(oneSide + mf.bndArr[inBndNum].bndLine[3].heading) * 2.0), 0.0);

            if (inBndNum == 0 && !mf.bndArr[inBndNum].IsPointInsideBoundary(point))
            {
                oneSide *= -1.0;
            }
            if (inBndNum != 0 && mf.bndArr[inBndNum].IsPointInsideBoundary(point))
            {
                oneSide *= -1.0;
            }

            //clear the headland point list
            hlLine.Clear();

            //determine how wide a headland space
            double totalHeadWidth = (mf.vehicle.toolWidth - mf.vehicle.toolOverlap) * widthSet;

            for (int i = 0; i < ptCount; i++)
            {
                //calculate the point inside the boundary
                point.easting  = mf.bndArr[inBndNum].bndLine[i].easting - (Math.Sin(oneSide + mf.bndArr[inBndNum].bndLine[i].heading) * totalHeadWidth);
                point.northing = mf.bndArr[inBndNum].bndLine[i].northing - (Math.Cos(oneSide + mf.bndArr[inBndNum].bndLine[i].heading) * totalHeadWidth);
                point.heading  = mf.bndArr[inBndNum].bndLine[i].heading;

                //only add if inside actual field boundary, outer boundary is 0 and opposite
                if (inBndNum == 0 && mf.bndArr[inBndNum].IsPointInsideBoundary(point))
                {
                    hlLine.Add(point);
                }
                if (inBndNum != 0 && !mf.bndArr[inBndNum].IsPointInsideBoundary(point))
                {
                    hlLine.Add(point);
                }
            }

            int headCount = hlLine.Count;

            //remove the points too close to boundary
            for (int i = 0; i < ptCount; i++)
            {
                for (int j = 0; j < headCount; j++)
                {
                    //make sure distance between headland and boundary is not less then width
                    distance = glm.Distance(mf.bndArr[inBndNum].bndLine[i], hlLine[j]);
                    if (distance < (totalHeadWidth * 0.98))
                    {
                        hlLine.RemoveAt(j);
                        headCount = hlLine.Count;
                        j         = 0;
                    }
                }
            }

            //fix the gaps and overlaps
            FixHeadlandList();

            //pre calculate all the constants and multiples
            PreCalcHeadlandLines();
        }
예제 #20
0
        public void FixTurnLine(double totalHeadWidth, List <vec3> curBnd, double spacing)
        {
            //count the points from the boundary
            int    lineCount = turnLine.Count;
            double distance  = 0;

            //int headCount = mf.bndArr[inTurnNum].bndLine.Count;
            int bndCount = curBnd.Count;

            //remove the points too close to boundary
            for (int i = 0; i < bndCount; i++)
            {
                for (int j = 0; j < lineCount; j++)
                {
                    //make sure distance between headland and boundary is not less then width
                    distance = glm.Distance(curBnd[i], turnLine[j]);
                    if (distance < (totalHeadWidth * 0.99))
                    {
                        turnLine.RemoveAt(j);
                        lineCount = turnLine.Count;
                        j         = -1;
                    }
                }
            }

            //make sure distance isn't too small between points on turnLine
            bndCount = turnLine.Count;

            //double spacing = mf.tool.toolWidth * 0.25;
            for (int i = 0; i < bndCount - 1; i++)
            {
                distance = glm.Distance(turnLine[i], turnLine[i + 1]);
                if (distance < spacing)
                {
                    turnLine.RemoveAt(i + 1);
                    bndCount = turnLine.Count;
                    i--;
                }
            }

            //make sure distance isn't too big between points on Turn
            bndCount = turnLine.Count;
            for (int i = 0; i < bndCount; i++)
            {
                int j = i + 1;
                if (j == bndCount)
                {
                    j = 0;
                }
                distance = glm.Distance(turnLine[i], turnLine[j]);
                if (distance > (spacing * 1.25))
                {
                    vec3 pointB = new vec3((turnLine[i].easting + turnLine[j].easting) / 2.0, (turnLine[i].northing + turnLine[j].northing) / 2.0, turnLine[i].heading);

                    turnLine.Insert(j, pointB);
                    bndCount = turnLine.Count;
                    i--;
                }
            }

            //make sure headings are correct for calculated points
            if (turnLine.Count > 0)
            {
                CalculateTurnHeadings();
            }

            //int cnt = turnLine.Count;
            //vec3[] arr = new vec3[cnt];
            //turnLine.CopyTo(arr);
            //turnLine.Clear();

            //double delta = 0;
            //for (int i = 0; i < arr.Length; i++)
            //{
            //    if (i == 0)
            //    {
            //        turnLine.Add(new vec3(arr[i].easting, arr[i].northing, arr[i].heading));
            //        continue;
            //    }
            //    delta += (arr[i - 1].heading - arr[i].heading);

            //    if (Math.Abs(delta) > 0.1)
            //    {
            //        vec3 pt = new vec3(arr[i].easting, arr[i].northing, arr[i].heading);

            //        turnLine.Add(pt);
            //        delta = 0;
            //    }
            //}
        }
예제 #21
0
        public void GetCurrentABLine(vec3 pivot)
        {
            //move the ABLine over based on the overlap amount set in vehicle
            double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;

            //x2-x1
            double dx = refABLineP2.easting - refABLineP1.easting;
            //z2-z1
            double dy = refABLineP2.northing - refABLineP1.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));

            //sign of distance determines which side of line we are on
            if (distanceFromRefLine > 0)
            {
                refLineSide = 1;
            }
            else
            {
                refLineSide = -1;
            }

            //absolute the distance
            distanceFromRefLine = Math.Abs(distanceFromRefLine);

            //Which ABLine is the vehicle on, negative is left and positive is right side
            howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);

            //generate that pass number as signed integer
            passNumber = Convert.ToInt32(refLineSide * howManyPathsAway);

            //calculate the new point that is number of implement widths over
            double toolOffset = mf.vehicle.toolOffset;
            vec2   point1;

            //depending which way you are going, the offset can be either side
            if (isABSameAsVehicleHeading)
            {
                point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.easting,
                                  (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.northing);
            }
            else
            {
                point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.easting,
                                  (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.northing);
            }

            //create the new line extent points for current ABLine based on original heading of AB line
            currentABLineP1.easting  = point1.easting - (Math.Sin(abHeading) * 40000.0);
            currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 40000.0);

            currentABLineP2.easting  = point1.easting + (Math.Sin(abHeading) * 40000.0);
            currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 40000.0);

            //get the distance from currently active AB line
            //x2-x1
            dx = currentABLineP2.easting - currentABLineP1.easting;
            //z2-z1
            dy = currentABLineP2.northing - currentABLineP1.northing;

            //save a copy of dx,dy in youTurn
            mf.yt.dxAB = dx; mf.yt.dyAB = dy;

            //how far from current AB Line is fix
            distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
                                                                                       * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
                                      / Math.Sqrt((dy * dy) + (dx * dx));

            //are we on the right side or not
            isOnRightSideCurrentLine = distanceFromCurrentLine > 0;

            //absolute the distance
            distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);

            //update base on autosteer settings and distance from line
            double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLine);

            mf.lookaheadActual = goalPointDistance;

            //Subtract the two headings, if > 1.57 its going the opposite heading as refAB
            abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
            if (abFixHeadingDelta >= Math.PI)
            {
                abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
            }

            // ** Pure pursuit ** - calc point on ABLine closest to current position
            double U = (((pivot.easting - currentABLineP1.easting) * dx)
                        + ((pivot.northing - currentABLineP1.northing) * dy))
                       / ((dx * dx) + (dy * dy));

            //point on AB line closest to pivot axle point
            rEastAB  = currentABLineP1.easting + (U * dx);
            rNorthAB = currentABLineP1.northing + (U * dy);

            if (abFixHeadingDelta >= glm.PIBy2)
            {
                isABSameAsVehicleHeading = false;
                goalPointAB.easting      = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
                goalPointAB.northing     = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
            }
            else
            {
                isABSameAsVehicleHeading = true;
                goalPointAB.easting      = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
                goalPointAB.northing     = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
            }

            //calc "D" the distance from pivot axle to lookahead point
            double goalPointDistanceDSquared
                = glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);

            //calculate the the new x in local coordinates and steering angle degrees based on wheelbase
            double localHeading = glm.twoPI - mf.fixHeading;

            ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                           + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));

            steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                        + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
                                                   / goalPointDistanceDSquared));
            if (steerAngleAB < -mf.vehicle.maxSteerAngle)
            {
                steerAngleAB = -mf.vehicle.maxSteerAngle;
            }
            if (steerAngleAB > mf.vehicle.maxSteerAngle)
            {
                steerAngleAB = mf.vehicle.maxSteerAngle;
            }

            //limit circle size for display purpose
            if (ppRadiusAB < -500)
            {
                ppRadiusAB = -500;
            }
            if (ppRadiusAB > 500)
            {
                ppRadiusAB = 500;
            }

            radiusPointAB.easting  = pivot.easting + (ppRadiusAB * Math.Cos(localHeading));
            radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading));

            //Convert to millimeters
            distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);

            //angular velocity in rads/sec  = 2PI * m/sec * radians/meters
            angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase;

            //clamp the steering angle to not exceed safe angular velocity
            if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
            {
                steerAngleAB = glm.toDegrees(steerAngleAB > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity)
                                                                           / (glm.twoPI * mf.pn.speed * 0.277777)))
                    : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
            }

            //distance is negative if on left, positive if on right
            if (isABSameAsVehicleHeading)
            {
                if (!isOnRightSideCurrentLine)
                {
                    distanceFromCurrentLine *= -1.0;
                }
            }

            //opposite way so right is left
            else
            {
                if (isOnRightSideCurrentLine)
                {
                    distanceFromCurrentLine *= -1.0;
                }
            }

            mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
            mf.guidanceLineSteerAngle  = (Int16)(steerAngleAB * 100);

            if (mf.yt.isYouTurnTriggered)
            {
                //do the pure pursuit from youTurn
                mf.yt.DistanceFromYouTurnLine();

                mf.seq.DoSequenceEvent();

                //now substitute what it thinks are AB line values with auto turn values
                steerAngleAB            = mf.yt.steerAngleYT;
                distanceFromCurrentLine = mf.yt.distanceFromCurrentLine;

                goalPointAB            = mf.yt.goalPointYT;
                radiusPointAB.easting  = mf.yt.radiusPointYT.easting;
                radiusPointAB.northing = mf.yt.radiusPointYT.northing;
                ppRadiusAB             = mf.yt.ppRadiusYT;
            }
        }
예제 #22
0
 //Add current position to stripList
 public void AddPoint(vec3 pivot)
 {
     vec3 point = new vec3(pivot.easting, pivot.northing, pivot.heading);
     ptList.Add(point);
 }
예제 #23
0
        private void btnMakeBoundaryCurve_Click(object sender, EventArgs e)
        {            //count the points from the boundary
            int ptCount = mf.bnd.bndList[0].fenceLine.Count;

            mf.curve.refList?.Clear();

            //outside point
            vec3 pt3 = new vec3();

            double moveDist = (double)nudDistance.Value * mf.inchOrCm2m;
            double distSq   = (moveDist) * (moveDist) * 0.999;

            //make the boundary tram outer array
            for (int i = 0; i < ptCount; i++)
            {
                //calculate the point inside the boundary
                pt3.easting = mf.bnd.bndList[0].fenceLine[i].easting -
                              (Math.Sin(glm.PIBy2 + mf.bnd.bndList[0].fenceLine[i].heading) * (moveDist));

                pt3.northing = mf.bnd.bndList[0].fenceLine[i].northing -
                               (Math.Cos(glm.PIBy2 + mf.bnd.bndList[0].fenceLine[i].heading) * (moveDist));

                pt3.heading = mf.bnd.bndList[0].fenceLine[i].heading;

                bool Add = true;

                for (int j = 0; j < ptCount; j++)
                {
                    double check = glm.DistanceSquared(pt3.northing, pt3.easting,
                                                       mf.bnd.bndList[0].fenceLine[j].northing, mf.bnd.bndList[0].fenceLine[j].easting);
                    if (check < distSq)
                    {
                        Add = false;
                        break;
                    }
                }

                if (Add)
                {
                    if (mf.curve.refList.Count > 0)
                    {
                        double dist = ((pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting) * (pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting))
                                      + ((pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing) * (pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing));
                        if (dist > 1)
                        {
                            mf.curve.refList.Add(pt3);
                        }
                    }
                    else
                    {
                        mf.curve.refList.Add(pt3);
                    }
                }
            }

            btnCancelTouch.Enabled = false;

            int cnt = mf.curve.refList.Count;

            if (cnt > 3)
            {
                //make sure distance isn't too big between points on Turn
                for (int i = 0; i < cnt - 1; i++)
                {
                    int j = i + 1;
                    //if (j == cnt) j = 0;
                    double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]);
                    if (distance > 1.2)
                    {
                        vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0,
                                               (mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0,
                                               mf.curve.refList[i].heading);

                        mf.curve.refList.Insert(j, pointB);
                        cnt = mf.curve.refList.Count;
                        i   = -1;
                    }
                }
                //who knows which way it actually goes
                mf.curve.CalculateTurnHeadings();

                mf.curve.isCurveSet = true;

                mf.curve.aveLineHeading = 0;

                //mf.curve.SmoothAB(4);
                //mf.curve.CalculateTurnHeadings();

                mf.curve.isCurveSet = true;

                //double offset = ((double)nudDistance.Value) / 200.0;

                mf.curve.curveArr.Add(new CCurveLines());
                mf.curve.numCurveLines        = mf.curve.curveArr.Count;
                mf.curve.numCurveLineSelected = mf.curve.numCurveLines;

                //array number is 1 less since it starts at zero
                int idx = mf.curve.curveArr.Count - 1;

                //create a name
                mf.curve.curveArr[idx].Name = "Boundary Curve";

                mf.curve.curveArr[idx].aveHeading = mf.curve.aveLineHeading;

                //write out the Curve Points
                foreach (vec3 item in mf.curve.refList)
                {
                    mf.curve.curveArr[idx].curvePts.Add(item);
                }

                mf.FileSaveCurveLines();

                //update the arrays
                btnMakeABLine.Enabled = false;
                btnMakeCurve.Enabled  = false;
                isMakingCurve         = false;
                isMakingAB            = false;
                start = 99999; end = 99999;

                FixLabelsCurve();
            }
            else
            {
                mf.curve.isCurveSet = false;
                mf.curve.refList?.Clear();
            }
            btnExit.Focus();
        }
예제 #24
0
        //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();

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

            //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;
            if (isSameWay)
            {
                start = pt - 35; if (start < 0) start = 0;
                stop = pt + 40; if (stop > ptCount) stop = ptCount + 1;
            }
            else
            {
                start = pt - 40; if (start < 0) start = 0;
                stop = pt + 35; 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);

                //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;
            }
        }
예제 #25
0
        private void btnLoadBoundaryFromGE_Click(object sender, EventArgs e)
        {
            if (sender is Button button)
            {
                Selectedreset = true;
                btnLoadBoundaryFromGE.Enabled = false;
                btnDelete.Enabled             = false;

                string fileAndDirectory;
                {
                    //create the dialog instance
                    OpenFileDialog ofd = new OpenFileDialog
                    {
                        //set the filter to text KML only
                        Filter = "KML files (*.KML)|*.KML",

                        //the initial directory, fields, for the open dialog
                        InitialDirectory = mf.fieldsDirectory + mf.currentFieldDirectory
                    };

                    //was a file selected
                    if (ofd.ShowDialog() == DialogResult.Cancel)
                    {
                        return;
                    }
                    else
                    {
                        fileAndDirectory = ofd.FileName;
                    }
                }

                //start to read the file
                string line        = null;
                string coordinates = null;
                int    startIndex;
                int    i = 0;

                using (System.IO.StreamReader reader = new System.IO.StreamReader(fileAndDirectory))
                {
                    if (button.Name == "btnLoadMultiBoundaryFromGE")
                    {
                        ResetAllBoundary();
                    }
                    else
                    {
                        i = mf.bnd.boundarySelected;
                    }

                    try
                    {
                        while (!reader.EndOfStream)
                        {
                            line = reader.ReadLine();

                            startIndex = line.IndexOf("<coordinates>");

                            if (startIndex != -1)
                            {
                                while (true)
                                {
                                    int endIndex = line.IndexOf("</coordinates>");

                                    if (endIndex == -1)
                                    {
                                        //just add the line
                                        if (startIndex == -1)
                                        {
                                            coordinates += line.Substring(0);
                                        }
                                        else
                                        {
                                            coordinates += line.Substring(startIndex + 13);
                                        }
                                    }
                                    else
                                    {
                                        if (startIndex == -1)
                                        {
                                            coordinates += line.Substring(0, endIndex);
                                        }
                                        else
                                        {
                                            coordinates += line.Substring(startIndex + 13, endIndex - (startIndex + 13));
                                        }
                                        break;
                                    }
                                    line       = reader.ReadLine();
                                    line       = line.Trim();
                                    startIndex = -1;
                                }

                                line = coordinates;
                                char[]   delimiterChars = { ' ', '\t', '\r', '\n' };
                                string[] numberSets     = line.Split(delimiterChars);

                                //at least 3 points
                                if (numberSets.Length > 2)
                                {
                                    mf.bnd.bndArr.Add(new CBoundaryLines());
                                    mf.turn.turnArr.Add(new CTurnLines());
                                    mf.gf.geoFenceArr.Add(new CGeoFenceLines());

                                    foreach (var item in numberSets)
                                    {
                                        string[] fix = item.Split(',');
                                        double.TryParse(fix[0], NumberStyles.Float, CultureInfo.InvariantCulture, out lonK);
                                        double.TryParse(fix[1], NumberStyles.Float, CultureInfo.InvariantCulture, out latK);
                                        double[] xy = mf.pn.DecDeg2UTM(latK, lonK);

                                        //match new fix to current position
                                        easting  = xy[0] - mf.pn.utmEast;
                                        northing = xy[1] - mf.pn.utmNorth;

                                        double east = easting;
                                        double nort = northing;

                                        //fix the azimuth error
                                        easting  = (Math.Cos(-mf.pn.convergenceAngle) * east) - (Math.Sin(-mf.pn.convergenceAngle) * nort);
                                        northing = (Math.Sin(-mf.pn.convergenceAngle) * east) + (Math.Cos(-mf.pn.convergenceAngle) * nort);

                                        //add the point to boundary
                                        vec3 bndPt = new vec3(easting, northing, 0);
                                        mf.bnd.bndArr[i].bndLine.Add(bndPt);
                                    }

                                    //fix the points if there are gaps bigger then
                                    mf.bnd.bndArr[i].CalculateBoundaryHeadings();
                                    mf.bnd.bndArr[i].PreCalcBoundaryLines();
                                    mf.bnd.bndArr[i].FixBoundaryLine(i, mf.tool.toolWidth);

                                    //boundary area, pre calcs etc
                                    mf.bnd.bndArr[i].CalculateBoundaryArea();
                                    mf.bnd.bndArr[i].PreCalcBoundaryLines();
                                    mf.bnd.bndArr[i].isSet = true;
                                    //if (i == 0) mf.bnd.bndArr[i].isOwnField = true;
                                    //else mf.bnd.bndArr[i].isOwnField = false;
                                    coordinates = "";
                                    i++;
                                }
                                else
                                {
                                    mf.TimedMessageBox(2000, gStr.gsErrorreadingKML, gStr.gsChooseBuildDifferentone);
                                }
                                if (button.Name == "btnLoadBoundaryFromGE")
                                {
                                    break;
                                }
                            }
                        }
                        mf.FileSaveBoundary();
                        mf.fd.UpdateFieldBoundaryGUIAreas();
                        UpdateChart();
                    }
                    catch (Exception)
                    {
                        return;
                    }
                }
            }
        }
예제 #26
0
        //determine distance from contour guidance line
        public void DistanceFromContourLine(vec3 pivot)
        {
            isValid = false;
            double minDistA = 1000000, minDistB = 1000000;
            int ptCount = ctList.Count;
            //distanceFromCurrentLine = 9999;
            if (ptCount > 0)
            {
                //find the closest 2 points to current fix
                for (int t = 0; t < ptCount; t++)
                {
                    double dist = ((pivot.easting - ctList[t].easting) * (pivot.easting - ctList[t].easting))
                                    + ((pivot.northing - ctList[t].northing) * (pivot.northing - ctList[t].northing));
                    if (dist < minDistA)
                    {
                        minDistB = minDistA;
                        B = A;
                        minDistA = dist;
                        A = t;
                    }
                    else if (dist < minDistB)
                    {
                        minDistB = dist;
                        B = t;
                    }
                }

                //just need to make sure the points continue ascending or heading switches all over the place
                if (A > B) { C = A; A = B; B = C; }

                //get the distance from currently active AB line
                //x2-x1
                double dx = ctList[B].easting - ctList[A].easting;
                //z2-z1
                double dy = ctList[B].northing - ctList[A].northing;

                if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dy) < Double.Epsilon) return;

                //abHeading = Math.Atan2(dz, dx);
                abHeading = ctList[A].heading;

                //how far from current AB Line is fix
                distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (ctList[B].easting
                            * ctList[A].northing) - (ctList[B].northing * ctList[A].easting))
                                / Math.Sqrt((dy * dy) + (dx * dx));

                //are we on the right side or not
                isOnRightSideCurrentLine = distanceFromCurrentLine > 0;

                //absolute the distance
                distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);

                // ** Pure pursuit ** - calc point on ABLine closest to current position
                double U = (((pivot.easting - ctList[A].easting) * (dx)) + ((pivot.northing - ctList[A].northing) * (dy)))
                            / ((dx * dx) + (dy * dy));

                rEastCT = ctList[A].easting + (U * (dx));
                rNorthCT = ctList[A].northing + (U * (dy));

                ////determine if the point is between 2 points initially determined
                //double minx, maxx, miny, maxy;

                //minx = Math.Min(ctList[A].northing, ctList[B].northing);
                //maxx = Math.Max(ctList[A].northing, ctList[B].northing);

                //miny = Math.Min(ctList[A].easting, ctList[B].easting);
                //maxy = Math.Max(ctList[A].easting, ctList[B].easting);

                //isValid = (rNorthCT >= minx && rNorthCT <= maxx) && (rEastCT >= miny && rEastCT <= maxy);
                //if (!isValid)
                //{
                //    //invalid distance so tell AS module
                //    distanceFromCurrentLine = 32000;
                //    mf.guidanceLineDistanceOff = 32000;
                //    return;
                //}

                //Subtract the two headings, if > 1.57 its going the opposite heading as refAB
                abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
                if (abFixHeadingDelta >= Math.PI) abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);

                //used for accumulating distance to find goal point
                double distSoFar;

                //how far should goal point be away  - speed * seconds * kmph -> m/s + min value
                double goalPointDistance = mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.27777777;

                //minimum of 4.0 meters look ahead
                if (goalPointDistance < 4.0) goalPointDistance = 4.0;

                // used for calculating the length squared of next segment.
                double tempDist = 0.0;

                if (abFixHeadingDelta >= glm.PIBy2)
                {
                    //counting down
                    isABSameAsFixHeading = false;
                    distSoFar = glm.Distance(ctList[A], rEastCT, rNorthCT);
                    //Is this segment long enough to contain the full lookahead distance?
                    if (distSoFar > goalPointDistance)
                    {
                        //treat current segment like an AB Line
                        goalPointCT.easting = rEastCT - (Math.Sin(ctList[A].heading) * goalPointDistance);
                        goalPointCT.northing = rNorthCT - (Math.Cos(ctList[A].heading) * goalPointDistance);
                    }

                    //multiple segments required
                    else
                    {
                        //cycle thru segments and keep adding lengths. check if start and break if so.
                        while (A > 0)
                        {
                            B--; A--;
                            tempDist = glm.Distance(ctList[B], ctList[A]);

                            //will we go too far?
                            if ((tempDist + distSoFar) > goalPointDistance)
                            {
                                //A++; B++;
                                break; //tempDist contains the full length of next segment
                            }
                            else
                            {
                                distSoFar += tempDist;
                            }
                        }

                        double t = (goalPointDistance - distSoFar); // the remainder to yet travel
                        t /= tempDist;

                        goalPointCT.easting = (((1 - t) * ctList[B].easting) + (t * ctList[A].easting));
                        goalPointCT.northing = (((1 - t) * ctList[B].northing) + (t * ctList[A].northing));
                    }
                }
                else
                {
                    //counting up
                    isABSameAsFixHeading = true;
                    distSoFar = glm.Distance(ctList[B], rEastCT, rNorthCT);

                    //Is this segment long enough to contain the full lookahead distance?
                    if (distSoFar > goalPointDistance)
                    {
                        //treat current segment like an AB Line
                        goalPointCT.easting = rEastCT + (Math.Sin(ctList[A].heading) * goalPointDistance);
                        goalPointCT.northing = rNorthCT + (Math.Cos(ctList[A].heading) * goalPointDistance);
                    }

                    //multiple segments required
                    else
                    {
                        //cycle thru segments and keep adding lengths. check if end and break if so.
                        // ReSharper disable once LoopVariableIsNeverChangedInsideLoop
                        while (B < ptCount - 1)
                        {
                            B++; A++;
                            tempDist = glm.Distance(ctList[B], ctList[A]);

                            //will we go too far?
                            if ((tempDist + distSoFar) > goalPointDistance)
                            {
                                //A--; B--;
                                break; //tempDist contains the full length of next segment
                            }

                            distSoFar += tempDist;
                        }

                        //xt = (((1 - t) * x0 + t * x1)
                        //yt = ((1 - t) * y0 + t * y1))

                        double t = (goalPointDistance - distSoFar); // the remainder to yet travel
                        t /= tempDist;

                        goalPointCT.easting = (((1 - t) * ctList[A].easting) + (t * ctList[B].easting));
                        goalPointCT.northing = (((1 - t) * ctList[A].northing) + (t * ctList[B].northing));
                    }
                }

                //calc "D" the distance from pivot axle to lookahead point
                double goalPointDistanceSquared = glm.DistanceSquared(goalPointCT.northing, goalPointCT.easting, pivot.northing, pivot.easting);

                //calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
                double localHeading = glm.twoPI - mf.fixHeading;
                ppRadiusCT = goalPointDistanceSquared / (2 * (((goalPointCT.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCT.northing - pivot.northing) * Math.Sin(localHeading))));

                steerAngleCT = glm.toDegrees(Math.Atan(2 * (((goalPointCT.easting - pivot.easting) * Math.Cos(localHeading))
                    + ((goalPointCT.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));

                if (steerAngleCT < -mf.vehicle.maxSteerAngle) steerAngleCT = -mf.vehicle.maxSteerAngle;
                if (steerAngleCT > mf.vehicle.maxSteerAngle) steerAngleCT = mf.vehicle.maxSteerAngle;

                if (ppRadiusCT < -500) ppRadiusCT = -500;
                if (ppRadiusCT > 500) ppRadiusCT = 500;

                radiusPointCT.easting = pivot.easting + (ppRadiusCT * Math.Cos(localHeading));
                radiusPointCT.northing = pivot.northing + (ppRadiusCT * Math.Sin(localHeading));

                //angular velocity in rads/sec  = 2PI * m/sec * radians/meters
                double angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleCT))) / mf.vehicle.wheelbase;

                //clamp the steering angle to not exceed safe angular velocity
                if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
                {
                    steerAngleCT = glm.toDegrees(steerAngleCT > 0 ?
                            (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))
                        : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
                }
                //Convert to centimeters
                distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);

                //distance is negative if on left, positive if on right
                //if you're going the opposite direction left is right and right is left
                //double temp;
                if (isABSameAsFixHeading)
                {
                    if (!isOnRightSideCurrentLine) distanceFromCurrentLine *= -1.0;
                }

                //opposite way so right is left
                else if (isOnRightSideCurrentLine)
                {
                    distanceFromCurrentLine *= -1.0;
                }

                //fill in the autosteer variables
                mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
                mf.guidanceLineSteerAngle = (Int16)(steerAngleCT * 100);
            }
            else
            {
                //invalid distance so tell AS module
                distanceFromCurrentLine = 32000;
                mf.guidanceLineDistanceOff = 32000;
            }
        }
예제 #27
0
        public void FixBoundaryLine(int bndNum )
        {
            double spacing;
            //boundary point spacing based on eq width
            //close if less then 30 ha, 60ha, more then 60
            if (area < 200000) spacing = 1.1;
            else if (area < 400000) spacing = 2.2;
            else spacing = 3.3;

            if (bndNum > 0) spacing *= 0.5;

            int bndCount = bndLine.Count;
            double distance;

            //make sure distance isn't too big between points on boundary
            for (int i = 0; i < bndCount; i++)
            {
                int j = i + 1;

                if (j == bndCount) j = 0;
                 distance = glm.Distance(bndLine[i], bndLine[j]);
                if (distance > spacing*1.5)
                {
                    vec3 pointB = new vec3((bndLine[i].easting + bndLine[j].easting) / 2.0,
                        (bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading);

                    bndLine.Insert(j, pointB);
                    bndCount = bndLine.Count;
                    i--;
                }
            }

            //make sure distance isn't too big between points on boundary
            bndCount = bndLine.Count;

            for (int i = 0; i < bndCount; i++)
            {
                int j = i + 1;

                if (j == bndCount) j = 0;
                distance = glm.Distance(bndLine[i], bndLine[j]);
                if (distance > spacing*1.6)
                {
                    vec3 pointB = new vec3((bndLine[i].easting + bndLine[j].easting) / 2.0,
                        (bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading);

                    bndLine.Insert(j, pointB);
                    bndCount = bndLine.Count;
                    i--;
                }
            }

            //make sure distance isn't too small between points on headland
            spacing *= 1.2;
            bndCount = bndLine.Count;
            for (int i = 0; i < bndCount - 1; i++)
            {
                distance = glm.Distance(bndLine[i], bndLine[i + 1]);
                if (distance < spacing)
                {
                    bndLine.RemoveAt(i + 1);
                    bndCount = bndLine.Count;
                    i--;
                }
            }

            //make sure headings are correct for calculated points
            CalculateBoundaryHeadings();

            double delta = 0;
            bndLineEar?.Clear();

            for (int i = 0; i < bndLine.Count; i++)
            {
                if (i == 0)
                {
                    bndLineEar.Add(new vec2(bndLine[i].easting, bndLine[i].northing));
                    continue;
                }
                delta += (bndLine[i - 1].heading - bndLine[i].heading);
                if (Math.Abs(delta) > 0.01)
                {
                    bndLineEar.Add(new vec2(bndLine[i].easting, bndLine[i].northing));
                    delta = 0;
                }
            }
        }
예제 #28
0
        public void GetCurrentABLine(vec3 pivot, vec3 steer)
        {
            double dx, dy;

            //build new current ref line if required
            if (!isABValid || ((mf.secondsSinceStart - lastSecond) > 0.66 && (!mf.isAutoSteerBtnOn || mf.mc.steerSwitchHigh)))
            {
                BuildCurrentABLineList(pivot);
            }

            //Check uturn first
            if (mf.yt.isYouTurnTriggered && mf.yt.DistanceFromYouTurnLine())//do the pure pursuit from youTurn
            {
                //now substitute what it thinks are AB line values with auto turn values
                steerAngleAB = mf.yt.steerAngleYT;
                distanceFromCurrentLinePivot = mf.yt.distanceFromCurrentLine;

                goalPointAB            = mf.yt.goalPointYT;
                radiusPointAB.easting  = mf.yt.radiusPointYT.easting;
                radiusPointAB.northing = mf.yt.radiusPointYT.northing;
                ppRadiusAB             = mf.yt.ppRadiusYT;
            }

            //Stanley
            else if (mf.isStanleyUsed)
            {
                mf.gyd.StanleyGuidanceABLine(currentABLineP1, currentABLineP2, pivot, steer);
            }

            //Pure Pursuit
            else
            {
                //get the distance from currently active AB line
                //x2-x1
                dx = currentABLineP2.easting - currentABLineP1.easting;
                //z2-z1
                dy = currentABLineP2.northing - currentABLineP1.northing;

                //save a copy of dx,dy in youTurn
                mf.yt.dxAB = dx; mf.yt.dyAB = dy;

                //how far from current AB Line is fix
                distanceFromCurrentLinePivot = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
                                                                                                * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
                                               / Math.Sqrt((dy * dy) + (dx * dx));

                //integral slider is set to 0
                if (mf.vehicle.purePursuitIntegralGain != 0 && !mf.isReverse)
                {
                    pivotDistanceError = distanceFromCurrentLinePivot * 0.2 + pivotDistanceError * 0.8;

                    if (counter2++ > 4)
                    {
                        pivotDerivative        = pivotDistanceError - pivotDistanceErrorLast;
                        pivotDistanceErrorLast = pivotDistanceError;
                        counter2         = 0;
                        pivotDerivative *= 2;

                        //limit the derivative
                        //if (pivotDerivative > 0.03) pivotDerivative = 0.03;
                        //if (pivotDerivative < -0.03) pivotDerivative = -0.03;
                        //if (Math.Abs(pivotDerivative) < 0.01) pivotDerivative = 0;
                    }

                    //pivotErrorTotal = pivotDistanceError + pivotDerivative;

                    if (mf.isAutoSteerBtnOn &&
                        Math.Abs(pivotDerivative) < (0.1) &&
                        mf.avgSpeed > 2.5 &&
                        !mf.yt.isYouTurnTriggered)
                    //&& Math.Abs(pivotDistanceError) < 0.2)

                    {
                        //if over the line heading wrong way, rapidly decrease integral
                        if ((inty < 0 && distanceFromCurrentLinePivot < 0) || (inty > 0 && distanceFromCurrentLinePivot > 0))
                        {
                            inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.04;
                        }
                        else
                        {
                            if (Math.Abs(distanceFromCurrentLinePivot) > 0.02)
                            {
                                inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.02;
                                if (inty > 0.2)
                                {
                                    inty = 0.2;
                                }
                                else if (inty < -0.2)
                                {
                                    inty = -0.2;
                                }
                            }
                        }
                    }
                    else
                    {
                        inty *= 0.95;
                    }
                }
                else
                {
                    inty = 0;
                }

                //Subtract the two headings, if > 1.57 its going the opposite heading as refAB
                abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
                if (abFixHeadingDelta >= Math.PI)
                {
                    abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
                }

                // ** Pure pursuit ** - calc point on ABLine closest to current position
                double U = (((pivot.easting - currentABLineP1.easting) * dx)
                            + ((pivot.northing - currentABLineP1.northing) * dy))
                           / ((dx * dx) + (dy * dy));

                //point on AB line closest to pivot axle point
                rEastAB  = currentABLineP1.easting + (U * dx);
                rNorthAB = currentABLineP1.northing + (U * dy);

                //update base on autosteer settings and distance from line
                double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();

                if (mf.isReverse ? isHeadingSameWay : !isHeadingSameWay)
                {
                    goalPointAB.easting  = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
                    goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
                }
                else
                {
                    goalPointAB.easting  = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
                    goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
                }

                //calc "D" the distance from pivot axle to lookahead point
                double goalPointDistanceDSquared
                    = glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);

                //calculate the the new x in local coordinates and steering angle degrees based on wheelbase
                double localHeading;

                if (isHeadingSameWay)
                {
                    localHeading = glm.twoPI - mf.fixHeading + inty;
                }
                else
                {
                    localHeading = glm.twoPI - mf.fixHeading - inty;
                }

                ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                               + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));

                steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                            + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
                                                       / goalPointDistanceDSquared));

                if (mf.ahrs.imuRoll != 88888)
                {
                    steerAngleAB += mf.ahrs.imuRoll * -mf.gyd.sideHillCompFactor;
                }

                if (steerAngleAB < -mf.vehicle.maxSteerAngle)
                {
                    steerAngleAB = -mf.vehicle.maxSteerAngle;
                }
                if (steerAngleAB > mf.vehicle.maxSteerAngle)
                {
                    steerAngleAB = mf.vehicle.maxSteerAngle;
                }

                //limit circle size for display purpose
                if (ppRadiusAB < -500)
                {
                    ppRadiusAB = -500;
                }
                if (ppRadiusAB > 500)
                {
                    ppRadiusAB = 500;
                }

                radiusPointAB.easting  = pivot.easting + (ppRadiusAB * Math.Cos(localHeading));
                radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading));

                if (mf.isAngVelGuidance)
                {
                    //angular velocity in rads/sec  = 2PI * m/sec * radians/meters
                    mf.setAngVel = 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase;
                    mf.setAngVel = glm.toDegrees(mf.setAngVel) * 100;

                    //clamp the steering angle to not exceed safe angular velocity
                    if (Math.Abs(mf.setAngVel) > 1000)
                    {
                        //mf.setAngVel = mf.setAngVel < 0 ? -mf.vehicle.maxAngularVelocity : mf.vehicle.maxAngularVelocity;
                        mf.setAngVel = mf.setAngVel < 0 ? -1000 : 1000;
                    }
                }

                //distance is negative if on left, positive if on right
                if (!isHeadingSameWay)
                {
                    distanceFromCurrentLinePivot *= -1.0;
                }

                //Convert to millimeters
                mf.guidanceLineDistanceOff = (short)Math.Round(distanceFromCurrentLinePivot * 1000.0, MidpointRounding.AwayFromZero);
                mf.guidanceLineSteerAngle  = (short)(steerAngleAB * 100);
            }
        }
예제 #29
0
        public void FindPointsDriveAround(vec3 fromPt, double headAB, ref vec3 start, ref vec3 stop)
        {
            //initial scan is straight ahead of pivot point of vehicle to find the right turnLine/boundary
            vec3 pt = new vec3();

            bool isFound          = false;
            int  closestTurnNum   = 99;
            int  closestTurnIndex = 99999;
            int  mazeDim          = mf.mazeGrid.mazeColXDim * mf.mazeGrid.mazeRowYDim;

            double cosHead = Math.Cos(headAB);
            double sinHead = Math.Sin(headAB);

            for (int b = 1; b < mf.maxCrossFieldLength; b += 2)
            {
                pt.easting  = fromPt.easting + (sinHead * b);
                pt.northing = fromPt.northing + (cosHead * b);

                if (mf.turn.turnArr[0].IsPointInTurnWorkArea(pt))
                {
                    for (int t = 1; t < mf.bnd.bndArr.Count; t++)
                    {
                        if (!mf.bnd.bndArr[t].isSet || mf.bnd.bndArr[t].isDriveThru)
                        {
                            continue;
                        }

                        if (mf.bnd.bndArr[t].isDriveAround)
                        {
                            if (mf.gf.geoFenceArr[t].IsPointInGeoFenceArea(pt))
                            {
                                isFound          = true;
                                closestTurnNum   = t;
                                closestTurnIndex = b;

                                start.easting  = fromPt.easting + (sinHead * b);
                                start.northing = fromPt.northing + (cosHead * b);
                                start.heading  = headAB;
                                break;
                            }
                        }
                        else
                        {
                            //its a uturn obstacle
                            if (mf.turn.turnArr[t].IsPointInTurnWorkArea(pt))
                            {
                                start.easting = 88888;
                                return;
                            }
                        }
                    }
                }
                else
                {
                    isFound        = true;
                    start.easting  = 88888;
                    closestTurnNum = 0;
                    return;
                }
                if (isFound)
                {
                    break;
                }
            }

            isFound = false;

            for (int b = closestTurnIndex + 200; b > closestTurnIndex; b--)
            {
                pt.easting  = fromPt.easting + (sinHead * b);
                pt.northing = fromPt.northing + (cosHead * b);

                if (mf.gf.geoFenceArr[closestTurnNum].IsPointInGeoFenceArea(pt))
                {
                    isFound = true;

                    stop.easting  = fromPt.easting + (sinHead * b);
                    stop.northing = fromPt.northing + (cosHead * b);
                    stop.heading  = headAB;
                }

                if (isFound)
                {
                    break;
                }
            }

            for (int i = 0; i < 30; i++)
            {
                start.easting  -= sinHead;
                start.northing -= cosHead;
                start.heading   = headAB;

                int iStart = (int)((((int)((start.northing - mf.minFieldY) / mf.mazeGrid.mazeScale)) * mf.mazeGrid.mazeColXDim)
                                   + (int)((start.easting - mf.minFieldX) / mf.mazeGrid.mazeScale));

                if (iStart >= mazeDim)
                {
                    isFound        = true;
                    start.easting  = 88888;
                    closestTurnNum = 0;
                    return;
                }

                if (mf.mazeGrid.mazeArr[iStart] == 0)
                {
                    break;
                }
            }

            for (int i = 0; i < 30; i++)
            {
                stop.easting  += sinHead;
                stop.northing += cosHead;
                stop.heading   = headAB;

                int iStop = (int)((((int)((stop.northing - mf.minFieldY) / mf.mazeGrid.mazeScale)) * mf.mazeGrid.mazeColXDim)
                                  + (int)((stop.easting - mf.minFieldX) / mf.mazeGrid.mazeScale));

                if (iStop >= mazeDim)
                {
                    isFound        = true;
                    start.easting  = 88888;
                    closestTurnNum = 0;
                    return;
                }

                if (mf.mazeGrid.mazeArr[iStop] == 0)
                {
                    break;
                }
            }
        }
예제 #30
0
        private void btnBPoint_Click(object sender, System.EventArgs e)
        {
            mf.curve.aveLineHeading  = 0;
            mf.curve.isOkToAddPoints = false;
            btnBPoint.Enabled        = false;
            btnAPoint.Enabled        = false;
            btnPausePlay.Enabled     = false;
            btnCancel.Enabled        = false;
            lvLines.Enabled          = false;

            int cnt = mf.curve.refList.Count;

            if (cnt > 3)
            {
                //make sure distance isn't too big between points on Turn
                for (int i = 0; i < cnt - 1; i++)
                {
                    int j = i + 1;
                    //if (j == cnt) j = 0;
                    double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]);
                    if (distance > 1.2)
                    {
                        vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0,
                                               (mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0,
                                               mf.curve.refList[i].heading);

                        mf.curve.refList.Insert(j, pointB);
                        cnt = mf.curve.refList.Count;
                        i   = -1;
                    }
                }

                //calculate average heading of line
                double x = 0, y = 0;
                mf.curve.isCurveSet = true;
                foreach (var pt in mf.curve.refList)
                {
                    x += Math.Cos(pt.heading);
                    y += Math.Sin(pt.heading);
                }
                x /= mf.curve.refList.Count;
                y /= mf.curve.refList.Count;
                mf.curve.aveLineHeading = Math.Atan2(y, x);
                if (mf.curve.aveLineHeading < 0)
                {
                    mf.curve.aveLineHeading += glm.twoPI;
                }

                //build the tail extensions
                mf.curve.AddFirstLastPoints();
                SmoothAB(4);
                mf.curve.CalculateTurnHeadings();

                mf.curve.isCurveSet = true;
                mf.EnableYouTurnButtons();
                //mf.FileSaveCurveLine();
                lblCurveExists.Text = gStr.gsCurveSet;

                ShowSavedPanel(true);

                btnAddAndGo.Enabled  = true;
                btnAddToFile.Enabled = true;
                btnAPoint.Enabled    = false;
                btnBPoint.Enabled    = false;
                btnPausePlay.Enabled = false;

                textBox1.BackColor = Color.LightGreen;
                textBox1.Enabled   = true;
                textBox1.Text      = (Math.Round(glm.toDegrees(mf.curve.aveLineHeading), 1)).ToString(CultureInfo.InvariantCulture)
                                     + "\u00B0" + mf.FindDirection(mf.curve.aveLineHeading)
                                     + DateTime.Now.ToString("hh:mm:ss", CultureInfo.InvariantCulture);
            }
            else
            {
                mf.curve.isCurveSet = false;
                mf.curve.refList?.Clear();

                lblCurveExists.Text = " > Off <";
                ShowSavedPanel(true);
                btnNewCurve.Enabled = true;
                btnCancel.Enabled   = true;
                lvLines.Enabled     = true;
            }

            lvLines.SelectedItems.Clear();
        }