예제 #1
0
파일: AlgCC_E.cs 프로젝트: Lannyland/IPPA
        // Algorithm specific implementation of the path planning
        protected override void DoPathPlanning()
        {
            bool blnClean = false;                              // Is there still probability left?
            int CurT = 0;                                       // Time used for current run (lawnmowing pattern)
            int RealT = 0;                                      // How much time left after current run
            int PatternStepCount = 0;                           // Used to remember last flight pattern inside box
            Point CurStart = new Point(curRequest.pStart.column, curRequest.pStart.row);    // Start point in each run
            Point End = new Point(curRequest.pEnd.column, curRequest.pEnd.row);             // End point for entire request
            List<Point> CurPathSegment = new List<Point>();     // Path planned for current run
            CurPathSegment.Add(CurStart);                       // Only do this once. Don't add Start again in future runs.

            int dist;
            Point CurPoint = new Point(CurStart.X, CurStart.Y);

            // Plan to do complete coverage multiple times if partial detection is used
            // Before distribution map is wiped clean
            while (!blnClean && RealT < curRequest.T)
            {
                // If no more time left, go straight to end point
                dist = MISCLib.ManhattanDistance(CurStart.X, CurStart.Y, End.X, End.Y);
                if (dist + 2 >= curRequest.T - RealT)
                {
                    goto BreakoutCC;
                }

                // First find bounding box that contains all the non-zero probability nodes
                bool EvenColumns = false;
                int Top, Bottom, Left, Right;
                Top = -1;
                Bottom = -1;
                Left = -1;
                Right = -1;

                RtwMatrix boundingbox = GetBox(ref EvenColumns, ref Top, ref Bottom, ref Left, ref Right);

                // If nothing left on map, exit while loop
                if (Top == -1 && Bottom == -1 && Left == -1 && Right == -1)
                {
                    blnClean = true;
                    break;
                }

                #region Move inside the box if not in

                // Reset pattern step count
                PatternStepCount = 0;
                // Move inside
                Point Start = CurStart;
                CurPoint = new Point(Start.X, Start.Y);
                if (boundingbox[Start.Y, Start.X] == 0)
                {
                    // Outside of the box, so plan shortest path to bounding box
                    Point Parent;
                    if (Path.Count == 0)
                    {
                        Parent = Start;
                    }
                    else if (Path.Count == 1)
                    {
                        Parent = Path[Path.Count - 1];
                    }
                    else
                    {
                        Parent = Path[Path.Count - 2];
                    }
                    if (Start.X < Left)
                    {
                        // Should go right
                        // Make sure it's a valid flight pattern for given UAV
                        Point Child = new Point(CurPoint.X + 1, CurPoint.Y);
                        if (!ValidMove(Parent, CurPoint, Child, End, curRequest.T - RealT))
                        {
                            if (CurPoint.Y < Top || CurPoint.Y == 0)
                            {
                                CurPoint.Y++;
                            }
                            else if (CurPoint.Y > Bottom || CurPoint.Y == mDist.Rows - 1)
                            {
                                CurPoint.Y--;
                            }
                            else
                            {
                                // Just go down (choices are up or down)
                                CurPoint.Y++;
                            }
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);

                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }
                        // Move right horizentally
                        while (CurPoint.X < Left)
                        {
                            CurPoint.X++;
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);

                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }
                    }
                    else if (Start.X > Right)
                    {
                        // Should go left
                        // Make sure it's a valid flight pattern for given UAV
                        Point Child = new Point(CurPoint.X - 1, CurPoint.Y);
                        if (!ValidMove(Parent, CurPoint, Child, End, curRequest.T - RealT))
                        {
                            if (CurPoint.Y < Top || CurPoint.Y == 0)
                            {
                                CurPoint.Y++;
                            }
                            else if (CurPoint.Y > Bottom || CurPoint.Y == mDist.Rows - 1)
                            {
                                CurPoint.Y--;
                            }
                            else
                            {
                                // Just go down (choices are up or down)
                                CurPoint.Y++;
                            }
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);

                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }
                        // Move left horizentally
                        while (CurPoint.X > Right)
                        {
                            CurPoint.X--;
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);

                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }
                    }
                    else
                    {
                        // No need to move horizentally
                    }
                    if (CurPoint.Y < Top)
                    {
                        // Should go down
                        // Make sure it's a valid flight pattern for given UAV
                        Point Child = new Point(CurPoint.X, CurPoint.Y + 1);
                        if (!ValidMove(Parent, CurPoint, Child, End, curRequest.T - RealT))
                        {
                            if (CurPoint.X < Left || CurPoint.X == 0)
                            {
                                CurPoint.X++;
                            }
                            else if (CurPoint.X > Right || CurPoint.X == mDist.Columns - 1)
                            {
                                CurPoint.X--;
                            }
                            else
                            {
                                // Just go right (choices are left or right)
                                CurPoint.X++;
                            }
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);

                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }
                        // Move down vertically
                        while (CurPoint.Y < Top)
                        {
                            CurPoint.Y++;
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);

                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }
                    }
                    else if (CurPoint.Y > Bottom)
                    {
                        // Should go up
                        // Make sure it's a valid flight pattern for given UAV
                        Point Child = new Point(CurPoint.X, CurPoint.Y - 1);
                        if (!ValidMove(Parent, CurPoint, Child, End, curRequest.T - RealT))
                        {
                            if (CurPoint.X < Left || CurPoint.X == 0)
                            {
                                CurPoint.X++;
                            }
                            else if (CurPoint.X > Right || CurPoint.X == mDist.Columns - 1)
                            {
                                CurPoint.X--;
                            }
                            else
                            {
                                // Just go right (choices are left or right)
                                CurPoint.X++;
                            }
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);

                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }
                        // Move up vertically
                        while (CurPoint.Y > Bottom)
                        {
                            CurPoint.Y--;
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);

                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }
                    }
                    else
                    {
                        // No need to move vertically
                    }
                }
                else
                {
                    // Inside the box. Let's add Current Node first
                    // Point already in path segment
                }
                #endregion

                #region Complete Coverage

                // Remember starting position inside bounding box
                Point boxstart = new Point(CurPoint.X, CurPoint.Y);
                // boxstart node counts as part of the pattern, increase counter
                PatternStepCount++;
                // tempt is current timestep, used to identify first step in while loop
                bool AtBoxStart = true;

                // Once inside bounding box fly the pattern until mxn-1 steps (complete coverage)
                if (EvenColumns)
                {
                    // Depending on the current position, decide which direction to go
                    // Do the following as long as there's still time or if I return back to boxstart
                    while (((CurPoint.X != boxstart.X || CurPoint.Y != boxstart.Y) && RealT <= curRequest.T) || AtBoxStart)
                    {
                        // Don't add boxstart, but add future nodes
                        if (!AtBoxStart)
                        {
                            // Add node to path
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);
                            // Increase time counter
                            PatternStepCount++;
                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }

                        // Move intelligently
                        if (CurPoint.X >= Left && CurPoint.X < Right && CurPoint.Y == Top)
                        {
                            // Top left corner. Go right
                            CurPoint.X++;
                        }
                        else if (CurPoint.X == Right && CurPoint.Y >= Top && CurPoint.Y < Bottom)
                        {
                            // Top right corner. Go down
                            CurPoint.Y++;
                        }
                        else if (CurPoint.Y == Bottom && (CurPoint.X - Left) % 2 == 1)
                        {
                            // Bottom right corners. Go left
                            CurPoint.X--;
                        }
                        else if (CurPoint.Y <= Bottom && CurPoint.Y > Top + 1 && (CurPoint.X - Left) % 2 == 0)
                        {
                            // Bottom left corners. Go up
                            CurPoint.Y--;
                        }
                        else if (CurPoint.Y == Top + 1 && CurPoint.X > Left && (CurPoint.X - Left) % 2 == 0)
                        {
                            // Second row right corners. Go left
                            CurPoint.X--;
                        }
                        else if (CurPoint.Y >= Top + 1 && CurPoint.Y < Bottom && (CurPoint.X - Left) % 2 == 1)
                        {
                            // Second row left corners. Go down
                            CurPoint.Y++;
                        }
                        else if (CurPoint.X == Left && CurPoint.Y == Top + 1)
                        {
                            // Point left of top right corner. Go Right
                            CurPoint.Y--;
                        }

                        AtBoxStart = false;
                    }
                }
                else
                {
                    // Turn the pattern 90 degrees clockwise
                    while (((CurPoint.X != boxstart.X || CurPoint.Y != boxstart.Y) && RealT < curRequest.T) || AtBoxStart)
                    {
                        // Don't add boxstart, but add future nodes
                        if (!AtBoxStart)
                        {
                            // Add node to path
                            AddNodeToPath(CurPathSegment, ref CurT, ref RealT, ref CurPoint);
                            // Increase pattern step counter
                            PatternStepCount++;
                            // If no more time left, go straight to end point
                            dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                            if (dist + 2 >= curRequest.T - RealT)
                            {
                                goto BreakoutCC;
                            }
                        }

                        if (CurPoint.X == Right && CurPoint.Y >= Top && CurPoint.Y < Bottom)
                        {
                            // Top right corner. Go down
                            CurPoint.Y++;
                        }
                        else if (CurPoint.X <= Right && CurPoint.X > Left && CurPoint.Y == Bottom)
                        {
                            // Bottom right corner. Go left
                            CurPoint.X--;
                        }
                        else if (CurPoint.X == Left && (CurPoint.Y - Top) % 2 == 1)
                        {
                            // Left bottom corners. Go up
                            CurPoint.Y--;
                        }
                        else if (CurPoint.X >= Left && CurPoint.X < Right - 1 && (CurPoint.Y - Top) % 2 == 0)
                        {
                            // Left top corners. Go right
                            CurPoint.X++;
                        }
                        else if (CurPoint.X == Right - 1 && CurPoint.Y > Top && (CurPoint.Y - Top) % 2 == 0)
                        {
                            // Second column from right bottom corners. Go up
                            CurPoint.Y--;
                        }
                        else if (CurPoint.X <= Right - 1 && CurPoint.X > Left && (CurPoint.Y - Top) % 2 == 1)
                        {
                            // Second column from right top corners. Go left
                            CurPoint.X--;
                        }
                        else if (CurPoint.X == Right - 1 && CurPoint.Y == Top)
                        {
                            // Point left of top right corner. Go Right
                            CurPoint.X++;
                        }

                        AtBoxStart = false;
                    }
                }

                #endregion

                // Add current segment of path to total path
                Path.AddRange(CurPathSegment);

                // Clear current segment of path
                CurPathSegment.Clear();

                // Reset timer for current run
                CurT = 0;

                // Remember new start point
                CurStart = new Point(Path[Path.Count - 1].X, Path[Path.Count - 1].Y);
            }

            // If all time used, we are done.
            if (RealT == curRequest.T)
            {
                return;
            }

            // After distribution map is wiped clean and still time left
            int FixedPatternStartAt = Path.Count - PatternStepCount;
            while (RealT < curRequest.T)
            {
                CurPoint = new Point(Path[FixedPatternStartAt].X, Path[FixedPatternStartAt].Y);
                Path.Add(CurPoint);
                RealT++;
                FixedPatternStartAt++;
                // If no more time left, go straight to end point
                dist = MISCLib.ManhattanDistance(CurPoint.X, CurPoint.Y, End.X, End.Y);
                if (dist + 2 >= curRequest.T - RealT)
                {
                    goto BreakoutCC;
                }
            }

            return;

            // Just enough time to go straight to end point now
            BreakoutCC:
            // Add current segment of path to total path
            if (CurPathSegment.Count > 0)
            {
                Path.AddRange(CurPathSegment);
                // Clear current segment of path
                CurPathSegment.Clear();
                CurPathSegment = null;
            }

            PathPlanningRequest newRequest = curRequest.Clone();
            newRequest.pStart.column = CurPoint.X;
            newRequest.pStart.row = CurPoint.Y;
            newRequest.T = curRequest.T - RealT;
            CDF -= lastCDF;
            mCurDist[CurPoint.Y, CurPoint.X] += (float)lastCDF;
            AlgLHCGWCONV myAlg = new AlgLHCGWCONV(newRequest, mCurDist, mDiff, Efficiency_UB, 3);
            if (Path.Count > 1)
            {
                myAlg.SetBeforeStart(Path[Path.Count - 2]);
            }
            myAlg.PlanPath();

            // Record all related paths stuff
            CDF += myAlg.GetCDF();
            Path.AddRange(myAlg.GetPath());
            myAlg = null;

            return;
        }
