diff --git a/src/cd.py b/src/cd.py
new file mode 100644
index 0000000000000000000000000000000000000000..f375ee9bc8d96c493910201c1f49d92e601dfa17
--- /dev/null
+++ b/src/cd.py
@@ -0,0 +1,361 @@
+#/usr/bin/python
+
+import math
+from scanutil import ccw
+
+#
+# test to implement convexdivide by Tommy Persson
+#
+
+# Input list of vertices
+
+# list of index for site vertices.
+
+#
+# from http://stackoverflow.com/questions/451426/how-do-i-calculate-the-surface-area-of-a-2d-polygon
+#
+
+#
+# site thingy when making ccw
+#
+# site info in result? Is it always an index?
+#
+
+def area(p):
+ return 0.5 * abs(sum(x0*y1 - x1*y0
+ for ((x0, y0), (x1, y1)) in segments(p)))
+
+def segments(p):
+ return zip(p, p[1:] + [p[0]])
+
+#
+# End of area computation
+#
+
+
+#
+# From http://www.toptal.com/python/computational-geometry-in-python-from-theory-to-implementation
+#
+
+#def ccw(A, B, C):
+# """Tests whether the turn formed by A, B, and C is ccw"""
+# return (B.x - A.x) * (C.y - A.y) > (B.y - A.y) * (C.x - A.x)
+
+def intersect(a1, b1, a2, b2):
+ """Returns True if line segments a1b1 and a2b2 intersect."""
+ return ccw(a1, b1, a2) != ccw(a1, b1, b2) and ccw(a2, b2, a1) != ccw(a2, b2, b1)
+
+def is_convex(area):
+ res = True
+ for i in range(len(area)):
+ j = (i + 1) % len(area)
+ k = (i + 2) % len(area)
+ print i, j, k
+ if not ccw(area[i], area[j], area[k]):
+ res = False
+ return res
+
+def seg_length(p, i):
+ i1 = (i+1) % len(p)
+ return math.sqrt((p[i][0]-p[i1][0])*(p[i][0]-p[i1][0])+(p[i][1]-p[i1][1])*(p[i][1]-p[i1][1]))
+
+def poly_length(p):
+ res = 0.0
+ for i in range(len(p)):
+ res += seg_length(p, i)
+# print "POLY_LENGTH:", i, res
+ return res
+
+def point_on_segment(p, i, length):
+ # in ccw direction, p is ccw
+ slen = seg_length(p, i)
+ i1 = (i+1) % len(p)
+ dx = p[i1][0]-p[i][0]
+ dy = p[i1][1]-p[i][1]
+ x = p[i][0] + dx * length/slen
+ y = p[i][1] + dy * length/slen
+ return (x, y)
+
+def move_cw(p, frac):
+ l = poly_length(p)
+ flen = l*frac
+ index = len(p)-1
+ slen = seg_length(p, index)
+# print "SLEN:", slen, flen, index, l, frac
+ while slen < flen:
+# print "MOVE_CW", index, slen, flen
+ index -= 1
+ flen -= slen
+ slen = seg_length(p, index)
+# print "INDEX:", index, flen, slen-flen
+ p1 = point_on_segment(p, index, slen-flen)
+# print "P1", p1, index
+ return p1, index
+
+def move_ccw(p, frac):
+ l = poly_length(p)
+ flen = l*frac
+ index = 0
+ slen = seg_length(p, index)
+# print "SLEN:", slen, flen, index, l, frac, p
+ while slen < flen:
+# print "MOVE_CW", index, slen, flen, p
+ index += 1
+ flen -= slen
+ slen = seg_length(p, index)
+# print "INDEX:", index, flen, slen
+ p1 = point_on_segment(p, index, flen)
+# print "P1", p1, index
+ return p1, index
+
+def make_ccw(area, w, s):
+ if ccw(area[0], area[1], area[2]):
+ return area, w, s
+ else:
+ a1 = list(reversed(area))
+ el = a1.pop()
+ a1.insert(0, el)
+ w1 = list(reversed(w))
+ el = w1.pop()
+ s1 = []
+ for site in s:
+ s1.append((len(w)-site) % len(w))
+ s1.sort()
+ w1.insert(0, el)
+ return a1, w1, s1
+
+#
+#
+#
+
+def get_area(w, start, end):
+ p = []
+ for i in range(start, end+1):
+# print "I:", i
+ p.append(w[i])
+ return area(p)
+
+def get_requested_area(index, areareq, totarea):
+ res = 0.0
+ for i in range(0, index+1):
+ res += areareq[i]*totarea
+ return res
+
+def area_lt(a0, a1):
+ return a0 < a1
+
+def area_gt(a0, a1):
+ return a0 > a1
+
+def area_eq(a0, a1):
+ if math.fabs(a0-a1) < 0.2:
+ return True
+ else:
+ return False
+
+def get_split_polygon(p, si, new_point, new_point_index):
+# print "GET_SPLIT_POLYGON P:", p, si, new_point, new_point_index
+ a = p[0:si+1]
+ a.append(new_point)
+ a += p[new_point_index+1:]
+# print "GET_SPLIT_POLYGON:", a
+ return a
+
+def get_split_polygon_ccw(p, si, new_point, new_point_index):
+# print "GET_SPLIT_POLYGON CCW:", p, si, new_point, new_point_index
+ a = []
+ a.append(new_point)
+ a += p[new_point_index+1:si+1]
+# print "GET_SPLIT_POLYGON:", a
+ return a
+
+def get_split_polygon_remain(p, si, new_point, new_point_index):
+# print "GET_SPLIT_POLYGON P:", p, si, new_point, new_point_index
+ a = p[si:new_point_index+1]
+ a.append(new_point)
+# print "GET_SPLIT_POLYGON_REMAIN:", a
+ return a
+
+def get_split_polygon_remain_ccw(p, si, new_point, new_point_index):
+# print "GET_SPLIT_POLYGON_REMAIN_CCW:", p, si, new_point, new_point_index
+ a = p[0:new_point_index+1]
+ a.append(new_point)
+ a += p[si:]
+# print "GET_SPLIT_POLYGON_REMAIN_CCW:", a
+ return a
+
+#
+# p is ths polygon
+# si is index in p for the endpoint of the line
+# new_point is the new point introduced
+# new point index is the index of the vertice before the new point
+# The area to compute is the one to the right of the line
+#
+#
+def get_splitarea(p, si, new_point, new_point_index):
+ a = get_split_polygon(p, si, new_point, new_point_index)
+ ar = area(a)
+# print "SPLITAREA", ar, a
+ return ar
+
+def get_splitarea_ccw(p, si, new_point, new_point_index):
+ a = get_split_polygon_ccw(p, si, new_point, new_point_index)
+ ar = area(a)
+# print "SPLITAREA", ar, a
+ return ar
+
+
+def convex_divide(ar, w, sites, areareq, totarea):
+ print "CONVEX_DIVIDE:", ar, w, sites, areareq, totarea
+ #
+ # Result after one iteration in w_r, w_l, sites_r, sites_l
+ #
+ w_r = []
+ w_l = []
+ sites_r = []
+ sites_l = []
+# print "TOTAREA:", totarea
+ siteindex = 0
+ windex = sites[siteindex]
+ reqarea = get_requested_area(siteindex, areareq, totarea)
+# print "REQ AREA:", reqarea
+ while get_area(w, 0, windex) < reqarea and windex != sites[-1]:
+# print "AREA LESS: ", windex, get_area(w, 0, windex), reqarea
+ if windex > 0:
+ if windex in sites:
+ siteindex = sites.index(windex)
+ reqarea = get_requested_area(siteindex, areareq, totarea)
+# print "REQ AREA CHANGED:", reqarea
+ windex += 1
+# print "WINDEX:", windex
+ if windex == sites[0] and get_area(w, 0, windex) > reqarea:
+ print "Move L_s CCW along PLR"
+ n = 10000
+ for i in range(n):
+ p, index = move_ccw(ar, i*1.0/n)
+ a = get_splitarea_ccw(ar, sites[0], p, index)
+# print "RES:", p, index, i*1.0/n, a, reqarea
+ if area_eq(a, reqarea):
+ print "FOUND", a, reqarea, p, index
+ w_r = get_split_polygon_ccw(ar, sites[0], p, index)
+ sites_r = [sites[0]]
+ w_l = get_split_polygon_remain_ccw(ar, sites[0], p, index)
+ sites_l = [x-(sites[0]-index-2) for x in sites[1:]]
+ areq_r = []
+ areq_l = [x/(1-areareq[0]) for x in areareq[1:]]
+ break
+ elif windex == sites[-1] and get_area(w, 0, windex) < reqarea:
+ print "Move L_s CW along PLR"
+ n = 10000
+ for i in range(n):
+ p, index = move_cw(ar, i*1.0/n)
+ a = get_splitarea(ar, sites[-1], p, index)
+# print "RES:", p, index, i*1.0/n, a, reqarea
+ if area_eq(a, reqarea):
+ print "FOUND", a, reqarea, p, index
+ w_r = get_split_polygon(ar, sites[-1], p, index)
+ sites_r = sites[0:-1]
+ w_l = get_split_polygon_remain(ar, sites[-1], p, index)
+ sites_l = [0]
+ areq_r = [x/(1-areareq[-1]) for x in areareq[:-1]]
+ areq_l = []
+ break
+ else:
+ print "INTERPOLATE"
+ index = windex -1
+ slen = seg_length(w, index)
+ n = 3000
+ for i in range(n):
+ le = (1-i*1.0/n)*slen
+ p = point_on_segment(w, index, le)
+# print "POINT:", p
+ poly = w[0:index+1]
+ poly.append(p)
+ a = area(poly)
+# print "RES:", p, index, i*1.0/n, a, reqarea
+ if area_eq(a, reqarea):
+ print "FOUND", a, reqarea, p, index
+ w_r = poly
+ sites_r = sites[0:index-1]
+ w_l = w[0:1]
+ w_l.append(p)
+ w_l += w[index+1:]
+ sites_l = [x-(index-1) for x in sites[index-1:]]
+ areq_r = [x/(1-sum(areareq[index-1:])) for x in areareq[0:index-1:]]
+ areq_l = [x/(1-sum(areareq[0:index-1])) for x in areareq[index-1:]]
+ break
+
+
+ print "w_r - sites_r:", w_r, " - ", sites_r, areq_r
+ print "w_l - sites_l:", w_l, " - ", sites_l, areq_l
+
+ # Fix area request
+
+ if len(sites_r) == 1:
+ right = [w_r, sites_r]
+ else:
+ right = [w_r, sites_r]
+ ## right = convex_divide(w_r, w_r, sites_r
+
+ if len(sites_l) == 1:
+ left = [w_l, sites_l]
+ else:
+ left = [w_l, sites_l]
+ ## right = convex_divide(w_r, w_r, sites_r
+
+ return [right, left]
+
+if __name__ == "__main__":
+ p = [(0, 0), (5,1), (5,8), (0,8)]
+ w = [(0, 0), (5,1), (5,8), (0,8)]
+ sites = [0, 2]
+
+ p2 = [(0, 0), (5,1), (5,8), (2.5, 10), (0,8)]
+ w2 = [(0, 0), (5,1), (5,8), (2.5, 10), (0,8)]
+ sites2 = [3, 4]
+
+ p1 = [(0, 0), (0, 8), (5,8), (5,1)]
+ w1 = [(0, 0), (0, 8), (5,8), (5,1)]
+ sites1 = [0, 1]
+
+ print "P1:", p1
+ print "W1:", w1
+ print "S1:", sites1
+
+ p1, w1, sites1 = make_ccw(p1, w1, sites1)
+
+ print "P1:", p1
+ print "W1:", w1
+ print "S1:", sites1
+
+ print "AREA P:", area(p)
+ #print "AREA PR:", area(pr)
+
+
+ print "IS_CONVEX", is_convex(p)
+ #print "IS_CONVEX", is_convex(pr)
+ #print "IS_CONVEX", is_convex(prr)
+
+ line = [[0, 0], [5,0], [8,0]]
+ print "AREA:", area(line)
+ print "IS_CONVEX", is_convex(line)
+
+ print "AREA3", get_area(p, 0, 2)
+
+ res = convex_divide(p, p, [0, 2], [0.5, 0.5], area(p))
+ print "RES:", res
+
+ res = convex_divide(p2, w2, sites2, [0.5, 0.5], area(p2))
+ print "RES:", res
+
+ res = convex_divide(p1, w1, sites1, [0.5, 0.5], area(p1))
+ print "RES:", res
+
+ p3 = [(0.0, 0.0), (10.0, 5.0), (7.2457566807008842, 5.6885608298247785), (-2.0, 8.0), (-4.0, 2.0)]
+ res = convex_divide (p3, p3, [0, 2], [0.5, 0.5], area(p3))
+ print "RES:", res
+
+
+ #print "LEN P:", poly_length(p)
+ #p1, index = move_cw(p, 0.5)
+ #print "MOVE_CW:", p1, index
diff --git a/src/genpoint.py b/src/genpoint.py
new file mode 100644
index 0000000000000000000000000000000000000000..c3d7c080e6e872b0c4d139c619264ba8d878c9df
--- /dev/null
+++ b/src/genpoint.py
@@ -0,0 +1,151 @@
+#!/usr/bin/python
+import math
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import matplotlib as mpl
+import numpy as np
+from scipy.optimize import fsolve
+from scipy.optimize import newton
+
+pi = 3.1415926535897932384626433832795028841971
+
+def genpoint(point, theta, line, coverage):
+ #print "THIS IS DEFINITELY HERE, MAYBE"
+ # print "GENPOINT point:", point
+ print "GENPOINT theta:", theta
+ print "GENPOINT line: y = ", line(1)-line(0), '*x+', line(0)
+ # print "GENPOINT coverage:", coverage
+
+ if theta > 2*pi:
+ theta = theta - 2*pi
+ #Gets the angle of the line and adjusts it so it lies in the interval [0,pi]
+ lineangle = math.atan2(line(1)-line(0), 1)
+ if lineangle<0:
+ lineangle = lineangle + pi
+
+ #Generates the path associates with the angle and point and a function for the intersection
+ path = lambda x: math.tan(theta)*(x-point[0]) + point[1]
+ fn = lambda x: path(x) - line(x)
+
+ fnprime = lambda x: fn(1)-fn(0)
+ fnprime2 = lambda x: 0
+ #The intersec coordinates is the intersection of the line and the path
+# x_intersec = newton(fn, 0, fprime=fnprime, fprime2=fnprime2)
+ x_intersec = newton(fn, 0, fprime=fnprime)
+# x_intersec = x_intersec[0]
+ y_intersec = line(x_intersec)
+ #print '1st xy-intersec', x_intersec, y_intersec
+
+ #The hypotenuse of the triangles associated with the path-intersection-point and the intersecting corner of the coverage-rectangle.
+ #print "theta: ", theta, "lineangle: ", lineangle
+ if theta==lineangle:
+ print "Lines are parallel! Theta:", theta, 'Lineangle:', lineangle
+ #print "THETA:", theta
+ #print "LINEANGLE:", lineangle
+ lefthyp = coverage[0]/(2*math.sin(theta-lineangle))
+ righthyp = coverage[0]/(2*math.sin(lineangle-theta))
+
+ #print 'righthyp:', righthyp
+
+ #A help function for getting for getting the corner points of the coverage-rectangle at the stopping coordinate.
+ def getcovcorners(corner, x_intersec, y_intersec):
+ if corner == 1:
+ covcorners = [(x_intersec, y_intersec),
+ (x_intersec-math.cos(theta)*coverage[1], y_intersec-math.sin(theta)*coverage[1]),
+ (x_intersec+math.sin(theta)*coverage[0]-math.cos(theta)*coverage[1],
+ y_intersec-math.cos(theta)*coverage[0]-math.sin(theta)*coverage[1]),
+ (x_intersec+math.sin(theta)*coverage[0], y_intersec-math.cos(theta)*coverage[0])]
+ elif corner == 2:
+ #'hello?'
+ covcorners=[(x_intersec, y_intersec),
+ (x_intersec-coverage[0]*math.sin(theta), y_intersec+coverage[0]*math.cos(theta)),
+ (x_intersec-coverage[0]*math.sin(theta)-coverage[1]*math.cos(theta),
+ y_intersec+coverage[0]*math.cos(theta)-math.sin(theta)*coverage[1]),
+ (x_intersec-math.cos(theta)*coverage[1], y_intersec-math.sin(theta)*coverage[1])]
+ else:
+ print "Error, input not allowed"
+ return covcorners
+
+ #If-block that handles the different cases associated with different angles and lines
+ if theta <= pi/2:
+ if lineangle - theta < pi/2 and lineangle - theta > 0:
+ x_intersec, y_intersec = x_intersec+righthyp*math.cos(lineangle), y_intersec+righthyp*math.sin(lineangle)
+ covcorners = getcovcorners(1,x_intersec,y_intersec)
+ #print "11"
+ # print 'xy-intersec, ', x_intersec, y_intersec
+ else:
+ x_intersec, y_intersec = (x_intersec-righthyp*math.cos(lineangle), y_intersec-righthyp*math.sin(lineangle))
+ covcorners = getcovcorners(2,x_intersec,y_intersec)
+ #print "12"
+ #print 'x_intersec, y_intersec', x_intersec, y_intersec
+
+ elif theta <= pi:
+ if lineangle+pi-theta < pi/2 or lineangle + pi - theta > pi:
+ x_intersec, y_intersec = (x_intersec-lefthyp*math.cos(lineangle),y_intersec-lefthyp*math.sin(lineangle))
+ covcorners = getcovcorners(1,x_intersec,y_intersec)
+ #print "21"
+
+ else:
+ x_intersec, y_intersec = (x_intersec-lefthyp*math.cos(lineangle+pi), y_intersec-lefthyp *math.sin(lineangle+pi))
+ covcorners = getcovcorners(2,x_intersec,y_intersec)
+ #print "22"
+
+ elif theta <= 3*pi/2:
+ if lineangle+pi-theta < pi/2 and lineangle+pi-theta > 0:
+ x_intersec, y_intersec = (x_intersec-lefthyp*math.cos(lineangle),y_intersec-lefthyp*math.sin(lineangle))
+ covcorners = getcovcorners(1,x_intersec,y_intersec)
+ #print "31"
+
+ else:
+ x_intersec, y_intersec = (x_intersec-lefthyp*math.cos(lineangle+pi), y_intersec-lefthyp *math.sin(lineangle+pi))
+ covcorners = getcovcorners(2,x_intersec,y_intersec)
+ #print "32"
+
+ elif theta <= 2*pi:
+ if lineangle+pi-theta < -pi/2 or lineangle+pi-theta > 0:
+ x_intersec, y_intersec = x_intersec+righthyp*math.cos(lineangle), y_intersec+righthyp*math.sin(lineangle)
+ covcorners = getcovcorners(1,x_intersec,y_intersec)
+ # print "41"
+
+ else:
+ x_intersec, y_intersec = (x_intersec-righthyp*math.cos(lineangle), y_intersec-righthyp*math.sin(lineangle))
+ covcorners = getcovcorners(2,x_intersec,y_intersec)
+ # print "42"
+
+ else:
+ print "Angle is outside of allowed interval. Angle is :", theta
+
+
+
+ xway=[x[0] for x in covcorners]
+ yway=[y[1] for y in covcorners]
+ waypoint=(sum(xway)/4,sum(yway)/4)
+ print 'GENPOINT, waypoint:_', waypoint
+ return waypoint
+
+"""
+ #Block for testing this function, uncomment to confirm that it works.
+
+ fig = plt.figure()
+ ax = fig.add_subplot(111)
+ plt.plot([point[0], waypoint[0]],[point[1], waypoint[1]])
+ covcorners.append(covcorners[0])
+ plt.plot([x[0] for x in covcorners], [y[1] for y in covcorners])
+ plt.plot(range(-30,30), map(line, range(-30,30)))
+ plt.xlim(min(point[0]-1, waypoint[0]-coverage[0]-1), max(point[1]+1, waypoint[0]+coverage[0]+1))
+ plt.ylim(min(point[1]-1,waypoint[1]-coverage[1]-1), max(point[1]+1, waypoint[1]+coverage[1]+1))
+ plt.xlabel('x')
+ plt.ylabel('y')
+ plt.grid(True)
+ plt.show()
+
+
+
+
+
+pi = 3.1415926535897932384626433832795028841971
+
+#genpoint((0.7749, -1.847), 1.446, lambda x: 0.5*(x-2.0)+6.0, (0.5,0.1))
+
+"""
+
diff --git a/src/line.py b/src/line.py
new file mode 100644
index 0000000000000000000000000000000000000000..f0321d16a5183d4d1308f6c9b58e4ec4a8614466
--- /dev/null
+++ b/src/line.py
@@ -0,0 +1,125 @@
+import math
+
+
+#Returns line on standard form (Ax+By+C = 0)
+def line_from_point(A, B):
+ if A[0] == B[0] and A[1] == B[1]:
+ raise ValueError('A = B in line')
+ #print 'Creating line', A, ', ', B, ',', (A[1]-B[1]), '*x+', (B[0]-A[0]), 'y + ', A[0]*B[1]-A[1]*B[0]
+ return lambda x, y: (A[1]-B[1])*x+(B[0]-A[0])*y + A[0]*B[1]-A[1]*B[0]
+
+def line_from_angle(p, theta):
+ #print 'Used theta:', theta, ' to create', (p[0]+math.cos(theta), p[1]+math.sin(theta)), 'from p:', p
+ return line_from_point(p, (p[0]+math.cos(theta), p[1]+math.sin(theta)))
+
+def line_angle(L):
+ if L(0, 1)==L(0, 0):
+ return math.pi*0.5
+ else:
+ return math.atan(-(L(1, 0)-L(0, 0))/(L(0, 1)-L(0, 0)))
+
+def intersect(L_1, L_2):
+ C_1 = L_1(0, 0)
+ C_2 = L_2(0, 0)
+ A_1 = L_1(1, 0) - C_1
+ A_2 = L_2(1, 0) - C_2
+ B_1 = L_1(0, 1) - C_1
+ B_2 = L_2(0, 1) - C_2
+ #print 'L_1', A_1, B_1, C_1, 'L_2:', A_2, B_2, C_2
+ if A_1*B_2 == A_2*B_1:
+ #print 'Failure in generating intersection: L_1 and L_2 are parallel!'
+ raise ValueError('L_1, L_2 parallel')
+ else:
+ x_intersect = (B_2*C_1-B_1*C_2)/(B_1*A_2-B_2*A_1)
+ y_intersect = (A_2*C_1-A_1*C_2)/(A_1*B_2-A_2*B_1)
+ #print 'Intersection at:, ', (x_intersect, y_intersect)
+ return (x_intersect, y_intersect)
+
+def x_coeff(L):
+ return L(1, 0)-L(0,0)
+
+def y_coeff(L):
+ return L(0, 1)-L(0,0)
+
+
+def y(L):
+ if y_coeff(L) !=0:
+ return lambda x: -(x_coeff(L)/y_coeff(L))*x-L(0,0)/y_coeff(L)
+ else:
+ raise ValueError('Y arbitrary for L')
+
+def x(L):
+ if x_coeff(L) !=0:
+ return lambda y: -(y_coeff(L)/x_coeff(L))*y-L(0,0)/x_coeff(L)
+ else:
+ raise ValueError('X arbitrary for L')
+
+def distance(a, b):
+ return math.sqrt((a[0]-b[0])**2+(a[1]-b[1])**2)
+
+def distance_squared(a,b):
+ return (a[0]-b[0])**2+(a[1]-b[1])**2
+
+def point_on_line(p, a, b):
+ #print p, a, b
+ epsilon = distance(a,b)*0.000001
+ #print 'POINT ON LINE?:', distance_squared(a,p)+distance_squared(b,p), distance_squared(a,b), epsilon
+ #print abs((distance(a,p)+distance(b,p)) - distance(a,b)) < epsilon
+ return abs((distance(a,p)+distance(b,p)) - distance(a,b)) < epsilon
+
+
+#does not work properly for negative values?
+#point_on_line can be used instead
+def point_approx_in(p, a, b):
+ eps = 0.000001
+ if p[0] <= min((1-eps)*a[0], (1-eps)*b[0], (1+eps)*a[0], (1+eps)*b[0]):
+ return False
+ elif p[0] >= max((1-eps)*a[0], (1-eps)*b[0], (1+eps)*a[0], (1+eps)*b[0]):
+ return False
+ elif p[1] <= min((1-eps)*a[1], (1-eps)*b[1], (1+eps)*a[1], (1+eps)*b[1]):
+ return False
+ elif p[1] >= max((1-eps)*a[1], (1-eps)*b[1], (1+eps)*a[1], (1+eps)*b[1]):
+ return False
+ else:
+ return True
+
+def signed_area(polygon):
+ area = 0
+ for i in range(0, len(polygon)-1):
+ area += 0.5 * (polygon[i+1][0]-polygon[i][0])*(polygon[i+1][1]+polygon[i][0])
+ return area
+
+
+
+def centre(polygon):
+ centre = [0, 0]
+ for i in range(0, len(polygon)):
+ centre[0] += polygon[i][0]/len(polygon)
+ centre[1] += polygon[i][1]/len(polygon)
+ return (centre[0], centre[1])
+
+def angle_from_points(a, b):
+ return math.atan2(a[1]-b[1],a[0]-b[0])
+
+def counterclockwise(polygon):
+ for i in range(0, len(polygon)):
+ p = [polygon[i-2], polygon[i-1], polygon[i]]
+ if signed_area(p) < 0:
+ #print 'not clockwise:', p
+ #print signed_area(p)
+ return False
+ return True
+
+#Sorts a list of coordinates p, if the coordinates are tied to something that can be sorted too
+def sort_ccw(p, something=False):
+ c = centre(p)
+ for i in range(0, len(p)):
+ for j in range(1, len(p)-i):
+ if angle_from_points(p[j], c) < angle_from_points(p[j-1], c):
+ p[j-1], p[j] = p[j], p[j-1]
+ if something:
+ something[j-1], something[j] = something[j], something[j-1]
+ if something:
+ return p, something
+ else:
+ return p
diff --git a/src/partitioningclient.py b/src/partitioningclient.py
index 2a46ca0ea626ce9131c6be4fac03958aabf82a49..6284c1fa2a04f633ab4488a484b0e9966a1f93c5 100755
--- a/src/partitioningclient.py
+++ b/src/partitioningclient.py
@@ -5,7 +5,7 @@ import sys
import time
import os
-from lrs_srvs.srv import *
+from lrs_scan_msgs.srv import *
from geometry_msgs.msg import *
from optparse import OptionParser
@@ -16,7 +16,7 @@ parser.add_option ("-t", "--test", action="store", type="int", dest="testcase",
def get_partitioning (ns, polygon, sites, area_req):
try:
- client = rospy.ServiceProxy(ns + "scanserver/get_partitioning", SPGetPartitioning)
+ client = rospy.ServiceProxy(ns + "partitioningserver/get_partitioning", SPGetPartitioning)
resp = client (polygon, sites, area_req)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
diff --git a/src/partitioningserver.py b/src/partitioningserver.py
index 036224390d7f7371c9c4a2804b9d6e31fe72101c..4fbcdbcfd7886f55aeaa7666a4827d652751e80d 100755
--- a/src/partitioningserver.py
+++ b/src/partitioningserver.py
@@ -79,14 +79,14 @@ def get_partitioning(data):
polygon = [(p.x, p.y) for p in data.polygon]
sites = [(p.x, p.y) for p in data.sites]
areareq = [x for x in data.area_request]
- print "scanserver POLYGON", polygon
- print "scanserver SITES", sites
- print "scanserver AREAREQ", areareq
+ print "partitioningserver POLYGON", polygon
+ print "partitioningserver SITES", sites
+ print "partitioningserver AREAREQ", areareq
part.set_polygon (polygon)
part.set_sites(sites)
# return SPGetPartitioningResponse([], True, 0, "")
areas = part.partition(areareq)
- print "scanserver AREAS", areas
+ print "partitioningserver AREAS", areas
res = []
for a in areas:
pa = PointArray()
@@ -111,7 +111,7 @@ if __name__ == "__main__":
ns = rospy.get_namespace ()
- s = rospy.Service('scanserver/get_partitioning', SPGetPartitioning, get_partitioning)
+ s = rospy.Service('partitioningserver/get_partitioning', SPGetPartitioning, get_partitioning)
print "Partitioning server spinning..."
diff --git a/src/robust_convexdivide.py b/src/robust_convexdivide.py
new file mode 100644
index 0000000000000000000000000000000000000000..adc82c16b0b6692508d58f577c1b81a5ec5b237a
--- /dev/null
+++ b/src/robust_convexdivide.py
@@ -0,0 +1,357 @@
+'''
+Created on Jul 14, 2014
+
+@author: aleer
+
+Algorithm developed by Susan Hert. See her paper called Polygon Area
+Decomposition for Multiple-Robot workspace division for more details.
+
+'''
+
+from copy import *
+from scanutil import check_and_fix, index_of_point
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import matplotlib as mpl
+import copy
+from line import *
+from scipy.optimize import fsolve as fsolve
+
+
+#Gets the area of a set of points (ordered in counterclockwise order)
+#that make up a polygon. If the points are in a clockwise order the
+#area returned is negative, so this function can be used to determine
+#the orientation of the points
+def getarea(poly):
+ poly.append(poly[0])
+ area=0
+ for i in range(0, len(poly)-1):
+ area = area+(poly[i][0]*poly[i+1][1]-poly[i][1]*poly[i+1][0])/2.0
+ del poly[len(poly)-1]
+ #print 'GETPOLY points:', poly, 'with area:', area
+ return area
+
+
+#Helperfunction that calculates the new unknown shared cornerpoint for the smaller polygons
+def getpolypoint(points, maxindex, reqarea, change):
+## points = check_and_fix(points0)
+ #print "CONVEXDIVIDE GETPOLYPOINT:", points, maxindex, reqarea, change
+ g=0
+ within = False
+ while not within:
+ if change == 'ccw':
+ #print 'ccw'
+ g = g+1
+ #y = lambda x: (points[g][1]-points[g-1][1])/(points[g][0]-points[g-1][0])*(x-points[g-1][0])+points[g-1][1]
+ l = line_from_point(points[g], points[g-1])
+
+ #function that helps us generate the areafunction by
+ #calculating the area of the known points
+ ahelp = 0
+ for i in range(g+1, maxindex+1):
+ ahelp = ahelp + points[i-1][0]*points[i][1] - points[i-1][1]*points[i][0]
+
+ #Generates the area function, with an unknown point that
+ #is then calculated using the desired area.
+ if abs(y_coeff(l)) > 0.001:
+ area = lambda x: (x*points[g][1] - y(l)(x)*points[g][0] + points[maxindex][0]*y(l)(x) - points[maxindex][1]*x + ahelp)/2.0 - reqarea
+ intersect = (fsolve(area, 1)[0], y(l)(fsolve(area, 1)[0]))
+ if (intersect[0] >= min(points[g][0], points[g-1][0]) and intersect[0] <= max(points[g][0], points[g-1][0])):
+ within = True
+ else:
+ #print '\n !!!!!---THAT JUST HAPPENED: ccw---!!!!! \n'
+ area = lambda y: (-l(0,0)/x_coeff(l)*points[g][1] - y*points[g][0] + points[maxindex][0]*y + points[maxindex][1]*l(0,0)/x_coeff(l)+ahelp)/2.0-reqarea
+ intersect = (-l(0,0)/x_coeff(l), fsolve(area, 1)[0])
+ if (intersect[1] >= min(points[g][1], points[g-1][1]) and intersect[1] <= max(points[g][1], points[g-1][1])):
+ within = True
+
+ #print 'intersect!: ', intersect, 'points!: ', points[g], points[g-1]
+
+
+ #print 'g', g, within
+
+ elif change == 'cw':
+ #print 'cw'
+ g = g-1
+ #y = lambda x: (points[g][1]-points[g+1][1])/(points[g][0]-points[g+1][0])*(x-points[g+1][0])+points[g+1][1]
+ #print 'g', g, g+1, len(points)
+ l = line_from_point(points[g], points[g+1])
+
+ ahelp = 0
+ for i in range(g+2, maxindex+1):
+ ahelp = ahelp + points[i-1][0]*points[i][1] - points[i-1][1]*points[i][0]
+ #print 'ahelp, maxindex:', ahelp/2.0, maxindex
+ if abs(y_coeff(l)) > 0.001:
+ area = lambda x: (x*points[g+1][1] - y(l)(x)*points[g+1][0] + points[maxindex][0]*y(l)(x) - points[maxindex][1]*x + ahelp)/2.0 - reqarea
+ intersect = (fsolve(area, 1)[0], y(l)(fsolve(area, 1)[0]))
+ #print 'y(l)(fsol...):', y(l)(fsolve(area,1)[0])
+ #print area(intersect[0])
+ if (intersect[0] >= min(points[g][0], points[g+1][0]) and intersect[0] <= max(points[g][0], points[g+1][0])):
+ within = True
+ #print l, 'reqarea', reqarea
+ #print (lambda x: area(x)+reqarea)(intersect[0])
+ else:
+ #print '\n !!!!!---THAT JUST HAPPENED: cw---!!!!! \n'
+ area = lambda y: (-l(0,0)/x_coeff(l)*points[g+1][1] - y*points[g+1][0] + points[maxindex][0]*y + points[maxindex][1]*l(0,0)/x_coeff(l)+ahelp)/2.0-reqarea
+ intersect = (-l(0,0)/x_coeff(l), fsolve(area, 1)[0])
+ if (intersect[1] >= min(points[g][1], points[g+1][1]) and intersect[1] <= max(points[g][1], points[g+1][1])):
+ within = True
+ #print l, 'reqarea', reqarea
+ #print (lambda x: area(x)+reqarea)(intersect[0])
+
+ #print 'intersect!: ', intersect, 'points!: ', points[g], points[g+1], l
+
+
+ elif change == 'line0':
+ #print 'l0'
+ #y = lambda x: (points[maxindex][1]-points[maxindex+1][1])/(points[maxindex][0]-points[maxindex+1][0])*(x-points[maxindex+1][0])+points[maxindex+1][1]
+ l = line_from_point(points[maxindex], points[maxindex+1])
+ #print '---L0---', points[maxindex], points[maxindex+1], y_coeff(l)
+ #print (points[maxindex][1]-points[maxindex+1][1])/(points[maxindex][0]-points[maxindex+1][0]), '*(x-', points[maxindex+1][0], ')+', points[maxindex+1][1]
+ ahelp = 0
+ for i in range(1, maxindex+1):
+ ahelp = ahelp + points[i-1][0]*points[i][1] - points[i-1][1]*points[i][0]
+ if abs(y_coeff(l)) > 0.00001:
+ area = lambda x: (x*points[0][1] - y(l)(x)*points[0][0] + points[maxindex][0]*y(l)(x) - points[maxindex][1]*x + ahelp)/2.0 - reqarea
+ intersect = (fsolve(area, 1)[0], y(l)(fsolve(area, 1)[0]))
+ else:
+ #print '\n !!!!!---THAT JUST HAPPENED: l0---!!!!! \n'
+ area = lambda y: (-l(0,0)/x_coeff(l)*points[0][1] - y*points[0][0] + points[maxindex][0]*y + points[maxindex][1]*l(0,0)/x_coeff(l)+ahelp)/2.0-reqarea
+ intersect = (-l(0,0)/x_coeff(l), fsolve(area, 1)[0])
+
+
+ #print 'intersect!: ', intersect, 'points!: ', points[maxindex], points[maxindex+1]
+ within=True
+
+
+ else:
+ print 'Invalid change-input.'
+ #print "CONVEXDIVIDE GETPOLYPOINT:", points, maxindex, reqarea, change, " -> ", intersect, g
+ return intersect, g
+
+def remove_non_vertices(polygon):
+ p = deepcopy(polygon)
+ for i in range(len(p)-1, -1, -1):
+ if p[i][0] == p[i-1][0] and p[i][1] == p[i-1][1]:
+ #print 'removing:', p[i], 'because equal to', p[i-1]
+ p.pop(i)
+ elif point_on_line(p[i-1], p[i], p[i-2]):
+ #print 'removing:', p[i-1], 'because on linesegment(', p[i], ',', p[i-2], ')'
+ p.pop(i-1)
+ return p
+
+
+
+def convexdividesub(points0, sites0, areareq):
+ points = [(x[0], x[1]) for x in points0]
+ siteindex = [index_of_point(points, sites0[i]) for i in range(0, len(sites0))]
+ #points = check_and_fix(points)
+ sites = [points[s] for s in siteindex]
+
+ #print 'POINTS:', points, 'SITES:', sites
+ k = points.index(sites[0])
+ print 'k and stuff', k, sites, 'points\n\n', points, 'siteindex', siteindex
+ ## k = index_of_point(points, sites[0])
+ line = (points[0], points[k])
+ rightsites = [sites.index(sites[0]),]
+ polyright = points[0:k+1]
+ polyleft = points[k:]
+ polyleft.append(points[0])
+ # for h in range(0, len(areareq)):
+ # areareq[h] = areareq[h] * getarea(points)
+
+ #plt.plot([x[0] for x in points], [y[1] for y in points])
+ #plt.plot([points[0][0], points[k][0]], [points[0][1], points[k][1]])
+ #plt.plot([x[0] for x in polyright] +[polyright[0][0]] + [x[0] for x in polyleft] + [polyleft[0][0]], \
+ # [y[1] for y in polyright] + [polyright[0][1]] + [y[1] for y in polyleft] + [polyleft[0][1]])
+ #plt.show()
+ #We create a line with endpoints in the first vertex and the first
+ #site, we then move the second endpoint around the polygon until
+ #either the area on the right side of the line is larger than the
+ #desired area or we can no longer sweep any further without
+ #getting all the sites on the same side of the line.
+ while getarea(polyright) < sum([areareq[item] for item in rightsites]) and not (points[k]==sites[len(sites)-1]):
+ """print 'CHECKING IF', points[k-1], 'IN RIGHTSITES'
+ if (k > 1) and (points[k-1] in sites) and not sites.index(points[k-1]) in rightsites:
+ rightsites.append(sites.index(points[k-1]))
+ print 'APPENDING ', points[k-1], 'TO RIGHTSITES'"""
+
+ k = k + 1
+ #print 'CHECKING IF', points[k-1], 'IN RIGHTSITES'
+ if (k > 1) and (points[k-1] in sites) and not sites.index(points[k-1]) in rightsites:
+ rightsites.append(sites.index(points[k-1]))
+ #print 'APPENDING ', points[k-1], 'TO RIGHTSITES'
+
+ line = (points[0], points[k])
+ #print line[1]
+ polyright = points[0:k+1]
+ #print 'polyright', polyright
+ polyleft = points[k:]
+ polyleft.append(points[0])
+ #plt.plot([x[0] for x in points], [y[1] for y in points])
+ #plt.plot([points[0][0], points[k][0]], [points[0][1], points[k][1]])
+ #plt.plot([x[0] for x in polyright] +[polyright[0][0]] + [x[0] for x in polyleft] + [polyleft[0][0]], \
+ # [y[1] for y in polyright] + [polyright[0][1]] + [y[1] for y in polyleft] + [polyleft[0][1]])
+ #plt.show()
+
+
+ #If the area of the side to the right of the line is greater than
+ #the desired area we instead sweep by moving the first point in a
+ #counter-clockwise order.
+ if line[1] == sites[0] and getarea(polyright) > sum([areareq[item] for item in rightsites]):
+ print 'ccw, sites, points', sites, points
+ intersect, offset = getpolypoint(points, points.index(sites[0]), sum([areareq[item] for item in rightsites]), 'ccw')
+ for l in range(1, offset):
+ polyleft.append(polyright[l])
+ print 'deleting:', polyright[0:offset]
+ del polyright[0:offset]
+ #print 'ccw', offset
+ polyright.insert(0, intersect)
+ polyleft.append(intersect)
+
+ #If the second point on the line is the same as the last site and
+ #the area to the right of the line is lesser than the desired area
+ #we move the first point clockwise around the polygon.
+ elif line[1] == sites[len(sites)-1] and getarea(polyright) < sum([areareq[item] for item in rightsites]):
+ print 'cw, sites, rareq, points: ', sites,[areareq[item] for item in rightsites], points
+ #print 'l0', line[0]
+ intersect, offset = getpolypoint(points, points.index(sites[len(sites)-1]), sum([areareq[item] for item in rightsites]), 'cw')
+ offset = (-1)*offset
+ polyleft.pop()
+ for l in range(1, offset):
+ polyright.insert(0, polyleft.pop())
+ polyright.insert(0, intersect)
+ #print 'cw, left, right', offset, polyleft, polyright
+ polyleft.append(intersect)
+
+ #If we are not in any of the above cases the area to the right of
+ #the line is greater than desired, and there exist a point between
+ #point[k] and [k-1] such that if we replace the second point of
+ #the line with it we get the desired area.
+ else:
+ intersect, _ = getpolypoint(points, points.index(line[1])-1, sum([areareq[item] for item in rightsites]), 'line0')
+ #print polyright, rightsites
+ if intersect not in polyright:
+ polyright[k] = intersect
+ #print polyright
+ print 'l0, sites, points', sites, points
+ #print 'line0'
+ #print polyleft
+ if intersect not in polyleft:
+ polyleft.insert(0, intersect)
+ #print polyleft
+
+ rareareq = [areareq[item] for item in rightsites]
+
+
+ #Up until now we stored the indexes of the sites in rightsites,
+ #but as an output we'd rather just give the actual site backs.
+ #print "NEWSITES rightsites index:", rightsites, sites
+ rightsites = [sites[item] for item in rightsites]
+ leftsites = [sites[item] for item in range(0, len(sites)) if not sites[item] in rightsites]
+ lareareq = [areareq[item] for item in range(0, len(sites)) if not sites[item] in rightsites]
+
+ #print 'NEWSITES LEFT AND RIGHT SITES', leftsites, rightsites
+ #We restore the area requirement to the way it was input in and
+ #divide it into two sets, one for each new polygon. for j in
+ #range(0, len(areareq)): areareq[j]=areareq[j] / getarea(points)
+ #print 'PLEFT LSITES LAREAREQ', polyleft, leftsites, lareareq,
+ #print 'RRIGHT RSITES RAREAREQ', polyright, rightsites, rareareq
+
+ print 'leftsites:\n', leftsites, 'rightsites:\n', rightsites
+ return polyleft, polyright, leftsites, rightsites, lareareq, rareareq
+
+
+#takes a set of points in counterclockwise order (the vertices and the
+#sites), the sites in counterclockwise order and the arearequirements
+#of the sites as an input. Returns the input split into two.
+
+def convexdivide(points0, sites0, areareq_in):
+ points = [(x[0], x[1]) for x in copy.deepcopy(points0)]
+ for i in range(len(points)-1, -1, -1):
+ if (points[i] == points[i-1]):
+ points.pop(i)
+
+ sites = copy.deepcopy(sites0)
+ print 'cd input:', points0, sites0, areareq_in
+
+ totarea = getarea(points)
+ areareq = copy.deepcopy(areareq_in)
+ for h in range(0, len(areareq)):
+ areareq[h] = areareq[h]*totarea
+
+ #main
+ newsites = [deepcopy(sites)]
+ newpoints = [deepcopy(points)]
+ newareq = [deepcopy(areareq)]
+
+ while len(newsites)<(len(sites)):
+ for j in range(0, len(newsites)):
+ if len(newsites[j])>1:
+ pleft, pright, sleft, sright, lareareq, rareareq = convexdividesub(newpoints[j], newsites[j], newareq[j])
+
+ newsites[j] = sright
+ newsites.insert(j+1, sleft)
+
+ newpoints[j] = pright
+ newpoints.insert(j+1, pleft)
+ newareq[j] = rareareq
+ newareq.insert(j+1, lareareq)
+
+ try:
+ newsites = [item[0] for item in newsites]
+ except:
+ print 'Newsites failure:', newsites
+ print '\n\npoints:', points0
+ print '\n\nsites:', sites0
+ raise ValueError('FODA-SE')
+
+ parts = []
+ for i in range(0, len(newpoints)):
+ parts.append(remove_non_vertices(newpoints[i]))
+ print 'newsites', newsites
+ return parts, newsites
+
+#polygon = [(0.0, 0.0), (3.0, 2.0), (2.0, 5.0), (-3.0, 4.0), (-4.0, 3.0), (-2, 1.0)]
+
+"""
+points = [[0.0, 0.0], [10.0, 5.0], [-2.0, 8.0], [-4.0, 2.0]]
+sites = [[0.0, 0.0], [10.0, 5.0]]
+areareq = [0.5, 0.5]
+
+
+
+newpoints, newsites, areareq = convexdivide(points, sites, areareq)
+
+print newpoints
+
+#print 'newpoints:', newpoints,
+#print 'newsites:', newsites, 'newpoints', newpoints
+
+for h in range(0, len(areareq)):
+ print areareq[h], getarea(newpoints[h])
+
+fig = plt.figure()
+ax = fig.add_subplot(111)
+#pright.append(pright[0])
+#pleft.append(pleft[0])
+
+points.append(points[0])
+plt.plot([x[0] for x in points], [y[1] for y in points])
+
+for k in range(0, len(newpoints)):
+ newpoints[k].append(newpoints[k][0])
+ plt.plot([x[0] for x in newpoints[k]], [y[1] for y in newpoints[k]])
+#plt.plot([x[0] for x in pright], [y[1] for y in pright], [x[0] for x in pleft], [y[1] for y in pleft])
+
+
+#plt.plot(range(-30,30), map(line, range(-30,30)))
+#plt.xlim(min(point[0]-1, waypoint[0]-coverage[0]-1), max(point[1]+1, waypoint[0]+coverage[0]+1))
+#plt.ylim(min(point[1]-1,waypoint[1]-coverage[1]-1), max(point[1]+1, waypoint[1]+coverage[1]+1))
+plt.xlabel('x')
+plt.ylabel('y')
+plt.grid(True)
+plt.show()
+
+"""
+
diff --git a/src/scan/__init__.py b/src/scan/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..ba30b7a0d117066d866ee2a82f432da499b44ecd
--- /dev/null
+++ b/src/scan/__init__.py
@@ -0,0 +1 @@
+__all__ = ["partition"]
diff --git a/src/scanutil.py b/src/scanutil.py
new file mode 100644
index 0000000000000000000000000000000000000000..98b0599da3539060d6b7187edc2de6238b40b2ed
--- /dev/null
+++ b/src/scanutil.py
@@ -0,0 +1,150 @@
+import copy
+
+import numpy as np
+import math
+
+def check_and_fix(poly):
+ #Fixes div-by-zero errors
+ polygon = copy.deepcopy(poly)
+ for i in range(0, len(polygon)):
+ if polygon[i][0] == 0 and polygon[i-1] == 0:
+ polygon[i][0] = 0.001
+ elif polygon[i][0] == polygon[i-1][0]:
+ print 'addressing divzero for, polygon[', i,']', polygon[i]
+ polygon[i] = (polygon[i][0]*1.0001, polygon[i][1])
+ #print 'polygon[',i,'] is now', polygon[i]
+ return polygon
+
+def ccw(A, B, C):
+ """Tests whether the turn formed by A, B, and C is ccw"""
+ return (B[0] - A[0]) * (C[1] - A[1]) > (B[1] - A[1]) * (C[0] - A[0])
+
+# true if p is betweem p0 and p1
+# def ccw_between(p0, p1, p):
+
+# project vector a on vector b
+# proj.x = ( (a.x*b.x + a.y*b.y) / (b.x*b.x + b.y*b.y) ) * b.x;
+# proj.y = ( (a.x*b.x + a.y*b.y) / (b.x*b.x + b.y*b.y) ) * b.y;
+# http://en.wikipedia.org/wiki/Dot_product
+# http://en.wikipedia.org/wiki/Vector_projection
+
+def project_point_on_line(p0, p1, p):
+ a = (p[0]-p0[0], p[1]-p0[1])
+ b = (p1[0]-p0[0], p1[1]-p0[1])
+# print "A, B:", a, b
+ dot = (float(a[0]*b[0] + a[1]*b[1])) / float((b[0]*b[0] + b[1]*b[1]))
+# print "DOT:", dot
+ x = dot * b[0]
+ y = dot * b[1]
+# print "X, Y:", x, y
+ return (x + p0[0], y + p0[1])
+
+def is_point_on_segment(p0, p1, p):
+ x0 = min(p0[0], p1[0])
+ y0 = min(p0[1], p1[1])
+ x1 = max(p0[0], p1[0])
+ y1 = max(p0[1], p1[1])
+# print "IS_POINT_ON_SEGMENT:", x0, x
+ if (p[0] >= x0) and (p[0] <= x1) and (p[1] >= y0) and (p[1] <= y1):
+ return True
+ else:
+ return False
+
+def get_nearest_corner(polygon, p):
+## print "PROJJJJ: get_nearest_corner:", polygon, p
+ dists = []
+ for point in polygon:
+ dists.append ((p[0]-point[0])*(p[0]-point[0])+(p[1]-point[1])*(p[1]-point[1]))
+ i = np.argmin(dists)
+ return polygon[i], i
+
+def snap_point_to_corner(polygon, p):
+ p, i = get_nearest_corner(polygon, p)
+ return p
+
+def project_point_with_index(poly0, point):
+ poly = [(float(x[0]), float(x[1])) for x in poly0]
+ points = []
+ dists = []
+ index = []
+ for i in range(0,len(poly)):
+ j = (i + 1) % len(poly)
+ p = project_point_on_line(poly[i], poly[j], point)
+## print "PROJJJJ:", i, poly[i], poly[j], p
+ if is_point_on_segment(poly[i], poly[j], p):
+## print "PROJJJJ POINT ON SEGMENT:", i, poly[i], poly[j], p
+ points.append (p)
+ dists.append ((p[0]-point[0])*(p[0]-point[0])+(p[1]-point[1])*(p[1]-point[1]))
+ index.append(i)
+
+ if len(points) > 0:
+## print "PROJJJJ: POINTS, DISTS, INDEX:", point, dists, index
+ i = np.argmin(dists)
+ return points[i], index[i]
+ else:
+ return get_nearest_corner(poly, point)
+
+def project_point(poly, point):
+ p, index = project_point_with_index(poly, point)
+ return p
+
+def near_equal(x, y):
+ return math.fabs(x-y)<0.00000001
+
+def equal_points(p0, p1):
+ return near_equal(p0[0], p1[0]) and near_equal(p0[1], p1[1])
+
+def index_of_point(l, p):
+ for i, pp in enumerate(l):
+ if equal_points(p, pp):
+ return i
+ return -1
+
+def remove_point(l, p):
+ i = index_of_point(l, p)
+ if i >= 0:
+ l.pop(i)
+
+def count_point_occ(l, p):
+ res = 0
+ for pp in l:
+ if equal_points(p, pp):
+ res += 1
+ return res
+
+def merge_polygon_sites(poly, startp):
+ polygon = [(x[0], x[1]) for x in poly]
+ startpoints = [(x[0], x[1]) for x in startp]
+
+ sites = []
+ sites_index = []
+
+ verbose = False
+
+ if verbose:
+ print "MERGE_POLYGON_SITES polygon", polygon
+ print "MERGE_POLYGON_SITES start points", startpoints
+
+ # original code removed an element if previous element was the same from polygon
+ # can that happen?
+
+ for uav in range(len(startpoints)):
+ # p, i = projectpoint_with_index(sitehelp, startpoints[uav], False)
+ p, i = project_point_with_index(polygon, startpoints[uav])
+ sites.append(p)
+ sites_index.append(i)
+
+ if verbose:
+ print "MERGE SITES (projected startpoints):", sites
+ print "MERGE SITES_INDEX (projected startpoints):", sites_index
+
+ offset = 0
+ for si, i in enumerate(sites_index):
+ polygon.insert(i+offset+1, sites[si])
+ offset += 1
+ if verbose:
+ print "MERGED POLYGON", polygon
+
+ return polygon, sites
+
+