protected ArrayList<Nodo> actualizarAbiertos(
ArrayList<Nodo> lAbiertos, ArrayList<Nodo> lCerrados, ArrayList<Nodo> lvecinos, Nodo nFinal) {
Nodo nVecino = null;
// ArrayList<Nodo> auxLAbiertos=lAbiertos;
boolean estaEnAbiertos = false;
boolean estaEnCerrados = false;
for (int i = 0; i < lvecinos.size(); i++) {
estaEnAbiertos = false;
estaEnCerrados = false;
nVecino = lvecinos.get(i);
for (int k = 0; k < lCerrados.size(); k++) {
if (nVecino.comparaEsNodo(lCerrados.get(k))) {
estaEnCerrados = true;
}
}
if (!estaEnCerrados) {
for (int j = 0; j < lAbiertos.size(); j++) {
if (nVecino.comparaEsNodo(lAbiertos.get(j))) {
estaEnAbiertos = true;
if (nVecino.getCosteF() < lAbiertos.get(j).getCosteF()) {
lAbiertos.remove(j);
lAbiertos.add(nVecino);
}
}
}
if (!estaEnAbiertos) lAbiertos.add(nVecino);
}
}
return lAbiertos;
}
/**
* Calculates a new destiny position to walk.
*
* <p>This method is called before the agent creates a <tt> TASK_GOTO_POSITION</tt> task. It will
* try (for 5 attempts) to generate a valid random point in a radius of 20 units. If it doesn't
* generate a valid position in this cycle, it will try it in next cycle. Once a position is
* calculated, agent updates its destination to the new position, and automatically calculates the
* new direction.
*
* <p><em> It's very useful to overload this method. </em>
*
* @return <tt> TRUE</tt>: valid position generated / <tt> FALSE</tt> cannot generate a valid
* position
*/
protected boolean GeneratePath() {
bHeVuelto = false;
m_iAStarPathIndex = 0; // inicilaizamos a 0
System.out.println(
"La posicion inicial por getposition es "
+ m_Movement.getPosition().x
+ "y z es "
+ m_Movement.getPosition().z);
Vector3D vNewDestination =
new Vector3D(m_Movement.getPosition().x, 0.0, m_Movement.getPosition().z);
ArrayList<Vector3D> lv3D =
new ArrayList<
Vector3D>(); // para ir metiendo los puntos que luego le pasaré en orden descendente a
// m_AStarPath
ArrayList<Nodo> lAbiertos = new ArrayList<Nodo>();
ArrayList<Nodo> lCerrados = new ArrayList<Nodo>();
ArrayList<Nodo> lvecinos = new ArrayList<Nodo>();
int xInit = (int) Math.floor(m_Movement.getPosition().x);
xInit /= 8;
int zInit = (int) Math.floor(m_Movement.getPosition().z);
zInit /= 8;
int xFinal = (int) Math.floor(m_Movement.getDestination().x);
xFinal /= 8;
int zFinal = (int) Math.floor(m_Movement.getDestination().z);
zFinal /= 8;
Nodo nodoInit = new Nodo(xInit, zInit, null);
Nodo nodoFinal = new Nodo(xFinal, zFinal, null);
Nodo nodoActual = null;
lAbiertos.add(nodoInit); // añado el nodo inicial a la lista de abiertos
boolean esNodoFinal = false;
while (!esNodoFinal) // (!lAbiertos.isEmpty()){
{
nodoActual =
this.obtenerNodoMejorCoste(
lAbiertos, nodoFinal); // obtener el nodo abierto de menor coste
for (int i = 0; i < lAbiertos.size(); i++) {
if (nodoActual.comparaEsNodo(lAbiertos.get(i))) {
lAbiertos.remove(i);
}
}
// System.out.println("2 Labiertos tiene"+lAbiertos.size());
// lAbiertos.remove(nodoActual);
lCerrados.add(nodoActual);
if (nodoActual.comparaEsNodo(nodoFinal)) {
esNodoFinal = true;
break;
}
lvecinos.clear();
lvecinos = this.calcularVecinos(lvecinos, nodoActual);
lAbiertos = this.actualizarAbiertos(lAbiertos, lCerrados, lvecinos, nodoFinal);
// System.out.println("Ha entrado en el bucle");
// System.out.println("Nodo Actual x:"+nodoActual.getPosX()+" y posicion z:
// "+nodoActual.getPosZ());
}
System.out.println("Ha salido");
boolean esInicial = false;
for (int h = 0; h < lCerrados.size(); h++) {
if (nodoFinal.comparaEsNodo(lCerrados.get(h))) {
nodoFinal = lCerrados.get(h);
}
}
Nodo nCurrent = nodoFinal;
Nodo nPadre = null;
while (!esInicial) // meto las posiciones de los nodos en un array de Vector3D
{
if (nCurrent.getPadre() == null) esInicial = true;
lv3D.add(new Vector3D(nCurrent.getPosX() * 8, 0, nCurrent.getPosZ() * 8));
nCurrent = nCurrent.getPadre();
}
Vector3D[] v3D = new Vector3D[lv3D.size()];
Vector3D[] v3DReturn = new Vector3D[1000];
for (int j = lv3D.size() - 1; j >= 0; j--) {
// for(int h=0;h<lv3D.size();h++)
// {
v3D[(lv3D.size() - 1) - j] = new Vector3D(lv3D.get(j).x, 0.0, lv3D.get(j).z);
// v3DReturn[j]=new Vector3D(lv3D.get(j).x,0.0,lv3D.get(j).z);
// }
}
// System.out.println(m_Movement.getPosition().x);
// System.out.println(m_Movement.getPosition().z);
double xInicial = lv3D.get(0).x;
double zInicial = lv3D.get(0).z;
double xINI = v3D[0].x;
double zINI = v3D[0].z;
// this.setvVolverPath(v3DReturn);//Me guardo el vector para volver
m_AStarPath = v3D;
if (CheckStaticPosition(m_AStarPath[0].x, m_AStarPath[0].z) == true) {
String posInit = " ( " + m_AStarPath[0].x + " , 0.0 , " + m_AStarPath[0].z + " ) ";
AddTask(CTask.TASK_WALKING_PATH, getAID(), posInit, 10000);
m_Movement.setDestination(vNewDestination);
System.out.println(
"la posicion inicial es " + vNewDestination.x + " y la z " + vNewDestination.z);
System.out.println(
"El destino actual es "
+ m_Movement.getDestination().x
+ " y la z "
+ m_Movement.getDestination().z);
return true;
}
return false;
}
