def _calculate_distance_outlanding_leg(self, leg, start_tp_fix,
outlanding_fix):
if leg == 0:
tp1 = self.waypoints[leg + 1]
bearing = calculate_bearing(start_tp_fix, outlanding_fix)
closest_area_fix = calculate_destination(start_tp_fix, tp1.r_max,
bearing)
distance = calculate_distance(self.start.fix, closest_area_fix)
distance -= calculate_distance(outlanding_fix, closest_area_fix)
elif leg == self.no_legs - 1: # take finish-point of task
distance = calculate_distance(start_tp_fix, self.finish.fix)
distance -= calculate_distance(self.finish.fix, outlanding_fix)
else:
tp1 = self.waypoints[leg + 1]
bearing = calculate_bearing(tp1.fix, outlanding_fix)
closest_area_fix = calculate_destination(tp1.fix, tp1.r_max,
bearing)
if leg == 0:
distance = calculate_distance(self.start.fix, closest_area_fix)
else:
distance = calculate_distance(start_tp_fix, closest_area_fix)
distance -= calculate_distance(outlanding_fix, closest_area_fix)
return distance
def set_orientation_angles(waypoints):
# sector orientations and angles
for index in range(len(waypoints)):
if index == 0: # necessary for index out of bounds
angle = calculate_bearing(waypoints[index + 1].fix,
waypoints[index].fix,
final_bearing=True)
waypoints[index].set_orientation_angle(angle_next=angle)
elif index == len(
waypoints) - 1: # necessary for index out of bounds
angle = calculate_bearing(waypoints[index - 1].fix,
waypoints[index].fix,
final_bearing=True)
waypoints[index].set_orientation_angle(angle_previous=angle)
else:
angle_start = calculate_bearing(waypoints[0].fix,
waypoints[index].fix,
final_bearing=True)
angle_previous = calculate_bearing(waypoints[index - 1].fix,
waypoints[index].fix,
final_bearing=True)
angle_next = calculate_bearing(waypoints[index + 1].fix,
waypoints[index].fix,
final_bearing=True)
waypoints[index].set_orientation_angle(
angle_start=angle_start,
angle_previous=angle_previous,
angle_next=angle_next)
def crossed_line(self, fix1, fix2):
distance1 = calculate_distance(fix1, self.fix)
distance2 = calculate_distance(fix2, self.fix)
if not self.is_line:
raise ValueError('Calling crossed_line on a sector!')
else:
if distance2 > self.r_max and distance1 > self.r_max:
return False
else: # either both within circle or only one, leading to small amount of false positives
bearing1 = calculate_bearing(self.fix, fix1)
bearing2 = calculate_bearing(self.fix, fix2)
angle_wrt_orientation1 = abs(
calculate_bearing_difference(self.orientation_angle,
bearing1))
angle_wrt_orientation2 = abs(
calculate_bearing_difference(self.orientation_angle,
bearing2))
if self.sector_orientation == "next": # start line
return angle_wrt_orientation1 < 90 < angle_wrt_orientation2
elif self.sector_orientation == "previous": # finish line
return angle_wrt_orientation2 < 90 < angle_wrt_orientation1
else:
raise ValueError(
"A line with this orientation is not implemented!")
def inside_sector(self, fix):
distance = calculate_distance(fix, self.fix)
bearing = calculate_bearing(self.fix, fix)
angle_wrt_orientation = abs(calculate_bearing_difference(self.orientation_angle, bearing))
if self.is_line:
raise ValueError('Calling inside_sector on a line')
elif self.r_min is not None:
inside_outer_sector = self.r_min - self.SEEYOU_SECTOR_MARGIN < distance < self.r_max + self.SEEYOU_SECTOR_MARGIN and angle_wrt_orientation < self.angle_max
inside_inner_sector = distance < self.r_min and angle_wrt_orientation < self.angle_min
return inside_outer_sector or inside_inner_sector
else: # self.r_min is None
return distance < self.r_max + self.SEEYOU_SECTOR_MARGIN and (180 - angle_wrt_orientation) < self.angle_max
def distance_moved_turnpoint(distance,
begin,
end,
moved_point,
move_direction='reduce'):
from math import sqrt, cos, pi, acos
if moved_point == "begin":
moved = begin
other = end
angle_reduction = 0
elif moved_point == "end":
moved = end
other = begin
angle_reduction = 0
elif moved_point == "both_end":
moved = end
other = begin
original_distance = calculate_distance(begin.fix, end.fix)
distance_moved_current = begin.r_max if begin.angle_max == 180 else begin.r_min
angle_reduction = abs(
acos((distance_moved_current**2 - distance**2 -
original_distance**2) /
(-2 * distance * original_distance))) * 180 / pi
else:
raise ValueError("Displaced point is not recognized: %s" %
moved_point)
displacement_dist = moved.r_max if moved.angle_max == 180 else moved.r_min
bearing1 = moved.orientation_angle
bearing2 = calculate_bearing(other.fix, other.fix)
if move_direction == 'increase':
angle = 180 - abs(calculate_bearing_difference(
bearing1, bearing2)) - angle_reduction
else:
angle = abs(calculate_bearing_difference(
bearing1, bearing2)) - angle_reduction
distance = sqrt(distance**2 + displacement_dist**2 - 2 * distance *
displacement_dist * cos(angle * pi / 180))
return distance
