List of usage examples for org.apache.commons.collections.buffer PriorityBuffer PriorityBuffer
public PriorityBuffer()
From source file:de._13ducks.cor.game.server.movement.SectorPathfinder.java
public static synchronized List<Node> findPath(SimplePosition start, SimplePosition target, FreePolygon startSector, MovementMap moveMap) { if (start == null || target == null) { System.out.println("FixMe: SPathfinder, irregular call: " + start + "-->" + target); return null; }//from w ww.java 2s .c om FreePolygon targetSector = moveMap.containingPoly(target.x(), target.y()); if (targetSector == null) { // Ziel ungltig abbrechen System.out.println("Irregular target. Aborting"); return null; } FakeNode startNode = new FakeNode(start.x(), start.y(), startSector); Node targetNode = new FakeNode(target.x(), target.y(), targetSector); targetNode.addPolygon(targetSector); // Der Startknoten muss die Member seines Polys kennen startNode.setReachableNodes(computeDirectReachable(startNode, startSector)); // Der Zielknoten muss den Membern seines Polys bekannt sein // Die Movement-Map darf aber nicht verndert werden. Des halb mssen einige Aufrufe intern abgefangen werden und das reingedoktert werden. List<Node> preTargetNodes = Arrays.asList(computeDirectReachable(targetNode, targetSector)); PriorityBuffer open = new PriorityBuffer(); // Liste fr entdeckte Knoten LinkedHashSet<Node> containopen = new LinkedHashSet<Node>(); // Auch fr entdeckte Knoten, hiermit kann viel schneller festgestellt werden, ob ein bestimmter Knoten schon enthalten ist. LinkedHashSet<Node> closed = new LinkedHashSet<Node>(); // Liste fr fertig bearbeitete Knoten double cost_t = 0; //Movement Kosten (gerade 5, diagonal 7, wird spter festgelegt) startNode.setCost(0); //Kosten fr das Startfeld (von dem aus berechnet wird) sind natrlich 0 open.add(startNode); //Startfeld in die openlist containopen.add(startNode); targetNode.setParent(null); //"Vorgngerfeld" vom Zielfeld noch nicht bekannt for (int j = 0; j < 40000; j++) { //Anzahl der maximalen Durchlufe, bis Wegfindung aufgibt if (open.isEmpty()) { //Abbruch, wenn openlist leer ist => es gibt keinen Weg return null; } // Sortieren nicht mehr ntig, PriorityBuffer bewahrt die Felder in der Reihenfolge ihrer Priority - also dem F-Wert auf Node current = (Node) open.remove(); //der Eintrag aus der openlist mit dem niedrigesten F-Wert rausholen und gleich lschen containopen.remove(current); if (current.equals(targetNode)) { //Abbruch, weil Weg von Start nach Ziel gefunden wurde targetNode.setParent(current.getParent()); //"Vorgngerfeld" von Ziel bekannt break; } // Aus der open wurde current bereits gelscht, jetzt in die closed verschieben closed.add(current); List<Node> neighbors = computeNeighbors(current, targetNode, preTargetNodes); for (Node node : neighbors) { if (closed.contains(node)) { continue; } // Kosten dort hin berechnen cost_t = current.movementCostTo(node); if (containopen.contains(node)) { //Wenn sich der Knoten in der openlist befindet, muss berechnet werden, ob es einen krzeren Weg gibt if (current.getCost() + cost_t < node.getCost()) { //krzerer Weg gefunden? node.setCost(current.getCost() + cost_t); //-> Wegkosten neu berechnen node.setValF(node.getCost() + node.getHeuristic()); //F-Wert, besteht aus Wegkosten vom Start + Luftlinie zum Ziel node.setParent(current); //aktuelles Feld wird zum Vorgngerfeld } } else { node.setCost(current.getCost() + cost_t); node.setHeuristic(Math.sqrt(Math.pow(Math.abs((targetNode.getX() - node.getX())), 2) + Math.pow(Math.abs((targetNode.getY() - node.getY())), 2))); // geschtzte Distanz zum Ziel //Die Zahl am Ende der Berechnung ist der Aufwand der Wegsuche //5 ist schnell, 4 normal, 3 dauert lange node.setParent(current); // Parent ist die RogPosition, von dem der aktuelle entdeckt wurde node.setValF(node.getCost() + node.getHeuristic()); //F-Wert, besteht aus Wegkosten vom Start aus + Luftlinie zum Ziel open.add(node); // in openlist hinzufgen containopen.add(node); } } } if (targetNode.getParent() == null) { //kein Weg gefunden return null; } ArrayList<Node> pathrev = new ArrayList<Node>(); //Pfad aus parents erstellen, von Ziel nach Start while (!targetNode.equals(startNode)) { pathrev.add(targetNode); targetNode = targetNode.getParent(); } pathrev.add(startNode); ArrayList<Node> path = new ArrayList<Node>(); //Pfad umkehren, sodass er von Start nach Ziel ist for (int k = pathrev.size() - 1; k >= 0; k--) { path.add(pathrev.get(k)); } // Der folgende Algorithmus braucht Polygon-Infos, diese also hier einfgen startNode.addPolygon(startSector); targetNode.addPolygon(targetSector); /** * An dieser Stelle muss der Weg nocheinmal berarbeitet werden. * Es kann nmlich durch neue Tweaks sein, dass dies die Knoten nicht direkt * verbunden sind (also keinen gemeinsamen Polygon haben) * Das tritt z.B. bei der Start- und Zieleinsprungpunkt-Variierung auf. */ for (int i = 0; i < path.size() - 1; i++) { Node n1 = path.get(i); Node n2 = path.get(i + 1); FreePolygon commonSector = commonSector(n1, n2); if (commonSector == null) { // Das hier ist der interessante Fall, die beiden Knoten sind nicht direkt verbunden, es muss ein Zwischenknoten eingefgt werden: // Dessen Punkt suchen Edge direct = new Edge(n1, n2); Node newNode = null; // Die Polygone von n1 durchprobieren for (FreePolygon currentPoly : n1.getPolygons()) { List<Edge> edges = currentPoly.calcEdges(); for (Edge testedge : edges) { // Gibts da einen Schnitt? SimplePosition intersection = direct.intersectionWithEndsNotAllowed(testedge); if (intersection != null) { // Kandidat fr den nchsten Polygon FreePolygon nextPoly = null; // Kante gefunden // Von dieser Kante die Enden suchen nextPoly = getOtherPoly(testedge.getStart(), testedge.getEnd(), currentPoly); newNode = intersection.toNode(); newNode.addPolygon(currentPoly); newNode.addPolygon(nextPoly); break; } } if (newNode != null) { break; } } if (newNode == null) { // Das drfte nicht passieren, der Weg ist legal gefunden worden, muss also eigentlich existieren System.out.println("[Pathfinder][ERROR]: Cannot insert Nodes into route, aborting!"); return null; } else { path.add(i + 1, newNode); } } } return path; //Pfad zurckgeben }
From source file:de._13ducks.cor.game.server.movement.SubSectorPathfinder.java
/** * Sucht einen Weg auf Freiflchen (FreePolygon) um ein Hindernis herum. * Beachtet weitere Hindernisse auf der "Umleitung". * Sucht die Route nur bis zum nchsten Ziel. * Der Mover darf sich nicht bereits auf einer Umleitung befinden, * diese muss ggf vorher gelscht worden sein. * @param mover/*from w w w . j a va 2 s.c o m*/ * @param obstacle * @return */ static List<SubSectorEdge> searchDiversion(Moveable mover, Moveable obstacle, SimplePosition target) { // Vorberprfung: Ist das Ziel berhaupt noch frei? List<Moveable> moversAroundTarget = Server.getInnerServer().moveMan.moveMap .moversAroundPoint(target.toFPP(), mover.getRadius() + 5); moversAroundTarget.remove(mover); // Falls drin for (Moveable m : moversAroundTarget) { if (m.getPrecisePosition().getDistance(target.toFPP()) < m.getRadius() + mover.getRadius() + ServerBehaviourMove.MIN_DISTANCE) { System.out.println("No div, target blocked!"); return null; } } /** * Wegsuche in 2 Schritten: * 1. Aufbauen eines geeigneten Graphen, der das gesamte Problem enthlt. * 2. Suchen einer Route in diesem Graphen mittels A* (A-Star). */ // Aufbauen des Graphen: ArrayList<SubSectorObstacle> graph = new ArrayList<SubSectorObstacle>(); // Der Graph selber LinkedList<Moveable> openObstacles = new LinkedList<Moveable>(); // Die Liste mit noch zu untersuchenden Knoten ArrayList<Moveable> closedObstacles = new ArrayList<Moveable>(); // Bearbeitete Knoten openObstacles.add(obstacle); // Startpunkt des Graphen. closedObstacles.add(mover); // Wird im Graphen nicht mitbercksichtigt. double radius = mover.getRadius() + ServerBehaviourMove.MIN_DISTANCE; while (!openObstacles.isEmpty()) { // Neues Element aus der Liste holen und als bearbeitet markieren. Moveable work = openObstacles.poll(); closedObstacles.add(work); SubSectorObstacle next = new SubSectorObstacle(work.getPrecisePosition().x(), work.getPrecisePosition().y(), work.getRadius()); // Zuerst alle Punkte des Graphen lschen, die jetzt nichtmehr erreichbar sind: for (SubSectorObstacle obst : graph) { obst.removeNearNodes(next, radius); } // Mit Graph vernetzen for (SubSectorObstacle node : graph) { if (node.inColRange(next, radius)) { // Schnittpunkte suchen SubSectorNode[] intersections = node.calcIntersections(next, radius); for (SubSectorNode n2 : intersections) { boolean reachable = true; for (SubSectorObstacle o : graph) { if (o.equals(node)) { continue; // Um den gehts jetzt ja gerade, natrlich liegen wir auf diesem Kreis } if (o.moveCircleContains(n2, radius)) { reachable = false; break; } } if (reachable) { // Schnittpunkt einbauen next.addNode(n2); node.addNode(n2); } } } } // Bearbeitetes selbst in Graph einfgen graph.add(next); // Weitere Hindernisse suchen, die jetzt relevant sind. List<Moveable> moversAround = Server.getInnerServer().moveMan.moveMap.moversAround(work, (work.getRadius() + radius) * 2); for (Moveable pmove : moversAround) { if (!closedObstacles.contains(pmove) && !openObstacles.contains(pmove)) { openObstacles.add(pmove); } } } // Jetzt drber laufen und Graph aufbauen: for (SubSectorObstacle obst : graph) { // Vorgensweise: // In jedem Hinderniss die Linie entlanglaufen und Knoten mit Kanten verbinden. // Ein Knoten darf auf einem Kreis immer nur in eine Richtung gehen. // (das sollte mithilfe seiner beiden, bekannten hindernisse recht einfach sein) // Die Lnge des Kreissegments lsst sich einfach mithilfe des winkels ausrechnen (Math.atan2(y,x) // Dann darf der A*. Bzw. Dijkstra, A* ist hier schon fast Overkill. // Alle Knoten ihrem Bogenma nach sortieren. obst.sortNodes(); obst.interConnectNodes(radius); } // Start- und Zielknoten einbauen und mit dem Graph vernetzten. SubSectorNode startNode = new SubSectorNode(mover.getPrecisePosition().x(), mover.getPrecisePosition().y()); SubSectorNode targetNode = new SubSectorNode(target.x(), target.y()); double min = Double.POSITIVE_INFINITY; SubSectorObstacle minObstacle = null; for (SubSectorObstacle obst : graph) { double newdist = Math.sqrt((obst.getX() - startNode.getX()) * (obst.getX() - startNode.getX()) + (obst.getY() - startNode.getY()) * (obst.getY() - startNode.getY())); newdist -= obst.getRadius() + radius; // Es interessiert uns der nchstmgliche Kreis, nicht das nchste Hinderniss if (newdist < min) { min = newdist; minObstacle = obst; } } // Punkt auf Laufkreis finden Vector direct = new Vector(startNode.getX() - minObstacle.getX(), startNode.getY() - minObstacle.getY()); direct = direct.normalize().multiply(minObstacle.getRadius() + radius); SubSectorNode minNode = new SubSectorNode(minObstacle.getX() + direct.getX(), minObstacle.getY() + direct.getY(), minObstacle); // In das Hinderniss integrieren: minObstacle.lateIntegrateNode(minNode); SubSectorEdge startEdge = new SubSectorEdge(startNode, minNode, min); if (!startNode.equals(minNode)) { startNode.addEdge(startEdge); minNode.addEdge(startEdge); } else { // Wir stehen schon auf dem minNode. // Die Einsprungkante ist nicht notwendig. startNode = minNode; } double min2 = Double.POSITIVE_INFINITY; SubSectorObstacle minObstacle2 = null; for (SubSectorObstacle obst : graph) { double newdist = Math.sqrt((obst.getX() - targetNode.getX()) * (obst.getX() - targetNode.getX()) + (obst.getY() - targetNode.getY()) * (obst.getY() - targetNode.getY())); newdist -= obst.getRadius() + radius; // Es interessiert uns der nchstmgliche Kreis, nicht das nchste Hinderniss if (newdist < min2) { min2 = newdist; minObstacle2 = obst; } } // Punkt auf Laufkreis finden Vector direct2 = new Vector(targetNode.getX() - minObstacle2.getX(), targetNode.getY() - minObstacle2.getY()); direct2 = direct2.normalize().multiply(minObstacle2.getRadius() + radius); SubSectorNode minNode2 = new SubSectorNode(minObstacle2.getX() + direct2.getX(), minObstacle2.getY() + direct2.getY(), minObstacle2); // In das Hinderniss integrieren: minObstacle2.lateIntegrateNode(minNode2); SubSectorEdge targetEdge = new SubSectorEdge(minNode2, targetNode, min2); if (!targetNode.equals(minNode2)) { targetNode.addEdge(targetEdge); minNode2.addEdge(targetEdge); } else { // Das Ziel ist schon auf dem Laufkreis. // Die Aussprungkante ist nicht ntig. targetNode = minNode2; } /** * Hier jetzt einen Weg suchen von startNode nach targetNode. * Die Kanten sind in node.myEdges * Die Ziele bekommt man mit edge.getOther(startNode) * Die Lnge (Wegkosten) stehen in edge.length (vorsicht: double-Wert!) */ PriorityBuffer open = new PriorityBuffer(); // Liste fr entdeckte Knoten LinkedHashSet<SubSectorNode> containopen = new LinkedHashSet<SubSectorNode>(); // Auch fr entdeckte Knoten, hiermit kann viel schneller festgestellt werden, ob ein bestimmter Knoten schon enthalten ist. LinkedHashSet<SubSectorNode> closed = new LinkedHashSet<SubSectorNode>(); // Liste fr fertig bearbeitete Knoten double cost_t = 0; //Movement Kosten (gerade 5, diagonal 7, wird spter festgelegt) open.add(startNode); while (open.size() > 0) { SubSectorNode current = (SubSectorNode) open.remove(); containopen.remove(current); if (current.equals(targetNode)) { //Abbruch, weil Weg von Start nach Ziel gefunden wurde //targetNode.setParent(current.getParent()); //"Vorgngerfeld" von Ziel bekannt break; } // Aus der open wurde current bereits gelscht, jetzt in die closed verschieben closed.add(current); ArrayList<SubSectorEdge> neighbors = current.getMyEdges(); for (SubSectorEdge edge : neighbors) { SubSectorNode node = edge.getOther(current); if (closed.contains(node)) { continue; } // Kosten dort hin berechnen cost_t = edge.getLength(); if (containopen.contains(node)) { //Wenn sich der Knoten in der openlist befindet, muss berechnet werden, ob es einen krzeren Weg gibt if (current.getCost() + cost_t < node.getCost()) { //krzerer Weg gefunden? node.setCost(current.getCost() + cost_t); //-> Wegkosten neu berechnen //node.setValF(node.cost + node.getHeuristic()); //F-Wert, besteht aus Wegkosten vom Start + Luftlinie zum Ziel node.setParent(current); //aktuelles Feld wird zum Vorgngerfeld } } else { node.setCost(current.getCost() + cost_t); //node.setHeuristic(Math.sqrt(Math.pow(Math.abs((targetNode.getX() - node.getX())), 2) + Math.pow(Math.abs((targetNode.getY() - node.getY())), 2))); // geschtzte Distanz zum Ziel //Die Zahl am Ende der Berechnung ist der Aufwand der Wegsuche //5 ist schnell, 4 normal, 3 dauert lange node.setParent(current); // Parent ist die RogPosition, von dem der aktuelle entdeckt wurde //node.setValF(node.cost + node.getHeuristic()); //F-Wert, besteht aus Wegkosten vom Start aus + Luftlinie zum Ziel open.add(node); // in openlist hinzufgen containopen.add(node); } } } if (targetNode.getParent() == null) { //kein Weg gefunden return null; } ArrayList<SubSectorNode> pathrev = new ArrayList<SubSectorNode>(); //Pfad aus parents erstellen, von Ziel nach Start while (!targetNode.equals(startNode)) { pathrev.add(targetNode); targetNode = targetNode.getParent(); } pathrev.add(startNode); ArrayList<SubSectorNode> path = new ArrayList<SubSectorNode>(); //Pfad umkehren, sodass er von Start nach Ziel ist for (int k = pathrev.size() - 1; k >= 0; k--) { path.add(pathrev.get(k)); } // Nachbearbeitung: // Wir brauchen eine Kanten-Liste mit arc/direct Informationen ArrayList<SubSectorEdge> finalPath = new ArrayList<SubSectorEdge>(); for (int i = 0; i < path.size() - 1; i++) { SubSectorNode from = path.get(i); SubSectorNode to = path.get(i + 1); SubSectorEdge edge = shortestCommonEdge(from, to); if (edge != null) { finalPath.add(edge); } else { throw new RuntimeException("ERROR Cannot find edge from " + from + " to " + to + " but it is part of the calculated path!!!"); } } return finalPath; //Pfad zurckgeben }