def singlearearoute(cell, inputwidth, Entrance, Exit): mintime = float('inf') optimalangle = 0 optimalpath = [] isoptrf = False Entrance = Point(Entrance) Exit = Point(Exit) for angle in range(0, 180, 90): angle = float(angle) rcell = cell.rotate(angle / 180 * math.pi) rentrance = Entrance.rotate(angle / 180 * math.pi) rexit = Exit.rotate(angle / 180 * math.pi) rcell = roundpolygon(rcell) waypoint = createallwaypoint(rcell, inputwidth) time = timeconsume(waypoint, rentrance, rexit) if (time < mintime): mintime = time optimalangle = angle optimalpath = waypoint isoptrf = False rvwaypoint = waypoint[:] rvwaypoint.reverse() time = timeconsume(rvwaypoint, rentrance, rexit) if (time < mintime): mintime = time optimalangle = angle optimalpath = rvwaypoint isoptrf = False rfcell = reflectpolygon(rcell) rfentrance = rentrance.reflect(xaxis) rfexit = rexit.reflect(xaxis) waypoint = createallwaypoint(rfcell, inputwidth) time = timeconsume(waypoint, rfentrance, rfexit) if (time < mintime): mintime = time optimalangle = angle optimalpath = waypoint isoptrf = True rvwaypoint = waypoint[:] rvwaypoint.reverse() time = timeconsume(rvwaypoint, rfentrance, rfexit) if (time < mintime): mintime = time optimalangle = angle optimalpath = rvwaypoint isoptrf = True for i in range(len(optimalpath)): if (isoptrf): optimalpath[i] = optimalpath[i].reflect(xaxis) optimalpath[i] = optimalpath[i].rotate(-optimalangle / 180 * math.pi) return optimalpath
def singlearearoute(cell, inputwidth, height, Entrance, Exit): mintime = float('inf') angle = findoptimalangle(cell) optimalpath = [] Entrance = Point(Entrance) Exit = Point(Exit) rcell = cell.rotate(angle) rentrance = Entrance.rotate(angle) rexit = Exit.rotate(angle) rcell = roundpolygon(rcell) waypoint1, waypoint2 = createallwaypoint(rcell, inputwidth, height) time = timeconsume(waypoint1, rentrance, rexit) if (time < mintime): mintime = time optimalpath = waypoint1 rvwaypoint = waypoint1[:] rvwaypoint.reverse() time = timeconsume(rvwaypoint, rentrance, rexit) if (time < mintime): mintime = time optimalpath = rvwaypoint time = timeconsume(waypoint2, rentrance, rexit) if (time < mintime): mintime = time optimalpath = waypoint2 rvwaypoint = waypoint2[:] rvwaypoint.reverse() time = timeconsume(rvwaypoint, rentrance, rexit) if (time < mintime): mintime = time optimalpath = rvwaypoint for i in range(len(optimalpath)): optimalpath[i] = optimalpath[i].rotate(-angle) return optimalpath
def findPathOld(roada, lanea, roadb, laneb, width, maxRadius=10., midPoint=5): dxa, dya = getRoadUnitVector(roada) dxb, dyb = getRoadUnitVector(roadb) pxa, pya = getOutPoint(roada, width, lanea) pxb, pyb = getInPoint(roadb, width, laneb) pa = Point(pxa, pya) pb = Point(pxb, pyb) da = Point(dxa, dya) db = Point(dxb, dyb) la = Line(pa, pa + da) lb = Line(pb, pb + db) inter = la.intersection(lb) # print(roada, roadb) disa = inter.disntance(pa) disb = inter.disntance(pb) distance = np.min([disa, disb, maxRadius]) ca = pa + da * (disa - distance) cb = pb - db * (disb - distance) o = (ca, ca + da.rotate(pi / 2)).intersection(cb, cb + db.rotate(pi / 2)) oa = ca - o ob = cb - o angle = oa.dot(ob) dangle = angle / midPoint path = [pa] for i in range(midPoint + 1): path.append(o + oa.rotate(dangle * i)) path.append(pb) # ix, iy = computeIntersection(pxa, pya, dxa, dya, pxb, pyb, dxb, dyb) # _width = (dxa * (pxb - pxa) + dya * (pyb - pya)) / 3 # print(dxa, dya, (pxb - pxa), (pyb - pya), _width) # return getOutTurnPoints(roada, _width, lanea, width) + getInTurnPoints(roadb, _width, laneb, width) return list(map(pointToDict2, path))