예제 #2
0
파일: AlgTopN.cs 프로젝트: Lannyland/IPPA
        // Join path segments together into one path
        private void JoinPathSegments(List<List<Point>> MidSegments, List<Point> allCentroids)
        {
            //TODO Deal with flying backwards

            // First join
            List<Point> SegFirstPath = SegFirst.GetPath();
            Point p1 = SegFirstPath[SegFirstPath.Count - 1];
            Point p2 = MidSegments[0][0];
            if (p1.X == p2.X && p1.Y == p2.Y)
            {
                Path.AddRange(SegFirstPath);
                Path.RemoveAt(Path.Count - 1);
                Path.AddRange(MidSegments[0]);
            }
            else
            {
                // Something is wrong!
                System.Windows.Forms.MessageBox.Show("Seg1 and Seg2 don't connect.");
                return;
            }
            // Find out path index order in MidSegments
            List<int> FinalRealOrder = new List<int>();
            for (int i = 0; i < FinalPerm.Count; i++)
            {
                bool swap = false;
                if (FinalPerm2 != null)
                {
                    for (int j = 0; j < FinalPerm2.Count; j++)
                    {
                        if (FinalPerm[i] == FinalPerm2[j])
                        {
                            swap = true;
                            break;
                        }
                    }
                }
                if (swap)
                {
                    FinalRealOrder.Add(FinalPerm[i] * 2 + 3);
                    FinalRealOrder.Add(FinalPerm[i] * 2 + 2);
                }
                else
                {
                    FinalRealOrder.Add(FinalPerm[i] * 2 + 2);
                    FinalRealOrder.Add(FinalPerm[i] * 2 + 3);
                }
            }
            // 0 1-2 3-4 5-6 ...
            FinalRealOrder.Insert(0, 0);

            //// Sanity Check:
            //int testDist = 0;
            //int d = 0;
            //d = MISCLib.ManhattanDistance(
            //    MidSegments[FinalRealOrder[0]][MidSegments[FinalRealOrder[0]].Count - 1],
            //    MidSegments[FinalRealOrder[1]][MidSegments[FinalRealOrder[1]].Count - 1]);
            //Console.Write(" 0-3: " + (d - 1));
            //if (d > 0)
            //{
            //    testDist = testDist + d + 1 - 2;
            //}
            //d = MISCLib.ManhattanDistance(
            //    MidSegments[FinalRealOrder[2]][MidSegments[FinalRealOrder[2]].Count - 1],
            //    MidSegments[FinalRealOrder[3]][MidSegments[FinalRealOrder[3]].Count - 1]);
            //Console.Write(" 2-7: " + (d - 1));
            //if (d > 0)
            //{
            //    testDist = testDist + d + 1 - 2;
            //}
            //d = MISCLib.ManhattanDistance(
            //    MidSegments[FinalRealOrder[4]][MidSegments[FinalRealOrder[4]].Count - 1],
            //    MidSegments[FinalRealOrder[5]][MidSegments[FinalRealOrder[5]].Count - 1]);
            //Console.Write(" 6-4: " + (d - 1));
            //if (d > 0)
            //{
            //    testDist = testDist + d + 1 - 2;
            //}
            //d = MISCLib.ManhattanDistance(
            //    MidSegments[FinalRealOrder[6]][MidSegments[FinalRealOrder[6]].Count - 1],
            //    MidSegments[1][MidSegments[1].Count - 1]);
            //Console.Write(" 5-1: " + (d - 1));
            //if (d > 0)
            //{
            //    testDist = testDist + d + 1 - 2;
            //}

            // Mid Joins
            for (int i = 0; i < FinalRealOrder.Count - 1; i = i + 2)
            {
                p1 = MidSegments[FinalRealOrder[i]][MidSegments[FinalRealOrder[i]].Count - 1];
                p2 = MidSegments[FinalRealOrder[i + 1]][MidSegments[FinalRealOrder[i + 1]].Count - 1];
                if ((p1.X == p2.X && p1.Y == p2.Y) ||
                    (p1.X == p2.X && Math.Abs(p1.Y - p2.Y) == 1) ||
                    (Math.Abs(p1.X - p2.X) == 1 && p1.Y == p2.Y))
                {
                    // The two paths are already connected.
                    // No need to plan path to connect to points.
                }
                else
                {
                    // Need to connect two segments
                    PathPlanningRequest newRequest = curRequest.Clone();
                    newRequest.UseEndPoint = true;
                    newRequest.pStart = new DistPoint(p1.Y, p1.X);
                    newRequest.pEnd = new DistPoint(p2.Y, p2.X);
                    newRequest.T = MISCLib.ManhattanDistance(p1, p2);
                    newRequest.AlgToUse = AlgType.LHCGWCONV;
                    AlgLHCGWCONV curSeg = new AlgLHCGWCONV(newRequest, mCurDist, mDiff, Efficiency_UB, 3);
                    curSeg.PlanPath();
                    mCurDist = curSeg.GetmCurDist();
                    Path.RemoveAt(Path.Count - 1);
                    Path.AddRange(curSeg.GetPath());
                    Path.RemoveAt(Path.Count - 1);
                    curSeg = null;
                }
                List<Point> reversePath = MidSegments[FinalRealOrder[i + 1]];
                reversePath.Reverse();
                Path.AddRange(reversePath);
                Path.AddRange(MidSegments[FinalRealOrder[i + 2]]);
            }
            // Last join
            if (curRequest.UseEndPoint)
            {
                p1 = MidSegments[FinalRealOrder[FinalRealOrder.Count - 1]][MidSegments[FinalRealOrder[FinalRealOrder.Count - 1]].Count - 1];
                p2 = MidSegments[1][MidSegments[1].Count - 1];
                List<Point> reversePath = MidSegments[1];
                reversePath.Reverse();
                if ((p1.X == p2.X && p1.Y == p2.Y) ||
                    (p1.X == p2.X && Math.Abs(p1.Y - p2.Y) == 1) ||
                    (Math.Abs(p1.X - p2.X) == 1 && p1.Y == p2.Y))
                {
                    // Two paths are already connected
                    // No need to reverse
                    Path.AddRange(reversePath);
                }
                else
                {
                    // Need to connect two segments
                    PathPlanningRequest newRequest = curRequest.Clone();
                    newRequest.UseEndPoint = true;
                    newRequest.pStart = new DistPoint(p1.Y, p1.X);
                    newRequest.pEnd = new DistPoint(p2.Y, p2.X);
                    newRequest.T = MISCLib.ManhattanDistance(p1, p2);
                    newRequest.AlgToUse = AlgType.LHCGWCONV;
                    AlgLHCGWCONV curSeg = new AlgLHCGWCONV(newRequest, mCurDist, mDiff, Efficiency_UB, 3);
                    curSeg.PlanPath();
                    mCurDist = curSeg.GetmCurDist();
                    Path.RemoveAt(Path.Count - 1);
                    Path.AddRange(curSeg.GetPath());
                    Path.RemoveAt(Path.Count - 1);
                    Path.AddRange(reversePath);
                    curSeg = null;
                }
                Path.RemoveAt(Path.Count - 1);

                List<Point> SegLastPath = SegLast.GetPath();
                SegLastPath.Reverse();
                p1 = MidSegments[1][MidSegments[1].Count - 1];      // Already reversed from previous step
                p2 = SegLastPath[0];
                if (p1.X == p2.X && p1.Y == p2.Y)
                {
                    Path.AddRange(SegLastPath);
                }
                else
                {
                    // Something is wrong!
                    System.Windows.Forms.MessageBox.Show("SegLast and the one before it don't connect.");
                    return;
                }
            }

            // In case distance from start to end is odd but T is even (or vise versa) for copter
            if (curRequest.VehicleType == UAVType.Copter)
            {
                if (Path.Count == curRequest.T)
                {
                    // Just hover at end point
                    Path.Add(Path[Path.Count - 1]);
                }
            }

            //// Debug code
            //// Sanity Check
            //for (int i = 0; i < 900; i++)
            //{
            //    if (MISCLib.ManhattanDistance(Path[i], Path[i + 1]) > 1)
            //    {
            //        Console.Write("Path is disconnected!");
            //        System.Windows.Forms.MessageBox.Show("Path is disconnected!");
            //    }
            //}

            //if (Path.Count != 901)
            //{
            //    Console.Write("Something is wrong with path length!\n");
            //    ComputeMinDist(allCentroids, MidSegments);
            //    System.Windows.Forms.MessageBox.Show("Something is wrong with path length!");
            //}
        }
