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