public static bool is_reachable(bool whole_path, int last_point, Auv current)
{
Waypoint last = current.get_waypoint(last_point);
double x_target = current.x_target;
double y_target = current.y_target;
double rho = Math.Atan2(y_target - last.y_est, x_target - last.x_est) * 180 / Math.PI;
if (rho <= 0)
{
rho = rho + 360;
}
if (last.theta_est < 0)
{
last.theta_est += 360;
}
double diff = Math.Abs(rho - last.theta_est);
if (diff < 30)
{
double dist = Math.Sqrt((last.y_est - y_target) * (last.y_est - y_target) + (last.x_est - x_target) * (last.x_est - x_target));
double prev_t = last.t_step;
double final_time = last.t_step + dist / MainClass.VELOCITY;
if (!intersects_path(whole_path, current, last_point, x_target, y_target, prev_t, final_time))
{
return(true);
}
}
return(false);
}
//public static bool intersects_path(bool whole_path, Auv current, int rand_point, double x_est, double y_est, double prev_t, double t_step)
//{
// Waypoint last_point = current.get_waypoint(rand_point);
// for (int i = 0; i < current.count; i++)
// {
// for (int j = 1; j < MainClass.curr_auvs[i].output.Count; j++)
// {
// Waypoint curr = MainClass.curr_auvs[i].output[j];
// Waypoint prev = MainClass.curr_auvs[i].output[j - 1];
// double curr_step = curr.t_step;
// double prev_step = prev.t_step;
// if (whole_path)
// {
// if (line_intersect(last_point.x_est, last_point.y_est, x_est, y_est, prev.x_est, prev.y_est, curr.x_est, curr.y_est))
// {
// return true;
// }
// }
// else
// {
// if (((prev_step > prev_t) && (prev_step < t_step)) || ((curr_step > prev_t) && (curr_step < t_step)) || ((prev_step < prev_t) && (curr_step > t_step)))
// {
// //Console.WriteLine("COLIDING");
// if (line_intersect(last_point.x_est, last_point.y_est, x_est, y_est, prev.x_est, prev.y_est, curr.x_est, curr.y_est))
// {
// return true;
// }
// }
// }
// }
// }
// return false;
//}
public static bool intersects_path(bool whole_path, Auv current, int rand_point, double x_est, double y_est, double prev_t, double t_step)
{
Waypoint last_point = current.get_waypoint(rand_point);
//for each of the previous auvs
for (int i = 0; i < current.count; i++)
{
int min_index = MainClass.curr_auvs[i].output_time.BinarySearch(prev_t);
int max_index = MainClass.curr_auvs[i].output_time.BinarySearch(t_step);
if (min_index < 0)
{
min_index = ~min_index;
}
if (max_index < 0)
{
max_index = ~max_index;
}
if (min_index != 0)
{
min_index -= 1;
}
if (max_index == MainClass.curr_auvs[i].output_time.Count)
{
max_index -= 1;
}
//Console.WriteLine("------------------------------");
//Console.WriteLine("Intersecting (" + prev_t + ", " + t_step + ") against");
foreach (double time in MainClass.curr_auvs[i].output_time)
{
//Console.WriteLine(time);
}
//Console.WriteLine("Min = " + min_index + ", Max = " + max_index + ", " + MainClass.curr_auvs[i].output_time.Count);
//Console.WriteLine("Time segement (" + prev_t + ", " + t_step + ") overlaps (" + MainClass.curr_auvs[i].output_time[min_index] + ", " + MainClass.curr_auvs[i].output_time[max_index] + ")");
int num_segments = max_index - min_index;
for (int j = 0; j < num_segments; j++)
{
Waypoint curr = MainClass.curr_auvs[i].output[min_index + j + 1];
Waypoint prev = MainClass.curr_auvs[i].output[min_index + j];
double curr_step = curr.t_step;
double prev_step = prev.t_step;
if (line_intersect(last_point.x_est, last_point.y_est, x_est, y_est, prev.x_est, prev.y_est, curr.x_est, curr.y_est))
{
return true;
}
}
}
return false;
}
//public static bool intersects_path(bool whole_path, Auv current, int rand_point, double x_est, double y_est, double prev_t, double t_step)
//{
// Waypoint last_point = current.get_waypoint(rand_point);
// for (int i = 0; i < current.count; i++)
// {
// for (int j = 1; j < MainClass.curr_auvs[i].output.Count; j++)
// {
// Waypoint curr = MainClass.curr_auvs[i].output[j];
// Waypoint prev = MainClass.curr_auvs[i].output[j - 1];
// double curr_step = curr.t_step;
// double prev_step = prev.t_step;
// if (whole_path)
// {
// if (line_intersect(last_point.x_est, last_point.y_est, x_est, y_est, prev.x_est, prev.y_est, curr.x_est, curr.y_est))
// {
// return true;
// }
// }
// else
// {
// if (((prev_step > prev_t) && (prev_step < t_step)) || ((curr_step > prev_t) && (curr_step < t_step)) || ((prev_step < prev_t) && (curr_step > t_step)))
// {
// //Console.WriteLine("COLIDING");
// if (line_intersect(last_point.x_est, last_point.y_est, x_est, y_est, prev.x_est, prev.y_est, curr.x_est, curr.y_est))
// {
// return true;
// }
// }
// }
// }
// }
// return false;
//}
public static bool intersects_path(bool whole_path, Auv current, int rand_point, double x_est, double y_est, double prev_t, double t_step)
{
Waypoint last_point = current.get_waypoint(rand_point);
//for each of the previous auvs
for (int i = 0; i < current.count; i++)
{
int min_index = MainClass.curr_auvs[i].output_time.BinarySearch(prev_t);
int max_index = MainClass.curr_auvs[i].output_time.BinarySearch(t_step);
if (min_index < 0)
{
min_index = ~min_index;
}
if (max_index < 0)
{
max_index = ~max_index;
}
if (min_index != 0)
{
min_index -= 1;
}
if (max_index == MainClass.curr_auvs[i].output_time.Count)
{
max_index -= 1;
}
//Console.WriteLine("------------------------------");
//Console.WriteLine("Intersecting (" + prev_t + ", " + t_step + ") against");
foreach (double time in MainClass.curr_auvs[i].output_time)
{
//Console.WriteLine(time);
}
//Console.WriteLine("Min = " + min_index + ", Max = " + max_index + ", " + MainClass.curr_auvs[i].output_time.Count);
//Console.WriteLine("Time segement (" + prev_t + ", " + t_step + ") overlaps (" + MainClass.curr_auvs[i].output_time[min_index] + ", " + MainClass.curr_auvs[i].output_time[max_index] + ")");
int num_segments = max_index - min_index;
for (int j = 0; j < num_segments; j++)
{
Waypoint curr = MainClass.curr_auvs[i].output[min_index + j + 1];
Waypoint prev = MainClass.curr_auvs[i].output[min_index + j];
double curr_step = curr.t_step;
double prev_step = prev.t_step;
if (line_intersect(last_point.x_est, last_point.y_est, x_est, y_est, prev.x_est, prev.y_est, curr.x_est, curr.y_est))
{
return(true);
}
}
}
return(false);
}
public static bool is_reachable(int last_point, Auv current)
{
Waypoint last = current.get_waypoint(last_point);
double x_target = current.x_target;
double y_target = current.y_target;
double rho = Math.Atan2(last.y_est - y_target, last.x_est - x_target)*180/Math.PI;
if (rho <= 0) {
rho = rho + 360;
}
double diff = Math.Abs(rho-last.theta_est);
if (diff < 30) {
double dist = Math.Sqrt((last.y_est - y_target)*(last.y_est - y_target)+(last.x_est - x_target)*(last.x_est - x_target));
double prev_t = last.t_step;
double final_time = last.t_step + dist/MainClass.VELOCITY;
if (!intersects_path(current, last_point, x_target, y_target, prev_t, final_time)) {
return true;
}
}
return false;
}
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 bool intersects_path(Auv current, int rand_point, double x_est, double y_est, double prev_t, double t_step)
{
Waypoint last_point = current.get_waypoint(rand_point);
for (int i = 0; i < current.count; i++)
{
for (int j = 0; j < MainClass.auvs.Count; j++)
{
Waypoint curr = MainClass.auvs[i].output[j];
Waypoint prev = MainClass.auvs[i].output[j - 1];
double curr_step = curr.t_step;
double prev_step = prev.t_step;
if (((prev_step > prev_t) && (prev_step < t_step)) || ((curr_step > prev_t) && (curr_step < t_step)) || ((prev_step < prev_t) && (curr_step > t_step)))
{
if (line_intersect(last_point.x_est, last_point.y_est, x_est, y_est, prev.x_est, prev.y_est, curr.x_est, curr.y_est))
{
return(true);
}
}
}
}
return(false);
}
public static bool intersects_path(Auv current, int rand_point, double x_est, double y_est, double prev_t, double t_step)
{
Waypoint last_point = current.get_waypoint(rand_point);
for (int i = 0; i < current.count; i++) {
for (int j = 0; j < MainClass.auvs.Count; j++) {
Waypoint curr = MainClass.auvs[i].output[j];
Waypoint prev = MainClass.auvs[i].output[j-1];
double curr_step = curr.t_step;
double prev_step = prev.t_step;
if (((prev_step > prev_t) && (prev_step < t_step)) || ((curr_step > prev_t) && (curr_step < t_step)) || ((prev_step < prev_t) && (curr_step > t_step))) {
if (line_intersect (last_point.x_est, last_point.y_est, x_est, y_est, prev.x_est, prev.y_est, curr.x_est, curr.y_est)) {
return true;
}
}
}
}
return false;
}
public void run_plan()
{
//Invalidate();
//Console.WriteLine("In Run Plan");
Thread old = t;
t = new Thread(new ThreadStart(run_simulation));
//Console.WriteLine("Making stuff");
if (!replan)
{
bounding_box = new List <double>();
obstacles = new List <Obstacle>();
}
locations = new List <List <double> >();
target_waypoints = new List <int>();
finished = new List <bool>();
actual_finished = new List <bool>();
whole_path_intersect = whole_path.Checked;
iterations = Convert.ToInt32(iteration_input.Text);
start = new List <GPS>();
end = new List <GPS>();
load_scenario();
if (replan)
{
for (int j = 0; j < MainClass.auvs.Count; j++)
{
Auv curr = MainClass.auvs[j];
double theta = Math.Atan2(curr.output[actual_target_waypoints[j]].y_est - actual_locations[j][1], curr.output[actual_target_waypoints[0]].x_est - actual_locations[j][0]) * (180 / Math.PI);
start.Add(new GPS(last_locations[j][0], last_locations[j][1], theta)); //CHANGE TO NOT ZERO
end.Add(new GPS(MainClass.auvs[j].x_target, MainClass.auvs[j].y_target, 0));
}
}
//Reset actual_locations
actual_locations = new List <List <double> >();
actual_target_waypoints = new List <int>();
MainClass.run_motion_planner(whole_path_intersect, iterations, start, end, bounding_box, obstacles, grid_width);
if (simulate.Checked)
{
//Console.WriteLine("Simulate Checked");
int count = 0;
foreach (Auv curr_auv in MainClass.auvs)
{
List <double> current_list = new List <double>();
current_list.Add(curr_auv.waypoints[0].x_est);
current_list.Add(curr_auv.waypoints[0].y_est);
finished.Add(false);
target_waypoints.Add(0);
locations.Add(current_list);
current_list = new List <double>();
current_list.Add(curr_auv.waypoints[0].x_est);
current_list.Add(curr_auv.waypoints[0].y_est);
actual_finished.Add(false);
actual_target_waypoints.Add(0);
actual_locations.Add(current_list);
//Console.WriteLine("Adding Point (" + locations[count][0] + ", " + locations[count][1] + ")");
count++;
}
t.Start();
old.Abort();
}
else
{
Invalidate();
}
}
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
}
}
