The minor memory consuming ode but semi-fast
package ia.astar;
import java.util.BitSet;
import java.util.HashMap;
public class AStarObj {
private static class Nodo {
float costeF;
float costeG;
int pos;
int idx;
public Nodo(int idx,float costeF, float costeG) {
this.idx=idx;
this.costeF=costeF;
this.costeG=costeG;
}
}
// Clase que gestiona la lista abierta de Nodos
private static class ListaAbierta {
int size = 0;
Nodo[] lista;
HashMap<Integer, Nodo> existe=new HashMap<Integer,Nodo>();
public ListaAbierta(int capacidadInicial) {
super();
size = 0;
lista = new Nodo[capacidadInicial];
}
public void push(int idx, float costeF, float costeG) {
size += 1;
Nodo nodo=new Nodo(idx,costeF,costeG);
existe.put(nodo.idx, nodo);
if(size>=lista.length) {
resize(size);
}
lista[size] = nodo;
nodo.pos=size;
fixup(size);
}
public Nodo pop() {
Nodo nodoAct=lista[1];
existe.remove(nodoAct.idx);
lista[1]=lista[size];
lista[1].pos=1;
lista[size]=null;
size--;
if (size > 1) {
fixdown(1);
}
return nodoAct;
}
public void fixup(int k) {
int j;
Nodo tmp;
while (k > 1) {
j = k >> 1;
if (lista[j].costeF <= lista[k].costeF) {
break;
}
tmp = lista[j];
lista[j] = lista[k];
lista[k] = tmp;
lista[j].pos = j;
lista[k].pos = k;
k = j;
}
}
public void fixdown(int k) {
int j;
Nodo tmp;
while (true) {
j = k << 1;
if (!((j <= size) && (j > 0))) {
break;
}
if ((j < size) && (lista[j].costeF > lista[j + 1].costeF)) {
j += 1;
}
if (lista[k].costeF <= lista[j].costeF) {
break;
}
tmp = lista[j];
lista[j] = lista[k];
lista[k] = tmp;
lista[j].pos=j;
lista[k].pos=k;
k = j;
}
}
public boolean existe(int idx) {
return (existe.containsKey(idx));
}
public Nodo getNodo(int idx) {
return existe.get(idx);
}
public void reordena(Nodo nodo) {
fixup(nodo.pos);
}
public boolean isVacia() {
return size == 0;
}
void resize(int indice) {
int newSize=indice*3/2+1;
Nodo[] tmp=new Nodo[newSize];
System.arraycopy(lista, 0, tmp, 0, lista.length);
lista=tmp;
}
}
public AStarObj() {
super();
}
/**
* Permite obtener uno de los caminos mas cortos entre una posicion de origen y una de destino.
* El mapeado puede ser de cualquier numero de dimensiones y distintos costes segun distintas direcciones
* El parametro mapa es el que define las caracteristicas del mapeado
*
* @param origen int Indice de la posicion origen
* @param destino int Indice de la posicion destino
* @param mapa AStarMapa Gestor del mapeado.
* @return int[] Indices de los nodos atravesados desde origen hasta destino, ambos incluidos
*/
public static int[] getCaminoIndexado(int origen, int destino, AStarMapa mapa) {
ListaAbierta abierta = null;
BitSet cerrada=new BitSet();
float costeF = 0; // Coste acumulado desde el origen + coste heuristico hasta el destino
float costeG = 0; // Coste acumulado desde el origen
HashMap<Integer, Integer> padre=new HashMap<Integer,Integer>();
Nodo nodoAct = null;
boolean encontrado = false;
// Si el nodo origen o destino no son transitables es imposible encontrar un camino
if (!(mapa.isTransitable(origen) && mapa.isTransitable(destino))) {
return null;
}
// Inicializa
abierta = new ListaAbierta(200);
// Coloca el nodo origen que iniciara el proceso
costeG = mapa.getCosteFijo(origen);
costeF = costeG + mapa.getCosteHeuristico(origen, destino);
abierta.push(origen, costeF, costeG);
while (!abierta.isVacia()) {
nodoAct = abierta.pop();
cerrada.set(nodoAct.idx);
if (nodoAct.idx == destino) { // Se ha encontrado la solucion
encontrado = true;
break;
}
int[] sucesores = mapa.getSucesores(nodoAct.idx); // Obtiene sucesores
for (int suc : sucesores) {
if (cerrada.get(suc)) {
continue; // Si ya esta explorado lo ignora
}
if (!mapa.isTransitable(suc)) {
continue; // Si no es transitable lo ignora
}
float newG = nodoAct.costeG + mapa.getCosteFijo(suc) + mapa.getCosteSucesor(nodoAct.idx, suc);
Nodo nodSuc=abierta.getNodo(suc);
if (nodSuc!=null) { // Si ya esta en abierta
if (newG < nodSuc.costeG) {
padre.put(suc, nodoAct.idx);
// actualiza costeF
nodSuc.costeF -= nodSuc.costeG;
nodSuc.costeF += newG;
nodSuc.costeG = newG;
abierta.reordena(nodSuc);
}
} else {
abierta.push(suc, newG + mapa.getCosteHeuristico(suc, destino), newG);
padre.put(suc,nodoAct.idx);
}
}
}
if (!encontrado) {
return null;
}
int cta = 0;
int nn = nodoAct.idx;
// Cuenta los nodos
do {
cta++;
nn = padre.get(nn);
} while (nn != origen);
// Carga el array con los nodos que forman el camino
cta++;
int[] retorno = new int[cta];
nn = nodoAct.idx;
do {
cta--;
retorno[cta] = nn;
nn = padre.get(nn);
} while (nn != origen);
retorno[0] = origen;
return retorno;
}
}
)