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
}
}