예제 #3
0
        // At current GW do LHC
        private bool PlanPathAtCurrentGW(RtwMatrix mGW, int index)
        {
            #region Deal with LHCGWCONV and LHCGWCONV_E algorithms
            // Console.WriteLine("Doing PlanPathAtCurrentGW once!");
            if (curRequest.AlgToUse == AlgType.LHCGWCONV || curRequest.AlgToUse == AlgType.LHCGWCONV_E)
            {
                // If LHCGWCONV, search multiple convolution kernal sizes
                int dim = Math.Max(mDist.Rows, mDist.Columns);
                for (int j = 5; j < dim; j += (int)(dim / ConvCount))
                {
                    // Console.Write("j=" + j + "\n");
                    AlgLHCGWCONV myAlg = null;
                    if (curRequest.UseParallelProcessing)
                    {
                        PathPlanningRequest curRequestCopy = curRequest.DeepClone();
                        RtwMatrix mGWCopy = mGW.Clone();
                        RtwMatrix mDiffCopy = mDiff.Clone();
                        myAlg = new AlgLHCGWCONV(curRequestCopy, mGWCopy, mDiffCopy, Efficiency_UB, j);
                        myAlg.SetBeforeStart(BeforeStart);
                        // Debug code
                        myAlg.conv = j;
                        myAlg.index = index;
                        lstThreads.Add(myAlg);
                    }
                    else
                    {
                        myAlg = new AlgLHCGWCONV(curRequest, mGW, mDiff, Efficiency_UB, j);
                        myAlg.SetBeforeStart(BeforeStart);
                        myAlg.PlanPath();

                        // Remember if true CDF is better
                        RememberBestPath(myAlg);

                        // Cleaning up
                        myAlg = null;

                        // If we already have the best path, then no need to continue
                        if (Math.Abs(Efficiency_UB - CDF) < 0.001)
                        {
                            return true;
                        }
                    }
                }
                //// Print one GW per line (3 conv each line)
                //curRequest.SetLog("\n");
            }
            #endregion

            #region Deal with LHCGWPF and LHCGWPF_E algorithms
            if (curRequest.AlgToUse == AlgType.LHCGWPF || curRequest.AlgToUse == AlgType.LHCGWPF_E)
            {
                // If LHCGWPF, search three convolution kernal sizes
                int dim = Math.Max(mDist.Rows, mDist.Columns);
                int Sigma = 0;
                for (int j = 0; j < PFCount; j++)
                {
                    //Console.Write("j=" + j + "\n");
                    Sigma += Convert.ToInt16(dim / 3);
                    AlgLHCGWCONV myAlg = null;
                    if (curRequest.UseParallelProcessing)
                    {
                        PathPlanningRequest curRequestCopy = curRequest.DeepClone();
                        RtwMatrix mGWCopy = mGW.Clone();
                        RtwMatrix mDiffCopy = mDiff.Clone();
                        myAlg = new AlgLHCGWCONV(curRequestCopy, mGWCopy, mDiffCopy, Efficiency_UB, Sigma);
                        myAlg.SetBeforeStart(BeforeStart);
                        // Debug code
                        myAlg.conv = j;
                        myAlg.index = index;
                        lstThreads.Add(myAlg);
                    }
                    else
                    {
                        myAlg = new AlgLHCGWCONV(curRequest, mGW, mDiff, Efficiency_UB, Sigma);
                        myAlg.SetBeforeStart(BeforeStart);
                        myAlg.PlanPath();

                        // Remember if true CDF is better
                        RememberBestPath(myAlg);

                        // Cleaning up
                        myAlg = null;

                        // If we already have the best path, then no need to continue
                        if (Math.Abs(Efficiency_UB - CDF) < 0.001)
                        {
                            return true;
                        }
                    }
                }
                // Print one GW per line (3 conv each line)
                //curRequest.SetLog("\n");
            }
            #endregion

            #region Deal with CONV and CONV_E algorithms
            if (curRequest.AlgToUse == AlgType.CONV || curRequest.AlgToUse == AlgType.CONV_E)
            {
                // If CONV, search multiple convolution kernal sizes
                int dim = Math.Max(mDist.Rows, mDist.Columns);
                for (int j = 3; j < dim; j += (int)(dim / ConvCount))
                {
                    //Console.Write("j=" + j + "\n");
                    AlgCONV myAlg = null;
                    if (curRequest.UseParallelProcessing)
                    {
                        PathPlanningRequest curRequestCopy = curRequest.DeepClone();
                        RtwMatrix mGWCopy = mGW.Clone();
                        RtwMatrix mDiffCopy = mDiff.Clone();
                        myAlg = new AlgCONV(curRequestCopy, mGWCopy, mDiffCopy, Efficiency_UB, j);
                        myAlg.SetBeforeStart(BeforeStart);
                        // Debug code
                        myAlg.conv = j;
                        myAlg.index = index;
                        lstThreads.Add(myAlg);
                    }
                    else
                    {
                        myAlg = new AlgCONV(curRequest, mGW, mDiff, Efficiency_UB, j);
                        myAlg.SetBeforeStart(BeforeStart);
                        myAlg.PlanPath();

                        // Remember if true CDF is better
                        RememberBestPath(myAlg);

                        // Cleaning up
                        myAlg = null;

                        // If we already have the best path, then no need to continue
                        if (Math.Abs(Efficiency_UB - CDF) < 0.001)
                        {
                            return true;
                        }
                    }
                }
                //// Print one GW per line (3 conv each line)
                //curRequest.SetLog("\n");
            }
            #endregion

            //// Debug code:
            //arrResponses = new PathPlanningResponse[lstThreads.Count];

            //for (int i = 0; i < lstThreads.Count; i++)
            //{
            //    int cur_i = i;
            //    StoreResults(cur_i);
            //}
            //// Now that all threads/tasks are done, find the best one
            //FindBestPath();

            return false;
        }
