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