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