def _generateRandomPoints(self):
self.randomPoints.clear()
self.startingPoint = sy.Point2D(self.width, self.height)
point = None
for num in range(NUMBER_OF_RANDOM_POINTS):
# generating unique random point on grid
while point is None or point in self.randomPoints:
point = sy.Point2D(rand.randrange(0, self.width),
rand.randrange(0, self.height))
self.randomPoints.append(point)
# selecting the starting point of the track
if point.x < self.startingPoint.x:
self.startingPoint = point
def distort(track_points):
new_points = []
for index, point in enumerate(track_points):
if index == 0 or index + 1 == len(track_points):
new_points.append(point)
continue
prev_point = track_points[index - 1]
next_point = track_points[index + 1]
# bounding box
box_min_x = prev_point.x if prev_point.x < next_point.x else next_point.x
box_max_x = prev_point.x if prev_point.x > next_point.x else next_point.x
delta_x = box_max_x - box_min_x
box_min_y = prev_point.y if prev_point.y < next_point.y else next_point.y
box_max_y = prev_point.y if prev_point.y > next_point.y else next_point.y
delta_y = box_max_y - box_min_y
# random distortion
distortion_perc = 0.25
max_distortion_x = distortion_perc * delta_x if rand.random(
) >= 0.5 else -distortion_perc * delta_x
max_distortion_y = distortion_perc * delta_y if rand.random(
) >= 0.5 else -distortion_perc * delta_y
new_point = sy.Point2D(point.x + max_distortion_x,
point.y + max_distortion_y)
new_points.append(new_point)
return new_points
def predict_direction(coords1, coords2, K):
p11 = coords1[0].T[0]
p12 = coords2[0].T[0]
line1 = sp.Line2D(sp.Point2D(p11[0], p11[1]), sp.Point2D(p12[0], p12[1]))
p21 = coords1[1].T[0]
p22 = coords2[1].T[0]
line2 = sp.Line2D(sp.Point2D(p21[0], p21[1]), sp.Point2D(p22[0], p22[1]))
p = line1.intersection(line2)[0]
k_inv = np.linalg.inv(K)
line = np.dot(k_inv, np.array([[p.x.evalf()], [p.y.evalf()], [1]]))
return line.T[0]
def _resizeTrack(self):
self._determine_edge_points()
# align track with window edges
x_offset_edge = self.leftPoint.x - TRACK_BORDER_WIDTH
y_offset_edge = self.bottomPoint.y - TRACK_BORDER_WIDTH
self.track_points = [
sy.Point2D(point.x - x_offset_edge, point.y - y_offset_edge)
for point in self.track_points
]
self._determine_edge_points()
# determining bounding box of track
track_width = self.rightPoint.x - self.leftPoint.x
track_height = self.topPoint.y - self.bottomPoint.y
width_multiplier = (self.width - 2 * TRACK_BORDER_WIDTH) / track_width
height_multiplier = (self.height -
2 * TRACK_BORDER_WIDTH) / track_height
self.track_points = [
sy.Point2D(int(point.x * width_multiplier),
int(point.y * height_multiplier))
for point in self.track_points
]
def test_smooth3(self):
list_initial = [
sy.Point2D(30, 30),
sy.Point2D(20, 20),
sy.Point2D(10, 10)
]
expected = [
sy.Point2D(30, 30),
sy.Point2D(25, 25),
sy.Point2D(20, 20),
sy.Point2D(15, 15),
sy.Point2D(10, 10)
]
self.assertEqual(Track.add_midpoints(list_initial), expected)
def _generateConvexHull(self):
print("GENERATING...")
current_point = self.startingPoint
free_points = self.randomPoints[:] # points that can still be used for track formation
self.track_points = [] # points that form the track
closed_loop = False
x_delta = 1 # if track is going forwards or backwards
while not closed_loop:
self.track_points.append(current_point)
if current_point != self.startingPoint:
free_points.remove(current_point)
# determine point with smallest angle to current point
ref_point = sy.Point2D(current_point.x, current_point.y - 1)
sAng_point = {'Point': None, 'Angle': None}
starting_search_dist = 120
while True:
for point in free_points:
if current_point == point:
continue
curr2ref = math.atan2(ref_point.y - current_point.y,
ref_point.x - current_point.x)
curr2point = math.atan2(point.y - current_point.y,
point.x - current_point.x)
angle = ((curr2point - curr2ref) * (180 / math.pi))
if angle < 0:
angle += 360
# restrict maximum distance to look for connecting point
mag = math.sqrt((point.x - current_point.x)**2 +
(point.y - current_point.y)**2)
if mag > starting_search_dist and len(free_points) > 1:
continue
# prevent going backwards
if x_delta < 0 and current_point.x <= point.x and point != self.startingPoint:
continue
# update point
if sAng_point['Point'] is None or sAng_point[
'Angle'] > angle:
sAng_point['Point'] = point
sAng_point['Angle'] = angle
if sAng_point['Point'] is None:
# no point found, increasing search distance
starting_search_dist += 10
else:
x_delta = sAng_point['Point'].x - current_point.x
break
current_point = sAng_point['Point']
# checking if loop is closed
if current_point == self.startingPoint:
self.track_points.append(self.startingPoint)
closed_loop = True
def line_intersection(self, line1, line2):
xdiff = (line1.p1.x - line1.p2.x, line2.p1.x - line2.p2.x)
ydiff = (line1.p1.y - line1.p2.y, line2.p1.y - line2.p2.y
) # Typo was here
def det(a, b):
return a[0] * b[1] - a[1] * b[0]
div = det(xdiff, ydiff)
if div == 0:
return []
d = (det(*[[line1.p1.x, line1.p1.y], [line1.p2.x, line1.p2.y]]),
det(*[[line2.p1.x, line2.p1.y], [line2.p2.x, line2.p2.y]]))
x = det(d, xdiff) / div
y = det(d, ydiff) / div
# test if intersection if within line segment
return [sy.Point2D(x, y)]
def add_midpoints(track_points):
new_points = [0 for i in range(len(track_points) * 2 - 1)]
for index1 in range(0, len(new_points), 2):
index2 = index1 + 2
mid_point_index = index1 + 1
point1 = track_points[int(index1 / 2)]
if index2 >= len(new_points):
new_points[index1] = point1
break
point2 = track_points[int(index2 / 2)]
if point1.x >= point2.x:
mid_point_x = point2.x + (point1.x - point2.x) / 2
elif point1.x < point2.x:
mid_point_x = point1.x + (point2.x - point1.x) / 2
if point1.y >= point2.y:
mid_point_y = point2.y + (point1.y - point2.y) / 2
elif point1.y < point2.y:
mid_point_y = point1.y + (point2.y - point1.y) / 2
new_points[index1] = point1
new_points[mid_point_index] = sy.Point2D(mid_point_x, mid_point_y)
return new_points
def to_sympy(self):
return sympy.Point2D(self._x, self._y)
def searchVitualPath(self):
self._virPathPntLst = [[] for x in range(self._robNum)]
self._virPathLst = [[] for x in range(self._robNum)]
for robID in range(self._robNum):
print('robID = ', robID)
path = self._virPathLst[robID]
pathPnt = self._virPathPntLst[robID]
stree = nx.Graph()
stree.add_edges_from(self._robStreeLst[robID].edges())
'''
此处需要修改
'''
for stcInd in self._stcVitualIndSet:
if stcInd in stree:
stree.remove_node(stcInd)
# nei = stree.neighbors(stcInd)
# stree.remove_edge((nei,stcInd))
rob_row = self._ins._robRowLst[robID]
rob_col = self._ins._robColLst[robID]
baseInd = GridInd(rob_row, rob_col)
lastInd = GridInd(rob_row, rob_col)
cenInd = self._s_map.gridInd2STCGridInd(
GridInd(row=self._robPosLst[robID][0],
col=self._robPosLst[robID][1]))
canInd = STCGridInd(cenInd.row, cenInd.col, cenInd.virType)
robPathSet = set()
for stcInd in self._robSetLst[robID]:
robPathSet.add(self._stcGraph.nodes[stcInd]['vert']._LBInd)
robPathSet.add(self._stcGraph.nodes[stcInd]['vert']._RBInd)
robPathSet.add(self._stcGraph.nodes[stcInd]['vert']._LTInd)
robPathSet.add(self._stcGraph.nodes[stcInd]['vert']._RTInd)
cenDir = DirType.left
lastDir = DirType.left
circleTime = 0
while True:
lastInd = baseInd
cenDir = lastDir
path.append(baseInd)
pathPnt.append((baseInd.row + 0.5, baseInd.col + 0.5))
# print('path = ', path)
baseNeiLst = self._s_map.baseMapNeighbors(baseInd)
# print('cenInd',cenInd)
# print('stcNeiLst',stcNeiLst)
# exit()
cenInd = self._s_map.gridInd2STCGridInd(baseInd)
stcNeiLst = list(stree.neighbors(cenInd))
baseNeiLstStc = []
for ind in baseNeiLst:
sg1 = sympy.Segment2D(
sympy.Point2D(ind[0] + 0.5, ind[1] + 0.5),
sympy.Point2D(baseInd[0] + 0.5, baseInd[1] + 0.5))
# print('sg1 ',sg1)
# print('len = ', len(stcNeiLst))
intersectionBool = False
for stcInd in stcNeiLst:
# print(stcInd)
pnt1 = sympy.Point2D(
self._stcGraph.nodes[cenInd]['vert']._pos_x,
self._stcGraph.nodes[cenInd]['vert']._pos_y)
pnt2 = sympy.Point2D(
self._stcGraph.nodes[stcInd]['vert']._pos_x,
self._stcGraph.nodes[stcInd]['vert']._pos_y)
sg2 = sympy.Segment2D(pnt1, pnt2)
# print('sg2',sg2)
# print(sg2.intersection(sg1))
if len(sg2.intersection(sg1)) != 0:
intersectionBool = True
break
# baseNeiLstStc.append(ind)
if not intersectionBool:
baseNeiLstStc.append(ind)
# is_Intersection(sg1,sg2):
# print(baseNeiLst)
# print(baseNeiLstStc)
chsSameMega = False
candLst = []
for ind in baseNeiLstStc:
if ind in path:
continue
if ind not in robPathSet:
continue
# 需要修改
_row = floor(ind.row / 2)
_col = floor(ind.col / 2)
if GridInd(_row, _col) in self._vitualIndSet:
continue
'''
需要修改
'''
if self._s_map.inSameSTCMegaBox(baseInd, ind):
baseInd = ind
chsSameMega = True
break
else:
candLst.append(ind)
# print(baseInd)
if not chsSameMega:
if len(candLst) != 0:
# print(cenDir)
# print(candLst)
# print('baseInd', baseInd)
if len(candLst) == 1:
baseInd = candLst[0]
# print('baseInd', baseInd)
else:
for cand in candLst:
if lastDir == getDir(baseInd, cand):
baseInd = cand
break
# if cenDir == DirType.center:
else:
'''
end construct vitual path
'''
break
# print(lastInd,baseInd)
lastDir = getDir(lastInd, baseInd)
circleTime += 1
if circleTime > 600:
pass
break