SHARE
TWEET

Untitled

a guest Dec 5th, 2019 76 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. def growSimpleRRT(points):
  2.     newPoints = {}
  3.     adjListMap = {}
  4.  
  5.     bool = True
  6.     count = 0
  7.     # Your code goes here
  8.     for i in points:
  9.         if len(newPoints) == 0:
  10.             newPoints[i] = points[i]
  11.             newPoints[i+1] = points[i+1]
  12.             adjListMap[i] = [i+1]
  13.             adjListMap[i+1] = [i]
  14.         elif len(newPoints) == 2 and bool:
  15.             bool = False
  16.             continue
  17.         else:
  18.             num = 0
  19.             index = 0
  20.             point1 = 0
  21.             point2 = 0
  22.             d = sys.maxsize                             #distance from random point to nearest point on line
  23.             x = 0
  24.             y = 0
  25.             for j in list(adjListMap):                  # j is the key in adjListMap. j just goes through each point
  26.                 for k in adjListMap[j]:                # k goes through every point that j is connected to
  27.                     m1 = (newPoints[k][1]-newPoints[j][1])/(newPoints[k][0]-newPoints[j][0])
  28.                     b1 = newPoints[j][1] - m1*newPoints[j][0]
  29.                     m2 = -1/m1
  30.                     b2 = points[i][1] - m2*points[i][0]
  31.                     newPointX = (b2-b1)/(m1-m2)
  32.                     newPointY = m1*newPointX + b1
  33.                     if not collisionDetection(adjListMap,newPoints,[points[i],(newPointX,newPointY)],j,k):
  34.                         if (newPoints[k][0] <= newPointX <= newPoints[j][0] or newPoints[k][0] >= newPointX >= newPoints[j][0]) and (newPoints[k][1] <= newPointY <= newPoints[j][1] or newPoints[k][1] >= newPointY >= newPoints[j][1]):
  35.                             if d > math.sqrt((newPointX-points[i][0])**2+(newPointY-points[i][1])**2):
  36.                                 d = math.sqrt((newPointX-points[i][0])**2+(newPointY-points[i][1])**2)
  37.                                 x = newPointX
  38.                                 y = newPointY
  39.                                 point1 = j
  40.                                 point2 = k
  41.                                 num = 0
  42.                         else:
  43.                             if newPoints[k][0] > newPoints[j][0]:
  44.                                 if newPointX > newPoints[k][0]:
  45.                                     if d > math.sqrt((newPoints[k][0]-points[i][0])**2+(newPoints[k][1]-points[i][1])**2):
  46.                                         d = math.sqrt((newPoints[k][0]-points[i][0])**2+(newPoints[k][1]-points[i][1])**2)
  47.                                         index = k
  48.                                         num = 1
  49.                                 else:
  50.                                     if d > math.sqrt((newPoints[j][0]-points[i][0])**2+(newPoints[j][1]-points[i][1])**2):
  51.                                         d = math.sqrt((newPoints[j][0]-points[i][0])**2+(newPoints[j][1]-points[i][1])**2)
  52.                                         index = j
  53.                                         num = 1
  54.                             else:
  55.                                 if newPointX > newPoints[j][0]:
  56.                                     if d > math.sqrt((newPoints[j][0]-points[i][0])**2+(newPoints[j][1]-points[i][1])**2):
  57.                                         d = math.sqrt((newPoints[j][0]-points[i][0])**2+(newPoints[j][1]-points[i][1])**2)
  58.                                         index = j
  59.                                         num = 1
  60.                                 else:
  61.                                     if d > math.sqrt((newPoints[k][0]-points[i][0])**2+(newPoints[k][1]-points[i][1])**2):
  62.                                         d = math.sqrt((newPoints[k][0]-points[i][0])**2+(newPoints[k][1]-points[i][1])**2)
  63.                                         index = k
  64.                                         num = 1
  65.  
  66.             if newPointX != points[i][0] and newPointY != points[i][1]:
  67.                 temp = i+count
  68.                 newPoints[temp] = points[i]
  69.                 if num == 0:
  70.                     count+=1
  71.                     newPoints[i+count] = (x,y)
  72.                     if adjListMap.get(temp) == None:
  73.                         adjListMap[temp] = [i+count]
  74.                     else:
  75.                         adjListMap[temp].append(i+count)
  76.                     adjListMap[i+count] = [temp]
  77.                     adjListMap[i+count].append(point1)
  78.                     adjListMap[i+count].append(point2)
  79.                     newList = adjListMap[point1]
  80.                     newList.remove(point2)
  81.                     newList.append(i+count)
  82.                     adjListMap[point1] = newList
  83.                     newList = adjListMap[point2]
  84.                     newList.remove(point1)
  85.                     newList.append(i+count)
  86.                     adjListMap[point2] = newList
  87.                 else:
  88.                     adjListMap[temp] = [index]
  89.                     adjListMap[index].append(temp)
  90.  
  91.  
  92.     return newPoints, adjListMap
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top