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
