AIMap.py 5.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133
  1. from random import choice
  2. import heapq
  3. from examples.tanks.mymath import D60, D105, D360
  4. import math
  5. FREE, OBSTACLE = range(2)
  6. class AIMap():
  7. def __init__(self, level):
  8. self.totalWidth = level.totalWidth
  9. self.totalHeight = level.totalHeight
  10. self.cellSize = level.cellSize
  11. obstacleMap = level.structure
  12. self.cellsX = self.totalWidth // self.cellSize
  13. self.cellsY = self.totalHeight // self.cellSize
  14. self.structure = [[ OBSTACLE if obstacleMap[x][y] else FREE for y in range(self.cellsY)] for x in range(self.cellsX)]
  15. def getNewExplore(self, position, tankAngle):
  16. cell = self.calculateCell(position)
  17. successors = self.getSuccessors(cell)
  18. good = []
  19. for (successor,wildcard) in successors :
  20. #successorCoords = self.calculateCoords(successor)
  21. diffAngle = math.fabs(tankAngle - self.getAngleToDest(cell,successor))
  22. if diffAngle > math.pi :
  23. diffAngle = D360-diffAngle
  24. value = len(self.getSuccessors(successor))
  25. if diffAngle <= D60 :
  26. good.append((successor,value+7))
  27. elif diffAngle <= D105 :
  28. good.append((successor,value+4))
  29. else :
  30. good.append((successor,value))
  31. max_value = 0
  32. result = []
  33. for (pos,value) in good :
  34. if value > max_value :
  35. result = [pos]
  36. max_value = value
  37. elif value == max_value :
  38. result.append(pos)
  39. return self.calculateCoords(choice(result))
  40. def getSuccessors(self, pos):
  41. successors = []
  42. (xpos, ypos) = pos
  43. i = 0
  44. for y in range(ypos-1,ypos+2):
  45. for x in range(xpos-1,xpos+2):
  46. i = i + 1
  47. if x >= 0 and x < self.cellsX and y >= 0 and y < self.cellsY :
  48. if i == 5 :
  49. continue
  50. elif self.structure[x][y] == FREE :
  51. if (x == xpos or y == ypos) :
  52. successors.append(((x,y),1))
  53. elif i == 1 :
  54. if self.structure[x+1][y] == FREE and self.structure[x][y+1] == FREE : successors.append(((x,y),1.4))
  55. elif i == 3 :
  56. if self.structure[x-1][y] == FREE and self.structure[x][y+1] == FREE : successors.append(((x,y),1.4))
  57. elif i == 7 :
  58. if self.structure[x+1][y] == FREE and self.structure[x][y-1] == FREE : successors.append(((x,y),1.4))
  59. elif i == 9 :
  60. if self.structure[x-1][y] == FREE and self.structure[x][y-1] == FREE : successors.append(((x,y),1.4))
  61. return successors
  62. def calculateCell(self, p):
  63. (xp, yp) = p
  64. return ( int(xp / self.cellSize) , int(yp / self.cellSize) )
  65. def calculateCoords(self,cell):
  66. return (self.cellSize * (cell[0]+0.5),self.cellSize * (cell[1]+0.5))
  67. def getAngleToDest(self,pos, des):
  68. (xpos, ypos) = pos
  69. (xdes, ydes) = des
  70. return math.atan2(ypos-ydes, xdes-xpos) % D360
  71. #------PathFinding----#
  72. def calculatePath(self, start, destination):
  73. #self.controller.stop()
  74. class PriorityQueue:
  75. def __init__(self):
  76. self.heap = []
  77. def push(self, item, priority):
  78. pair = (priority,item)
  79. heapq.heappush(self.heap,pair)
  80. def pop(self):
  81. popped = heapq.heappop(self.heap)
  82. return popped[1]#item
  83. def isEmpty(self):
  84. return len(self.heap) == 0
  85. #A*
  86. start_cell = self.calculateCell(start)
  87. destination_cell = self.calculateCell(destination)
  88. explored = {}
  89. fringe = PriorityQueue();
  90. startNode = (start_cell, 0, 0) #(state,parent,cost to get to this state)
  91. fringe.push(startNode, startNode[2])
  92. while (True) :
  93. while (True) :
  94. currentNode = fringe.pop()
  95. if currentNode[0] not in explored : #same node on the fringe multiple times. we only want the lowest cost node expanded.
  96. break
  97. explored[currentNode[0]] = currentNode
  98. if currentNode[0] == destination_cell :
  99. break
  100. successors = self.getSuccessors(currentNode[0])
  101. for (successor,cost) in successors :
  102. totalcost = currentNode[2] + cost
  103. if successor not in explored : #nodes that are not expanded yet, but are already on the fringe will still be added.
  104. node = (successor, currentNode, totalcost)
  105. heuristic = ((successor[0] - destination[0]) ** 2 + (successor[1] - destination[1]) ** 2 ) ** 0.5
  106. fringe.push(node,(totalcost+heuristic))
  107. newpoints = []
  108. while ( currentNode[0] != start_cell) :
  109. newpoints.insert(0, self.calculateCoords(currentNode[0]))
  110. currentNode = currentNode[1]
  111. return newpoints