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)