def trouverChemin(self, x1, y1, x2, y2, _listePointInteret):
collisionEntre = self.collider.collisionEntre #local var perf optim
startElements = self.pointContenuListe(x1, y1, _listePointInteret)
endElements = self.pointContenuListe(x2, y2, _listePointInteret)
blockingElements = startElements + list(
set(endElements) - set(startElements)) #merge removing duplicates
t = time.time()
#self.computeElementsCollisonList(_listePointInteret)
#print "Init Collisions: " + str(time.time() - t)
listePointInteret = sorted(
_listePointInteret,
key=lambda pointInteret: Ligne("", x1, y1, x1, y1).distanceAvec(
pointInteret.forme))
self.resetCollisonLists(listePointInteret)
#self.fenetre = None
initLine = Ligne("", x1, y1, x2, y2)
if self.fenetre:
initLine.setCouleur("violet")
initLine.dessiner(self.fenetre)
pathList = [[initLine]]
countTest = 0
tl = time.time()
while len(pathList) > 0:
#print("{} loop: {}".format(time.time()-tl, countTest))
tl = time.time()
countTest += 1
#print(pathList)
path = pathList.pop(0) #get firt path
if self.fenetre:
#self.fenetre.clear()
for l in path:
l.setCouleur("blue")
l.dessiner(self.fenetre)
self.fenetre.win.redraw()
#time.sleep(0.02)
ligne = path.pop() #get last line
collidePoint = self.enCollisionCarte(ligne, listePointInteret,
False)
if not collidePoint:
path.append(ligne)
#print countTest
t = time.time()
self.simplifierChemin(path, listePointInteret,
blockingElements)
#print "\tSimplifyPathTime: " + str(time.time() - t)
#print "Chemin Simplified len ", len(path)
return path
else:
collidedObjects = [collidePoint]
self.computeCollisonList(collidePoint, listePointInteret)
collidedObjects += collidePoint.collisionList
#find objects coliding current object
"""for obj in listePointInteret:
if obj == collidePoint:
continue
formeCollidePoint = collidePoint.zoneEvitement.forme if collidePoint.zoneEvitement else collidePoint.forme
formeObj = obj.zoneEvitement.forme if obj.zoneEvitement else obj.forme
if self.collider.collisionEntre(formeCollidePoint, formeObj):
collidedObjects.append(obj)"""
points = []
for collidedObj in collidedObjects:
#List collided object's points
objectPoints = []
forme = collidedObj.zoneEvitement.forme if collidedObj.zoneEvitement else collidedObj.forme
#print("colided with {} {}".format(forme.__class__.__name__, collidedObj.nom ))
if isinstance(forme, Ligne):
objectPoints.append(
self.extendLinePoint(forme, forme.x1, forme.y1))
objectPoints.append(
self.extendLinePoint(forme, forme.x2, forme.y2))
if isinstance(forme, Rectangle):
objectPoints.append(
self.extendLinePoint(forme, forme.x1, forme.y1))
objectPoints.append(
self.extendLinePoint(forme, forme.x2, forme.y1))
objectPoints.append(
self.extendLinePoint(forme, forme.x1, forme.y2))
objectPoints.append(
self.extendLinePoint(forme, forme.x2, forme.y2))
if isinstance(forme, Polygone):
for p in forme.pointList:
objectPoints.append(
self.extendLinePoint(forme, p["x"], p["y"]))
if isinstance(forme, Cercle):
nbpoint = 8
for i in numpy.arange(0, math.pi * 2,
math.pi * 2 / (nbpoint + 1)):
x = forme.x + forme.rayon * 1. * math.sin(i)
y = forme.y + forme.rayon * 1. * math.cos(i)
objectPoints.append(
self.extendLinePoint(forme, x, y))
"""c = Cercle("", x, y, 10, "white")
c.dessiner(self.fenetre)
self.fenetre.win.redraw()"""
#Exclude points colliding with direct line
for p in objectPoints:
#Exclude out of table
if p["x"] < 0 or p["x"] > self.largeur or p[
"y"] < 0 or p["y"] > self.longueur:
continue
#Exclude same point
if ligne.x1 == p["x"] and ligne.y1 == p["y"]:
continue
#Exclude point already in path
found = False
for l in path:
if (l.x1 == p["x"] and l.y1 == p["y"]) or (
l.x2 == p["x"] and l.y2 == p["y"]):
found = True
break
if found:
continue
#Exculde from collisions
directLine = Ligne("", ligne.x1, ligne.y1, p["x"],
p["y"])
if collisionEntre(directLine, forme):
continue
if self.enCollisionCarte(directLine, listePointInteret,
False):
continue
if self.fenetre:
c = Cercle("", p["x"], p["y"], 10, "black")
c.dessiner(self.fenetre)
self.fenetre.win.redraw()
points.append(p)
for p in points:
"""c = Cercle("", p["x"], p["y"], 10, "orange")
c.dessiner(self.fenetre)
self.fenetre.win.redraw()"""
newPath = list(path)
newPath.append(
Ligne("", ligne.x1, ligne.y1, p["x"], p["y"]))
newPath.append(
Ligne("", p["x"], p["y"], ligne.x2, ligne.y2))
pathList.append(newPath)
#Explore Paths with points
#print("points: {}".format(len(points)))
#Remove path ending at same point
pai = 0
while pai < len(pathList):
pa = pathList[pai]
#Look if other path reach same line
pa_length = 0
stop = False
lai = 0
while lai < len(pa) - 1:
la = pa[lai]
pa_length += la.getlongeur()
#look for same line in other path
pbi = 0
while pbi < len(pathList):
if pai == pbi:
pbi += 1
continue
pb = pathList[pbi]
pb_length = 0
lbi = 0
while lbi < len(pb) - 1:
lb = pb[lbi]
pb_length += lb.getlongeur()
if la.x1 == lb.x1 and la.y1 == lb.y1 and la.x2 == lb.x2 and la.y2 == lb.y2:
if pa_length < pb_length:
pbi -= 1
pathList.pop(pbi)
break
else:
pathList.pop(pai)
pai -= 1
stop = True
break
lbi += 1
if stop:
break
pbi += 1
lai += 1
if stop:
break
pai += 1
shortestPathMap = {}
conservedPath = []
"""for currentPath in pathList:
length = 0
endLine = None
for l in range(1, len(currentPath)):
if l == len(currentPath)-1:
endLine = currentPath[l]
else:
length += currentPath[l].getlongeur()
if not endLine:
conservedPath.append(currentPath)
continue
x = endLine.x1
y = endLine.y1
key = str(x)+","+str(y)
if (key in shortestPathMap and shortestPathMap[key]["length"] > length) or not key in shortestPathMap:
shortestPathMap[key] = {"length": length, "path": currentPath}
for key, noeud in shortestPathMap.iteritems():
conservedPath.append(noeud["path"])
pathList = conservedPath"""
return []
"""
def trouverChemin(self, x1, y1, x2, y2, _listePointInteret):
self.graph.nettoyer()
#Point contained in objects
startElements = self.pointContenuListe(x1, y1, _listePointInteret)
endElements = self.pointContenuListe(x2, y2, _listePointInteret)
blockingElements = startElements + list(
set(endElements) - set(startElements)) #merge removing duplicates
notBlockingElements = list(
set(_listePointInteret) - set(blockingElements))
directLine = Ligne("", x1, y1, x2, y2)
if not self.enCollisionCarte(directLine, notBlockingElements):
return [directLine]
elif self.fenetre:
directLine.setCouleur("violet")
directLine.dessiner(self.fenetre)
startNode = self.graph.trouverPointProche(x1, y1)
endNode = self.graph.trouverPointProche(x2, y2)
if startNode == None or endNode == None:
print "Start or end node not found"
return None
listnoeud = deque()
listnoeud.append(startNode)
self.graph.marquer(startNode)
while len(listnoeud) > 0:
currentNode = listnoeud.popleft()
if currentNode == endNode:
break
for noeud in self.graph.getVoisin(currentNode):
if not self.graph.estMaquer(noeud):
addNode = True
if len(noeud.colisionObject) != 0:
for elem in noeud.colisionObject:
if not elem in blockingElements:
self.graph.marquer(noeud)
addNode = False
break
if (addNode):
self.graph.setPere(noeud, currentNode)
self.graph.marquer(noeud)
listnoeud.append(noeud)
listPoint = []
lastNode = endNode
currentNode = self.graph.getPere(endNode)
if currentNode == None:
pass
elif self.graph.getPere(currentNode) == None:
#from endNode
listPoint.append([x2, y2])
#trip
listPoint.append([lastNode.x, lastNode.y])
listPoint.append([currentNode.x, currentNode.y])
#to startNode
listPoint.append([x1, y1])
else:
#from endNode
listPoint.append([x2, y2])
listPoint.append([lastNode.x, lastNode.y])
#trip
while self.graph.getPere(currentNode) != None:
listPoint.append([currentNode.x, currentNode.y])
currentNode = self.graph.getPere(currentNode)
# to startNode
listPoint.append([x1, y1])
tmpList = list(_listePointInteret)
for elem in blockingElements:
try:
tmpList.remove(elem)
except ValueError:
print "ChercheurChemin Err:" + elem.nom #shouldn't pop anymore
listPoint.reverse()
listChemin = []
for i in range(1, len(listPoint)):
p1 = listPoint[i - 1]
p2 = listPoint[i]
line = Ligne("", p1[0], p1[1], p2[0], p2[1])
listChemin.append(line)
print "Chemin len ", len(listChemin)
self.simplifierChemin(listChemin, tmpList)
print "Chemin Simplified len ", len(listChemin)
return listChemin
