// Method to actually generate all mid path segments and store in MidSegments list.
private void PlanMidPathSegments(List<List<Point>> MidSegments, Point newStart, Point newEnd, List<Point> allCentroids,
RtwMatrix mDistAfterSegFirstSegLast, int remainingT)
{
// In order to draw path, have to make these available to other sections of codes
frmMap map = new frmMap();
Bitmap CurBMP = new Bitmap(mDist.Columns, mDist.Rows);
ImgLib.MatrixToImage(ref mDist, ref CurBMP);
map.Text = "UAV trajectory and coverage";
map.setImage(CurBMP);
if (curRequest.DrawPath)
{
// Showing path and map remains as we plan
map.Show();
map.resetImage();
// Draw first segment and last segment
for (int i = 0; i < SegFirst.GetPath().Count; i++)
{
Point p = SegFirst.GetPath()[i];
map.setPointColor(p, VacuumProbability(p));
map.Refresh();
}
if (curRequest.UseEndPoint)
{
for (int i = 0; i < SegLast.GetPath().Count; i++)
{
Point p = SegLast.GetPath()[i];
map.setPointColor(p, VacuumProbability(p));
map.Refresh();
}
}
}
mCurDist = mDistAfterSegFirstSegLast;
// Initialize all mid segment paths
for (int i = 0; i < allCentroids.Count * 2 + 2; i++)
{
List<Point> curSeg = new List<Point>();
MidSegments.Add(curSeg);
}
// First identify all starting nodes and add to paths
MidSegments[0].Add(newStart);
if (curRequest.UseEndPoint)
{
MidSegments[1].Add(newEnd);
}
// Loop through all centroids (excluding newStart and NewEnd)
for (int i = 0; i < allCentroids.Count; i++)
{
// Add a pair list to allLooseEndsPairs
List<Point> curPair = new List<Point>();
// First add centroid
curPair.Add(allCentroids[i]);
// Update probability map
mCurDist[allCentroids[i].Y, allCentroids[i].X] = VacuumProbability(allCentroids[i]);
// Then add the best valid neighbor
Point bestNeighbor = FindBestNeighbor(allCentroids[i]);
// Update probability map
mCurDist[bestNeighbor.Y, bestNeighbor.X] = VacuumProbability(bestNeighbor);
// Add pair to list
curPair.Add(bestNeighbor);
MidSegments[i * 2 + 2].Add(curPair[0]);
MidSegments[i * 2 + 3].Add(curPair[1]);
AllLooseEndsPairs.Add(curPair);
// Show path planning process
if (curRequest.DrawPath)
{
map.setPointColor(curPair[0], mCurDist[curPair[0].Y, curPair[0].X]);
map.setPointColor(curPair[1], mCurDist[curPair[1].Y, curPair[1].X]);
map.Refresh();
}
}
// Deduct time
remainingT -= allCentroids.Count * 2;
// Loop through remaining flight time.
int thresholdT = remainingT;
for (int t = 0; t < remainingT; t++)
{
// Each start node find best neighbor and then choose best one to add to path
float maxP = 0;
int bestIndex = 0;
Point bestPoint = new Point();
for (int i = 0; i < MidSegments.Count; i++)
{
if (!curRequest.UseEndPoint && i == 1)
{
// Don't plan
}
else
{
Point bestNeighbor = FindBestNeighbor(MidSegments[i][MidSegments[i].Count - 1]);
if (i == 0)
{
bestPoint = bestNeighbor;
bestIndex = i;
}
float curP = GetPartialDetection(bestNeighbor);
if (curP > maxP)
{
maxP = curP;
bestIndex = i;
bestPoint = bestNeighbor;
}
}
}
// Fly the best point
// Add to mid path segment
MidSegments[bestIndex].Add(bestPoint);
// update loose ends pairs
if (bestIndex > 1)
{
if (bestIndex % 2 == 0)
{
AllLooseEndsPairs[(bestIndex - 2) / 2][0] = bestPoint;
}
else
{
AllLooseEndsPairs[(bestIndex - 2) / 2][1] = bestPoint;
}
}
// Vacuum probability
mCurDist[bestPoint.Y, bestPoint.X] = VacuumProbability(bestPoint);
// Increasing path by one point means worst comes to worst, it takes two time steps to recover when joining segmengs
thresholdT += 2;
int result = EnoughTimeLeftToConnect(MidSegments, allCentroids, remainingT, ref thresholdT, t);
if (result == 0)
{
System.Windows.Forms.MessageBox.Show("Something went wrong with EnoughTimeLeftToConnect method!");
break;
}
else if (result == 1)
{
// Just enough time to connect all loose ends in the remembered perm. Remember the perm.
// Console.WriteLine("result = 1");
break;
}
else if (result == 3)
{
// Hover at one of the end points (the one with the best vacuum)
HoverAtOneEndPoint(MidSegments);
// Console.WriteLine("result = 3");
break;
}
// Console.Write("\n");
// Show path planning process
if (curRequest.DrawPath)
{
map.setPointColor(bestPoint, mCurDist[bestPoint.Y, bestPoint.X]);
map.Refresh();
}
}
}
// Performing the path planning task
public void Run()
{
// Use the right algorithm
switch (curRequest.AlgToUse)
{
case AlgType.CC:
curAlg = new AlgCC(curRequest, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.CC_E:
curAlg = new AlgSearchReverse(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.LHCGWCONV:
curAlg = new AlgGlobalWarming(curRequest, ModeCount, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.LHCGWCONV_E:
curAlg = new AlgSearchReverse(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.LHCGWPF:
curAlg = new AlgGlobalWarming(curRequest, ModeCount, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.LHCGWPF_E:
curAlg = new AlgSearchReverse(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.LHCRandom:
curAlg = new AlgLHCRandom(curRequest, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.LHCRandom_E:
curAlg = new AlgSearchReverse(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.Random:
curAlg = new AlgRandom(curRequest, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.Random_E:
curAlg = new AlgSearchReverse(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.CONV:
curAlg = new AlgGlobalWarming(curRequest, ModeCount, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.CONV_E:
curAlg = new AlgSearchReverse(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.PF:
curAlg = new AlgPFLooper(curRequest, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.PF_E:
curAlg = new AlgSearchReverse(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.TopTwo:
curAlg = new AlgTopTwo(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.TopTwo_E:
curAlg = new AlgTopTwo(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.TopN:
curAlg = new AlgTopN(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.TopN_E:
curAlg = new AlgTopN(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.TopTwoH:
curAlg = new AlgTopTwoH(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.TopTwoH_E:
curAlg = new AlgTopTwoH(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.TopNH:
curAlg = new AlgTopNH(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.TopNH_E:
curAlg = new AlgTopNH(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.EA:
curAlg = new AlgEA(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.EA_E:
curAlg = new AlgSearchReverse(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.RealTime:
curAlg = new AlgRealTime(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
case AlgType.RealTime_E:
curAlg = new AlgRealTime(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curAlg.PlanPath();
break;
}
// Set things ready for getters
Efficiency = curAlg.GetEfficiency();
RunTime = curAlg.GetRunTime();
Path = curAlg.GetPath();
curAlg.Shout();
// Debug code, show actual path
if (curRequest.DrawPath)
{
//// Draw coverage
//Bitmap CurBMP = new Bitmap(mDistReachable.Columns, mDistReachable.Rows);
//ImgLib.MatrixToImage(ref mDistReachable, ref CurBMP);
//frmMap map = new frmMap();
//map.Text = "Actual UAV path";
//map.setImage(CurBMP);
//map.Show();
//map.resetImage();
//map.DrawCoverage(Path);
//map.Refresh();
//// Draw path
//Bitmap CurBMP2 = new Bitmap(mDistReachable.Columns, mDistReachable.Rows);
//ImgLib.MatrixToImage(ref mDistReachable, ref CurBMP2);
//frmMap map2 = new frmMap();
//map2.Text = "UAV trajectory simulation";
//map2.setImage(CurBMP2);
//map2.Show();
//map2.resetImage();
//map2.DrawPath(Path);
//map2.Refresh();
// Draw path with map remains
Bitmap CurBMP3 = new Bitmap(mDistReachable.Columns, mDistReachable.Rows);
ImgLib.MatrixToImage(ref mDistReachable, ref CurBMP3);
frmMap map3 = new frmMap();
map3.Text = "UAV trajectory and coverage";
map3.setImage(CurBMP3);
map3.Show();
map3.resetImage();
List<float> remains = curAlg.ShowCoverage();
Color c = Color.FromArgb(255, 0, 0);
for (int i=0; i<Path.Count; i++)
{
Point p = Path[i];
map3.setPointColor(p, c);
map3.Refresh();
map3.setPointColor(p, remains[i]);
map3.Refresh();
}
// Drawing real path
MISCLib.ShowImage(MISCLib.DrawPath(Path), "Real Path");
}
// Log results
if (ProjectConstants.DebugMode)
{
curRequest.SetLog("Run time: " + curAlg.GetRunTime() + "\n");
curRequest.SetLog("Best CDF: " + curAlg.GetCDF() + "\n");
curRequest.SetLog("Best Efficiency: " + curAlg.GetEfficiency() + "\n");
}
//// Log Path
//for (int i = 0; i < curAlg.GetPath().Count; i++)
//{
// Point p = curAlg.GetPath()[i];
// curRequest.SetLog(p.X + "," + p.Y + "\n");
//}
}
// When the test button is pressed (Count how many modes)
private void btnTest_Click(object sender, EventArgs e)
{
#region Test Mode Count
//DateTime startTime = DateTime.Now;
//CountDistModes myCount;
//if (chkUseDiff.Checked)
//{
// RtwMatrix mDistReachable = test.DistMap.Clone();
// RtwMatrix mDiffReachable = test.DiffMap.Clone();
// RtwMatrix mRealModes = new RtwMatrix(CurDiffMap.Rows, CurDiffMap.Columns);
// for (int i = 0; i < mRealModes.Rows; i++)
// {
// for (int j = 0; j < mRealModes.Columns; j++)
// {
// mRealModes[i, j] = mDistReachable[i, j] *
// (float)test.DiffRates[Convert.ToInt32(mDiffReachable[i, j])];
// }
// }
// myCount = new CountDistModes(mRealModes);
// // Debug code: Showing the product of dist and diff
// frmMap mapReal = new frmMap();
// Bitmap CurBMPReal = new Bitmap(mRealModes.Columns, mRealModes.Rows);
// ImgLib.MatrixToImage(ref mRealModes, ref CurBMPReal);
// mapReal.Text = "Real probability distribution with respect to difficulty map";
// mapReal.setImage(CurBMPReal);
// mapReal.Show();
// mapReal.resetImage();
//}
//else
//{
// myCount = new CountDistModes(CurDistMap);
//}
//Log(myCount.GetCount().ToString() + "\n");
//DateTime stopTime = DateTime.Now;
//TimeSpan duration = stopTime - startTime;
//Log("Computation took " + duration.ToString() + " seconds.\n");
//// Show mode nodes
//RtwMatrix myModes = myCount.GetModes().Clone();
//for (int i = 0; i < myModes.Rows; i++)
//{
// for (int j = 0; j < myModes.Columns; j++)
// {
// if (myModes[i, j] > 0)
// {
// myModes[i, j] = 255;
// }
// }
//}
//// Convert matrix to image
//Bitmap CurBMP = new Bitmap(myModes.Columns, myModes.Rows);
//ImgLib.MatrixToImage(ref myModes, ref CurBMP);
//// Showing map in map form
//frmMap myModesForm = new frmMap(this);
//myModesForm.Text = "Modes Map";
//myModesForm.setImage(CurBMP);
//myModesForm.Show();
//myCount = null;
#endregion
#region Test permutation
//int[] intInput = { 1, 2, 3, 4};
//Log(ShowPermutations<int>(intInput, 3));
//string[] stringInput = { "Hello", "World", "Foo" };
//Log(ShowPermutations<string>(stringInput, 2));
#endregion
#region Test MATLAB
//////////////////////
//// Input Parameters
//////////////////////
//// create an array ar for the real part of "a"
//System.Array ar = new double[2];
//ar.SetValue(11, 0);
//ar.SetValue(12, 1);
//// create an array ai for the imaginary part of "a"
//System.Array ai = new double[2];
//ai.SetValue(1, 0);
//ai.SetValue(2, 1);
//// create an array br for the real part of "b"
//System.Array br = new double[2];
//br.SetValue(21, 0);
//br.SetValue(22, 1);
//// create an array bi for the imaginary part of "b"
//System.Array bi = new double[2];
//bi.SetValue(3, 0);
//bi.SetValue(4, 1);
///////////////////////
//// Output Parameters
///////////////////////
//// initialize variables for return value from ML
//System.Array cr = new double[2];
//System.Array ci = new double[2];
//System.Array dr = new double[2];
//System.Array di = new double[2];
//////////////////////////
//// Call MATLAB function
//////////////////////////
//// call appropriate function/method based on Mode
//// use MATLAB engine
//UseEngine(ar, ai, br, bi, ref cr, ref ci, ref dr, ref di);
//Log("ar = " + ar.GetValue(0).ToString() + " " + ar.GetValue(1).ToString() + "\n");
//Log("ai = " + ai.GetValue(0).ToString() + " " + ai.GetValue(1).ToString() + "\n");
//Log("br = " + br.GetValue(0).ToString() + " " + br.GetValue(1).ToString() + "\n");
//Log("bi = " + bi.GetValue(0).ToString() + " " + bi.GetValue(1).ToString() + "\n");
//Log("cr = " + cr.GetValue(0).ToString() + " " + cr.GetValue(1).ToString() + "\n");
//Log("ci = " + ci.GetValue(0).ToString() + " " + ci.GetValue(1).ToString() + "\n");
//Log("dr = " + dr.GetValue(0).ToString() + " " + dr.GetValue(1).ToString() + "\n");
//Log("di = " + di.GetValue(0).ToString() + " " + di.GetValue(1).ToString() + "\n");
#endregion
#region Test MATH.NET EVD
//DenseMatrix m = new DenseMatrix(new[,] { { 81.1887, -18.4630 }, { -18.4630, 115.9033 } });
//System.Numerics.Complex[] d = m.Evd().EigenValues().ToArray();
//double a = d[0].Real;
//double b = d[1].Real;
//Log(a + " " + b);
#endregion
#region Test Arrays
//int n = 3;
//Array arrModes = new double[n];
//Array arrMUs = new double[n, 2];
//Array arrSigmaXSigmaY = new double[n];
//for (int i = 0; i < n; i++)
//{
// // Means
// arrMUs.SetValue(21, i, 0);
// arrMUs.SetValue(22, i, 1);
//}
#endregion
#region Compute Efficiency with existing path
#region Sanity Check
// Make sure maps are loaded
if (chkUseDist.Checked && CurDistMap == null)
{
System.Windows.Forms.MessageBox.Show("Please load a probability distribution map first!");
return;
}
if (chkUseDiff.Checked && CurDiffMap == null)
{
System.Windows.Forms.MessageBox.Show("Please load a task-difficulty map first!");
return;
}
// Make sure distribution map and task-difficulty map have same size
if (chkUseDiff.Checked && chkUseDist.Checked)
{
if (CurDistMap.Rows != CurDiffMap.Rows || CurDistMap.Columns != CurDiffMap.Columns)
{
System.Windows.Forms.MessageBox.Show("Please make sure the distribution map and the " +
"task-difficulty map must be the same size!");
return;
}
}
// Use default distribution map or task-difficulty map if only one is checked
if (chkUseDiff.Checked && !chkUseDist.Checked)
{
CurDistMap = new RtwMatrix(CurDiffMap.Rows, CurDiffMap.Columns);
}
if (!chkUseDiff.Checked && chkUseDist.Checked)
{
CurDiffMap = new RtwMatrix(CurDistMap.Rows, CurDistMap.Columns);
}
#endregion
// Create request object
PathPlanningRequest newRequest = new PathPlanningRequest();
#region Setting Request Object Properties
newRequest.UseDistributionMap = chkUseDist.Checked;
newRequest.UseTaskDifficultyMap = chkUseDiff.Checked;
newRequest.UseHierarchy = chkHierarchy.Checked;
newRequest.UseCoarseToFineSearch = chkCoaseToFine.Checked;
newRequest.UseParallelProcessing = chkParallel.Checked;
if (rbtnFixWing.Checked)
{
newRequest.VehicleType = UAVType.FixWing;
}
if (rbtnCopter.Checked)
{
newRequest.VehicleType = UAVType.Copter;
}
if (rbtnFixedAmount.Checked)
{
newRequest.DetectionType = DType.FixAmount;
}
if (rbtnFixedAmountPercent.Checked)
{
newRequest.DetectionType = DType.FixAmountInPercentage;
}
if (rbtnFixedPercent.Checked)
{
newRequest.DetectionType = DType.FixPercentage;
}
newRequest.DetectionRate = Convert.ToDouble(ntxtDetectionRate.Value);
newRequest.DistMap = CurDistMap;
newRequest.DiffMap = CurDiffMap;
newRequest.UseEndPoint = chkUseEndPoint.Checked;
newRequest.T = trbFlightTime.Value;
newRequest.pStart.column = Convert.ToInt16(ntxtSX.Value);
newRequest.pStart.row = Convert.ToInt16(ntxtSY.Value);
newRequest.pEnd.column = Convert.ToInt16(ntxtEX.Value);
newRequest.pEnd.row = Convert.ToInt16(ntxtEY.Value);
newRequest.AlgToUse = AlgType.LHCGWCONV;
newRequest.DrawPath = chkShowPath.Checked;
#endregion
#region Find max task-difficulty and compute diff rates only once
if (chkUseDiff.Checked)
{
newRequest.MaxDifficulty = Convert.ToInt32(CurDiffMap.MinMaxValue()[1]);
// Set task-difficulty rates
double[] DiffRates = new double[newRequest.MaxDifficulty + 1];
double rate = 1.0 / (newRequest.MaxDifficulty + 1);
for (int i = 0; i < newRequest.MaxDifficulty + 1; i++)
{
DiffRates[i] = 1 - i * rate;
}
newRequest.DiffRates = DiffRates;
}
if (!newRequest.SanityCheck())
{
System.Windows.Forms.MessageBox.Show(newRequest.GetLog());
return;
}
#endregion
// Read in existing path using hard-coded file path
string BAPathFileName = @"H:\Research\20 New IPPAs for Partial Detection Conference Paper\Selected Cases Maps\BAPath.csv";
RtwMatrix BAPath = MISCLib.ReadInMap(BAPathFileName);
#region First do reachable area (Dist and Diff)
RtwMatrix mDistReachable;
RtwMatrix mDiffReachable;
if (newRequest.T < newRequest.DistMap.Rows + newRequest.DistMap.Columns)
{
mDistReachable = newRequest.DistMap.Clone();
mDiffReachable = newRequest.DiffMap.Clone();
if (!ComputeReachableArea(newRequest, mDistReachable, mDiffReachable))
{
// Cannot plan path.
return;
}
}
else
{
mDistReachable = newRequest.DistMap.Clone();
mDiffReachable = newRequest.DiffMap.Clone();
}
#endregion
// Then do efficiency lower bound
ComputeEfficiencyUB myELB = new ComputeEfficiencyUB(newRequest, mDistReachable, mDiffReachable);
double Efficiency_UB = myELB.GetEfficiency_UB();
List<Point> TeleportPath = myELB.GetTeleportPath();
myELB = null;
// Creating path planning object
AlgPathPlanning curAlg = new AlgGlobalWarming(newRequest, 1, mDistReachable, mDiffReachable, Efficiency_UB);
// Call method to compute existing path CDF
List<Point> Path = new List<Point>();
for(int i=0; i<BAPath.Rows; i++)
{
Point p = new Point(Convert.ToInt16(BAPath[i,0]), Convert.ToInt16(BAPath[i,1]));
Path.Add(p);
}
float CDF = curAlg.GetTrueCDF(Path);
// Compute efficiency
double Efficiency = 0;
if (Efficiency_UB == 0)
{
Efficiency = 1;
}
else
{
Efficiency = CDF / Efficiency_UB;
}
// Compute CDF for graph
Console.WriteLine("Print Existing Path CDF Graph:");
curAlg.PrintCDFGraph(TeleportPath, newRequest.DistMap);
Console.WriteLine("Print BAPath CDF Graph:");
curAlg.PrintCDFGraph(Path, newRequest.DistMap);
curAlg.SetPath(Path);
// Show results
Log("----------------------------------------------\n");
Log("Efficiency: " + Efficiency.ToString() + "\n");
Log("----------------------------------------------");
Log("----------------------------------------------\n");
#region Show path
if (newRequest.DrawPath)
{
// Draw path with map remains
Bitmap CurBMP3 = new Bitmap(mDistReachable.Columns, mDistReachable.Rows);
ImgLib.MatrixToImage(ref mDistReachable, ref CurBMP3);
frmMap map3 = new frmMap();
map3.Text = "UAV trajectory and coverage";
map3.setImage(CurBMP3);
map3.Show();
map3.resetImage();
List<float> remains = curAlg.ShowCoverage();
Color c = Color.FromArgb(255, 0, 0);
for (int i = 0; i < Path.Count; i++)
{
Point p = Path[i];
map3.setPointColor(p, c);
map3.Refresh();
map3.setPointColor(p, remains[i]);
map3.Refresh();
}
// Drawing real path
MISCLib.ShowImage(MISCLib.DrawPath(Path), "Real Path");
}
#endregion
#endregion
}