s464968/AIprojekt-wozek
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

To jeszcze nie działa. Testowy commit

Browse Source
This commit is contained in:
tonywesoly 2022-05-12 21:01:31 +02:00
parent a33806e32a
commit b771ac0f00
3 changed files with 161 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
Astar.py
View File

@ -1,60 +1,14 @@
import math
from operator import ne
import pygame
from Global_variables import Global_variables as G_var
from Min_heap import Min_heap
from Node import Node, State
from Package import Package
from Shelf import Shelf
class State:
def __init__(self, direction, x, y):
self.direction = direction # kierunek w ktorym "patrzy wozek"
self.x = x
self.y = y
def get_direction(self):
return self.direction
def get_x(self):
return self.x
def get_y(self):
return self.y
def goal_test(self, goal): # sprawdza czy osiagnelismy cel
if self.x == goal[0] and self.y == goal[1]:
return True
else:
return False
class Node:
def __init__(self, state, walkable):
self.state = state
self.direction = state.direction
self.walkable = walkable
self.g_cost = 0
self.h_cost = 0
self.parent = None
def get_action(self):
return self.action
def get_direction(self):
return self.direction
def get_parent(self):
return self.parent
def f_cost(self):
if self.walkable:
return self.g_cost + self.h_cost
else:
# return 0
return math.inf
class Pathfinding:
def __init__(self, enviroment_2d):
# self.grid = []
@ -89,39 +43,46 @@ class Pathfinding:
start_node = self.grid[starting_state.x][starting_state.y]
target_node = self.grid[target_state.x][target_state.y]
fringe = []
# fringe = []
fringe = Min_heap()
explored = []
is_target_node_walkable = True
if not target_node.walkable:
target_node.walkable = True
is_target_node_walkable = False
fringe.append(start_node)
# fringe.append(start_node)
fringe.insert(start_node)
while len(fringe) > 0:
current_node = fringe[0]
for i in range(1, len(fringe)):
if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost):
current_node = fringe[i]
# while len(fringe) > 0:
while fringe.count() > 0:
# current_node = fringe[0]
current_node = fringe.extract()
# for i in range(1, len(fringe)):
# if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost):
# current_node = fringe[i]
fringe.remove(current_node)
# fringe.remove(current_node)
explored.append(current_node)
if current_node.state == target_node.state:
path = self.retrace_path(start_node,target_node)
self.path = path
return
for neighbour in self.succ(current_node):
if not neighbour.walkable or neighbour in explored:
# if neighbour in explored:
continue
new_movement_cost_to_neighbour = current_node.g_cost + self.get_distance(current_node,neighbour)
if new_movement_cost_to_neighbour < neighbour.g_cost or not neighbour in fringe:
# if new_movement_cost_to_neighbour < neighbour.g_cost or not neighbour in fringe:
if new_movement_cost_to_neighbour < neighbour.g_cost or not fringe.contains(neighbour):
neighbour.g_cost = new_movement_cost_to_neighbour
neighbour.h_cost = self.get_distance(neighbour,target_node)
neighbour.parent = current_node
if not neighbour in fringe:
fringe.append(neighbour)
# if not neighbour in fringe:
if not fringe.contains(neighbour):
fringe.insert(neighbour)
target_node.walkable = is_target_node_walkable

76
Min_heap.py Normal file
View File

@ -0,0 +1,76 @@
from cgitb import small
from heapq import heapify
import math
from multiprocessing.dummy import Array
from Node import Node, State
class Min_heap:
def __init__(self):
self.items = []
def parent(self,i):
return (i - 1) >> 1
def left(self,i):
return (i << 1) + 1
def right(self,i):
return (i << 1) + 2
def heapify(self, i):
l = self.left(i)
r = self.right(i)
if l < len(self.items) - 1 and self.items[l] < self.items[i]:
smallest = l
else:
smallest = i
if r < len(self.items) - 1 and self.items[r] < self.items[smallest]:
smallest = r
if smallest != i:
self.items[i], self.items[smallest] = self.items[smallest], self.items[i]
self.items[i].heap_index, self.items[smallest].heap_index = self.items[smallest].heap_index, self.items[i].heap_index
self.heapify(smallest)
def extract(self):
if len(self.items) < 1:
print("STOS PUSTY!")
return
min = self.items[0]
# self.items[0] = self.items[last_item_index]
# self.items[0] = self.items.pop()
self.items[0] = self.items[len(self.items) - 1]
self.items.pop()
self.heapify(0)
return min
def decrese_key(self, index, item):
if item > self.items[index]:
print("Nowy klucz wiekszy od klucza aktualnego!")
return
self.items[index] = item
while index > 0 and self.items[self.parent(index)] > self.items[index]:
self.items[index], self.items[self.parent(
index)] = self.items[self.parent(index)], self.items[index]
self.items[index].heap_index, self.items[self.parent(
index)].heap_index = self.items[self.parent(index)].heap_index, self.items[index].heap_index
index = self.parent(index)
def insert(self, item):
temp_node = Node(State(0,0,0),False)
temp_node.h_cost = math.inf
self.items.append(temp_node)
item.heap_index = len(self.items) - 1
self.decrese_key(len(self.items) - 1, item)
def count(self):
return len(self.items)
def contains(self, item):
in_range = len(self.items) > item.heap_index
contains = False
if in_range:
contains = self.items[item.heap_index] == item
return in_range and contains

64
Node.py Normal file
View File

@ -0,0 +1,64 @@
import math
class State:
def __init__(self, direction, x, y):
self.direction = direction # kierunek w ktorym "patrzy wozek"
self.x = x
self.y = y
def get_direction(self):
return self.direction
def get_x(self):
return self.x
def get_y(self):
return self.y
def goal_test(self, goal): # sprawdza czy osiagnelismy cel
if self.x == goal[0] and self.y == goal[1]:
return True
else:
return False
class Node:
def __init__(self, state, walkable):
self.state = state
self.direction = state.direction
self.walkable = walkable
self.g_cost = 0
self.h_cost = 0
self.parent = None
self.heap_index = 0
def get_action(self):
return self.action
def get_direction(self):
return self.direction
def get_parent(self):
return self.parent
def f_cost(self):
if self.walkable:
return self.g_cost + self.h_cost
else:
# return 0
return math.inf
# if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost):
def __lt__(self, other):
if self.f_cost() == other.f_cost():
return self.h_cost < other.h_cost
return self.f_cost() < other.f_cost()
def __gt__(self,other):
if self.f_cost() == other.f_cost():
return self.h_cost > other.h_cost
return self.f_cost() > other.f_cost()
def __eq__(self,other):
return self.f_cost() == other.f_cost() and self.h_cost == other.h_cost