예제 #4
0
파일: AlgTopN.cs 프로젝트: Lannyland/IPPA
        // Plan first or last path segment
        private void StraightToClosestCentroid(Point StartOrEnd, List<Point> allCentroids, ref Point newPoint,
            ref int remainingT, ref AlgLHCGWCONV SegFirstLast, ref RtwMatrix mDistAfterSegFirstSegLast)
        {
            // Find centroid closest to start
            int closestCentroidIndex = -1;
            int d = curRequest.T;
            for (int i = 0; i < allCentroids.Count; i++)
            {
                int dist = MISCLib.ManhattanDistance(StartOrEnd, allCentroids[i]);
                if (dist < d)
                {
                    closestCentroidIndex = i;
                    d = dist;
                }
            }
            // Time used up is d
            remainingT = remainingT - d - 1;

            // Remember the closest centroid and remove it from list.
            newPoint = allCentroids[closestCentroidIndex];
            allCentroids.RemoveAt(closestCentroidIndex);
            // Plan path from start to this centroid
            PathPlanningRequest newRequest = curRequest.Clone();
            newRequest.UseEndPoint = true;
            newRequest.pStart = new DistPoint(StartOrEnd.Y, StartOrEnd.X);
            newRequest.pEnd = new DistPoint(newPoint.Y, newPoint.X);
            newRequest.T = d;
            newRequest.AlgToUse = AlgType.LHCGWCONV;
            SegFirstLast = new AlgLHCGWCONV(newRequest, mDistAfterSegFirstSegLast, mDiff, Efficiency_UB, 3);
            SegFirstLast.PlanPath();
            mDistAfterSegFirstSegLast = SegFirstLast.GetmCurDist();
        }
