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