Java tutorial
/******************************************************************************* * Copyright 2014 Rafael Garcia Moreno. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. ******************************************************************************/ package com.bladecoder.engine.polygonalpathfinder; import java.util.ArrayList; import java.util.Collection; import com.badlogic.gdx.math.Polygon; import com.badlogic.gdx.math.Vector2; import com.badlogic.gdx.utils.Json; import com.badlogic.gdx.utils.Json.Serializable; import com.badlogic.gdx.utils.JsonValue; import com.bladecoder.engine.assets.EngineAssetManager; import com.bladecoder.engine.model.BaseActor; import com.bladecoder.engine.model.ObstacleActor; import com.bladecoder.engine.pathfinder.AStarPathFinder; import com.bladecoder.engine.pathfinder.NavContext; import com.bladecoder.engine.pathfinder.NavGraph; import com.bladecoder.engine.pathfinder.PathFinder; import com.bladecoder.engine.util.EngineLogger; import com.bladecoder.engine.util.PolygonUtils; /** * Finds the shortest path between 2 points in a world defined by a walkzone and * several obstacles. * * @author rgarcia */ public class PolygonalNavGraph implements NavGraph<NavNodePolygonal>, Serializable { private static final int MAX_PATHFINDER_SEARCH_DISTANCE = 50; private static final Vector2 tmp = new Vector2(); private static final Vector2 tmp2 = new Vector2(); private Polygon walkZone; private final ArrayList<Polygon> obstacles = new ArrayList<Polygon>(); final private PathFinder<NavNodePolygonal> pathfinder = new AStarPathFinder<NavNodePolygonal>(this, MAX_PATHFINDER_SEARCH_DISTANCE, new ManhattanDistance()); final private NavPathPolygonal resultPath = new NavPathPolygonal(); final private NavNodePolygonal startNode = new NavNodePolygonal(); final private NavNodePolygonal targetNode = new NavNodePolygonal(); final private ArrayList<NavNodePolygonal> graphNodes = new ArrayList<NavNodePolygonal>(); public ArrayList<Vector2> findPath(float sx, float sy, float tx, float ty) { resultPath.clear(); Vector2 source = new Vector2(sx, sy); Vector2 target = new Vector2(tx, ty); // 1. First verify if both the start and target points of the path are // inside the polygon. If the end point is outside the polygon clamp it // back inside. if (!PolygonUtils.isPointInside(walkZone, sx, sy, true)) { EngineLogger.debug("PolygonalPathFinder: Source not in polygon!"); return resultPath.getPath(); } if (!PolygonUtils.isPointInside(walkZone, tx, ty, true)) { PolygonUtils.getClampedPoint(walkZone, tx, ty, target); // if (!PolygonUtils.isPointInside(walkZone, target.x, target.y, true)) { // EngineLogger.debug("PolygonalPathFinder: CLAMPED FAILED!!"); // return resultPath.getPath(); // } } for (Polygon o : obstacles) { if (PolygonUtils.isPointInside(o, target.x, target.y, false)) { PolygonUtils.getClampedPoint(o, target.x, target.y, target); // If the clamped point is not in the walkzone // we search for the first vertex inside if (!PolygonUtils.isPointInside(walkZone, target.x, target.y, true)) { getFirstVertexInsideWalkzone(o, target); // We exit after processing the first polygon with the point inside. // Overlaped obstacles are not supported break; } } } // 2. Then start by checking if both points are in line-of-sight. If // they are, theres no need for pathfinding, just walk there! if (inLineOfSight(source.x, source.y, target.x, target.y)) { EngineLogger.debug("PolygonalPathFinder: Direct path found"); resultPath.getPath().add(source); resultPath.getPath().add(target); return resultPath.getPath(); } // 3. Otherwise, add the start and end points of your path as new // temporary nodes to the graph. // AND Connect them to every other node that they can see on the graph. addStartEndNodes(source.x, source.y, target.x, target.y); // 5. Run your A* implementation on the graph to get your path. This // path is guaranteed to be as direct as possible! pathfinder.findPath(null, startNode, targetNode, resultPath); return resultPath.getPath(); } /** * Search the first polygon vertex inside the walkzone. * * @param p the polygon * @param target the vertex found */ private void getFirstVertexInsideWalkzone(Polygon p, Vector2 target) { float verts[] = p.getTransformedVertices(); for (int i = 0; i < verts.length; i += 2) { if (PolygonUtils.isPointInside(walkZone, verts[i], verts[i + 1], true)) { target.x = verts[i]; target.y = verts[i + 1]; return; } } } public void createInitialGraph(Collection<BaseActor> actors) { graphNodes.clear(); // 1.- Add WalkZone convex nodes float verts[] = walkZone.getTransformedVertices(); for (int i = 0; i < verts.length; i += 2) { if (!PolygonUtils.isVertexConcave(walkZone, i)) { graphNodes.add(new NavNodePolygonal(verts[i], verts[i + 1])); } } // 2.- Add obstacles concave nodes obstacles.clear(); for (BaseActor a : actors) { if (a instanceof ObstacleActor && a.isVisible()) obstacles.add(a.getBBox()); } for (Polygon o : obstacles) { verts = o.getTransformedVertices(); for (int i = 0; i < verts.length; i += 2) { if (PolygonUtils.isVertexConcave(o, i) && PolygonUtils.isPointInside(walkZone, verts[i], verts[i + 1], false)) { graphNodes.add(new NavNodePolygonal(verts[i], verts[i + 1])); } } } // 3.- CALC LINE OF SIGHTs for (int i = 0; i < graphNodes.size() - 1; i++) { NavNodePolygonal n1 = graphNodes.get(i); for (int j = i + 1; j < graphNodes.size(); j++) { NavNodePolygonal n2 = graphNodes.get(j); if (inLineOfSight(n1.x, n1.y, n2.x, n2.y)) { n1.neighbors.add(n2); n2.neighbors.add(n1); } } } } private boolean inLineOfSight(float p1X, float p1Y, float p2X, float p2Y) { tmp.set(p1X, p1Y); tmp2.set(p2X, p2Y); if (!PolygonUtils.inLineOfSight(tmp, tmp2, walkZone, false)) { return false; } for (Polygon o : obstacles) { if (!PolygonUtils.inLineOfSight(tmp, tmp2, o, true)) { return false; } } return true; } private void addStartEndNodes(float sx, float sy, float tx, float ty) { startNode.x = sx; startNode.y = sy; targetNode.x = tx; targetNode.y = ty; startNode.neighbors.clear(); for (NavNodePolygonal n : graphNodes) { n.neighbors.removeValue(targetNode, true); if (inLineOfSight(startNode.x, startNode.y, n.x, n.y)) { startNode.neighbors.add(n); } if (inLineOfSight(targetNode.x, targetNode.y, n.x, n.y)) { n.neighbors.add(targetNode); } } } public Polygon getWalkZone() { return walkZone; } public void setWalkZone(Polygon walkZone) { this.walkZone = walkZone; } public ArrayList<NavNodePolygonal> getGraphNodes() { return graphNodes; } @Override public boolean blocked(NavContext<NavNodePolygonal> context, NavNodePolygonal targetNode) { return false; } @Override public float getCost(NavContext<NavNodePolygonal> context, NavNodePolygonal targetNode) { return 1; } private void addObstacleToGrapth(Polygon poly) { float verts[] = poly.getTransformedVertices(); for (int i = 0; i < verts.length; i += 2) { if (PolygonUtils.isVertexConcave(poly, i) && PolygonUtils.isPointInside(walkZone, verts[i], verts[i + 1], false)) { NavNodePolygonal n1 = new NavNodePolygonal(verts[i], verts[i + 1]); for (int j = 0; j < graphNodes.size(); j++) { NavNodePolygonal n2 = graphNodes.get(j); if (inLineOfSight(n1.x, n1.y, n2.x, n2.y)) { n1.neighbors.add(n2); n2.neighbors.add(n1); } } graphNodes.add(n1); } } } public void addDinamicObstacle(Polygon poly) { int idx = obstacles.indexOf(poly); // CHECK TO AVOID ADDING THE ACTOR SEVERAL TIMES if (idx == -1) { obstacles.add(poly); addObstacleToGrapth(poly); } } public boolean removeDinamicObstacle(Polygon poly) { boolean exists = obstacles.remove(poly); if (!exists) return false; float verts[] = poly.getTransformedVertices(); for (int i = 0; i < verts.length; i += 2) { if (PolygonUtils.isVertexConcave(poly, i) && PolygonUtils.isPointInside(walkZone, verts[i], verts[i + 1], false)) { for (int j = 0; j < graphNodes.size(); j++) { NavNodePolygonal n = graphNodes.get(j); if (n.x == verts[i] && n.y == verts[i + 1]) { graphNodes.remove(n); j--; for (NavNodePolygonal n2 : graphNodes) { n2.neighbors.removeValue(n, true); } } } } } return true; } @Override public void write(Json json) { Polygon p = new Polygon(walkZone.getVertices()); p.setPosition(walkZone.getX() / walkZone.getScaleX(), walkZone.getY() / walkZone.getScaleY()); json.writeValue("walkZone", p); } @Override public void read(Json json, JsonValue jsonData) { float worldScale = EngineAssetManager.getInstance().getScale(); walkZone = json.readValue("walkZone", Polygon.class, jsonData); walkZone.setScale(worldScale, worldScale); walkZone.setPosition(walkZone.getX() * worldScale, walkZone.getY() * worldScale); } }