예제 #5
0
파일: AlgTopTwo.cs 프로젝트: Lannyland/IPPA
 // Method to plan path for segment 4
 private void PlanPathSeg4()
 {
     PathPlanningRequest newRequest4 = curRequest.Clone();
     newRequest4.UseEndPoint = true;
     newRequest4.pStart = new DistPoint(End.Y, End.X);
     newRequest4.pEnd = new DistPoint(Centroid2.Y, Centroid2.X);
     newRequest4.T = t4;
     newRequest4.AlgToUse = AlgType.LHCGWCONV;
     Seg4 = new AlgLHCGWCONV(newRequest4, mDistAfterSeg1Seg4, mDiff, Efficiency_UB, 3);
     Seg4.PlanPath();
     mDistAfterSeg1Seg4 = Seg4.GetmCurDist();
 }
예제 #6
0
파일: AlgTopTwo.cs 프로젝트: Lannyland/IPPA
        // Method to plan path for segment 2 given t2 and t3
        private void PlanPathSeg2Seg3(int index)
        {
            int dim = Math.Max(mDist.Rows, mDist.Columns);
            for (int j = 5; j < dim; j += (int)(dim / ProjectConstants.ConvCount))
            {

                // Plan path from closest centroid to end with current allocated t2
                PathPlanningRequest newRequest2 = curRequest.Clone();
                newRequest2.UseEndPoint = true;
                newRequest2.pStart = new DistPoint(Centroid1.Y, Centroid1.X);
                newRequest2.pEnd = new DistPoint(End1.Y, End1.X);
                newRequest2.AlgToUse = AlgType.LHCGWCONV_E;
                newRequest2.T = t2;
                // Seg2 = new AlgGlobalWarming(newRequest2, myModes.GetModeCount(), mDistAfterSeg1Seg4, mDiff, Efficiency_UB);
                // Seg2.SetGWCount(1);
                // Seg2.SetConvCount(3);
                if (curRequest.UseParallelProcessing)
                {
                    PathPlanningRequest newRequestCopy = newRequest2.DeepClone();
                    RtwMatrix mCopy = mDistAfterSeg1Seg4.Clone();
                    RtwMatrix mDiffCopy = mDiff.Clone();
                    AlgLHCGWCONV myAlg = new AlgLHCGWCONV(newRequestCopy, mCopy, mDiffCopy, Efficiency_UB, j);
                    myAlg.conv = 3;
                    myAlg.index = index * 2;
                    lstThreads.Add(myAlg);
                }
                else
                {
                    Seg2 = new AlgLHCGWCONV(newRequest2, mDistAfterSeg1Seg4, mDiff, Efficiency_UB, j);
                    Seg2.PlanPath();
                }

                // Plan path from the other centroid to end with allocated t3
                PathPlanningRequest newRequest3 = curRequest.Clone();
                newRequest3.UseEndPoint = true;
                newRequest3.pStart = new DistPoint(Centroid2.Y, Centroid2.X);
                newRequest3.pEnd = new DistPoint(End2.Y, End2.X);
                newRequest3.AlgToUse = AlgType.LHCGWCONV_E;
                newRequest3.T = t3;
                // Seg3 = new AlgGlobalWarming(newRequest3, myModes.GetModeCount(), Seg2.GetmCurDist(), mDiff, Efficiency_UB);
                // Seg3.SetGWCount(1);
                // Seg3.SetConvCount(3);
                if (curRequest.UseParallelProcessing)
                {
                    PathPlanningRequest newRequestCopy = newRequest3.DeepClone();
                    RtwMatrix mCopy = mDistAfterSeg1Seg4.Clone();
                    RtwMatrix mDiffCopy = mDiff.Clone();
                    AlgLHCGWCONV myAlg = new AlgLHCGWCONV(newRequestCopy, mCopy, mDiffCopy, Efficiency_UB, j);
                    myAlg.conv = 3;
                    myAlg.index = index * 2 + 1;
                    lstThreads.Add(myAlg);
                }
                else
                {
                    Seg3 = new AlgLHCGWCONV(newRequest3, Seg2.GetmCurDist(), mDiff, Efficiency_UB, j);
                    Seg3.PlanPath();
                }

                // Cleaning up
                newRequest2 = null;
                newRequest3 = null;

                // Remember if true CDF is better
                if (!curRequest.UseParallelProcessing)
                {
                    RememberBestPath();
                }
            }
        }
