Guest User

Untitled

a guest
Dec 5th, 2019
84
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