private LinkedList <Point> BestFirstSearch(int a, int b, Robot r, Obstacle[] o)
{
int[,] visited = new int[width, height];
Console.WriteLine("Start Point: " + a + "," + b + " = " + MyGetPixel(a, b));
for (int i = 0; i < width; i++)
{
for (int j = 0; j < height; j++)
{
if (MyGetPixel(i, j) == 255)
{
visited[i, j] = 1;
}
}
}
LinkedList <Point> open = new LinkedList <Point>();
RobotPath robotpath = new RobotPath(r.GetNumOfPoly(), r.GetPolygons(), r.GetInitCenterPoint(), initP);
open = new LinkedList <Point>();
open.AddLast(getP(a, b));
visited[a, b] = 1;
bool success = false;
while (open.Count != 0 && !success)
{
List <Point> neighbors = new List <Point>();
Point target = open.Last();
if (target.X > 1 && visited[target.X - 1, target.Y] == 0)
{
neighbors.Add(getP(target.X - 1, target.Y));
}
if (target.X < 127 && visited[target.X + 1, target.Y] == 0)
{
neighbors.Add(getP(target.X + 1, target.Y));
}
if (target.Y > 1 && visited[target.X, target.Y - 1] == 0)
{
neighbors.Add(getP(target.X, target.Y - 1));
}
if (target.Y < 127 && visited[target.X, target.Y + 1] == 0)
{
neighbors.Add(getP(target.X, target.Y + 1));
}
if (neighbors.Count == 0)
{
open.RemoveLast();
}
else
{
int min = findMin(neighbors);
for (int i = 0; i < neighbors.Count; i++)
{
if (MyGetPixel(neighbors[i].X, neighbors[i].Y) == min && visited[neighbors[i].X, neighbors[i].Y] == 0 && robotpath.MoveWithCenter(neighbors[i], o) == true)
{
Console.WriteLine("Min: " + neighbors[i].X + "," + neighbors[i].Y + " value = " + min);
Console.WriteLine(robotpath.MoveWithCenter(neighbors[i], o));
//install x’ in T with a pointer toward x
open.AddLast(neighbors[i]);
visited[neighbors[i].X, neighbors[i].Y] = 1;
if (neighbors[i].X == goalP[0].X && neighbors[i].Y == goalP[0].Y)
{
success = true;
}
break;
}
else if (i == (neighbors.Count - 1))
{
Console.WriteLine("i = " + i);
open.RemoveLast();
Console.WriteLine("open.Last: " + open.Last.Value.X + "," + open.Last.Value.Y);
break;
}
}
neighbors.Clear();
}
}
if (success)
{
//return the constructed path by tracing the pointers in T from Xgoal back to Xinit
Console.WriteLine(open.Count);
return(open);
}
else
{
return(null);
}
}
private void button2_Click(object sender, EventArgs e)
{
if (collision == true || outOfBorder == true)
{
label1.Text = "Some Collisions occurs.\n\nCannot find the path.";
}
else
{
label1.Text = "Wait For a while...";
drawPoly.Clear();
Obstacle[] obstacles = new Obstacle[readobstacle.GetNumOfObstacle()];
for (int i = 0; i < readobstacle.GetNumOfObstacle(); i++)
{
obstacles[i] = readobstacle.GetObstacle(i);
}
for (int i = 0; i < readrobot.GetNumOfRobots(); i++)
{
Robot robot = readrobot.GetRobot(i);
bmp = new MyBitmap(128, 128, /*robot.GetControlPoint().Length*/ 1);
SetObstacleField();
bmp.SetRobotPixel(robot);
bmp.GetTotalPixel();
LinkedList <Point> path = bmp.FindPath(robot, obstacles);
/*LinkedList<Point>[] path = new LinkedList<Point>[robot.GetControlPoint().Length];
* for(int j = 0; j < path.Length; j++)
* {
* path[j] = new LinkedList<Point>();
* bmp.FindPath(robot, obstacles, j);
* }*/
if (path == null)
{
Console.WriteLine("Found Fail !");
label1.Text = "Found Fail !";
}
else
{
label1.Text = "Found Success !";
findSuccess = true;
PointF moveCenter = robot.GetInitCenterPoint();
Point[] controlP = new Point[robot.GetControlPoint().Length];
for (int c = 0; c < robot.GetControlPoint().Length; c++)
{
controlP[i] = new Point();
controlP[i].X = (int)robot.GetControlPoint()[i].X;
controlP[i].Y = (int)robot.GetControlPoint()[i].Y;
}
while (path.Count != 0)
{
Point point = path.First.Value;
RobotPath robotpaths = new RobotPath(robot.GetNumOfPoly(), robot.GetPolygons(), moveCenter, controlP);
Polygon[] poly = (Polygon[])robotpaths.GetTransfer(point).Clone();
moveCenter = robotpaths.GetPlanCenter();
DrawRobotPath(poly, 0);
//drawPoly.AddLast(shallowCopy(poly,moveCenter));
path.RemoveFirst();
}
}
}
bmp.Clear();
}
}
