// Constructor
public AlgTopNH(PathPlanningRequest _curRequest, int _ModeCount, RtwMatrix _mModes, RtwMatrix _mDistReachable,
RtwMatrix _mDiffReachable, double _Efficiency_UB)
: base(_curRequest, _mDistReachable, _mDiffReachable, _Efficiency_UB)
{
ModeCount = _ModeCount;
mModes = _mModes;
}
// Constructor
public ComputeEfficiencyUB(PathPlanningRequest _curRequest, RtwMatrix _mDistReachable, RtwMatrix _mDiffReachable)
{
curRequest = _curRequest;
mDistReachable = _mDistReachable;
mDiffReachable = _mDiffReachable;
Compute();
}
// Constructor
public AlgSearchReverse(PathPlanningRequest _curRequest, int _ModeCount, RtwMatrix _mMode,
RtwMatrix _mDistReachable, RtwMatrix _mDiffReachable, double _Efficiency_UB)
: base(_curRequest, _mDistReachable, _mDiffReachable, _Efficiency_UB)
{
ModeCount = _ModeCount;
mMode = _mMode;
}
// Constructor
public MapModes(int _ModeCount, RtwMatrix _mModes, PathPlanningRequest _curRequest)
{
ModeCount = _ModeCount;
mModes = _mModes;
curRequest = _curRequest;
N = curRequest.TopN;
FindTopNModes();
}
// Scale matrix back
public static void ScaleBack(ref RtwMatrix matrixOut, float max)
{
for (int y = 0; y < matrixOut.Rows; y++)
{
for (int x = 0; x < matrixOut.Columns; x++)
{
matrixOut[y, x] = matrixOut[y, x] / 255 * max;
}
}
}
// Constructor
public PathPlanningTask(PathPlanningRequest _curRequest, int _ModeCount, RtwMatrix _mModes,
RtwMatrix _mDistReachable, RtwMatrix _mDiffReachable, double _Efficiency_UB)
{
curRequest = _curRequest;
ModeCount = _ModeCount;
mModes = _mModes;
mDistReachable = _mDistReachable;
mDiffReachable = _mDiffReachable;
Efficiency_UB = _Efficiency_UB;
}
// Constructor
public AlgGlobalWarming(PathPlanningRequest _curRequest, int _ModeCount,
RtwMatrix _mDistReachable, RtwMatrix _mDiffReachable, double _Efficiency_UB)
: base(_curRequest, _mDistReachable, _mDiffReachable, _Efficiency_UB)
{
ModeCount = _ModeCount;
GWCount = ProjectConstants.GWCount;
ConvCount = ProjectConstants.ConvCount;
CTFGWCoraseLevel = ProjectConstants.CTFGWCoraseLevel;
CTFGWLevelCount = ProjectConstants.CTFGWLevelCount;
PFCount = ProjectConstants.PFCount;
}
// Static function to convert matrix back to gray bitmap image
public static void MatrixToImage(ref RtwMatrix m, ref Bitmap bm)
{
for (int y = 0; y < m.Rows; y++)
{
for (int x = 0; x < m.Columns; x++)
{
int grayScale = Convert.ToInt32(m[y, x]);
Color c = Color.FromArgb(grayScale, grayScale, grayScale);
bm.SetPixel(x, y, c);
}
}
}
// Static function to convert bitmap image object to matrix
public static RtwMatrix ImageToMatrix(ref Bitmap bm)
{
RtwMatrix m = new RtwMatrix(bm.Height, bm.Width);
for (int y = 0; y < bm.Size.Height; y++)
{
for (int x = 0; x < bm.Size.Width; x++)
{
Color c = bm.GetPixel(x, y);
m[y, x] = (float)c.R;
}
}
return m;
}
// Constructor
public AlgPathPlanning(PathPlanningRequest _curRequest,
RtwMatrix _mDist, RtwMatrix _mDiff, double _Efficiency_UB)
{
// Start timer
startTime = DateTime.Now;
curRequest = _curRequest;
mDist = _mDist;
mDiff = _mDiff;
// Clone distribution map so we can modify it
mCurDist = mDist.Clone();
Efficiency_UB = _Efficiency_UB;
}
// Method to convert Protocal Buffer version of Matrix to RtwMatrix
public static RtwMatrix PBMatrixToRtwMatrix(ProtoBuffer.PathPlanningRequest.Types.Matrix mPB)
{
RtwMatrix mMap = null;
if (mPB.RowList.Count > 0)
{
mMap = new RtwMatrix(mPB.RowList.Count, mPB.RowList[0].CellList.Count);
for (int i = 0; i < mPB.RowList.Count; i++)
{
for (int j = 0; j < mPB.RowList[0].CellList.Count; j++)
{
mMap[i, j] = mPB.RowList[i].CellList[j];
}
}
}
return mMap;
}
public AlgTopTwo(MapModes _curModes, PathPlanningRequest _curRequest, int _ModeCount, RtwMatrix _mModes, RtwMatrix _mDistReachable,
RtwMatrix _mDiffReachable, double _Efficiency_UB)
: base(_curRequest, _mDistReachable, _mDiffReachable, _Efficiency_UB)
{
// Start timer
//DateTime startTime = DateTime.Now;
myModes = _curModes;
//DateTime stopTime = DateTime.Now;
//TimeSpan duration = stopTime - startTime;
//double RunTime = duration.TotalSeconds;
//System.Windows.Forms.MessageBox.Show("Run time " + RunTime + " seconds!");
CTFTTCoraseLevel = ProjectConstants.CTFTTCoraseLevel;
CTFTTLevelCount = ProjectConstants.CTFTTLevelCount;
mDistAfterSeg1Seg4 = mDist;
}
public AlgTopN(MapModes _curModes, PathPlanningRequest _curRequest, int _ModeCount, RtwMatrix _mModes, RtwMatrix _mDistReachable,
RtwMatrix _mDiffReachable, double _Efficiency_UB)
: base(_curRequest, _mDistReachable, _mDiffReachable, _Efficiency_UB)
{
//DateTime startTime = DateTime.Now;
myModes = _curModes;
//DateTime stopTime = DateTime.Now;
//TimeSpan duration = stopTime - startTime;
//double RunTime = duration.TotalSeconds;
//System.Windows.Forms.MessageBox.Show("Run time " + RunTime + " seconds!");
Start = new Point(curRequest.pStart.column, curRequest.pStart.row);
if (curRequest.UseEndPoint)
{
End = new Point(curRequest.pEnd.column, curRequest.pEnd.row);
}
N = curRequest.TopN;
}
// Method to convert RtwMatrix to Protocal Buffer version of Matrix
private static ProtoBuffer.PathPlanningRequest.Types.Matrix RtwMatrixToPBMatrix(RtwMatrix m)
{
ProtoBuffer.PathPlanningRequest.Types.Matrix.Builder newMBuilder = ProtoBuffer.PathPlanningRequest.Types.Matrix.CreateBuilder();
for (int i = 0; i < m.Rows; i++)
{
ProtoBuffer.PathPlanningRequest.Types.MatrixRow.Builder rowBuilder = ProtoBuffer.PathPlanningRequest.Types.MatrixRow.CreateBuilder();
for (int j = 0; j < m.Columns; j++)
{
rowBuilder.AddCell(m[i, j]);
}
ProtoBuffer.PathPlanningRequest.Types.MatrixRow newRow = rowBuilder.Build();
rowBuilder = null;
newMBuilder.AddRow(newRow);
}
ProtoBuffer.PathPlanningRequest.Types.Matrix newM = newMBuilder.Build();
newMBuilder = null;
return newM;
}
protected float GetPartialDetection(Point p, RtwMatrix m)
{
float original = 0;
float current = m[p.Y, p.X];
float amountToDeduct = 0;
// Always do fixed percent of remaining for DiffRates
// Based on detection type, compute differently
switch (curRequest.DetectionType)
{
case DType.FixAmount:
// Ignore task-difficulty map
amountToDeduct = (float)curRequest.DetectionRate;
break;
case DType.FixAmountInPercentage:
if (curRequest.UseTaskDifficultyMap)
{
original = mDistReachable[p.Y, p.X];
amountToDeduct = (float)(original * curRequest.DiffRates[Convert.ToInt32(mDiffReachable[p.Y, p.X])] * curRequest.DetectionRate);
}
else
{
amountToDeduct = (float)(current * curRequest.DetectionRate);
}
break;
case DType.FixPercentage:
if (curRequest.UseTaskDifficultyMap)
{
amountToDeduct = (float)(current * curRequest.DiffRates[Convert.ToInt32(mDiffReachable[p.Y, p.X])] * curRequest.DetectionRate);
}
else
{
amountToDeduct = (float)(current * curRequest.DetectionRate);
}
break;
}
if (amountToDeduct > current)
{
amountToDeduct = current;
}
return amountToDeduct;
}
//inverse
public static RtwMatrix operator !(RtwMatrix mtx)
{
if(mtx.Determinant() == 0)
throw new RtwMatrixException("Attempt to invert a singular matrix");
//return null;
//inverse of a 2x2 matrix
if(mtx.Rows == 2 && mtx.Columns == 2)
{
RtwMatrix tempMtx = new RtwMatrix(2,2);
tempMtx[0,0] = mtx[1,1];
tempMtx[0,1] = -mtx[0,1];
tempMtx[1,0] = -mtx[1,0];
tempMtx[1,1] = mtx[0,0];
return tempMtx / mtx.Determinant();
}
return mtx.Adjoint()/mtx.Determinant();
}
//returns a minor of a matrix with respect to an element
public RtwMatrix Minor(int row, int column)
{
if(this.Rows < 2 || this.Columns < 2)
throw new RtwMatrixException("Minor not available");
int i, j = 0;
RtwMatrix minorMtx = new RtwMatrix(this.Rows - 1, this.Columns - 1);
//find the minor with respect to the first element
for(int k = 0; k < minorMtx.Rows; k++)
{
if(k >= row)
i = k + 1;
else
i = k;
for(int l = 0; l < minorMtx.Columns; l++)
{
if(l >= column)
j = l + 1;
else
j = l;
minorMtx[k,l] = this[i,j];
}
}
return minorMtx;
}
//determinent
public float Determinant()
{
float determinent = 0;
if(this.Rows != this.Columns)
throw new RtwMatrixException("Attempt to find the determinent of a non square matrix");
//return 0;
//get the determinent of a 2x2 matrix
if(this.Rows == 2 && this.Columns == 2)
{
determinent = (this[0,0] * this[1,1]) - (this[0,1] * this[1,0]);
return determinent;
}
RtwMatrix tempMtx = new RtwMatrix(this.Rows - 1, this.Columns - 1);
//find the determinent with respect to the first row
for(int j = 0; j < this.Columns; j++)
{
tempMtx = this.Minor(0, j);
//recursively add the determinents
determinent += (int)Math.Pow(-1, j) * this[0,j] * tempMtx.Determinant();
}
return determinent;
}
//clone matrix
public RtwMatrix Clone()
{
RtwMatrix new_matrix = new RtwMatrix(this.Rows, this.Columns);
for (int i = 0; i < new_matrix.Rows; i++)
{
for (int j = 0; j < new_matrix.Columns; j++)
{
new_matrix[i, j] = this[i, j];
}
}
return new_matrix;
}
//adjoint matrix
public RtwMatrix Adjoint()
{
if(this.Rows < 2 || this.Columns < 2)
throw new RtwMatrixException("Adjoint matrix not available");
RtwMatrix tempMtx = new RtwMatrix(this.Rows-1 , this.Columns-1);
RtwMatrix adjMtx = new RtwMatrix (this.Columns , this.Rows);
for(int i = 0; i < this.Rows; i++)
{
for(int j = 0; j < this.Columns;j++)
{
tempMtx = this.Minor(i, j);
//put the determinent of the minor in the transposed position
adjMtx[j,i] = (int)Math.Pow(-1,i+j) * tempMtx.Determinant();
}
}
return adjMtx;
}
//transpose
public static RtwMatrix operator ~(RtwMatrix mtx)
{
RtwMatrix result = new RtwMatrix(mtx.Columns,mtx.Rows);
for(int i = 0; i < mtx.Rows; i++)
{
for(int j = 0; j < mtx.Columns; j++)
{
result[j,i] = mtx[i,j];
}
}
return result;
}
//gradiant magnitude
public static RtwMatrix operator ^(RtwMatrix lmtx, RtwMatrix rmtx)
{
if (lmtx.Columns != rmtx.Columns || lmtx.Rows != rmtx.Rows)
throw new RtwMatrixException("Attempt to element multiply matrices with unmatching row and column indexes");
//return null;
RtwMatrix result = new RtwMatrix(lmtx.Rows, lmtx.Columns);
for (int i = 0; i < lmtx.Rows; i++)
{
for (int j = 0; j < lmtx.Columns; j++)
{
result[i, j] = (float)Math.Sqrt(lmtx[i, j] * lmtx[i,j] + rmtx[i,j] * rmtx[i, j]);
}
}
return result;
}
//devision by const
public static RtwMatrix operator /(RtwMatrix mtx, float val)
{
if(val == 0)
throw new RtwMatrixException("Attempt to devide the matrix by zero");
//return null;
RtwMatrix result = new RtwMatrix(mtx.Rows,mtx.Columns);
for(int i = 0; i < mtx.Rows; i++)
{
for(int j = 0; j < mtx.Columns; j++)
{
result[i,j] = mtx[i,j] / val;
}
}
return result;
}
//subtraction
public static RtwMatrix operator -(RtwMatrix lmtx, RtwMatrix rmtx)
{
if(lmtx.Rows != rmtx.Rows || lmtx.Columns != rmtx.Columns)
throw new RtwMatrixException("Attempt to subtract matrixes of different sizes");
//return null;
RtwMatrix result = new RtwMatrix(lmtx.Rows,lmtx.Columns);
for(int i = 0; i < lmtx.Rows; i++)
{
for(int j = 0; j < lmtx.Columns; j++)
{
result[i,j] = lmtx[i,j] - rmtx[i,j];
}
}
return result;
}
//addition with const
public static RtwMatrix operator +(RtwMatrix lmtx, float val)
{
RtwMatrix result = new RtwMatrix(lmtx.Rows, lmtx.Columns);
for (int i = 0; i < lmtx.Rows; i++)
{
for (int j = 0; j < lmtx.Columns; j++)
{
result[i, j] = lmtx[i, j] + val;
}
}
return result;
}
public void Run()
{
int BatchCount = 1;
if (curRequest.BatchRun)
{
BatchCount = curRequest.RunTimes;
}
// First do reachable area (Dist and Diff)
RtwMatrix mDistReachable;
RtwMatrix mDiffReachable;
if (curRequest.T < curRequest.DistMap.Rows + curRequest.DistMap.Columns)
{
mDistReachable = curRequest.DistMap.Clone();
mDiffReachable = curRequest.DiffMap.Clone();
if (!ComputeReachableArea(mDistReachable, mDiffReachable))
{
// Cannot plan path.
return;
}
}
else
{
mDistReachable = curRequest.DistMap.Clone();
mDiffReachable = curRequest.DiffMap.Clone();
}
// Then do mode count (If Diff map is used, multiply first)
CountDistModes myCount;
if (curRequest.UseTaskDifficultyMap)
{
RtwMatrix mRealModes = new RtwMatrix(mDistReachable.Rows, mDistReachable.Columns);
for (int i = 0; i < mRealModes.Rows; i++)
{
for (int j = 0; j < mRealModes.Columns; j++)
{
mRealModes[i, j] = mDistReachable[i, j] *
(float)curRequest.DiffRates[Convert.ToInt32(mDiffReachable[i, j])];
}
}
myCount = new CountDistModes(mRealModes);
}
else
{
myCount = new CountDistModes(mDistReachable);
}
int ModeCount = myCount.GetCount();
RtwMatrix mModes = myCount.GetModes();
myCount = null;
//Console.WriteLine("ModeCount = " + ModeCount);
// Then do efficiency lower bound
ComputeEfficiencyUB myELB = new ComputeEfficiencyUB(curRequest, mDistReachable, mDiffReachable);
double Efficiency_UB = myELB.GetEfficiency_UB();
myELB = null;
// Set sample rate based on max value in mdistReachable.
float max = mDistReachable.MinMaxValue()[1];
ProjectConstants.DownSample_Rate = max / 10f;
// Do the batch run of Path Planning
List<double> AllRunTimes = new List<double>();
List<double> AllEfficiencies = new List<double>();
for (int i = 0; i < BatchCount; i++)
{
// Run them sequencially and don't use multiple threads.
PathPlanningTask curTask = new PathPlanningTask(curRequest, ModeCount, mModes, mDistReachable, mDiffReachable, Efficiency_UB);
curTask.Run();
AllRunTimes.Add(curTask.GetRunTime());
AvgRunTime += curTask.GetRunTime();
AllEfficiencies.Add(curTask.GetEfficiency());
AvgEfficiency += curTask.GetEfficiency();
Path = curTask.GetPath();
curTask = null;
}
AvgRunTime = AvgRunTime / BatchCount;
StdRunTime = ComputeStDev(AllRunTimes, AvgRunTime);
AvgEfficiency = AvgEfficiency / BatchCount;
StdEfficiency = ComputeStDev(AllEfficiencies, AvgEfficiency);
// Log path planning activities
if (ProjectConstants.DebugMode)
{
curRequest.SetLog("----------------------------------------------\n");
curRequest.SetLog("Average run time: " + AvgRunTime.ToString() + "\n");
curRequest.SetLog("Standard deviation: " + StdRunTime.ToString() + "\n");
curRequest.SetLog("Average efficiency: " + AvgEfficiency.ToString() + "\n");
curRequest.SetLog("Standard deviation: " + StdEfficiency.ToString() + "\n");
curRequest.SetLog("----------------------------------------------");
curRequest.SetLog("----------------------------------------------\n");
}
}
// Compute the reachable area and might as well compute distance to closest non-zero node
private bool ComputeReachableArea(RtwMatrix mDistReachable, RtwMatrix mDiffReachable)
{
// Code is cleaner to just deal two cases seperately.
if (!curRequest.UseEndPoint)
{
Point Start = new Point(curRequest.pStart.column, curRequest.pStart.row);
int d = curRequest.T;
for (int y = 0; y < mDistReachable.Rows; y++)
{
for (int x = 0; x < mDistReachable.Columns; x++)
{
int dist = MISCLib.ManhattanDistance(x, y, Start.X, Start.Y);
if (dist >= curRequest.T)
{
// Wipe cell in both maps clean
mDistReachable[y, x] = 0;
mDiffReachable[y, x] = 0;
}
if (dist < d && mDistReachable[y, x] != 0)
{
d = dist;
}
}
}
curRequest.d = d;
}
else
{
Point Start = new Point(curRequest.pStart.column, curRequest.pStart.row);
Point End = new Point(curRequest.pEnd.column, curRequest.pEnd.row);
int d = curRequest.T;
int dist = MISCLib.ManhattanDistance(Start.X, Start.Y, End.X, End.Y);
if (dist > curRequest.T)
{
// Impossible to get from A to B in allowed flight time
System.Windows.Forms.MessageBox.Show("Impossible! Extend flight time!");
return false;
}
if (curRequest.VehicleType == UAVType.FixWing && curRequest.T % 2 != dist % 2)
{
// Impossible to get from A to B in the exact allowed flight time
System.Windows.Forms.MessageBox.Show("Impossible to reach end point at time T! Add 1 or minus 1!");
return false;
}
for (int y = 0; y < mDistReachable.Rows; y++)
{
for (int x = 0; x < mDistReachable.Columns; x++)
{
int dist_AC = MISCLib.ManhattanDistance(x, y, Start.X, Start.Y);
int dist_BC = MISCLib.ManhattanDistance(x, y, End.X, End.Y);
if ((dist_AC + dist_BC) > curRequest.T)
{
// Wipe cell in both maps clean
mDistReachable[y, x] = 0;
mDiffReachable[y, x] = 0;
}
dist = MISCLib.ManhattanDistance(x, y, Start.X, Start.Y);
if (dist < d && mDistReachable[y, x] != 0)
{
d = dist;
}
}
}
curRequest.d = d;
}
return true;
}
//multiplication by matrix
public static RtwMatrix operator *(RtwMatrix lmtx, RtwMatrix rmtx)
{
if(lmtx.Columns != rmtx.Rows)
throw new RtwMatrixException("Attempt to multiply matrices with unmatching row and column indexes");
//return null;
RtwMatrix result = new RtwMatrix(lmtx.Rows,rmtx.Columns);
for(int i = 0; i < lmtx.Rows; i++)
{
for(int j = 0; j < rmtx.Columns; j++)
{
for(int k = 0; k < rmtx.Rows; k++)
{
result[i,j] += lmtx[i,k] * rmtx[k,j];
}
}
}
return result;
}
public void DrawReachableRegion(RtwMatrix mask)
{
// Apply mask
int rows = mask.Rows;
int columns = mask.Columns;
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < columns; j++)
{
if (mask[i, j] != 1)
{
Color c = new Color();
c = Color.FromArgb(0, 0, 0);
DisplayMap.SetPixel(j, i, c);
}
}
}
// Showing the image
pbMap.Image = DisplayMap;
}
//multiplication by const
public static RtwMatrix operator *(RtwMatrix mtx, float val)
{
RtwMatrix result = new RtwMatrix(mtx.Rows,mtx.Columns);
for(int i = 0; i < mtx.Rows; i++)
{
for(int j = 0; j < mtx.Columns; j++)
{
result[i,j] = mtx[i,j] * val;
}
}
return result;
}