public static void init_objects(List <GPS> start, List <GPS> end, List <double> bounding_box, double new_grid_width) { //Initialize global variables width = (int)Math.Abs(bounding_box[0] - bounding_box[2]); height = (int)Math.Abs(bounding_box[1] - bounding_box[3]); num_auvs = start.Count; //Initialize auvs for (int i = 0; i < start.Count; i++) { //Console.WriteLine ("Adding " + i); //Declare new auv and first waypoint int j = num_auvs - i; Auv current = new Auv(i, end[i].long_coord, end[i].lat_coord); current.add_waypoint(128, 0, 0, start[i].long_coord, start[i].lat_coord, start[i].heading, -1, 0); //Auv current = new Auv(i, 180, j * 40 + 5); //current.add_waypoint(128, 0, 0, 20, (i + 1) * 40 + 20, 0, -1, 0); //Console.WriteLine ("Added first waypoint"); //Add current auv to auvs curr_auvs.Add(current); //Console.WriteLine ("Add new auv to list"); } }
public static double VELOCITY = 1; //Velocity in m/s #endregion Fields #region Methods public static void init_objects(List<GPS> start, List<GPS> end, List<double> bounding_box, double new_grid_width) { //Initialize global variables width = (int)Math.Abs(bounding_box[0] - bounding_box[2]); height = (int)Math.Abs(bounding_box[1] - bounding_box[3]); num_auvs = start.Count; //Initialize auvs for (int i = 0; i < start.Count; i++) { //Console.WriteLine ("Adding " + i); //Declare new auv and first waypoint int j = num_auvs - i; Auv current = new Auv(i, end[i].long_coord, end[i].lat_coord); current.add_waypoint(128, 0, 0, start[i].long_coord, start[i].lat_coord, start[i].heading, -1, 0); //Auv current = new Auv(i, 180, j * 40 + 5); //current.add_waypoint(128, 0, 0, 20, (i + 1) * 40 + 20, 0, -1, 0); //Console.WriteLine ("Added first waypoint"); //Add current auv to auvs curr_auvs.Add(current); //Console.WriteLine ("Add new auv to list"); } }
public static double VELOCITY = 2; //Velocity in m/s #endregion Fields #region Methods public static void Main(string[] args) { //Initialize obstacle array List<Obstacle> obstacles = new List<Obstacle>(); //Initialize auvs for (int i = 0; i < num_auvs; i++) { //Declare new auv and first waypoint int j = num_auvs - i; Auv current = new Auv(i, 180, j*40 + 20); current.add_waypoint(128, 0, 0, 20, (i+1)*40 + 20, 0, -1, 0); //Add current auv to auvs auvs.Add(current); //Plot Data } //Define obstacles List<Point> obstaclepoints = new List<Point>(); obstaclepoints.Add(new Point(50, 50)); obstaclepoints.Add(new Point(50, 75)); obstaclepoints.Add(new Point(75, 75)); obstaclepoints.Add(new Point(75, 50)); obstacles.Add(new Obstacle(obstaclepoints)); //Prob Roadmap for (int a = 0; a < auvs.Count; a++) { //Start with next waypoint after start int count = 1; int last_point = 0; //Create current line to target List<Point> current_line = new List<Point>(); current_line.Add (new Point(auvs[a].x_target, auvs[a].y_target)); current_line.Add (new Point(auvs[a].get_waypoint(last_point).x_est, auvs[a].get_waypoint(last_point).y_est)); //While there is no path to target, keep adding to tree while (intersect.intersect_obstacles(obstacles, current_line) || !intersect.is_reachable(last_point, auvs[a])) { Random rand = new Random(); int yaw_rand = rand.Next(0, 255); //Change to double? int dt_rand = rand.Next(0, MAX_DT); Waypoint rand_point = auvs[a].pick_waypoint(); double prev_t = rand_point.t_step; double t_step = prev_t + dt_rand; Waypoint new_point = sim_motion(rand_point, yaw_rand, dt_rand, t_step, count); List<Point> new_line = new List<Point>(); new_line.Add (new Point(rand_point.x_est, rand_point.y_est)); new_line.Add (new Point(new_point.x_est, new_point.y_est)); if (!intersect.intersect_obstacles(obstacles, new_line) && !intersect.intersects_path(auvs[a], rand_point.index, new_point.x_est, new_point.y_est, rand_point.t_step, dt_rand)) { auvs[a].add_waypoint(new_point); last_point = count; count++; } } //Create output } }
public static void Main(string[] args) { //Initialize obstacle array List <Obstacle> obstacles = new List <Obstacle>(); //Initialize auvs for (int i = 0; i < num_auvs; i++) { //Declare new auv and first waypoint int j = num_auvs - i; Auv current = new Auv(i, 180, j * 40 + 20); current.add_waypoint(128, 0, 0, 20, (i + 1) * 40 + 20, 0, -1, 0); //Add current auv to auvs auvs.Add(current); //Plot Data } //Define obstacles List <Point> obstaclepoints = new List <Point>(); obstaclepoints.Add(new Point(50, 50)); obstaclepoints.Add(new Point(50, 75)); obstaclepoints.Add(new Point(75, 75)); obstaclepoints.Add(new Point(75, 50)); obstacles.Add(new Obstacle(obstaclepoints)); //Prob Roadmap for (int a = 0; a < auvs.Count; a++) { //Start with next waypoint after start int count = 1; int last_point = 0; //Create current line to target List <Point> current_line = new List <Point>(); current_line.Add(new Point(auvs[a].x_target, auvs[a].y_target)); current_line.Add(new Point(auvs[a].get_waypoint(last_point).x_est, auvs[a].get_waypoint(last_point).y_est)); //While there is no path to target, keep adding to tree while (intersect.intersect_obstacles(obstacles, current_line) || !intersect.is_reachable(last_point, auvs[a])) { Random rand = new Random(); int yaw_rand = rand.Next(0, 255); //Change to double? int dt_rand = rand.Next(0, MAX_DT); Waypoint rand_point = auvs[a].pick_waypoint(); double prev_t = rand_point.t_step; double t_step = prev_t + dt_rand; Waypoint new_point = sim_motion(rand_point, yaw_rand, dt_rand, t_step, count); List <Point> new_line = new List <Point>(); new_line.Add(new Point(rand_point.x_est, rand_point.y_est)); new_line.Add(new Point(new_point.x_est, new_point.y_est)); if (!intersect.intersect_obstacles(obstacles, new_line) && !intersect.intersects_path(auvs[a], rand_point.index, new_point.x_est, new_point.y_est, rand_point.t_step, dt_rand)) { auvs[a].add_waypoint(new_point); last_point = count; count++; } } //Create output } }