Działa konwersja nmt dla asc i xyz.
This commit is contained in:
336
src/main/java/pl/wat/ms4ds/terrain/AStar.java
Normal file
336
src/main/java/pl/wat/ms4ds/terrain/AStar.java
Normal file
@@ -0,0 +1,336 @@
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user