def test_geofun(self): p1 = Position(52, -178) l1 = Line(p1, Position(51, -177)) self.assertAlmostEqual(52, l1.p1.lat) l2 = Line(Position(48, 178), Position(52, -178)) self.assertAlmostEqual(131147, l1.v.r, delta=1) self.assertAlmostEqual(529213, l2.v.r, delta=1)
def set_sections_orientation(self, point_on_water): # Determine the orientation of each section for section in self.sections: l = Line(section, section.first) # Create a point just right of the first segment p = section + l.v * 0.5 + Vector(l.v.a + 0.5 * math.pi, 1) l = Line(point_on_water, p) intersects = self.__intersects(l) section.orientation = len(intersects) % 2
def test_navigable(self): # Check dutch coastline just north of Hoek van Holland l_miss = Line(Position(52.1, 4.122), Position(52.01, 4.122)) l_hit = Line(Position(52.01, 4.122), Position(52.00, 4.124)) n_miss = self.chart.is_navigable(l_miss)[0] self.assertTrue(n_miss) n_hit = self.chart.is_navigable(l_hit)[0] self.assertFalse(n_hit) r = self.chart.is_navigable((l_miss, l_hit)) n_miss = r[0] n_hit = r[1] self.assertTrue(n_miss) self.assertFalse(n_hit)
def get_on_land(self, position): if self._chart is not None: line = Line(self._start, position) hits = self._chart.intersections(line) return len(hits) % 2 == 1 else: return False
def clean_path(path): """Try and remove points and still not hit land""" prev_len = len(path) for i in xrange(len(path) - 2, 0, -1): l = Line(path[i - 1], path[i + 1]) if not self.hit(l): path.pop(i) return len(path) != prev_len
def test_line_doesnt_self_intersect(self): p1 = Position(0.0, 0.0) p2 = Position(0.1, 0.1) l1 = Line(p1, p2) l2 = Line(p1, p2) l3 = Line(p2, p1) self.assertFalse(l1.intersects(l2)) self.assertFalse(l1.intersects(l3)) self.assertFalse(l3.intersects(l1))
def similar(self, other): if other is None: return False if len(self) != len(other): return False for p1, p2 in zip(self, other): l = Line(p1, p2) # When points are more that 10m apart, consider them different if l.v.r > 20: return False return True
def prevent_beaching(self, heading, look_ahead=None): if look_ahead == None: # 30 seconds of sailing look_ahead = self.boat.motion.speed * 30 # We'll construct a future course line... boat_position = self.boat.position # ... project it ahead... sail_vector = Vector(heading, look_ahead) future_position = boat_position + sail_vector sail_line = Line(self.boat.position, future_position) # Check if the projected line hits land... if self.chart.hit(sail_line): # ... and if so, tack or gybe away from it wind = self.boat.condition.wind wind_angle = normalize_angle_pipi(wind[0] - heading) self.log("Tacked/gybed to avoid hitting land") return True, normalize_angle_2pi(heading + 2 * wind_angle) # Also, we want to keep a clear line of sight to the waypoint track, waypoint = self.navigator.active_leg bearing = waypoint - self.boat.position view_line = Line(self.boat.position, waypoint) if self.chart.hit(view_line): # The only way, we could have gotten something in the view # line is that we were reaching or tacking away from the # track. Tack or gybe now to get back. off_bearing_angle = normalize_angle_pipi(heading - bearing[0]) off_track = self.navigator.get_cross_track() # Check if heading is 'outside' bearing if (off_track > 0) == (off_bearing_angle > 0): # ...or tack/gybe when they don't wind = self.boat.condition.wind wind_angle = normalize_angle_pipi(wind[0] - heading) self.log("Tacked/gybed to avoid land getting in line of sight") return True, normalize_angle_2pi(heading + 2 * wind_angle) # Nothing needed to be done. Return the originally suggested heading return False, heading
def adjust_if_beaching(self, heading): track, waypoint = self.navigator.active_leg bearing = waypoint - self.boat.position view_line = Line(self.boat.position, waypoint) if self.chart.hit(view_line): # Actually, this shouldn't have happened as "prevent_beaching" should # have kept a clear view line already, but a verification won't hurt # when actually steering a direct course . # Steer towards the track perpendicularly to reduce the cte. # When the track itself hits land, we're fsckd, but that is someone # elses error and cannot be corrected here self.log("Navigate to track to avoid land") return True, self.navigator.to_track()[0] else: return False, heading
def push_out(points, offset=42, chart=None): if points is None: return None result = [] i = iter(points) try: cur = i.next() result.append(Position(cur)) try: nxt = i.next() v_from = nxt - cur try: while True: prv = cur cur = nxt nxt = i.next() v_to = v_from v_from = nxt - cur ad = angle_diff(v_from.a, v_to.a) o = offset if ad > 0: a = normalize_angle_2pi(v_to.a + 0.5 * (ad - pi)) elif ad < 0: a = normalize_angle_2pi(v_to.a + 0.5 * (ad + pi)) else: a = 0 o = 0 v = Vector(a, o) p = cur + v if chart is not None: l = Line(cur + v * 0.01, p) segment, intersect = chart.intersect(l) if intersect is not None: p = cur + (intersect - cur) * 0.5 result.append(p) except StopIteration: result.append(Position(nxt)) except StopIteration: pass except StopIteration: pass return result
def load_cell(self, cell_elem, cell): polys = get_elements(cell_elem, 'poly') for poly in polys: pl = [] points = get_elements(poly, 'point') if points: # Start with the last point point = points[-1] lat = float(point.getAttribute('lat')) lon = float(point.getAttribute('lon')) pos1 = Position(deg_to_rad(lat), deg_to_rad(lon)) for point in points: lat = float(point.getAttribute('lat')) lon = float(point.getAttribute('lon')) pos2 = Position(deg_to_rad(lat), deg_to_rad(lon)) # There are duplicates in the list (why is a bit unclear to me) if not (pos2 == pos1): # Avoid adding grid lines looks_like_grid = False if pos1.lat == pos2.lat: # This line is horizontal rounded = round(pos1.lat / self.cellsize) rounded *= self.cellsize if floats_equal(pos1.lat, rounded): looks_like_grid = True if pos1.lon == pos2.lon: # This line is vertical rounded = round(pos1.lon / self.cellsize) rounded *= self.cellsize if floats_equal(pos1.lon, rounded): looks_like_grid = True if not looks_like_grid: pl.append(Line(pos1, pos2)) pos1 = pos2 if len(pl) > 0: cell.append(pl)
def legs(self): last = self.start for mark in self.marks: yield Line(last, mark.target) last = mark.target yield Line(last, self.finish)
def test_line_doesnt_intersect_at_end(self): p1 = Position(0.0, 0.0) p2 = Position(0.1, 0.1) p3 = Position(0.1, 0.0) l1 = Line(p1, p2) l2 = Line(p1, p3) l3 = Line(p3, p1) l4 = Line(p3, p2) self.assertFalse(l1.intersects(l2)) self.assertFalse(l1.intersects(l3)) self.assertFalse(l1.intersects(l4)) self.assertFalse(l2.intersects(l1)) self.assertFalse(l3.intersects(l1)) self.assertFalse(l3.intersects(l1)) self.assertFalse(l4.intersects(l1))
def __init__(self, *args, **kwargs): super(Course, self).__init__() chart = None if len(args) > 0: waypoints = args[0] if len(args) > 1: finish_radius = args[1] if len(args) > 2: chart = args[2] try: waypoints = kwargs['waypoints'] except KeyError: pass try: finish_radius = kwargs['finish_radius'] except KeyError: pass try: chart = kwargs['map'] except KeyError: pass try: chart = kwargs['chart'] except KeyError: pass self._chart = chart self._start = Position(waypoints[0][0], waypoints[0][1]) self._finish = Finish(waypoints[-1][0], waypoints[-1][1]) # Enrich waypoints to turn them into marks for i in range(1, len(waypoints) - 1): prev = Position(waypoints[i - 1][0], waypoints[i - 1][1]) mark = Mark(waypoints[i][0], waypoints[i][1]) nxt = Position(waypoints[i + 1][0], waypoints[i + 1][1]) v1 = mark - prev v2 = nxt - mark a1 = angle_diff(v2.a, v1.a) mark.to_port = a1 < 0 mark.on_land = self.get_on_land(mark) if mark.to_port: a2 = normalize_angle_2pi(v1.a + 0.5 * (a1 + pi)) else: a2 = normalize_angle_2pi(v1.a + 0.5 * (a1 - pi)) mark.target_vector = Vector(a2, 1) if mark.on_land: intersection = None dist = 0 for j in range(-49, 50): a = a2 + j * (0.01 + 0.003 * math.fabs(a1)) perp = Line(mark, mark + Vector(a, 42000)) chart_segment, intersect = self._chart.intersect(perp) # We may not find an intersection for all angles if intersect is not None: v = intersect - mark if v.r > dist: dist = v.r intersection = intersect if intersection is None: raise Exception( 'No waterfront found for Mark %d. Mark far inland?' % i) mark.set_target(intersection + mark.target_vector * 42) else: mark.set_target(mark + mark.target_vector * 42) self._marks.append(mark) # Construct finish line finish_leg = self._finish - Position(waypoints[-2][0], waypoints[-2][1]) self._finish.left = self._finish + Vector(finish_leg.a - half_pi, finish_radius) self._finish.right = self._finish + Vector(finish_leg.a + half_pi, finish_radius)
def segments(self): prev_p = None for p in self: if prev_p is not None: yield Line(prev_p, p) prev_p = p