Heurística

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <math.h>

#define ALTURA 5
#define LARGURA 5
#define MAX_SUCESSORES 8
#define VISUAL 1

typedef struct {char posicao[ALTURA][LARGURA];
} MAPA;


typedef struct {
	       int posX;
	       int posY;
} ESTADO;


typedef struct NOH {
	       ESTADO estado;
	       float g;
	       float h;
	       struct NOH *pAntecessor;
	       struct NOH *pSucessores[MAX_SUCESSORES];
	       struct NOH *pListaAntecessor;
	       struct NOH *pListaSucessor;
} NOH;


float calculaH(ESTADO aAtual, ESTADO aObjetivo) {
  return ((float) sqrt( (double)abs(aAtual.posX-aObjetivo.posX)+
                          (double)abs(aAtual.posY-aObjetivo.posY)));
}

float calculaG(int posAtual, int posProx) {
      if(posAtual==posProx) {
     //Continuo na mesma direcao
              return (float) 1;  //custo apenas do passo
      } else if(abs(posAtual - posProx) > (MAX_SUCESSORES/2)) {
      //Movimento maior que 180 graus, viro na outra direcao
              return (float) ((MAX_SUCESSORES - abs(posAtual - posProx)) //custo da virada
                             + (float) 1.0); //custo do passo
      } else {
      //Movimento menor que 180 graus
              return (float) (abs(posAtual-posProx) //custo da virada
                             + (float) 1.0); //custo do passo
      }
}

NOH *criarNOH() {
	NOH *pNOHCriado;
	int i;
	pNOHCriado=(NOH *) malloc(sizeof (NOH));
	if (pNOHCriado == 0)  {
	    printf("Falha ao alocar Mem¢ria!");
		exit(1);
	}
	for (i=0; i<MAX_SUCESSORES; i++) {
		pNOHCriado->pSucessores[i] = 0;
	}
	pNOHCriado->pListaAntecessor = 0;
	pNOHCriado->pListaSucessor = 0;
	return pNOHCriado;
}


NOH *naLista(ESTADO aEstado, NOH * aLista) {
	NOH *resultado;
	NOH *pTemp;
	resultado=0;
	pTemp=aLista;
	while ((pTemp!=0)&&(resultado==0)) {
		if ((pTemp->estado.posX==aEstado.posX)&&(pTemp->estado.posY==aEstado.posY)) {
			resultado=pTemp;
		}
		pTemp=pTemp->pListaSucessor;
	}
	return resultado;
}