예제 #7
0
파일: AlgTopTwo.cs 프로젝트: Lannyland/IPPA
 // Method to plan path for segment 1
 private void PlanPathSeg1()
 {
     PathPlanningRequest newRequest1 = curRequest.Clone();
     newRequest1.UseEndPoint = true;
     newRequest1.pEnd = new DistPoint(Centroid1.Y, Centroid1.X);
     newRequest1.T = t1;
     newRequest1.AlgToUse = AlgType.LHCGWCONV;
     Seg1 = new AlgLHCGWCONV(newRequest1, mDist, mDiff, Efficiency_UB, 3);
     Seg1.PlanPath();
     mDistAfterSeg1Seg4 = Seg1.GetmCurDist();
 }
예제 #8
0
파일: AlgTopTwo.cs 프로젝트: Lannyland/IPPA
        // Method to perform extensive search in the T space
        private void ExtensiveSearch()
        {
            //TODO For now just assume copter (not checking the if flying backward at joint of Seg1 Seg2 and joint of Seg3 Seg4.
            int t2Min = MISCLib.ManhattanDistance(Centroid1, End1);
            int t3Max = curRequest.T - t1 - t4 - t2Min - 2;
            int t3Min = MISCLib.ManhattanDistance(Centroid2, End2);
            int t2Max = curRequest.T - t1 - t4 - t3Min - 2;
            if (curRequest.VehicleType == UAVType.Copter)
            {
                t3Max += 2;
                t2Max += 2;
            }
            StepSize = Convert.ToInt16(Math.Round(Convert.ToDouble(t2Max - t2Min) / ProjectConstants.SearchResolution));

            // Now let's search
            t2 = t2Min;
            t3 = t3Max;
            int count = 0;
            while (t2 <= t2Max && StepSize>0)
            {
                count++;
                PlanPathSeg2Seg3(count - 1);

                //// Debug: Log
                //if (curRequest.UseEndPoint)
                //{
                //    curRequest.SetLog((Seg1.GetCDF() + Seg2.GetCDF() + Seg3.GetCDF() + Seg4.GetCDF()).ToString() + "\n");
                //}
                //else
                //{
                //    curRequest.SetLog((Seg1.GetCDF() + Seg2.GetCDF() + Seg3.GetCDF()).ToString() + "\n");
                //}

                // Next search
                t2 += StepSize;
                t3 -= StepSize;

                // Free memory
                Seg2 = null;
                Seg3 = null;
            }

            // Console.WriteLine("Count = " + count);
        }