dmonde/lib/AEtoile.py

#from __future__ import unicode_literals
# -*- coding: utf-8 -*-
"""algorithme de recherche du plus court chemin entre deux cases"""
from math import *


def distance(coord1, coord2):
    return sqrt((coord1[0] - coord2[0])**2 + (coord1[1] - coord2[1])**2)

def distanceCarree(coord1, coord2):
    """bcp plus rapide a calculer que la distance reelle"""
    return (coord1[0] - coord2[0])**2 + (coord1[1] - coord2[1])**2

def chemin(plateau, origine, cible):
    """retourne la liste des coord a traverser pour atteindre l'objectif
        se base sur la fonction estFranchissable() des cases"""

    nO = N(origine)
    nO.parent = None
    nO.coutG = 0
    nO.cout = 0

    filOuvert = []   #"liste ouverte": noeuds etudies et a etudier
    filFerme = [nO]    #"liste fermee":  noeuds retenus
    chemin = []
    position = nO
    echec = False
    it = 0
    #on continue jusqu'a tomber sur la case cible, ou jusqu'a un echec
    while position.coord != cible and not echec:
        it += 1
        print "##", it, position.coord
        
        #on etudie tous les voisins de la case en cours
        for coord in plateau.cases[position.coord].voisins:
            
            #on elimine les cases deja retenues (celles qui sont dans le fil ferme)
            trouve = False
            for nTest in filFerme:
                if nTest.coord == coord:
                    trouve = True
            if not trouve:
                
                #on elimine les cases infranchissables
                if plateau.cases[coord].estFranchissable():
                    noeud = N(coord)
                    
                    #on calcule le cout de la case
                    noeud.creer(position, cible)

                    #si le noeud est trouve dans la liste ouverte, on le compare a celui-ci
                    # si ce nouveau noeud a un meilleur cout, on remplace
                    # sinon on ajoute ce noeud a la liste ouverte
                    trouve = False
                    for nTest in filOuvert:
                        if nTest.coord == noeud.coord:
                            nTest.parent = noeud.parent
                            nTest.cout = noeud.cout
                            trouve = True
                            print " | teste", coord, "maj", noeud.coutG, noeud.coutH
                            
                    if not trouve:
                        filOuvert.append(noeud)
                        print " | teste", coord, "ajout", noeud.coutG, noeud.coutH

        #on parcourt les noeuds de la liste ouverte
        #et on cherche le meilleur noeud (le cout le plus faible)
        #si trouve: on l'ajoute a la liste fermee, on le retire de la liste ouverte et on continue
        #sinon, le chemin n'existe pas
        meilleur = None                
                        
        for noeud in filOuvert:
            if meilleur == None:
                meilleur = noeud
            else:
                if noeud.cout < meilleur.cout:
                    meilleur = noeud
        print " >> retenu", meilleur.coord, meilleur.cout
                
        if meilleur:
            filFerme.append(meilleur)
            filOuvert.remove(meilleur)
            position = meilleur
        else:
            echec = True

        for n in reversed(filFerme):    
            chemin.append(n.coord)
            
    return chemin                    
                    
class N():
    """noeud du chemin"""
    def __init__(self, coord):
        self.parent = None    #coord du noeud qui a amene a ce noeud-ci
        self.coord = coord
        self.coutG = 0
        self.coutH = 0
        self.cout = 0

##    def __repr__(self):
##        return "{} ({}+{}= {}) |".format(self.coord, self.coutG, self.coutH, self.cout)

    def creer(self, parent, cible):
        self.parent = parent
        self.coutH = distanceCarree(self.coord, cible)
        self.coutG = self.parent.coutG + 1
        self.cout =  self.coutG + self.coutH


### pour les tests
##
##class Plateau():
##    def __init__(self):
##        self.cases = {}
##
##class Case():
##    def __init__(self, coord):
##        self.coord = coord
##        self.voisins = []
##        self.majVoisins()
##        
##    def majVoisins(self):
##        for x in range(1, 6):
##            for y in range(1, 6):
##                if x in [self.coord[0] - 1, self.coord[0], self.coord[0] + 1] and \
##                   y in [self.coord[1] - 1, self.coord[1], self.coord[1] + 1] and \
##                   (x, y) != self.coord:
##                    self.voisins.append((x, y))
##
##    def estFranchissable(self):
##        return not (self.coord[0]==2 and self.coord[1] in [2, 3, 4, 5])
##
##
##plateau = Plateau()
##
##for x in range(1, 6):
##    for y in range(1, 6):
##        plateau.cases[(x, y)] = Case((x, y))
##
##
##depart = (1, 2)
##arrivee = (5, 5)
##        
##print chemin(plateau, depart, arrivee)
##