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