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 __init__(self, *args, **kwargs):
super(Course, self).__init__()
self._marks = []
self._start = Position()
self._finish = Finish()
if len(args) > 0:
it = iter(args[0])
try:
prev_p = it.next()
self._start = Position(prev_p[0], prev_p[1])
p = it.next()
while True:
next_p = it.next()
mark = self.mark_factory(p[0], p[1])
s1 = p - prev_p
s2 = next_p - p
s3 = next_p - prev_p
mark.to_port = angle_diff(s1.a, s3.a) >= 0
if mark.to_port:
s3.a = s3.a + half_pi
else:
s3.a = s3.a - half_pi
s3.r = 1
mark.set_target(mark + s3)
self._marks.append(mark)
prev_p = p
p = next_p
except StopIteration:
self._finish = Finish(p[0], p[1])
def testIntersection(self):
p1 = Position(0.99, 0.99)
p2 = Position(0.99, 1.0)
p3 = Position(1.0, 1.0)
p4 = Position(1.0, 0.99)
# Square
line1 = Line(p1, p2)
line2 = Line(p2, p3)
line3 = Line(p3, p4)
line4 = Line(p4, p1)
line5 = Line(p1, p3)
# Cross
line6 = Line(p2, p4)
line7 = Line(p4, p2)
pa = line5.intersection(line6)
pb = line6.intersection(line5)
pc = line5.intersection(line7)
bb = pb - pa
bc = pc - pa
#print pos_to_str(pa)
#print pos_to_str(pb), vec_to_str(bb)
#print pos_to_str(pc), vec_to_str(bc)
self.assertTrue(bb[1] < 200)
self.assertTrue(bc[1] < 200)
def testHit(self):
self.chart.load(dirname + '/Canary_Brazil.xml')
# Attempt to hit Sao Antao (Cabo Verde) from all sides
pos_to = Position(0.297869, -0.43921)
bearing = Vector(0, 30000)
pos_from = pos_to - bearing
segment = Line(pos_from, pos_to)
hit = self.chart.hit(segment)
self.assertTrue(hit)
bearing = Vector(1.57, 25000)
pos_from = pos_to - bearing
segment = Line(pos_from, pos_to)
hit = self.chart.hit(segment)
self.assertTrue(hit)
bearing = Vector(3.14, 25000)
pos_from = pos_to - bearing
segment = Line(pos_from, pos_to)
hit = self.chart.hit(segment)
self.assertTrue(hit)
bearing = Vector(4.71, 30000)
pos_from = pos_to - bearing
segment = Line(pos_from, pos_to)
hit = self.chart.hit(segment)
self.assertTrue(hit)
# This point is not on the island
pos1 = Position(0.3019, -0.4363)
pos2 = pos1 - bearing
segment = Line(pos1, pos2)
hit = self.chart.hit(segment)
self.assertFalse(hit)
def testRouteAround(self):
p1 = Position(*deg_to_rad(39.3082, 26.4316))
p2 = Position(*deg_to_rad(39.3365, 26.4245))
self.chart.load(dirname + '/Turkey.xml')
self.chart.save_to_kml(dirname + '/Turkey.kml')
l = Line(p1, p2)
routes = self.chart.route_around(l)
self.assertTrue(routes)
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 gen_waypoints():
result = []
# Rotterdam start
result.append(Position(deg_to_rad(52.00), deg_to_rad(4.10)))
# Somewhere on the english coast a little north of newcastle
result.append(Position(deg_to_rad(55.64), deg_to_rad(-1.55)))
# Skagen
result.append(Position(deg_to_rad(57.74), deg_to_rad(10.60)))
# South of laeso island
result.append(Position(deg_to_rad(57.20), deg_to_rad(11.15)))
# Goteborg finish
result.append(Position(deg_to_rad(57.61), deg_to_rad(11.70)))
return result
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 trace_poly(p_last, p, right):
points = result[right]
a = line.v.a # Angle of line from last point to current point
b = a
a_max = a
p_max = None
p_start = p
p_next = None
while True:
# Poly ended (edge of map?) or looped back to start
if p is None or ((p_next is not None) and (p == p_start)):
if p_max == p_last:
result[right] = None
break
try:
# Cumulative angles
ar = p - line.p1
a += angle_diff(ar.a, a)
br = line.p2 - p # Target bearing
b += angle_diff(br.a, b)
# Distance to target exceeds distance hint, invalidate this
# solution
if br.r > distance_hint:
result[right] = None
break
# We've gone more than halfway around the target: stop for
# now
if abs(b - line.v.a) > pi:
break
# Check if we've found a new 'outer' point
if angle_bigger(a, a_max, right):
a_max = a
p_max = p
finally:
p_next = p.other_link(p_last)
p_last = p
p = p_next
assert p_max is not None, '%f %f %f %s' % (line.v.a, a, a_max,
str(right))
v = p_max - line.p1
p = p_max + veer_vector(v, right)
points.append(Position(p))
points.append(Position(line.p2))
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 testValidRoute(self):
p1 = Position(0.9, 0.9)
p2 = Position(1.0, 1.0)
p3 = Position(1.0, 1.1)
pt1 = Position(0.99, 1.0)
pt2 = Position(1.01, 1.0)
r1 = Route((p1, pt1, p3))
r2 = Route((p1, pt2, p3))
c = Course((p1, p2, p3))
boat = SailBoat()
chart = Chart()
router = Router(boat=boat, course=c, chart=chart)
self.assertFalse(router.valid_route(r1))
self.assertTrue(router.valid_route(r2))
c.marks[0].to_port = True
self.assertTrue(router.valid_route(r1))
self.assertFalse(router.valid_route(r2))
def load_course(self, course):
self.course = []
waypoints = get_elements(course, 'waypoint')
for waypoint in waypoints:
lat = deg_to_rad(float(get_child_text_value(waypoint, 'lat')))
lon = deg_to_rad(float(get_child_text_value(waypoint, 'lon')))
self.course.append(Position(lat, lon))
self.finish_radius = nm_to_m(
float(get_child_text_value(course, 'goal_radius')))
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 testOuterPoints(self):
self.chart.load(dirname + '/Canary_Brazil.xml')
# Attempt to pass Sao Antao (Cabo Verde) from north to south
pos_from = Position(0.297869, -0.43921) + Vector(0, 30000)
pos_to = Position(0.297869, -0.43921) + Vector(math.pi, 30000)
segment = Line(pos_from, pos_to)
# First verify that we're hitting
hit = self.chart.hit(segment)
self.assertTrue(hit, 'Expected island hit')
outer_points = self.chart.route_around(segment)
self.assertEquals(2, len(outer_points))
self.assertTrue(not outer_points[0] is None \
and not outer_points[1] is None, \
'Island, so expected two ways to pass (both sides)')
# ..now from south to north and the start a little offset to the east
# so we'll also hit Sao Vicente
pos_from = Position(0.297869, -0.43921) + Vector(math.pi - 0.2, 35000)
pos_to = Position(0.297869, -0.43921) + Vector(0, 30000)
segment = Line(pos_from, pos_to)
# First verify that we're hitting
hit = self.chart.hit(segment)
self.assertTrue(hit, 'Expected island hit')
outer_points = self.chart.route_around(segment)
self.assertEquals(2, len(outer_points))
route = Route(outer_points[0])
route.save_to_kml(dirname + '/outer_points_first_0.kml')
route = Route(outer_points[1])
route.save_to_kml(dirname + '/outer_points_first_1.kml')
outer_points = self.chart.find_paths(segment)
for i, points in enumerate(outer_points):
path = Path(points)
path.save_to_kml('result_%d.kml' % i)
# Expected 3 lines around both islands
self.assertTrue(len(outer_points) >= 3)
class Situation(object):
__position = Position(0, 0)
heading = 0
def __init__(self, *args, **kwargs):
self.time = datetime.utcnow()
@property
def position(self):
return self.__position
@position.setter
def position(self, value):
self.__position = Position(value[0], value[1])
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 gen_waypoints_pb3_2011():
"""Race along the shores of estonia"""
# Good race for collision detection
result = []
result.append(Position(*deg_to_rad(58.3700, 24.5000)))
result.append(Position(*deg_to_rad(58.9036, 23.3503)))
result.append(Position(*deg_to_rad(58.9500, 23.5000)))
result.append(Position(*deg_to_rad(59.2200, 23.5200)))
result.append(Position(*deg_to_rad(59.4870, 24.7080)))
result.append(Position(*deg_to_rad(59.4690, 24.8190)))
return result
def point_elem_to_position(elem):
lat = elem.getAttribute('latitude')
lon = elem.getAttribute('longitude')
lat = deg_to_rad(float(lat))
lon = deg_to_rad(float(lon))
return Position(lat, lon)
def route_around(self, line, distance_hint=1E8):
poly_part, intersect = self.__hit(line)
if poly_part is None:
# We didn't hit anything :)
return [[Position(line.p1), Position(line.p2)]]
_log.debug('routing line %s - %s around %s' %
(str(line.p1), str(line.p2), str(intersect)))
# We're hitting so set up left and right around as
# the line sections from current position to hit point
result = [[Position(line.p1)], [Position(line.p1)]]
# Find the points of the map line that is being intersected
# (This should not really be necessary.. as the poly_part
# could already contain this information, but doesn't at
# the moment. The poly_part points are not ChartPoints)
p1 = self.__find_point(poly_part.p1)
p2 = self.__find_point(poly_part.p2)
b1 = p1 - line.p1
b2 = p2 - line.p1
right = side_of(b2, b1, True)
def trace_poly(p_last, p, right):
points = result[right]
a = line.v.a # Angle of line from last point to current point
b = a
a_max = a
p_max = None
p_start = p
p_next = None
while True:
# Poly ended (edge of map?) or looped back to start
if p is None or ((p_next is not None) and (p == p_start)):
if p_max == p_last:
result[right] = None
break
try:
# Cumulative angles
ar = p - line.p1
a += angle_diff(ar.a, a)
br = line.p2 - p # Target bearing
b += angle_diff(br.a, b)
# Distance to target exceeds distance hint, invalidate this
# solution
if br.r > distance_hint:
result[right] = None
break
# We've gone more than halfway around the target: stop for
# now
if abs(b - line.v.a) > pi:
break
# Check if we've found a new 'outer' point
if angle_bigger(a, a_max, right):
a_max = a
p_max = p
finally:
p_next = p.other_link(p_last)
p_last = p
p = p_next
assert p_max is not None, '%f %f %f %s' % (line.v.a, a, a_max,
str(right))
v = p_max - line.p1
p = p_max + veer_vector(v, right)
points.append(Position(p))
points.append(Position(line.p2))
# Trace both directions
trace_poly(p1, p2, right)
trace_poly(p2, p1, not right)
try:
result.remove(None)
result.remove(None)
raise Exception('Expected at least one route around')
except ValueError:
pass
return result
class Finish(Position):
left = Position()
right = Position()
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 position(self, value):
self.__position = Position(value[0], value[1])
