import bge
import collections
from mathutils import Vector
def myRound(x, prec=4, base=.125):
return round(base * round(float(x)/base),prec)
def shortest_path(graph, start, goal):
try:
return next(bfs_paths(graph, start, goal))
except StopIteration:
return None
def bfs_paths(graph, start, goal):
queue = [(start, [start])]
while queue:
(vertex, path) = queue.pop(0)
for next in graph[vertex] - set(path):
if next == goal:
yield path + [next]
else:
queue.append((next, path + [next]))
def main():
cont = bge.logic.getCurrentController()
own = cont.owner
Target = own.scene.objects['agent']
pos = (myRound(own.worldPosition.x),myRound(own.worldPos ition.y))
if 'offsetsBig' not in own:
own['Tile']=own.scene.objects['world']
own['offsetsSmall']={}
index=0
w = 4
for x in range(w):
for y in range(w):
own['offsetsSmall'].update({ index:(-(w/2)+x,-(w/2)+y)})
index+=1
own['offsetsBig']={}
index=0
w = 10
for x in range(w):
for y in range(w):
own['offsetsBig'].update({ index:(-(w/2)+x,-(w/2)+y)})
index+=1
#clear last position
if 'GridPos' in own:
for pos in own['GridPos']:
if (pos[0],pos[1]) in own['Tile']['grid']:
gridPos = own['Tile']['grid'][(pos[0],pos[1])]
gridPos[0]=False
for vert in gridPos[1]:
vert.color = [1,0,0,1]
del own['GridPos']
#mark position center
own['GridPos']=[]
if (pos[0],pos[1]) in own['Tile']['grid']:
gridPos = own['Tile']['grid'][(pos[0],pos[1])]
gridPos[0]=True
for vert in gridPos[1]:
vert.color = [0,0,1,1]
own['GridPos'].append((pos[0],pos[1]))
if 'Path' not in own:
pathC=[]
offset_1 = own['offsetsBig']
#mark points around position
else:
offset_1 = own['offsetsSmall']
#T = Target.worldPosition
#T.z = own.worldPosition.z
pos = own.worldPosition
pos = Vector([myRound(pos.x),myRound(pos.y),0])
for offset in offset_1:
offsets = offset_1[offset]
#print(offsets)
pos1 = pos + Vector([offsets[0]*.125,offsets[1]*.125,0])
if (pos1[0],pos1[1]) in own['Tile']['grid']:
gridPos = own['Tile']['grid'][(pos1[0],pos1[1])]
m = (Vector([pos1[0],pos1[1]])-Vector([pos[0],pos[1]])).magnitude
if m<own['size']:
#occupy these tiles
gridPos[0]=True
for vert in gridPos[1]:
vert.color = [0,0,1,1]
own['GridPos'].append((pos1[0],pos1[1]))
if 'Path' not in own:
pathC.append([gridPos,Vector([pos1[0],pos1[1]])])
elif 'Path' not in own:
#path find these tiles
if gridPos[0]==False:
pathC.append([gridPos,Vector([pos1[0],pos1[1]])])
for vert in gridPos[1]:
vert.color = [0,1,0,1]
if 'Path' not in own:
d = own.getDistanceTo(Vector([Target.worldPosition.x,Target.worldPosition.y,own. worldPosition.z]))
#print(d)
if d>.25:
print('building graph')
graph = {}
index =0
storedG = None
storedG_D = 100
storedS = None
storedS_D = 100
nodeKey = {}
for pos in pathC:
index2=0
posX = Vector([pos[1][0],pos[1][1],own.worldPosition.z])
D =Target.getDistanceTo(posX)
if D<storedG_D:
storedG = index
storedG_D=D
D =own.getDistanceTo(posX)
if D<storedS_D:
storedS = index
storedS_D=D
nodeKey.update({index:posX})
for pos2 in pathC:
if pos[1]!=pos2[1]:
d = (pos[1]-pos2[1]).magnitude
if d<.225:
if own['Debug']==True:
#$print(d)
p1 = Vector([pos[1][0],pos[1][1],.125])
p2 = Vector([pos2[1][0],pos2[1][1],.125])
#print(p1)
bge.render.drawLine(p1,p2,(.5+bge.logic.getRandomF loat()*.5,0,0))
if (index) not in graph:
graph.update({ index:set([index2])} )
else:
graph[index].add(index2)
index2+=1
index+=1
own['NKey']=nodeKey
own['Path']= shortest_path(graph, storedS, storedG)
else:
index =0
if len(own['Path'])>0:
for point in own['Path']:
p = own['NKey'][point]
if index!=0 and own['Debug']==True:
p2 = own['NKey'][own['Path'][index-1]]
bge.render.drawLine(p,p2,(1,0,0,1))
else:
if own.getDistanceTo(p)<.25:
own['Path'].pop(0)
break
else:
p.z=own.worldPosition.z
local = p-own.worldPosition
local = own.worldOrientation.inverted()*local
if local.x>0:
va=own.getVectTo(p)[1]
if abs(local.y)>.5:
if local.y>0:
own.applyRotation((0,0,.1),1)
else:
own.applyRotation((0,0,-.1),1)
else:
own.alignAxisToVect(va,0,.1)
own.alignAxisToVect([0,0,1],2,1)
own.applyForce(va*1,0)
#own.applyForce((0,0,9.8),0)
if abs(local.y)-local.x<0:
own.applyForce([8,0,0],1)
own.localLinearVelocity.y*=.1
else:
own.localLinearVelocity.x*=.1
own.localLinearVelocity.y*=.1
if local.y>-.01:
own.applyRotation((0,0,.05),1)
else:
own.applyRotation((0,0,-.05),1)
index+=1
own['R']=str(own['Path'])
else:
del own['Path']
main()
http://pasteall.org/664588/python -partialy commented code
Attachments
MarkGrid_working2.blend (1.01 MB)