def calc_baseline(p1: QPointF, p2: QPointF, p_mouse: QPointF):
if p1 == p2:
return p1, p2
p1_s = Point(p1.x(), p1.y())
p2_s = Point(p2.x(), p2.y())
p_m = Point(p_mouse.x(), p_mouse.y())
line = Line(p1_s, p2_s)
if line.contains(p_m):
return p1, p2
perpendicular_1 = line.perpendicular_line(p1_s)
perpendicular_2 = line.perpendicular_line(p2_s)
para = line.parallel_line(p_m)
prompt_p_1 = para.intersection(perpendicular_1)
prompt_p_2 = para.intersection(perpendicular_2)
q_p_1 = QPointF(prompt_p_1[0].x, prompt_p_1[0].y)
q_p_2 = QPointF(prompt_p_2[0].x, prompt_p_2[0].y)
return q_p_1, q_p_2
from sympy import Point, Line, Segment p1, p2, p3 = Point(0, 0), Point(1, 1), Point(1, 2) l1 = Line(p1, p2) print l1.contains(Point(-1, -1)) print l1.contains(Point(1, 1)) print l1.contains(Point(0.1, 0.1))
def createallwaypoint(rboundary):
#convert to rotated frame
#define the width,generate cut position
cutpos = np.arange(bounds[0] + width, bounds[2] + width, width)
pointpos = np.arange(bounds[0] + width / 2, bounds[2] + width / 2, width)
cutlines = formperpenicularlines(cutpos)
#form sub-polygons cut by cutlines
subpolygons = []
remain = rboundary
for line in cutlines:
subpolygon, remain = splitpolygon(remain, line)
subpolygons.append(subpolygon)
if not (remain):
break
#design waypoints in subpolygons
i = 0
waypoints = []
for polygon in subpolygons:
sides = polygon.sides
vertices = polygon.vertices
lines = []
for side in sides:
if (side.is_parallel(yaxis)):
lines.append(side)
if (len(lines) == 2): #formed by 2 cuts
if (lines[0].p1.x < lines[1].p1.x):
leftline = lines[0]
rightline = lines[1]
else:
leftline = lines[1]
rightline = lines[0]
if (leftline.p1.y < leftline.p2.y):
leftline = Line(leftline.p2, leftline.p1)
if (rightline.p1.y < rightline.p2.y):
rightline = Line(rightline.p2, rightline.p1)
#seprate vertices into upper part and lower part
leftupindex = findpoint(vertices, leftline.p1)
leftdownindex = findpoint(vertices, leftline.p2)
rightupindex = findpoint(vertices, rightline.p1)
rightdownindex = findpoint(vertices, rightline.p2)
if (rightupindex < leftupindex):
uppoints = list(vertices[rightupindex:leftupindex + 1])
else:
uppoints = list(vertices[rightupindex:len(vertices)])
uppoints.extend(vertices[0:leftupindex + 1])
if (leftdownindex < rightdownindex):
downpoints = list(vertices[leftdownindex:rightdownindex + 1])
else:
downpoints = list(vertices[leftdownindex:len(vertices)])
downpoints.extend(vertices[0:leftupindex + 1])
index = findbound(uppoints)
buttomindex = index[1] #upper min y
if (index[1] == index[3]): #upper part is segment
up1 = None
else:
cut = xaxis.parallel_line(uppoints[buttomindex])
up1, temp = splitpolygon(polygon, cut)
index = findbound(downpoints)
topindex = index[3] #lower max y
if (index[1] == index[3]): #lower part is segment
down1 = None
else:
cut = xaxis.parallel_line(downpoints[topindex])
temp, down1 = splitpolygon(polygon, cut)
waypoint = []
if (up1 and down1):
waypoint = createwaypoint(up1, down1, pointpos[i])
elif (up1): #lower part is segment
upbound = up1.bounds
upmin = upbound[1] #ymin
upmax = upbound[3] #ymax
buttom = downpoints[topindex].y #y of lower part
if (upmax - upmin <= width / 2):
waypoint = [Point(pointpos[i], upmax - width / 2)]
else:
cutpt = Point(pointpos[i], upmax - width / 2)
cut = yaxis.perpendicular_line(cutpt)
crosspoint = polygon.intersection(cut)
if (crosspoint[0].x < crosspoint[1].x):
leftcross = crosspoint[0]
rightcross = crosspoint[1]
else:
leftcross = crosspoint[1]
rightcross = crosspoint[0]
if (leftcross.x <= pointpos[i]
and rightcross.x >= pointpos[i]):
waypoint = [Point(pointpos[i], upmax - width / 2)]
elif (leftcross.x > pointpos[i]):
waypoint = [leftcross]
else:
waypoint = [rightcross]
if (upmin - buttom > width):
waypoint.append(Point(pointpos[i], upmin - width / 2))
waypoint.append(Point(pointpos[i], buttom + width / 2))
elif (down1):
downbound = down1.bounds
downmin = downbound[1] #ymin
downmax = downbound[3] #ymax
top = uppoints[buttomindex].y #upper segment y
if (downmax - downmin <= width / 2):
waypoint = [Point(pointpos[i], downmin + width / 2)]
else:
cutpt = Point(pointpos[i], downmin + width / 2)
cut = yaxis.perpendicular_line(cutpt)
crosspoint = polygon.intersection(cut)
if (crosspoint[0].x < crosspoint[1].x):
leftcross = crosspoint[0]
rightcross = crosspoint[1]
else:
leftcross = crosspoint[1]
rightcross = crosspoint[0]
if (leftcross.x <= pointpos[i]
and rightcross.x >= pointpos[i]):
waypoint = [Point(pointpos[i], downmin + width / 2)]
elif (leftcross.x > pointpos[i]):
waypoint = [leftcross]
else:
waypoint = [rightcross]
if (top - downmax >= width):
waypoint.append(Point(pointpos[i], downmax + width / 2))
waypoint.append(Point(pointpos[i], top - width / 2))
waypoint.reverse()
else:
buttom = downpoints[topindex].y
top = uppoints[buttomindex].y
if (top - buttom <= width):
waypoint = [Point(pointpos[i], (top + buttom) / 2)]
else:
waypoint = [
Point(pointpos[i], top - width / 2),
Point(pointpos[i], buttom + width / 2)
]
elif (len(lines) == 1):
index = findbound(vertices)
leftindex = index[0]
rightindex = index[2]
line = lines[0]
if (line.p1.y < line.p2.y):
line = Line(line.p2, line.p1)
bound = polygon.bounds
ymax = bound[3]
ymin = bound[1]
xmin = bound[0]
xmax = bound[2]
if (line.contains(vertices[leftindex])): #cut line at left
if (ymax - ymin <= width):
waypoint = [Point(xmax - width / 2, (ymin + ymax) / 2)]
else:
cutpt = Point(pointpos[i], ymax - width / 2)
cut = yaxis.perpendicular_line(cutpt)
crosspoint = polygon.intersection(cut)
if (crosspoint[0].x < crosspoint[1].x):
crosspoint = crosspoint[1]
else:
crosspoint = crosspoint[0]
if (crosspoint.x > pointpos[i]):
waypoint = [Point(pointpos[i], ymax - width / 2)]
else:
waypoint = [crosspoint]
cutpt = Point(pointpos[i], ymin + width / 2)
cut = yaxis.perpendicular_line(cutpt)
crosspoint = polygon.intersection(cut)
if (crosspoint[0].x < crosspoint[1].x):
crosspoint = crosspoint[1]
else:
crosspoint = crosspoint[0]
if (crosspoint.x > pointpos[i]):
waypoint.append(Point(pointpos[i], ymin + width / 2))
else:
waypoint.append(crosspoint)
#see if segment consist of 2 waypoints can cover rightmost part
way = Segment(*waypoint)
wp = Point(vertices[rightindex].x - width / 2,
vertices[rightindex].y)
limit = xaxis.perpendicular_line(wp)
if not (way.intersection(limit)):
waypoint.insert(1, wp)
else: #cut line at right
if (ymax - ymin <= width):
waypoint = [Point(xmin + width / 2, (ymax + ymin) / 2)]
else:
cutpt = Point(pointpos[i], ymax - width / 2)
cut = yaxis.perpendicular_line(cutpt)
crosspoint = polygon.intersection(cut)
if (crosspoint[0].x < crosspoint[1].x):
crosspoint = crosspoint[0]
else:
crosspoint = crosspoint[1]
if (crosspoint.x < pointpos[i]):
waypoint = [Point(pointpos[i], ymax - width / 2)]
else:
waypoint = [crosspoint]
cutpt = Point(pointpos[i], ymin + width / 2)
cut = yaxis.perpendicular_line(cutpt)
crosspoint = polygon.intersection(cut)
if (crosspoint[0].x < crosspoint[1].x):
crosspoint = crosspoint[0]
else:
crosspoint = crosspoint[1]
if (crosspoint.x < pointpos[i]):
waypoint.append(Point(pointpos[i], ymin + width / 2))
else:
waypoint.append(crosspoint)
way = Segment(*waypoint)
wp = Point(vertices[leftindex].x + width / 2,
vertices[leftindex].y)
limit = xaxis.perpendicular_line(wp)
if not (way.intersection(limit)):
waypoint.insert(1, wp)
else: #width of the area less than sensor width
bound = polygon.bounds
if (bound[3] - bound[1] < width):
waypoint = [Point(pointpos[i], (bound[1] + bound[3]) / 2)]
else:
ypos = np.array(bound[3] - width / 2, bound[1] + width / 2,
width)
ypos = ypos.reshape(len(ypos), 1)
waypoint = ypos.insert(0, pointpos[i], axis=1)
i = i + 1
if (i % 2 == 1):
waypoint.reverse()
waypoints.extend(waypoint)
return waypoints
from sympy import Point, Line, Segment p1, p2, p3 = Point(0, 0), Point(1, 1), Point(1, 2) l1 = Line(p1, p2) print l1.contains(Point(-1,-1)) print l1.contains(Point(1,1)) print l1.contains(Point(0.1,0.1))
Point(pointpos[i], top - width / 2),
Point(pointpos[i], buttom + width / 2)
]
elif (len(lines) == 1):
index = findbound(vertices)
leftindex = index[0]
rightindex = index[2]
line = lines[0]
if (line.p1.y < line.p2.y):
line = Line(line.p2, line.p1)
bound = polygon.bounds
ymax = bound[3]
ymin = bound[1]
xmin = bound[0]
xmax = bound[2]
if (line.contains(vertices[leftindex])): #cut line at left
if (ymax - ymin <= width):
waypoint = [Point(xmax - width / 2, (ymin + ymax) / 2)]
else:
cutpt = Point(pointpos[i], ymax - width / 2)
cut = yaxis.perpendicular_line(cutpt)
crosspoint = polygon.intersection(cut)
if (crosspoint[0].x < crosspoint[1].x):
crosspoint = crosspoint[1]
else:
crosspoint = crosspoint[0]
if (crosspoint.x > pointpos[i]):
waypoint = [Point(pointpos[i], ymax - width / 2)]
else:
waypoint = [crosspoint]
cutpt = Point(pointpos[i], ymin + width / 2)
