337 lines
14 KiB
Java
337 lines
14 KiB
Java
package pl.wat.ms4ds.terrain;
|
|
|
|
import pl.wat.ms4ds.common.EGeoDirection;
|
|
import pl.wat.ms4ds.common.ERodzajDzialania;
|
|
import pl.wat.ms4ds.common.ERodzajPodwozia;
|
|
|
|
import java.util.*;
|
|
|
|
public final class AStar {
|
|
|
|
private static class Node {
|
|
|
|
int x;
|
|
int y;
|
|
boolean koncowy;
|
|
boolean zamkniety;
|
|
double kosztOdStartu;
|
|
double kosztZHeurystyka;
|
|
Node poprzednik;
|
|
EGeoDirection zKierunku;
|
|
|
|
private static Hashtable<Node, Node> allNodes = new Hashtable<>();
|
|
private static Node roboczy = new Node(0, 0);
|
|
|
|
static Node dajAStarNode(int x, int y) {
|
|
if (x < 0 || y < 0) {
|
|
return null;
|
|
}
|
|
roboczy.x = x;
|
|
roboczy.y = y;
|
|
Node node = allNodes.get(roboczy);
|
|
if (node == null) {
|
|
node = new Node(x, y);
|
|
allNodes.put(node, node);
|
|
}
|
|
return node;
|
|
}
|
|
|
|
ArrayList<Node> dajNiezamknietychSasiadow() {
|
|
ArrayList<Node> wynik = new ArrayList<>();
|
|
Node sasiad;
|
|
sasiad = Node.dajAStarNode(this.x - 1, this.y - 1);
|
|
if (null != sasiad && !sasiad.zamkniety)
|
|
wynik.add(sasiad);
|
|
sasiad = Node.dajAStarNode(this.x - 1, this.y);
|
|
if (null != sasiad && !sasiad.zamkniety)
|
|
wynik.add(sasiad);
|
|
sasiad = Node.dajAStarNode(this.x - 1, this.y + 1);
|
|
if (null != sasiad && !sasiad.zamkniety)
|
|
wynik.add(sasiad);
|
|
sasiad = Node.dajAStarNode(this.x, this.y - 1);
|
|
if (null != sasiad && !sasiad.zamkniety)
|
|
wynik.add(sasiad);
|
|
sasiad = Node.dajAStarNode(this.x, this.y + 1);
|
|
if (null != sasiad && !sasiad.zamkniety)
|
|
wynik.add(sasiad);
|
|
sasiad = Node.dajAStarNode(this.x + 1, this.y - 1);
|
|
if (null != sasiad && !sasiad.zamkniety)
|
|
wynik.add(sasiad);
|
|
sasiad = Node.dajAStarNode(this.x + 1, this.y);
|
|
if (null != sasiad && !sasiad.zamkniety)
|
|
wynik.add(sasiad);
|
|
sasiad = Node.dajAStarNode(this.x + 1, this.y + 1);
|
|
if (null != sasiad && !sasiad.zamkniety)
|
|
wynik.add(sasiad);
|
|
return wynik;
|
|
}
|
|
|
|
static void reset() {
|
|
allNodes.clear();
|
|
}
|
|
|
|
Node(int x, int y) {
|
|
this.x = x;
|
|
this.y = y;
|
|
koncowy = false;
|
|
zamkniety = false;
|
|
kosztOdStartu = 0.0;
|
|
kosztZHeurystyka = Double.MAX_VALUE;
|
|
poprzednik = null;
|
|
zKierunku = EGeoDirection.UNDEFINED;
|
|
}
|
|
|
|
Node(Coord.Grid id) {
|
|
this(id.x, id.y);
|
|
}
|
|
|
|
@Override
|
|
public String toString() {
|
|
return "AStarNode{" +
|
|
"x=" + x +
|
|
", y=" + y +
|
|
", koncowy=" + koncowy +
|
|
'}';
|
|
}
|
|
|
|
@Override
|
|
public int hashCode() {
|
|
final int prime = 31;
|
|
int result = prime + x;
|
|
result = prime * result + y;
|
|
return result;
|
|
}
|
|
|
|
@Override
|
|
public boolean equals(Object obj) {
|
|
if (this == obj) {
|
|
return true;
|
|
}
|
|
if (obj == null) {
|
|
return false;
|
|
}
|
|
if (obj instanceof Node other) {
|
|
if (x != other.x) {
|
|
return false;
|
|
}
|
|
return y == other.y;
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Funkcja wyznacza drogę po sąsiednich kwadratach przechodzącą przez punkty profilujące punktyProfilujace
|
|
*
|
|
* @param punktyProfilujace punkty profilujące drogę (droga musi je zawierać)
|
|
* @param staryKierunek kierunek z którego osiągnięto punkt startowy (1. punkt profilujący)
|
|
* @param podwozie rodzaj podwozia
|
|
* @param rodzajDzialania rodzaj działania, w ramach którego określana jest droga
|
|
* @return uporządkowana kolekcja współrzędnych kolejnych kwadratów drogi zawierająca kwadrat startowy i docelowy lub kolekcja pusta, gdy nie istnieje droga
|
|
*/
|
|
public static ArrayList<Coord.Grid> wyznaczDroge(Coord.Grid[] punktyProfilujace, EGeoDirection staryKierunek,
|
|
ERodzajPodwozia podwozie, ERodzajDzialania rodzajDzialania) {
|
|
if (null == punktyProfilujace || 0 == punktyProfilujace.length) {
|
|
return null;
|
|
}
|
|
Node.reset();
|
|
ArrayList<Coord.Grid> wynik = new ArrayList<>();
|
|
Coord.Grid start;
|
|
Coord.Grid stop = punktyProfilujace[0];
|
|
ArrayList<Coord.Grid> odcinek;
|
|
for (int i = 1; i < punktyProfilujace.length; i++) {
|
|
start = stop;
|
|
stop = punktyProfilujace[i];
|
|
EGeoDirection staryKier;
|
|
if (wynik.size() > 1) {
|
|
staryKier = GeomUtils.kierunekDlaSasiada(wynik.get(wynik.size() - 2), wynik.get(wynik.size() - 1));
|
|
} else {
|
|
staryKier = staryKierunek;
|
|
}
|
|
if (i == 1) {
|
|
odcinek = wyznaczDroge(start, stop, staryKier, true, podwozie, rodzajDzialania);
|
|
} else {
|
|
odcinek = wyznaczDroge(start, stop, staryKier, false, podwozie, rodzajDzialania);
|
|
}
|
|
if (odcinek.size() == 0) {
|
|
// gdy nie istnieje droga między danymi punktami profilującymi, to zwracam kolekcję pustą
|
|
return odcinek;
|
|
}
|
|
wynik.addAll(odcinek);
|
|
Node.reset();
|
|
}
|
|
return wynik;
|
|
}
|
|
|
|
/**
|
|
* Funkcja wyznacza drogę po sąsiednich kwadratach od kwadratu startowego start do docelowego stop
|
|
*
|
|
* @param start współrzędne kwadratu startowego
|
|
* @param stop współrzędne kwadratu docelowego
|
|
* @param staryKierunek kierunek z którego osiągnięto punkt startowy (1. punkt profilujący)
|
|
* @param podwozie rodzaj podwozia
|
|
* @param rodzajDzialania rodzaj działania, w ramach którego określana jest droga
|
|
* @param zawieraStartowy parametr wskazujący, czy wyznaczona droga ma zawierać kwadrat startowy
|
|
* @return uporządkowana kolekcja współrzędnych kolejnych kwadratów drogi zawierająca kwadrat startowy (jeśli zawieraStartowy==true) i docelowy lub kolekcja pusta, gdy nie istnieje droga
|
|
*/
|
|
public static ArrayList<Coord.Grid> wyznaczDroge(Coord.Grid start, Coord.Grid stop, EGeoDirection staryKierunek,
|
|
boolean zawieraStartowy, ERodzajPodwozia podwozie, ERodzajDzialania rodzajDzialania) {
|
|
|
|
PriorityQueue<Node> listaOtwarta = new PriorityQueue<>(100, (o1, o2) -> Double.compare(o1.kosztZHeurystyka, o2.kosztZHeurystyka));
|
|
boolean naPrzelaj = false;
|
|
switch (rodzajDzialania) {
|
|
case DZIALANIE_ATAK:
|
|
case DZIALANIE_MAGIC_MOVEMENT:
|
|
case DZIALANIE_OBRONA:
|
|
case DZIALANIE_OPL:
|
|
case DZIALANIE_WRIA:
|
|
case DZIALANIE_ROZMIESZCZENIE:
|
|
case DZIALANIE_ZESRODKOWANIE:
|
|
case DZIALANIE_MINOWANIE:
|
|
naPrzelaj = true;
|
|
break;
|
|
default:
|
|
break;
|
|
|
|
}
|
|
Node aktualny = Node.dajAStarNode(stop.x, stop.y);
|
|
aktualny.koncowy = true;
|
|
ArrayList<Node> sasiedzi;
|
|
aktualny = Node.dajAStarNode(start.x, start.y);
|
|
aktualny.zKierunku = staryKierunek;
|
|
listaOtwarta.add(aktualny);
|
|
ArrayList<Coord.Grid> wynik = new ArrayList<Coord.Grid>();
|
|
int licznik_zabezpieczajacy = 200000;
|
|
while (listaOtwarta.size() > 0 && licznik_zabezpieczajacy-- > 0) {
|
|
aktualny = listaOtwarta.remove();
|
|
if (aktualny.koncowy) {
|
|
while (null != aktualny) {
|
|
wynik.add(new Coord.Grid(aktualny.x, aktualny.y));
|
|
aktualny = aktualny.poprzednik;
|
|
}
|
|
if (!zawieraStartowy) {
|
|
//Usuwam poczatek drogi
|
|
wynik.remove(wynik.size() - 1);
|
|
}
|
|
Collections.reverse(wynik);
|
|
return wynik;
|
|
}
|
|
sasiedzi = aktualny.dajNiezamknietychSasiadow();
|
|
for (Node sasiad : sasiedzi) {
|
|
// double stopienPrzejezdnosci = Teren.getStopienPrzejezdnosci(aktualny.x, aktualny.y, sasiad.x, sasiad.y,
|
|
// aktualny.zKierunku, podwozie);
|
|
double stopienPrzejezdnosci = 1;
|
|
if (stopienPrzejezdnosci < 0.005f) {
|
|
continue;
|
|
}
|
|
if (naPrzelaj) {
|
|
stopienPrzejezdnosci = Math.max(Teren.minStopienPrzejezdNaPrzelaj, stopienPrzejezdnosci);
|
|
}
|
|
double nowyKosztOdStartu = aktualny.kosztOdStartu + Coord.Grid.distance(aktualny.x, aktualny.y, sasiad.x, sasiad.y) / stopienPrzejezdnosci;
|
|
double nowyKosztZHeurystyka = nowyKosztOdStartu + Coord.Grid.distance(sasiad.x, sasiad.y, stop.x, stop.y);
|
|
if (sasiad.kosztZHeurystyka > nowyKosztZHeurystyka) {
|
|
//UPDATE kosztow i zmiany w kolejce
|
|
sasiad.kosztOdStartu = nowyKosztOdStartu;
|
|
sasiad.kosztZHeurystyka = nowyKosztZHeurystyka;
|
|
sasiad.poprzednik = aktualny;
|
|
sasiad.zKierunku = GeomUtils.kierunekDlaSasiada(aktualny.x, aktualny.y, sasiad.x, sasiad.y);
|
|
listaOtwarta.remove(sasiad);
|
|
listaOtwarta.add(sasiad);
|
|
}
|
|
}
|
|
aktualny.zamkniety = true;
|
|
}
|
|
return wynik;
|
|
}
|
|
|
|
/**
|
|
* Funkcja wyznacza drogę po sąsiednich kwadratach od kwadratu startowego start do docelowego stop
|
|
*
|
|
* @param start współrzędne kwadratu startowego
|
|
* @param stop współrzędne kwadratu docelowego
|
|
* @param staryKierunek kierunek z którego osiągnięto punkt startowy (1. punkt profilujący)
|
|
* @param podwozie rodzaj podwozia
|
|
* @param rodzajDzialania rodzaj działania, w ramach którego określana jest droga
|
|
* @param zawieraStartowy parametr wskazujący, czy wyznaczona droga ma zawierać kwadrat startowy
|
|
* @return uporządkowana kolekcja współrzędnych kolejnych kwadratów drogi zawierająca kwadrat startowy (jeśli zawieraStartowy==true) i docelowy lub kolekcja pusta, gdy nie istnieje droga
|
|
*/
|
|
public static ArrayList<Coord.Grid> wyznaczDrogeNew(Coord.Grid start, Coord.Grid stop, EGeoDirection staryKierunek,
|
|
boolean zawieraStartowy, ERodzajPodwozia podwozie,
|
|
ERodzajDzialania rodzajDzialania,
|
|
double szerokoscPokonywRowow,
|
|
double glebokoscBrodzenia,
|
|
double predkoscPlywania) {
|
|
PriorityQueue<Node> listaOtwarta = new PriorityQueue<>(100, new Comparator<Node>() {
|
|
public int compare(Node o1, Node o2) {
|
|
return Double.compare(o1.kosztZHeurystyka, o2.kosztZHeurystyka);
|
|
}
|
|
});
|
|
// PriorityQueue<AStarNode> listaOtwarta = new PriorityQueue<>(100, (AStarNode o1, AStarNode o2) -> Double.compare(o1.kosztZHeurystyka, o2.kosztZHeurystyka));
|
|
boolean naPrzelaj = false;
|
|
switch (rodzajDzialania) {
|
|
case DZIALANIE_ATAK:
|
|
case DZIALANIE_MAGIC_MOVEMENT:
|
|
case DZIALANIE_OBRONA:
|
|
case DZIALANIE_OPL:
|
|
case DZIALANIE_WRIA:
|
|
case DZIALANIE_ROZMIESZCZENIE:
|
|
case DZIALANIE_ZESRODKOWANIE:
|
|
case DZIALANIE_MINOWANIE:
|
|
naPrzelaj = true;
|
|
break;
|
|
default:
|
|
break;
|
|
|
|
}
|
|
Node aktualny = Node.dajAStarNode(stop.x, stop.y);
|
|
aktualny.koncowy = true;
|
|
ArrayList<Node> sasiedzi;
|
|
aktualny = Node.dajAStarNode(start.x, start.y);
|
|
aktualny.zKierunku = staryKierunek;
|
|
listaOtwarta.add(aktualny);
|
|
ArrayList<Coord.Grid> wynik = new ArrayList<Coord.Grid>();
|
|
int licznik_zabezpieczajacy = 200000;
|
|
while (listaOtwarta.size() > 0 && licznik_zabezpieczajacy-- > 0) {
|
|
aktualny = listaOtwarta.remove();
|
|
if (aktualny.koncowy) {
|
|
while (null != aktualny) {
|
|
wynik.add(new Coord.Grid(aktualny.x, aktualny.y));
|
|
aktualny = aktualny.poprzednik;
|
|
}
|
|
if (!zawieraStartowy) {
|
|
//Usuwam poczatek drogi
|
|
wynik.remove(wynik.size() - 1);
|
|
}
|
|
Collections.reverse(wynik);
|
|
return wynik;
|
|
}
|
|
sasiedzi = aktualny.dajNiezamknietychSasiadow();
|
|
for (Node sasiad : sasiedzi) {
|
|
// double stopienPrzejezdnosci = Teren.getStopienPrzejezdnosciNew(aktualny.x, aktualny.y, sasiad.x, sasiad.y,
|
|
// aktualny.zKierunku, podwozie, szerokoscPokonywRowow, glebokoscBrodzenia, predkoscPlywania);
|
|
double stopienPrzejezdnosci = 1;
|
|
if (stopienPrzejezdnosci < 0.005f) {
|
|
continue;
|
|
}
|
|
if (naPrzelaj) {
|
|
stopienPrzejezdnosci = Math.max(Teren.minStopienPrzejezdNaPrzelaj, stopienPrzejezdnosci);
|
|
}
|
|
double nowyKosztOdStartu = aktualny.kosztOdStartu + Coord.Grid.distance(aktualny.x, aktualny.y, sasiad.x, sasiad.y) / stopienPrzejezdnosci;
|
|
double nowyKosztZHeurystyka = nowyKosztOdStartu + Coord.Grid.distance(sasiad.x, sasiad.y, stop.x, stop.y);
|
|
if (sasiad.kosztZHeurystyka > nowyKosztZHeurystyka) {
|
|
//UPDATE kosztow i zmiany w kolejce
|
|
sasiad.kosztOdStartu = nowyKosztOdStartu;
|
|
sasiad.kosztZHeurystyka = nowyKosztZHeurystyka;
|
|
sasiad.poprzednik = aktualny;
|
|
sasiad.zKierunku = GeomUtils.kierunekDlaSasiada(aktualny.x, aktualny.y, sasiad.x, sasiad.y);
|
|
listaOtwarta.remove(sasiad);
|
|
listaOtwarta.add(sasiad);
|
|
}
|
|
}
|
|
aktualny.zamkniety = true;
|
|
}
|
|
return wynik;
|
|
}
|
|
|
|
}
|