void imprimeTela(MAPA *mp, ESTADO *pos) {
	int i,j;
	for(i=0;i<ALTURA;i++) {
		for(j=0;j<LARGURA;j++) {
			if(j==0)
				printf("|");

			if(pos->posX !=j || pos->posY !=i)
				printf("%c",
					((mp->posicao[j][i]=='0') ? 0xB0 : 0xB2)
				);
			else
				printf("%c", 0x01);

			if(j==LARGURA-1)
				printf("|");
		} //for j
		printf("
");
	} //for i
	printf("
");
	getch();
}

int main(void) {
	MAPA mapaDoProblema;
	ESTADO objetivo;
	NOH *pRaiz;
	NOH *pAbertos;
	NOH *pFechados;
	NOH *pTemp;
	int achou;
	NOH *jaExiste;
	NOH *pIncluindo;
	int incluindo;
        int posAtual=0;
	int k;
	int acompanha = VISUAL;

	/* monta o mapa do problema */
	int i, j;

	pTemp=0;

	for (i=0; i<ALTURA; i++) {
		for (j=0; j<LARGURA; j++)  {
			mapaDoProblema.posicao[j][i]='0';
		}
	}
	mapaDoProblema.posicao[2][0]='1';
	mapaDoProblema.posicao[1][1]='1';
	mapaDoProblema.posicao[2][1]='1';
	mapaDoProblema.posicao[3][1]='1';
	mapaDoProblema.posicao[2][2]='1';
	mapaDoProblema.posicao[1][3]='1';
	mapaDoProblema.posicao[2][3]='1';
	mapaDoProblema.posicao[4][3]='1';
	/* mapa do problema montado*/

	objetivo.posX=4;
	objetivo.posY=0;

	pRaiz=(NOH *) malloc(sizeof (NOH));
	if (pRaiz == 0) {
	   printf("Falha ao alocar Memória!");
	   exit(1);
	}

	pRaiz->estado.posX=0;
	pRaiz->estado.posY=0;
	pRaiz->g=0;
	pRaiz->h=calculaH(pRaiz->estado, objetivo);
	pRaiz->pAntecessor = 0;

	/*
	printf("--------------------------------- 
");
	printf("DADOS INICIAIS
");
	printf("X = "); fflush(stdin);
	scanf("%d
", &(pRaiz->estado.posX));
	printf("Y = "); fflush(stdin);
	scanf("%d
", &(pRaiz->estado.posY));
	printf("--------------------------------- 
");
	printf("--------------------------------- 
");
	printf("OBJETIVO
");
	printf("X = "); fflush(stdin);
	scanf("%d
", &(objetivo.posX));
	printf("Y = "); fflush(stdin);
	scanf("%d
", &(objetivo.posY));
	printf("--------------------------------- 
");
	*/

	for (i=0; i<MAX_SUCESSORES; i++) {
		pRaiz->pSucessores[i] = 0;
        }
	pRaiz->pListaAntecessor = 0;
	pRaiz->pListaSucessor = 0;

	pAbertos=pRaiz;
	pFechados=0;
	achou=0;

	if(acompanha)
		printf("

 Limpando 

");

	while ( (achou==0) && (pAbertos!=0) )  {
		//Tira o n¢ de abertos e coloca em fechados.
		pTemp=pAbertos;
		if(acompanha)
			printf("Analisando No %d %d 
", pTemp->estado.posX, pTemp->estado.posY);

		pAbertos=(NOH *) pTemp->pListaSucessor;
		if (pAbertos!=0) {
			pAbertos->pListaAntecessor=0;
		}
		if (pFechados!=0) {
			pFechados->pListaAntecessor=pTemp;
		}
		pTemp->pListaSucessor=pFechados;
		pFechados=pTemp;

		//Verifica se eh um estado meta
		if ((pTemp->estado.posX==objetivo.posX)&&(pTemp->estado.posY==objetivo.posY)) {
                        NOH *pFinal;
			achou=1;
                        //Retorna Resultado
			printf("Resultado: %d,%d

", pTemp->estado.posX, pTemp->estado.posY);
			pTemp = pFechados;
                        while(pTemp->pAntecessor!=0) {
                              printf("-> %d,%d 
", pTemp->estado.posX, pTemp->estado.posY);
                              imprimeTela(&mapaDoProblema, &(pTemp->estado));
                              pTemp = pTemp->pAntecessor;
                        }
			imprimeTela(&mapaDoProblema, &(pTemp->estado));
			
		} else {//Gerar os sucessores...

			//Gera sucessor Norte
			if ((pTemp->estado.posY>0)&&(mapaDoProblema.posicao[pTemp->estado.posX][pTemp->estado.posY-1]=='0')) {
				pTemp->pSucessores[0]= criarNOH();
				pTemp->pSucessores[0]->estado.posX=pTemp->estado.posX;
				pTemp->pSucessores[0]->estado.posY=pTemp->estado.posY-1;
				pTemp->pSucessores[0]->g=pTemp->g + calculaG(posAtual,0);
				pTemp->pSucessores[0]->h=calculaH(pTemp->pSucessores[0]->estado, objetivo);
				pTemp->pSucessores[0]->pAntecessor = pTemp;
				
				if(acompanha) {
					printf("Gerando Sucessor Norte %d %d - ", pTemp->estado.posX, pTemp->estado.posY-1);
					printf("g= %f, h= %f 
",pTemp->pSucessores[0]->g, pTemp->pSucessores[0]->h);
				}
			}
			
			//Gera sucessor Nordeste
			if ( (pTemp->estado.posY>0 && pTemp->estado.posX<4) &&
				 (mapaDoProblema.posicao[pTemp->estado.posX+1][pTemp->estado.posY-1]=='0')) {
				pTemp->pSucessores[1]= criarNOH();
				pTemp->pSucessores[1]->estado.posX=pTemp->estado.posX+1;
				pTemp->pSucessores[1]->estado.posY=pTemp->estado.posY-1;
				pTemp->pSucessores[1]->g=pTemp->g + calculaG(posAtual,1);
				pTemp->pSucessores[1]->h=calculaH(pTemp->pSucessores[1]->estado, objetivo);
				pTemp->pSucessores[1]->pAntecessor = pTemp;

				if(acompanha) {
					printf("Gerando Sucessor Nordeste %d %d - ", pTemp->estado.posX+1, pTemp->estado.posY-1);
					printf("g= %f, h= %f 
",pTemp->pSucessores[1]->g, pTemp->pSucessores[1]->h);
				}
			}

			//Gera sucessor Leste
			if ((pTemp->estado.posX<4)&&(mapaDoProblema.posicao[pTemp->estado.posX+1][pTemp->estado.posY]=='0')) {
				pTemp->pSucessores[2]= criarNOH();
				pTemp->pSucessores[2]->estado.posX=pTemp->estado.posX+1;
				pTemp->pSucessores[2]->estado.posY=pTemp->estado.posY;
				pTemp->pSucessores[2]->g=pTemp->g + calculaG(posAtual,2);
				pTemp->pSucessores[2]->h=calculaH(pTemp->pSucessores[2]->estado, objetivo);
				pTemp->pSucessores[2]->pAntecessor = pTemp;
				if(acompanha) {
					printf("Gerando Sucessor Leste %d %d - ", pTemp->estado.posX+1, pTemp->estado.posY);
					printf("g= %f, h= %f 
",pTemp->pSucessores[2]->g, pTemp->pSucessores[2]->h);
				}
			}

			//Gera sucessor Sudeste
			if ( (pTemp->estado.posY<4 && pTemp->estado.posX<4)&&
				 (mapaDoProblema.posicao[pTemp->estado.posX+1][pTemp->estado.posY+1]=='0')) {
				pTemp->pSucessores[3]= criarNOH();
				pTemp->pSucessores[3]->estado.posX=pTemp->estado.posX+1;
				pTemp->pSucessores[3]->estado.posY=pTemp->estado.posY+1;
				pTemp->pSucessores[3]->g=pTemp->g + calculaG(posAtual,3);
				pTemp->pSucessores[3]->h=calculaH(pTemp->pSucessores[3]->estado, objetivo);
				pTemp->pSucessores[3]->pAntecessor = pTemp;
				if(acompanha) {
					printf("Gerando Sucessor Sudeste %d %d - ", pTemp->estado.posX+1, pTemp->estado.posY+1);
					printf("g= %f, h= %f 
",pTemp->pSucessores[3]->g, pTemp->pSucessores[3]->h);
				}
			}

			//Gera sucessor Sul
			if ((pTemp->estado.posY<4)&&(mapaDoProblema.posicao[pTemp->estado.posX][pTemp->estado.posY+1]=='0')) {
				pTemp->pSucessores[4]= criarNOH();
				pTemp->pSucessores[4]->estado.posX=pTemp->estado.posX;
				pTemp->pSucessores[4]->estado.posY=pTemp->estado.posY+1;
				pTemp->pSucessores[4]->g=pTemp->g + calculaG(posAtual,4);
				pTemp->pSucessores[4]->h=calculaH(pTemp->pSucessores[4]->estado, objetivo);
				pTemp->pSucessores[4]->pAntecessor = pTemp;
				if(acompanha) {
					printf("Gerando Sucessor Sul %d %d - ", pTemp->estado.posX, pTemp->estado.posY+1);
					printf("g= %f, h= %f 
",pTemp->pSucessores[4]->g, pTemp->pSucessores[4]->h);
				}
			}

			//Gera sucessor Sudoeste
			if ( (pTemp->estado.posY<4 && pTemp->estado.posX>0)&&
				 (mapaDoProblema.posicao[pTemp->estado.posX-1][pTemp->estado.posY+1]=='0')) {
				pTemp->pSucessores[5]= criarNOH();
				pTemp->pSucessores[5]->estado.posX=pTemp->estado.posX-1;
				pTemp->pSucessores[5]->estado.posY=pTemp->estado.posY+1;
				pTemp->pSucessores[5]->g=pTemp->g + calculaG(posAtual,5);
				pTemp->pSucessores[5]->h=calculaH(pTemp->pSucessores[5]->estado, objetivo);
				pTemp->pSucessores[5]->pAntecessor = pTemp;
				if(acompanha) {
					printf("Gerando Sucessor Sudoeste %d %d - ", pTemp->estado.posX-1, pTemp->estado.posY+1);
					printf("g= %f, h= %f 
",pTemp->pSucessores[5]->g, pTemp->pSucessores[5]->h);
				}
			}
  
			//Gera sucessor Oeste
			if ((pTemp->estado.posX>0)&&(mapaDoProblema.posicao[pTemp->estado.posX-1][pTemp->estado.posY]=='0')) {
				pTemp->pSucessores[6]= criarNOH();
				pTemp->pSucessores[6]->estado.posX=pTemp->estado.posX-1;
				pTemp->pSucessores[6]->estado.posY=pTemp->estado.posY;
				pTemp->pSucessores[6]->g=pTemp->g + calculaG(posAtual,6);
				pTemp->pSucessores[6]->h=calculaH(pTemp->pSucessores[6]->estado, objetivo);
				pTemp->pSucessores[6]->pAntecessor = pTemp;
				if(acompanha) {
					printf("Gerando Sucessor Oeste %d %d - ", pTemp->estado.posX-1, pTemp->estado.posY);
					printf("g= %f, h= %f 
",pTemp->pSucessores[6]->g, pTemp->pSucessores[6]->h);
				}
			}

			//Gera sucessor Noroeste
			if ( (pTemp->estado.posY>0 && pTemp->estado.posX>4) &&
				 (mapaDoProblema.posicao[pTemp->estado.posX-1][pTemp->estado.posY-1]=='0')) {
				pTemp->pSucessores[7]= criarNOH();
				pTemp->pSucessores[7]->estado.posX=pTemp->estado.posX-1;
				pTemp->pSucessores[7]->estado.posY=pTemp->estado.posY-1;
				pTemp->pSucessores[7]->g=pTemp->g + calculaG(posAtual,7);
				pTemp->pSucessores[7]->h=calculaH(pTemp->pSucessores[7]->estado, objetivo);
				pTemp->pSucessores[7]->pAntecessor = pTemp;
				if(acompanha) {
					printf("Gerando Sucessor Noroeste %d %d - ", pTemp->estado.posX-1, pTemp->estado.posY-1);
					printf("g= %f, h= %f 
",pTemp->pSucessores[7]->g, pTemp->pSucessores[7]->h);
				}
			}
                        if(acompanha)
                                     getch();
                                     
			for (k=0; k<MAX_SUCESSORES; k++) {
				if (pTemp->pSucessores[k]!=0) {
					//printf("Analisando sucessor %d 
", k);
					jaExiste=naLista(pTemp->pSucessores[k]->estado, pAbertos);
					if (jaExiste!=0) {
						//Verifica se o novo caminho eh melhor e atualiza o no.
						free(pTemp->pSucessores[k]);
						pTemp->pSucessores[k]=jaExiste;
					} else {
						jaExiste=naLista(pTemp->pSucessores[k]->estado, pFechados);
						if (jaExiste!=0) {
							//Verifica se o novo caminho eh melhor e atualiza o noh
							free(pTemp->pSucessores[k]);
							pTemp->pSucessores[k]=jaExiste;
                                                        posAtual = k;
						} else {//inclui o novo no em Abertos
							if (pAbertos==0) {
								pAbertos=pTemp->pSucessores[k];
							} else {
								pIncluindo=pAbertos;
								incluindo=1;
								while (incluindo==1) {
									if (pIncluindo->g+pIncluindo->h < pTemp->pSucessores[k]->g+pTemp->pSucessores[k]->h) {
										if (pIncluindo->pListaSucessor!=0) {
											pIncluindo=pIncluindo->pListaSucessor;
										} else {
											pIncluindo->pListaSucessor=pTemp->pSucessores[k];
											incluindo=0;
                                                                                        posAtual = k;
										}
									} else {
										//inclui o noh antes de pIncluindo;
										pTemp->pSucessores[k]->pListaSucessor=pIncluindo;
										pTemp->pSucessores[k]->pListaAntecessor=pIncluindo->pListaAntecessor;
										pIncluindo->pListaAntecessor=pTemp->pSucessores[k];
										incluindo=0;
									}
								}
							}
						}
					}
				}
			}
		}
	}

	if (achou!=0)  {
		printf("
A solução foi encontrada!
");
		/* Mostrar resultado */
	} else  {
		printf("Fracasso, não foi poss¡vel encontrar uma solução!
");
	}

	while (pAbertos!=0) {
		pTemp=pAbertos;
		pAbertos=(NOH *) pTemp->pListaSucessor;
		free(pTemp);
	}

	while (pFechados!=0) {
		pTemp=pFechados;
		pFechados=(NOH *) pTemp->pListaSucessor;
		free(pTemp);
	}

        return 0;
}