diff --git a/core/src/mindustry/Vars.java b/core/src/mindustry/Vars.java index 6d4006b074..552f0ac54f 100644 --- a/core/src/mindustry/Vars.java +++ b/core/src/mindustry/Vars.java @@ -241,7 +241,6 @@ public class Vars implements Loadable{ public static WaveSpawner spawner; public static BlockIndexer indexer; public static Pathfinder pathfinder; - public static HierarchyPathFinder hpath; public static ControlPathfinder controlPath; public static FogControl fogControl; @@ -315,7 +314,6 @@ public class Vars implements Loadable{ indexer = new BlockIndexer(); pathfinder = new Pathfinder(); controlPath = new ControlPathfinder(); - hpath = new HierarchyPathFinder(); fogControl = new FogControl(); bases = new BaseRegistry(); logicVars = new GlobalVars(); diff --git a/core/src/mindustry/ai/ControlPathfinder.java b/core/src/mindustry/ai/ControlPathfinder.java index f6b166270e..f50345a554 100644 --- a/core/src/mindustry/ai/ControlPathfinder.java +++ b/core/src/mindustry/ai/ControlPathfinder.java @@ -7,6 +7,8 @@ import arc.math.*; import arc.math.geom.*; import arc.struct.*; import arc.util.*; +import mindustry.annotations.Annotations.*; +import mindustry.content.*; import mindustry.core.*; import mindustry.game.EventType.*; import mindustry.game.*; @@ -17,14 +19,13 @@ import mindustry.world.*; import static mindustry.Vars.*; import static mindustry.ai.Pathfinder.*; -//TODO remove/replace -public class ControlPathfinder{ - //TODO this FPS-based update system could be flawed. - private static final long maxUpdate = Time.millisToNanos(30); - private static final int updateFPS = 60; - private static final int updateInterval = 1000 / updateFPS; +//https://webdocs.cs.ualberta.ca/~mmueller/ps/hpastar.pdf +//https://www.gameaipro.com/GameAIPro/GameAIPro_Chapter23_Crowd_Pathfinding_and_Steering_Using_Flow_Field_Tiles.pdf +public class ControlPathfinder implements Runnable{ private static final int wallImpassableCap = 1_000_000; + public static boolean showDebug; + public static final PathCost costGround = (team, tile) -> @@ -75,295 +76,1188 @@ public class ControlPathfinder{ costNaval ); - public static boolean showDebug = false; + private static final long maxUpdate = Time.millisToNanos(12); + private static final int updateStepInterval = 200; + private static final int updateFPS = 30; + private static final int updateInterval = 1000 / updateFPS, invalidateCheckInterval = 1000; - //static access probably faster than object access - static int wwidth, wheight; - //increments each tile change - static volatile int worldUpdateId; + static final int clusterSize = 12; - /** Current pathfinding threads, contents may be null */ - @Nullable PathfindThread[] threads; - /** for unique target IDs */ - int lastTargetId = 1; - /** requests per-unit */ - ObjectMap requests = new ObjectMap<>(); + static final int[] offsets = { + 1, 0, //right: bottom to top + 0, 1, //top: left to right + 0, 0, //left: bottom to top + 0, 0 //bottom: left to right + }; + + static final int[] moveDirs = { + 0, 1, + 1, 0, + 0, 1, + 1, 0 + }; + + static final int[] nextOffsets = { + 1, 0, + 0, 1, + -1, 0, + 0, -1 + }; + + //maps team -> pathCost -> flattened array of clusters in 2D + //(what about teams? different path costs?) + Cluster[][][] clusters; + + int cwidth, cheight; + + //temporarily used for resolving connections for intra-edges + IntSet usedEdges = new IntSet(); + //tasks to run on pathfinding thread + TaskQueue queue = new TaskQueue(); + + //individual requests based on unit - MAIN THREAD ONLY + ObjectMap unitRequests = new ObjectMap<>(); + + Seq threadPathRequests = new Seq<>(false); + + //TODO: very dangerous usage; + //TODO - it is accessed from the main thread + //TODO - it is written to on the pathfinding thread + //maps position in world in (x + y * width format) | type (bitpacked to long) to a cache of flow fields + LongMap fields = new LongMap<>(); + //MAIN THREAD ONLY + Seq fieldList = new Seq<>(false); + + //these are for inner edge A* (temporary!) + IntFloatMap innerCosts = new IntFloatMap(); + PathfindQueue innerFrontier = new PathfindQueue(); + + //ONLY modify on pathfinding thread. + IntSet clustersToUpdate = new IntSet(); + IntSet clustersToInnerUpdate = new IntSet(); + + //PATHFINDING THREAD - requests that should be recomputed + ObjectSet invalidRequests = new ObjectSet<>(); + + /** Current pathfinding thread */ + @Nullable Thread thread; + + //path requests are per-unit + static class PathRequest{ + final Unit unit; + final int destination, team, costId; + //resulting path of nodes + final IntSeq resultPath = new IntSeq(); + + //node index -> total cost + @Nullable IntFloatMap costs = new IntFloatMap(); + //node index (NodeIndex struct) -> node it came from TODO merge them, make properties of FieldCache? + @Nullable IntIntMap cameFrom = new IntIntMap(); + //frontier for A* + @Nullable PathfindQueue frontier = new PathfindQueue(); + + //main thread only! + long lastUpdateId = state.updateId; + + //both threads + volatile boolean notFound = false; + volatile boolean invalidated = false; + //old field assigned before everything was recomputed + @Nullable volatile FieldCache oldCache; + + boolean lastRaycastResult = false; + int lastRaycastTile, lastWorldUpdate; + int lastTile; + @Nullable Tile lastTargetTile; + + PathRequest(Unit unit, int team, int costId, int destination){ + this.unit = unit; + this.costId = costId; + this.team = team; + this.destination = destination; + } + } + + static class FieldCache{ + final PathCost cost; + final int costId; + final int team; + final int goalPos; + //frontier for flow fields + final IntQueue frontier = new IntQueue(); + //maps cluster index to field weights; 0 means uninitialized + final IntMap fields = new IntMap<>(); + final long mapKey; + + //main thread only! + long lastUpdateId = state.updateId; + + //TODO: how are the nodes merged? CAN they be merged? + + FieldCache(PathCost cost, int costId, int team, int goalPos){ + this.cost = cost; + this.team = team; + this.goalPos = goalPos; + this.costId = costId; + this.mapKey = Pack.longInt(goalPos, costId); + } + } + + static class Cluster{ + IntSeq[] portals = new IntSeq[4]; + //maps rotation + index of portal to list of IntraEdge objects + LongSeq[][] portalConnections = new LongSeq[4][]; + } public ControlPathfinder(){ + Events.on(ResetEvent.class, event -> stop()); + Events.on(WorldLoadEvent.class, event -> { stop(); - wwidth = world.width(); - wheight = world.height(); + + //TODO: can the pathfinding thread even see these? + unitRequests = new ObjectMap<>(); + fields = new LongMap<>(); + fieldList = new Seq<>(false); + + clusters = new Cluster[256][][]; + cwidth = Mathf.ceil((float)world.width() / clusterSize); + cheight = Mathf.ceil((float)world.height() / clusterSize); + start(); }); - //only update the world when a solid block is removed or placed, everything else doesn't matter - Events.on(TilePreChangeEvent.class, e -> { - if(e.tile.solid()){ - worldUpdateId ++; - } - }); - Events.on(TileChangeEvent.class, e -> { - if(e.tile.solid()){ - worldUpdateId ++; - } - }); - Events.on(ResetEvent.class, event -> stop()); + e.tile.getLinkedTiles(t -> { + int x = t.x, y = t.y, mx = x % clusterSize, my = y % clusterSize, cx = x / clusterSize, cy = y / clusterSize, cluster = cx + cy * cwidth; + + //is at the edge of a cluster; this means the portals may have changed. + if(mx == 0 || my == 0 || mx == clusterSize - 1 || my == clusterSize - 1){ + + if(mx == 0) queueClusterUpdate(cx - 1, cy); //left + if(my == 0) queueClusterUpdate(cx, cy - 1); //bottom + if(mx == clusterSize - 1) queueClusterUpdate(cx + 1, cy); //right + if(my == clusterSize - 1) queueClusterUpdate(cx, cy + 1); //top + + queueClusterUpdate(cx, cy); + //TODO: recompute edge clusters too. + }else{ + //there is no need to recompute portals for block updates that are not on the edge. + queue.post(() -> clustersToInnerUpdate.add(cluster)); + } + }); + + //TODO: recalculate affected flow fields? or just all of them? how to reflow? + }); //invalidate paths Events.run(Trigger.update, () -> { - for(var req : requests.values()){ + for(var req : unitRequests.values()){ //skipped N update -> drop it if(req.lastUpdateId <= state.updateId - 10){ + req.invalidated = true; //concurrent modification! - Core.app.post(() -> requests.remove(req.unit)); - req.thread.queue.post(() -> req.thread.requests.remove(req)); + queue.post(() -> threadPathRequests.remove(req)); + Core.app.post(() -> unitRequests.remove(req.unit)); + } + } + + for(var field : fieldList){ + //skipped N update -> drop it + if(field.lastUpdateId <= state.updateId - 30){ + //make sure it's only modified on the main thread...? but what about calling get() on this thread?? + queue.post(() -> fields.remove(field.mapKey)); + Core.app.post(() -> fieldList.remove(field)); } } }); - Events.run(Trigger.draw, () -> { - if(!showDebug) return; + if(showDebug){ + Events.run(Trigger.draw, () -> { + int team = player.team().id; + int cost = 0; - for(var req : requests.values()){ - if(req.frontier == null) continue; Draw.draw(Layer.overlayUI, () -> { - if(req.done){ - int len = req.result.size; - int rp = req.rayPathIndex; - if(rp < len && rp >= 0){ - Draw.color(Color.royal); - Tile tile = tile(req.result.items[rp]); - Lines.line(req.unit.x, req.unit.y, tile.worldx(), tile.worldy()); - } + Lines.stroke(1f); - for(int i = 0; i < len; i++){ - Draw.color(Tmp.c1.set(Color.white).fromHsv(i / (float)len * 360f, 1f, 0.9f)); - int pos = req.result.items[i]; - Fill.square(pos % wwidth * tilesize, pos / wwidth * tilesize, 3f); + if(clusters[team] != null && clusters[team][cost] != null){ + for(int cx = 0; cx < cwidth; cx++){ + for(int cy = 0; cy < cheight; cy++){ - if(i == req.pathIndex){ - Draw.color(Color.green); - Lines.square(pos % wwidth * tilesize, pos / wwidth * tilesize, 5f); - } - } - }else{ - var view = Core.camera.bounds(Tmp.r1); - int len = req.frontier.size; - float[] weights = req.frontier.weights; - int[] poses = req.frontier.queue; - for(int i = 0; i < Math.min(len, 1000); i++){ - int pos = poses[i]; - if(view.contains(pos % wwidth * tilesize, pos / wwidth * tilesize)){ - Draw.color(Tmp.c1.set(Color.white).fromHsv((weights[i] * 4f) % 360f, 1f, 0.9f)); + var cluster = clusters[team][cost][cy * cwidth + cx]; + if(cluster != null){ + Lines.stroke(0.5f); + Draw.color(Color.gray); + Lines.stroke(1f); - Lines.square(pos % wwidth * tilesize, pos / wwidth * tilesize, 4f); + Lines.rect(cx * clusterSize * tilesize - tilesize/2f, cy * clusterSize * tilesize - tilesize/2f, clusterSize * tilesize, clusterSize * tilesize); + + + for(int d = 0; d < 4; d++){ + IntSeq portals = cluster.portals[d]; + if(portals != null){ + + for(int i = 0; i < portals.size; i++){ + int pos = portals.items[i]; + int from = Point2.x(pos), to = Point2.y(pos); + float width = tilesize * (Math.abs(from - to) + 1), height = tilesize; + + portalToVec(cluster, cx, cy, d, i, Tmp.v1); + + Draw.color(Color.brown); + Lines.ellipse(30, Tmp.v1.x, Tmp.v1.y, width / 2f, height / 2f, d * 90f - 90f); + + LongSeq connections = cluster.portalConnections[d] == null ? null : cluster.portalConnections[d][i]; + + if(connections != null){ + Draw.color(Color.forest); + for(int coni = 0; coni < connections.size; coni ++){ + long con = connections.items[coni]; + + portalToVec(cluster, cx, cy, IntraEdge.dir(con), IntraEdge.portal(con), Tmp.v2); + + float + x1 = Tmp.v1.x, y1 = Tmp.v1.y, + x2 = Tmp.v2.x, y2 = Tmp.v2.y; + Lines.line(x1, y1, x2, y2); + + } + } + } + } + } + } } } } - Draw.reset(); + + for(var fields : fieldList){ + try{ + for(var entry : fields.fields){ + int cx = entry.key % cwidth, cy = entry.key / cwidth; + for(int y = 0; y < clusterSize; y++){ + for(int x = 0; x < clusterSize; x++){ + int value = entry.value[x + y * clusterSize]; + Tmp.c1.a = 1f; + Lines.stroke(0.8f, Tmp.c1.fromHsv(value * 3f, 1f, 1f)); + Draw.alpha(0.5f); + Fill.square((x + cx * clusterSize) * tilesize, (y + cy * clusterSize) * tilesize, tilesize / 2f); + } + } + } + }catch(Exception ignored){} //probably has some concurrency issues when iterating but I don't care, this is for debugging + } }); - } - }); + + Draw.reset(); + }); + } } - - /** @return the next target ID to use as a unique path identifier. */ - public int nextTargetId(){ - return lastTargetId ++; + void queueClusterUpdate(int cx, int cy){ + if(cx >= 0 && cy >= 0 && cx < cwidth && cy < cheight){ + queue.post(() -> clustersToUpdate.add(cx + cy * cwidth)); + } } - /** - * @return whether a path is ready. - * @param pathId a unique ID for this location query, which should change every time the 'destination' vector is modified. - * */ - public boolean getPathPosition(Unit unit, int pathId, Vec2 destination, Vec2 out){ - return getPathPosition(unit, pathId, destination, out, null); + //debugging only! + void portalToVec(Cluster cluster, int cx, int cy, int direction, int portalIndex, Vec2 out){ + int pos = cluster.portals[direction].items[portalIndex]; + int from = Point2.x(pos), to = Point2.y(pos); + int addX = moveDirs[direction * 2], addY = moveDirs[direction * 2 + 1]; + float average = (from + to) / 2f; + + float + x = (addX * average + cx * clusterSize + offsets[direction * 2] * (clusterSize - 1) + nextOffsets[direction * 2] / 2f) * tilesize, + y = (addY * average + cy * clusterSize + offsets[direction * 2 + 1] * (clusterSize - 1) + nextOffsets[direction * 2 + 1] / 2f) * tilesize; + + out.set(x, y); } - /** - * @return whether a path is ready. - * @param pathId a unique ID for this location query, which should change every time the 'destination' vector is modified. - * @param noResultFound extra return value for storing whether no valid path to the destination exists (thanks java!) - * */ - public boolean getPathPosition(Unit unit, int pathId, Vec2 destination, Vec2 out, @Nullable boolean[] noResultFound){ - if(noResultFound != null){ - noResultFound[0] = false; - } - - //uninitialized - if(threads == null || !world.tiles.in(World.toTile(destination.x), World.toTile(destination.y))) return false; - - PathCost costType = unit.type.pathCost; - int team = unit.team.id; - - //if the destination can be trivially reached in a straight line, do that. - if((!requests.containsKey(unit) || requests.get(unit).curId != pathId) && !raycast(team, costType, unit.tileX(), unit.tileY(), World.toTile(destination.x), World.toTile(destination.y))){ - out.set(destination); - return true; - } - - //destination is impassable, can't go there. - if(solid(team, costType, world.packArray(World.toTile(destination.x), World.toTile(destination.y)), false)){ - return false; - } - - //check for request existence - if(!requests.containsKey(unit)){ - PathfindThread thread = Structs.findMin(threads, t -> t.requestSize); - - var req = new PathRequest(thread); - req.unit = unit; - req.cost = costType; - req.destination.set(destination); - req.curId = pathId; - req.team = team; - req.lastUpdateId = state.updateId; - req.lastPos.set(unit); - req.lastWorldUpdate = worldUpdateId; - //raycast immediately when done - req.raycastTimer = 9999f; - - requests.put(unit, req); - - //add to thread so it gets processed next update - thread.queue.post(() -> thread.requests.add(req)); - }else{ - var req = requests.get(unit); - req.lastUpdateId = state.updateId; - req.team = unit.team.id; - if(req.curId != req.lastId || req.curId != pathId){ - req.pathIndex = 0; - req.rayPathIndex = -1; - req.done = false; - req.foundEnd = false; - } - - req.destination.set(destination); - req.curId = pathId; - - //check for the unit getting stuck every N seconds - if(req.done && (req.stuckTimer += Time.delta) >= 60f * 1.5f){ - req.stuckTimer = 0f; - //force recalculate - if(req.lastPos.within(unit, 1.5f)){ - req.forceRecalculate(); - } - req.lastPos.set(unit); - } - - if(req.done){ - int[] items = req.result.items; - int len = req.result.size; - int tileX = unit.tileX(), tileY = unit.tileY(); - float range = 4f; - - float minDst = req.pathIndex < len ? unit.dst2(world.tiles.geti(items[req.pathIndex])) : 0f; - int idx = req.pathIndex; - - //find closest node that is in front of the path index and hittable with raycast - for(int i = len - 1; i >= idx; i--){ - Tile tile = tile(items[i]); - float dst = unit.dst2(tile); - //TODO maybe put this on a timer since raycasts can be expensive? - if(dst < minDst && !permissiveRaycast(team, costType, tileX, tileY, tile.x, tile.y)){ - if(avoid(req.team, req.cost, items[i + 1])){ - range = 0.5f; - } - - req.pathIndex = Math.max(dst <= range * range ? i + 1 : i, req.pathIndex); - minDst = Math.min(dst, minDst); - }else if(dst <= 1f){ - req.pathIndex = Math.min(Math.max(i + 1, req.pathIndex), len - 1); - } - } - - if(req.rayPathIndex < 0){ - req.rayPathIndex = req.pathIndex; - } - - if((req.raycastTimer += Time.delta) >= 50f){ - for(int i = len - 1; i > req.pathIndex; i--){ - int val = items[i]; - if(!raycast(team, costType, tileX, tileY, val % wwidth, val / wwidth)){ - req.rayPathIndex = i; - break; - } - } - req.raycastTimer = 0; - } - - if(req.rayPathIndex < len && req.rayPathIndex >= 0){ - Tile tile = tile(items[req.rayPathIndex]); - out.set(tile); - - if(req.rayPathIndex > 0){ - float angleToNext = tile(items[req.rayPathIndex - 1]).angleTo(tile); - float angleToDest = unit.angleTo(tile); - //force recalculate when the unit moves backwards - if(Angles.angleDist(angleToNext, angleToDest) > 80f && !unit.within(tile, 1f)){ - req.forceRecalculate(); - } - } - - if(avoid(req.team, req.cost, items[req.rayPathIndex])){ - range = 0.5f; - } - - if(unit.within(tile, range)){ - req.pathIndex = req.rayPathIndex = Math.max(req.pathIndex, req.rayPathIndex + 1); - } - }else{ - //implicit done - out.set(unit); - //end of path, we're done here? reset path? what??? - } - - if(noResultFound != null){ - noResultFound[0] = !req.foundEnd; - } - } - - return req.done; - } - - return false; - } /** Starts or restarts the pathfinding thread. */ private void start(){ stop(); - if(net.client()) return; - //TODO currently capped at 6 threads, might be a good idea to make it more? - threads = new PathfindThread[Mathf.clamp(Runtime.getRuntime().availableProcessors() - 1, 1, 6)]; - for(int i = 0; i < threads.length; i ++){ - threads[i] = new PathfindThread("ControlPathfindThread-" + i); - threads[i].setPriority(Thread.MIN_PRIORITY); - threads[i].setDaemon(true); - threads[i].start(); - } + thread = new Thread(this, "Control Pathfinder"); + thread.setPriority(Thread.MIN_PRIORITY); + thread.setDaemon(true); + thread.start(); } /** Stops the pathfinding thread. */ private void stop(){ - if(threads != null){ - for(var thread : threads){ - thread.interrupt(); + if(thread != null){ + thread.interrupt(); + thread = null; + } + queue.clear(); + } + + /** @return a cluster at coordinates; can be null if not cluster was created yet*/ + @Nullable Cluster getCluster(int team, int pathCost, int cx, int cy){ + return getCluster(team, pathCost, cx + cy * cwidth); + } + + /** @return a cluster at coordinates; can be null if not cluster was created yet*/ + @Nullable Cluster getCluster(int team, int pathCost, int clusterIndex){ + if(clusters == null) return null; + + Cluster[][] dim1 = clusters[team]; + + if(dim1 == null) return null; + + Cluster[] dim2 = dim1[pathCost]; + + if(dim2 == null) return null; + + return dim2[clusterIndex]; + } + + /** @return the cluster at specified coordinates; never null. */ + Cluster getCreateCluster(int team, int pathCost, int cx, int cy){ + return getCreateCluster(team, pathCost, cx + cy * cwidth); + } + + /** @return the cluster at specified coordinates; never null. */ + Cluster getCreateCluster(int team, int pathCost, int clusterIndex){ + Cluster result = getCluster(team, pathCost, clusterIndex); + if(result == null){ + return updateCluster(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth); + }else{ + return result; + } + } + + Cluster updateCluster(int team, int pathCost, int cx, int cy){ + //TODO: what if clusters are null for thread visibility reasons? + + Cluster[][] dim1 = clusters[team]; + + if(dim1 == null){ + dim1 = clusters[team] = new Cluster[Team.all.length][]; + } + + Cluster[] dim2 = dim1[pathCost]; + + if(dim2 == null){ + dim2 = dim1[pathCost] = new Cluster[cwidth * cheight]; + } + + Cluster cluster = dim2[cy * cwidth + cx]; + if(cluster == null){ + cluster = dim2[cy * cwidth + cx] = new Cluster(); + }else{ + //reset data + for(var p : cluster.portals){ + p.clear(); } } - threads = null; - requests.clear(); + + PathCost cost = idToCost(pathCost); + + for(int direction = 0; direction < 4; direction++){ + int otherX = cx + Geometry.d4x(direction), otherY = cy + Geometry.d4y(direction); + //out of bounds, no portals in this direction + if(otherX < 0 || otherY < 0 || otherX >= cwidth || otherY >= cheight){ + continue; + } + + Cluster other = dim2[otherX + otherY * cwidth]; + IntSeq portals; + + if(other == null){ + //create new portals at direction + portals = cluster.portals[direction] = new IntSeq(4); + }else{ + //share portals with the other cluster + portals = cluster.portals[direction] = other.portals[(direction + 2) % 4]; + + //clear the portals, they're being recalculated now + portals.clear(); + } + + int addX = moveDirs[direction * 2], addY = moveDirs[direction * 2 + 1]; + int + baseX = cx * clusterSize + offsets[direction * 2] * (clusterSize - 1), + baseY = cy * clusterSize + offsets[direction * 2 + 1] * (clusterSize - 1), + nextBaseX = baseX + Geometry.d4[direction].x, + nextBaseY = baseY + Geometry.d4[direction].y; + + int lastPortal = -1; + boolean prevSolid = true; + + for(int i = 0; i < clusterSize; i++){ + int x = baseX + addX * i, y = baseY + addY * i; + + //scan for portals + if(solid(team, cost, x, y) || solid(team, cost, nextBaseX + addX * i, nextBaseY + addY * i)){ + int previous = i - 1; + //hit a wall, create portals between the two points + if(!prevSolid && previous >= lastPortal){ + //portals are an inclusive range + portals.add(Point2.pack(previous, lastPortal)); + } + prevSolid = true; + }else{ + //empty area encountered, mark the location of portal start + if(prevSolid){ + lastPortal = i; + } + prevSolid = false; + } + } + + //at the end of the loop, close any un-initialized portals; this is copy pasted code + int previous = clusterSize - 1; + if(!prevSolid && previous >= lastPortal){ + //portals are an inclusive range + portals.add(Point2.pack(previous, lastPortal)); + } + } + + updateInnerEdges(team, cost, cx, cy, cluster); + + return cluster; + } + + void updateInnerEdges(int team, int cost, int cx, int cy, Cluster cluster){ + updateInnerEdges(team, idToCost(cost), cx, cy, cluster); + } + + void updateInnerEdges(int team, PathCost cost, int cx, int cy, Cluster cluster){ + int minX = cx * clusterSize, minY = cy * clusterSize, maxX = Math.min(minX + clusterSize - 1, wwidth - 1), maxY = Math.min(minY + clusterSize - 1, wheight - 1); + + usedEdges.clear(); + + //clear all connections, since portals changed, they need to be recomputed. + cluster.portalConnections = new LongSeq[4][]; + + for(int direction = 0; direction < 4; direction++){ + var portals = cluster.portals[direction]; + if(portals == null) continue; + + int addX = moveDirs[direction * 2], addY = moveDirs[direction * 2 + 1]; + + for(int i = 0; i < portals.size; i++){ + usedEdges.add(Point2.pack(direction, i)); + + int + portal = portals.items[i], + from = Point2.x(portal), to = Point2.y(portal), + average = (from + to) / 2, + x = (addX * average + cx * clusterSize + offsets[direction * 2] * (clusterSize - 1)), + y = (addY * average + cy * clusterSize + offsets[direction * 2 + 1] * (clusterSize - 1)); + + for(int otherDir = 0; otherDir < 4; otherDir++){ + var otherPortals = cluster.portals[otherDir]; + if(otherPortals == null) continue; + + for(int j = 0; j < otherPortals.size; j++){ + + if(!usedEdges.contains(Point2.pack(otherDir, j))){ + + int + other = otherPortals.items[j], + otherFrom = Point2.x(other), otherTo = Point2.y(other), + otherAverage = (otherFrom + otherTo) / 2, + ox = cx * clusterSize + offsets[otherDir * 2] * (clusterSize - 1), + oy = cy * clusterSize + offsets[otherDir * 2 + 1] * (clusterSize - 1), + otherX = (moveDirs[otherDir * 2] * otherAverage + ox), + otherY = (moveDirs[otherDir * 2 + 1] * otherAverage + oy); + + //duplicate portal; should never happen. + if(Point2.pack(x, y) == Point2.pack(otherX, otherY)){ + continue; + } + + float connectionCost = innerAstar( + team, cost, + minX, minY, maxX, maxY, + x + y * wwidth, + otherX + otherY * wwidth, + (moveDirs[otherDir * 2] * otherFrom + ox), + (moveDirs[otherDir * 2 + 1] * otherFrom + oy), + (moveDirs[otherDir * 2] * otherTo + ox), + (moveDirs[otherDir * 2 + 1] * otherTo + oy) + ); + + if(connectionCost != -1f){ + if(cluster.portalConnections[direction] == null) cluster.portalConnections[direction] = new LongSeq[cluster.portals[direction].size]; + if(cluster.portalConnections[otherDir] == null) cluster.portalConnections[otherDir] = new LongSeq[cluster.portals[otherDir].size]; + if(cluster.portalConnections[direction][i] == null) cluster.portalConnections[direction][i] = new LongSeq(8); + if(cluster.portalConnections[otherDir][j] == null) cluster.portalConnections[otherDir][j] = new LongSeq(8); + + //TODO: can there be duplicate edges?? + cluster.portalConnections[direction][i].add(IntraEdge.get(otherDir, j, connectionCost)); + cluster.portalConnections[otherDir][j].add(IntraEdge.get(direction, i, connectionCost)); + } + } + } + } + } + } + } + + //distance heuristic: manhattan + private static float heuristic(int a, int b){ + int x = a % wwidth, x2 = b % wwidth, y = a / wwidth, y2 = b / wwidth; + return Math.abs(x - x2) + Math.abs(y - y2); + } + + private static int tcost(int team, PathCost cost, int tilePos){ + return cost.getCost(team, pathfinder.tiles[tilePos]); + } + + private static float tileCost(int team, PathCost type, int a, int b){ + //currently flat cost + return cost(team, type, b); + } + + /** @return -1 if no path was found */ + float innerAstar(int team, PathCost cost, int minX, int minY, int maxX, int maxY, int startPos, int goalPos, int goalX1, int goalY1, int goalX2, int goalY2){ + var frontier = innerFrontier; + var costs = innerCosts; + + frontier.clear(); + costs.clear(); + + //TODO: this can be faster and more memory efficient by making costs a NxN array... probably? + costs.put(startPos, 0); + frontier.add(startPos, 0); + + if(goalX2 < goalX1){ + int tmp = goalX1; + goalX1 = goalX2; + goalX2 = tmp; + } + + if(goalY2 < goalY1){ + int tmp = goalY1; + goalY1 = goalY2; + goalY2 = tmp; + } + + while(frontier.size > 0){ + int current = frontier.poll(); + + int cx = current % wwidth, cy = current / wwidth; + + //found the goal (it's in the portal rectangle) + if((cx >= goalX1 && cy >= goalY1 && cx <= goalX2 && cy <= goalY2) || current == goalPos){ + return costs.get(current); + } + + for(Point2 point : Geometry.d4){ + int newx = cx + point.x, newy = cy + point.y; + int next = newx + wwidth * newy; + + if(newx > maxX || newy > maxY || newx < minX || newy < minY || tcost(team, cost, next) == impassable) continue; + + float add = tileCost(team, cost, current, next); + + if(add < 0) continue; + + float newCost = costs.get(current) + add; + + if(newCost < costs.get(next, Float.POSITIVE_INFINITY)){ + costs.put(next, newCost); + float priority = newCost + heuristic(next, goalPos); + frontier.add(next, priority); + } + } + } + + return -1f; + } + + int makeNodeIndex(int cx, int cy, int dir, int portal){ + //to make sure there's only one way to refer to each node, the direction must be 0 or 1 (referring to portals on the top or right edge) + + //direction can only be 2 if cluster X is 0 (left edge of map) + if(dir == 2 && cx != 0){ + dir = 0; + cx --; + } + + //direction can only be 3 if cluster Y is 0 (bottom edge of map) + if(dir == 3 && cy != 0){ + dir = 1; + cy --; + } + + return NodeIndex.get(cx + cy * cwidth, dir, portal); + } + + //uses A* to find the closest node index to specified coordinates + //this node is used in cluster A* + /** @return MAX_VALUE if no node is found */ + private int findClosestNode(int team, int pathCost, int tileX, int tileY){ + int cx = tileX / clusterSize, cy = tileY / clusterSize; + + if(cx < 0 || cy < 0 || cx >= cwidth || cy >= cheight){ + return Integer.MAX_VALUE; + } + + PathCost cost = idToCost(pathCost); + Cluster cluster = getCreateCluster(team, pathCost, cx, cy); + int minX = cx * clusterSize, minY = cy * clusterSize, maxX = Math.min(minX + clusterSize - 1, wwidth - 1), maxY = Math.min(minY + clusterSize - 1, wheight - 1); + + int bestPortalPair = Integer.MAX_VALUE; + float bestCost = Float.MAX_VALUE; + + //A* to every node, find the best one (I know there's a better algorithm for this, probably dijkstra) + for(int dir = 0; dir < 4; dir++){ + var portals = cluster.portals[dir]; + if(portals == null) continue; + + for(int j = 0; j < portals.size; j++){ + + int + other = portals.items[j], + otherFrom = Point2.x(other), otherTo = Point2.y(other), + otherAverage = (otherFrom + otherTo) / 2, + ox = cx * clusterSize + offsets[dir * 2] * (clusterSize - 1), + oy = cy * clusterSize + offsets[dir * 2 + 1] * (clusterSize - 1), + otherX = (moveDirs[dir * 2] * otherAverage + ox), + otherY = (moveDirs[dir * 2 + 1] * otherAverage + oy); + + float connectionCost = innerAstar( + team, cost, + minX, minY, maxX, maxY, + tileX + tileY * wwidth, + otherX + otherY * wwidth, + (moveDirs[dir * 2] * otherFrom + ox), + (moveDirs[dir * 2 + 1] * otherFrom + oy), + (moveDirs[dir * 2] * otherTo + ox), + (moveDirs[dir * 2 + 1] * otherTo + oy) + ); + + //better cost found, update and return + if(connectionCost != -1f && connectionCost < bestCost){ + bestPortalPair = Point2.pack(dir, j); + bestCost = connectionCost; + } + } + } + + if(bestPortalPair != Integer.MAX_VALUE){ + return makeNodeIndex(cx, cy, Point2.x(bestPortalPair), Point2.y(bestPortalPair)); + } + + + return Integer.MAX_VALUE; + } + + //distance heuristic: manhattan + private float clusterNodeHeuristic(int team, int pathCost, int nodeA, int nodeB){ + int + clusterA = NodeIndex.cluster(nodeA), + dirA = NodeIndex.dir(nodeA), + portalA = NodeIndex.portal(nodeA), + clusterB = NodeIndex.cluster(nodeB), + dirB = NodeIndex.dir(nodeB), + portalB = NodeIndex.portal(nodeB), + rangeA = getCreateCluster(team, pathCost, clusterA).portals[dirA].items[portalA], + rangeB = getCreateCluster(team, pathCost, clusterB).portals[dirB].items[portalB]; + + float + averageA = (Point2.x(rangeA) + Point2.y(rangeA)) / 2f, + x1 = (moveDirs[dirA * 2] * averageA + (clusterA % cwidth) * clusterSize + offsets[dirA * 2] * (clusterSize - 1) + nextOffsets[dirA * 2] / 2f), + y1 = (moveDirs[dirA * 2 + 1] * averageA + (clusterA / cwidth) * clusterSize + offsets[dirA * 2 + 1] * (clusterSize - 1) + nextOffsets[dirA * 2 + 1] / 2f), + + averageB = (Point2.x(rangeB) + Point2.y(rangeB)) / 2f, + x2 = (moveDirs[dirB * 2] * averageB + (clusterB % cwidth) * clusterSize + offsets[dirB * 2] * (clusterSize - 1) + nextOffsets[dirB * 2] / 2f), + y2 = (moveDirs[dirB * 2 + 1] * averageB + (clusterB / cwidth) * clusterSize + offsets[dirB * 2 + 1] * (clusterSize - 1) + nextOffsets[dirB * 2 + 1] / 2f); + + return Math.abs(x1 - x2) + Math.abs(y1 - y2); + } + + @Nullable IntSeq clusterAstar(PathRequest request, int pathCost, int startNodeIndex, int endNodeIndex){ + var result = request.resultPath; + + if(startNodeIndex == endNodeIndex){ + result.clear(); + result.add(startNodeIndex); + return result; + } + + var team = request.team; + + if(request.costs == null) request.costs = new IntFloatMap(); + if(request.cameFrom == null) request.cameFrom = new IntIntMap(); + if(request.frontier == null) request.frontier = new PathfindQueue(); + + //note: these are NOT cleared, it is assumed that this function cleans up after itself at the end + //is this a good idea? don't know, might hammer the GC with unnecessary objects too + var costs = request.costs; + var cameFrom = request.cameFrom; + var frontier = request.frontier; + + cameFrom.put(startNodeIndex, startNodeIndex); + costs.put(startNodeIndex, 0); + frontier.add(startNodeIndex, 0); + + boolean foundEnd = false; + + while(frontier.size > 0){ + int current = frontier.poll(); + + if(current == endNodeIndex){ + foundEnd = true; + break; + } + + int cluster = NodeIndex.cluster(current), dir = NodeIndex.dir(current), portal = NodeIndex.portal(current); + int cx = cluster % cwidth, cy = cluster / cwidth; + Cluster clust = getCreateCluster(team, pathCost, cluster); + LongSeq innerCons = clust.portalConnections[dir] == null || portal >= clust.portalConnections[dir].length ? null : clust.portalConnections[dir][portal]; + + //edges for the cluster the node is 'in' + if(innerCons != null){ + checkEdges(request, team, pathCost, current, endNodeIndex, cx, cy, innerCons); + } + + //edges that this node 'faces' from the other side + int nextCx = cx + Geometry.d4[dir].x, nextCy = cy + Geometry.d4[dir].y; + if(nextCx >= 0 && nextCy >= 0 && nextCx < cwidth && nextCy < cheight){ + Cluster nextCluster = getCreateCluster(team, pathCost, nextCx, nextCy); + int relativeDir = (dir + 2) % 4; + LongSeq outerCons = nextCluster.portalConnections[relativeDir] == null ? null : nextCluster.portalConnections[relativeDir][portal]; + if(outerCons != null){ + checkEdges(request, team, pathCost, current, endNodeIndex, nextCx, nextCy, outerCons); + } + } + } + + //null them out, so they get GC'ed later + //there's no reason to keep them around and waste memory, since this path may never be recalculated + request.costs = null; + request.cameFrom = null; + request.frontier = null; + + if(foundEnd){ + result.clear(); + + int cur = endNodeIndex; + while(cur != startNodeIndex){ + result.add(cur); + cur = cameFrom.get(cur); + } + + result.reverse(); + + return result; + } + return null; + } + + private void checkEdges(PathRequest request, int team, int pathCost, int current, int goal, int cx, int cy, LongSeq connections){ + for(int i = 0; i < connections.size; i++){ + long con = connections.items[i]; + float cost = IntraEdge.cost(con); + int otherDir = IntraEdge.dir(con), otherPortal = IntraEdge.portal(con); + int next = makeNodeIndex(cx, cy, otherDir, otherPortal); + + float newCost = request.costs.get(current) + cost; + + if(newCost < request.costs.get(next, Float.POSITIVE_INFINITY)){ + request.costs.put(next, newCost); + + request.frontier.add(next, newCost + clusterNodeHeuristic(team, pathCost, next, goal)); + request.cameFrom.put(next, current); + } + } + } + + private void updateFields(FieldCache cache, long nsToRun){ + var frontier = cache.frontier; + var fields = cache.fields; + var goalPos = cache.goalPos; + var pcost = cache.cost; + var team = cache.team; + + long start = Time.nanos(); + int counter = 0; + + //actually do the flow field part + while(frontier.size > 0){ + int tile = frontier.removeLast(); + int baseX = tile % wwidth, baseY = tile / wwidth; + int curWeightIndex = (baseX / clusterSize) + (baseY / clusterSize) * cwidth; + + //TODO: how can this be null??? serious problem! + int[] curWeights = fields.get(curWeightIndex); + if(curWeights == null) continue; + + int cost = curWeights[baseX % clusterSize + ((baseY % clusterSize) * clusterSize)]; + + if(cost != impassable){ + for(Point2 point : Geometry.d4){ + + int + dx = baseX + point.x, dy = baseY + point.y, + clx = dx / clusterSize, cly = dy / clusterSize; + + if(clx < 0 || cly < 0 || dx >= wwidth || dy >= wheight) continue; + + int nextWeightIndex = clx + cly * cwidth; + + int[] weights = nextWeightIndex == curWeightIndex ? curWeights : fields.get(nextWeightIndex); + + //out of bounds; not allowed to move this way because no weights were registered here + if(weights == null) continue; + + int newPos = tile + point.x + point.y * wwidth; + + //can't move back to the goal + if(newPos == goalPos) continue; + + if(dx - clx * clusterSize < 0 || dy - cly * clusterSize < 0) continue; + + int newPosArray = (dx - clx * clusterSize) + (dy - cly * clusterSize) * clusterSize; + + int otherCost = pcost.getCost(team, pathfinder.tiles[newPos]); + int oldCost = weights[newPosArray]; + + //a cost of 0 means uninitialized, OR it means we're at the goal position, but that's handled above + if((oldCost == 0 || oldCost > cost + otherCost) && otherCost != impassable){ + frontier.addFirst(newPos); + weights[newPosArray] = cost + otherCost; + } + } + } + + //every N iterations, check the time spent - this prevents extra calls to nano time, which itself is slow + if(nsToRun >= 0 && (counter++) >= updateStepInterval){ + counter = 0; + if(Time.timeSinceNanos(start) >= nsToRun){ + return; + } + } + } + } + + private void addFlowCluster(FieldCache cache, int cluster, boolean addingFrontier){ + addFlowCluster(cache, cluster % cwidth, cluster / cwidth, addingFrontier); + } + + private void addFlowCluster(FieldCache cache, int cx, int cy, boolean addingFrontier){ + //out of bounds + if(cx < 0 || cy < 0 || cx >= cwidth || cy >= cheight) return; + + var fields = cache.fields; + int key = cx + cy * cwidth; + + if(!fields.containsKey(key)){ + fields.put(key, new int[clusterSize * clusterSize]); + + if(addingFrontier){ + for(int dir = 0; dir < 4; dir++){ + int ox = cx + nextOffsets[dir * 2], oy = cy + nextOffsets[dir * 2 + 1]; + + if(ox < 0 || oy < 0 || ox >= cwidth || ox >= cheight) continue; + + var otherField = cache.fields.get(ox + oy * cwidth); + + if(otherField == null) continue; + + int + relOffset = (dir + 2) % 4, + movex = moveDirs[relOffset * 2], + movey = moveDirs[relOffset * 2 + 1], + otherx1 = offsets[relOffset * 2] * (clusterSize - 1), + othery1 = offsets[relOffset * 2 + 1] * (clusterSize - 1); + + //scan the edge of the cluster + for(int i = 0; i < clusterSize; i++){ + int x = otherx1 + movex * i, y = othery1 + movey * i; + + //check to make sure it's not 0 (uninitialized flowfield data) + if(otherField[x + y * clusterSize] > 0){ + int worldX = x + ox * clusterSize, worldY = y + oy * clusterSize; + + //add the world-relative position to the frontier, so it recalculates + cache.frontier.addFirst(worldX + worldY * wwidth); + + if(showDebug){ + Core.app.post(() -> Fx.placeBlock.at(worldX *tilesize, worldY * tilesize, 1f)); + } + } + } + } + } + } + } + + private void initializePathRequest(PathRequest request, int team, int costId, int unitX, int unitY, int goalX, int goalY){ + PathCost pcost = idToCost(costId); + + int goalPos = (goalX + goalY * wwidth); + + int node = findClosestNode(team, costId, unitX, unitY); + int dest = findClosestNode(team, costId, goalX, goalY); + + if(dest == Integer.MAX_VALUE){ + request.notFound = true; + //no node found (TODO: invalid state??) + return; + } + + var nodePath = clusterAstar(request, costId, node, dest); + + FieldCache cache = fields.get(Pack.longInt(goalPos, costId)); + //if true, extra values are added on the sides of existing field cells that face new cells. + boolean addingFrontier = true; + + //create the cache if it doesn't exist, and initialize it + if(cache == null){ + cache = new FieldCache(pcost, costId, team, goalPos); + fields.put(cache.mapKey, cache); + FieldCache fcache = cache; + //register field in main thread for iteration + Core.app.post(() -> fieldList.add(fcache)); + cache.frontier.addFirst(goalPos); + addingFrontier = false; //when it's a new field, there is no need to add to the frontier to merge the flowfield + } + + if(nodePath != null){ + int cx = unitX / clusterSize, cy = unitY / clusterSize; + + addFlowCluster(cache, cx, cy, addingFrontier); + + for(int i = -1; i < nodePath.size; i++){ + int + current = i == -1 ? node : nodePath.items[i], + cluster = NodeIndex.cluster(current), + dir = NodeIndex.dir(current), + dx = Geometry.d4[dir].x, + dy = Geometry.d4[dir].y, + ox = cluster % cwidth + dx, + oy = cluster / cwidth + dy; + + addFlowCluster(cache, cluster, addingFrontier); + + //store directional/flipped version of cluster + if(ox >= 0 && oy >= 0 && ox < cwidth && oy < cheight){ + int other = ox + oy * cwidth; + + addFlowCluster(cache, other, addingFrontier); + } + } + } + } + + private PathCost idToCost(int costId){ + return ControlPathfinder.costTypes.get(costId); } public static boolean isNearObstacle(Unit unit, int x1, int y1, int x2, int y2){ return raycast(unit.team().id, unit.type.pathCost, x1, y1, x2, y2); } + @Deprecated + public int nextTargetId(){ + return 0; + } + + @Deprecated + public boolean getPathPosition(Unit unit, int pathId, Vec2 destination, Vec2 out){ + return getPathPosition(unit, pathId, destination, out, null); + } + + @Deprecated + public boolean getPathPosition(Unit unit, int pathId, Vec2 destination, Vec2 out, @Nullable boolean[] noResultFound){ + return getPathPosition(unit, destination, destination, out, noResultFound); + } + + public boolean getPathPosition(Unit unit, Vec2 destination, Vec2 mainDestination, Vec2 out, @Nullable boolean[] noResultFound){ + int costId = unit.type.pathCostId; + PathCost cost = idToCost(costId); + + int + team = unit.team.id, + tileX = unit.tileX(), + tileY = unit.tileY(), + packedPos = world.packArray(tileX, tileY), + destX = World.toTile(mainDestination.x), + destY = World.toTile(mainDestination.y), + actualDestX = World.toTile(destination.x), + actualDestY = World.toTile(destination.y), + destPos = destX + destY * wwidth; + + PathRequest request = unitRequests.get(unit); + + unit.hitboxTile(Tmp.r3); + //tile rect size has tile size factored in, since the ray cannot have thickness + float tileRectSize = tilesize + Tmp.r3.height; + + int lastRaycastTile = request == null || world.tileChanges != request.lastWorldUpdate ? -1 : request.lastRaycastTile; + boolean raycastResult = request != null && request.lastRaycastResult; + + //cache raycast results to run every time the world updates, and every tile the unit crosses + if(lastRaycastTile != packedPos){ + //near the destination, standard raycasting tends to break down, so use the more permissive 'near' variant that doesn't take into account edges of walls + raycastResult = unit.within(destination, tilesize * 2.5f) ? !raycastRect(unit.x, unit.y, destination.x, destination.y, team, cost, tileX, tileY, actualDestX, actualDestY, tileRectSize) : !raycast(team, cost, tileX, tileY, actualDestX, actualDestY); + + if(request != null){ + request.lastRaycastTile = packedPos; + request.lastRaycastResult = raycastResult; + request.lastWorldUpdate = world.tileChanges; + } + } + + //if the destination can be trivially reached in a straight line, do that. + if(raycastResult){ + out.set(destination); + return true; + } + + boolean any = false; + + long fieldKey = Pack.longInt(destPos, costId); + + //use existing request if it exists. + if(request != null && request.destination == destPos){ + request.lastUpdateId = state.updateId; + + Tile tileOn = unit.tileOn(), initialTileOn = tileOn; + //TODO: should fields be accessible from this thread? + FieldCache fieldCache = fields.get(fieldKey); + + if(fieldCache != null && tileOn != null){ + FieldCache old = request.oldCache; + //nullify the old field to be GCed, as it cannot be relevant anymore (this path is complete) + if(fieldCache.frontier.isEmpty() && old != null){ + request.oldCache = null; + } + + fieldCache.lastUpdateId = state.updateId; + int maxIterations = 30; //TODO higher/lower number? is this still too slow? + int i = 0; + boolean recalc = false; + + //TODO last pos can change if the flowfield changes. + if(initialTileOn.pos() != request.lastTile || request.lastTargetTile == null){ + boolean anyNearSolid = false; + + //find the next tile until one near a solid block is discovered + while(i ++ < maxIterations){ + int value = getCost(fieldCache, old, tileOn.x, tileOn.y); + + Tile current = null; + int minCost = 0; + for(int dir = 0; dir < 4; dir ++){ + Point2 point = Geometry.d4[dir]; + int dx = tileOn.x + point.x, dy = tileOn.y + point.y; + + Tile other = world.tile(dx, dy); + + if(other == null) continue; + + int packed = world.packArray(dx, dy); + int otherCost = getCost(fieldCache, old, dx, dy), relCost = otherCost - value; + + if(relCost > 2 || otherCost <= 0){ + anyNearSolid = true; + } + + if((value == 0 || otherCost < value) && otherCost != impassable && (otherCost != 0 || packed == destPos) && (current == null || otherCost < minCost) && passable(unit.team.id, cost, packed)){ + current = other; + minCost = otherCost; + } + } + + //TODO raycast spam = extremely slow + //...flowfield integration spam is also really slow. + if(!(current == null || (costId == costIdGround && current.dangerous() && !tileOn.dangerous()))){ + + //when anyNearSolid is false, no solid tiles have been encountered anywhere so far, so raycasting is a waste of time + if(anyNearSolid && !tileOn.dangerous() && raycastRect(unit.x, unit.y, current.x * tilesize, current.y * tilesize, team, cost, initialTileOn.x, initialTileOn.y, current.x, current.y, tileRectSize)){ + + //TODO this may be a mistake + if(tileOn == initialTileOn){ + recalc = true; + any = true; + } + + break; + }else{ + tileOn = current; + any = true; + + if(current.array() == destPos){ + break; + } + } + + }else{ + break; + } + } + + request.lastTargetTile = any ? tileOn : null; + if(showDebug && tileOn != null){ + Fx.placeBlock.at(tileOn.worldx(), tileOn.worldy(), 1); + } + } + + if(request.lastTargetTile != null){ + out.set(request.lastTargetTile); + request.lastTile = recalc ? -1 : initialTileOn.pos(); + return true; + } + } + }else if(request == null){ + + //queue new request. + unitRequests.put(unit, request = new PathRequest(unit, team, costId, destPos)); + + PathRequest f = request; + + //on the pathfinding thread: initialize the request + queue.post(() -> { + threadPathRequests.add(f); + recalculatePath(f); + }); + + out.set(destination); + + return true; + } + + if(noResultFound != null){ + noResultFound[0] = request.notFound; + } + return false; + } + + private void recalculatePath(PathRequest request){ + initializePathRequest(request, request.team, request.costId, request.unit.tileX(), request.unit.tileY(), request.destination % wwidth, request.destination / wwidth); + } + + private int getCost(FieldCache cache, FieldCache old, int x, int y){ + //fall back to the old flowfield when possible - it's best not to use partial results from the base cache + if(old != null){ + return getCost(old, x, y, false); + } + return getCost(cache, x, y, true); + } + + private int getCost(FieldCache cache, int x, int y, boolean requeue){ + int[] field = cache.fields.get(x / clusterSize + (y / clusterSize) * cwidth); + if(field == null){ + if(!requeue) return 0; + //request a new flow cluster if one wasn't found; this may be a spammed a bit, but the function will return early once it's created the first time + queue.post(() -> addFlowCluster(cache, x / clusterSize, y / clusterSize, true)); + return 0; + } + return field[(x % clusterSize) + (y % clusterSize) * clusterSize]; + } + private static boolean raycast(int team, PathCost type, int x1, int y1, int x2, int y2){ int ww = wwidth, wh = wheight; int x = x1, dx = Math.abs(x2 - x), sx = x < x2 ? 1 : -1; @@ -374,17 +1268,6 @@ public class ControlPathfinder{ if(avoid(team, type, x + y * wwidth)) return true; if(x == x2 && y == y2) return false; - //TODO no diagonals???? is this a good idea? - /* - //no diagonal ver - if(2 * err + dy > dx - 2 * err){ - err -= dy; - x += sx; - }else{ - err += dx; - y += sy; - }*/ - //diagonal ver e2 = 2 * err; if(e2 > -dy){ @@ -396,30 +1279,6 @@ public class ControlPathfinder{ err += dx; y += sy; } - - } - - return true; - } - - private static boolean permissiveRaycast(int team, PathCost type, int x1, int y1, int x2, int y2){ - int ww = wwidth, wh = wheight; - int x = x1, dx = Math.abs(x2 - x), sx = x < x2 ? 1 : -1; - int y = y1, dy = Math.abs(y2 - y), sy = y < y2 ? 1 : -1; - int err = dx - dy; - - while(x >= 0 && y >= 0 && x < ww && y < wh){ - if(solid(team, type, x + y * wwidth, true)) return true; - if(x == x2 && y == y2) return false; - - //no diagonals - if(2 * err + dy > dx - 2 * err){ - err -= dy; - x += sx; - }else{ - err += dx; - y += sy; - } } return true; @@ -449,22 +1308,65 @@ public class ControlPathfinder{ return 0; } - static boolean cast(int team, PathCost cost, int from, int to){ - return raycast(team, cost, from % wwidth, from / wwidth, to % wwidth, to / wwidth); + private static boolean overlap(int team, PathCost type, int x, int y, float startX, float startY, float endX, float endY, float rectSize){ + if(x < 0 || y < 0 || x >= wwidth || y >= wheight) return false; + if(!passable(team, type, x + y * wwidth)){ + return Intersector.intersectSegmentRectangleFast(startX, startY, endX, endY, x * tilesize - rectSize/2f, y * tilesize - rectSize/2f, rectSize, rectSize); + } + return false; } - private Tile tile(int pos){ - return world.tiles.geti(pos); + private static boolean raycastRect(float startX, float startY, float endX, float endY, int team, PathCost type, int x1, int y1, int x2, int y2, float rectSize){ + int ww = wwidth, wh = wheight; + int x = x1, dx = Math.abs(x2 - x), sx = x < x2 ? 1 : -1; + int y = y1, dy = Math.abs(y2 - y), sy = y < y2 ? 1 : -1; + int e2, err = dx - dy; + + while(x >= 0 && y >= 0 && x < ww && y < wh){ + if( + !passable(team, type, x + y * wwidth) || + overlap(team, type, x + 1, y, startX, startY, endX, endY, rectSize) || + overlap(team, type, x - 1, y, startX, startY, endX, endY, rectSize) || + overlap(team, type, x, y + 1, startX, startY, endX, endY, rectSize) || + overlap(team, type, x, y - 1, startX, startY, endX, endY, rectSize) + ) return true; + + if(x == x2 && y == y2) return false; + + //diagonal ver + e2 = 2 * err; + if(e2 > -dy){ + err -= dy; + x += sx; + } + + if(e2 < dx){ + err += dx; + y += sy; + } + } + + return true; } - //distance heuristic: manhattan - private static float heuristic(int a, int b){ - int x = a % wwidth, x2 = b % wwidth, y = a / wwidth, y2 = b / wwidth; - return Math.abs(x - x2) + Math.abs(y - y2); + private static boolean avoid(int team, PathCost type, int tilePos){ + int cost = cost(team, type, tilePos); + return cost == impassable || cost >= 2; } - private static int tcost(int team, PathCost cost, int tilePos){ - return cost.getCost(team, pathfinder.tiles[tilePos]); + private static boolean passable(int team, PathCost cost, int pos){ + int amount = cost.getCost(team, pathfinder.tiles[pos]); + //edge case: naval reports costs of 6000+ for non-liquids, even though they are not technically passable + return amount != impassable && !(cost == costNaval && amount >= 6000); + } + + private static boolean solid(int team, PathCost type, int x, int y){ + return x < 0 || y < 0 || x >= wwidth || y >= wheight || solid(team, type, x + y * wwidth, true); + } + + private static boolean solid(int team, PathCost type, int tilePos, boolean checkWall){ + int cost = cost(team, type, tilePos); + return cost == impassable || (checkWall && cost >= 6000); } private static int cost(int team, PathCost cost, int tilePos){ @@ -477,246 +1379,162 @@ public class ControlPathfinder{ return cost.getCost(team, pathfinder.tiles[tilePos]); } - private static boolean avoid(int team, PathCost type, int tilePos){ - int cost = cost(team, type, tilePos); - return cost == impassable || cost >= 2; - } + private void clusterChanged(int team, int pathCost, int cx, int cy){ + int index = cx + cy * cwidth; - private static boolean solid(int team, PathCost type, int tilePos, boolean checkWall){ - int cost = cost(team, type, tilePos); - return cost == impassable || (checkWall && cost >= 6000); - } - - private static float tileCost(int team, PathCost type, int a, int b){ - //currently flat cost - return cost(team, type, b); - } - - static class PathfindThread extends Thread{ - /** handles task scheduling on the update thread. */ - TaskQueue queue = new TaskQueue(); - /** pathfinding thread access only! */ - Seq requests = new Seq<>(); - /** volatile for access across threads */ - volatile int requestSize; - - public PathfindThread(String name){ - super(name); + for(var req : threadPathRequests){ + long mapKey = Pack.longInt(req.destination, pathCost); + var field = fields.get(mapKey); + if((field != null && field.fields.containsKey(index)) || req.notFound){ + invalidRequests.add(req); + } } - @Override - public void run(){ - while(true){ - //stop on client, no updating - if(net.client()) return; - try{ - if(state.isPlaying()){ - queue.run(); - requestSize = requests.size; + } - //total update time no longer than maxUpdate - for(var req : requests){ - //TODO this is flawed with many paths - req.update(maxUpdate / requests.size); + private void updateClustersComplete(int clusterIndex){ + for(int team = 0; team < clusters.length; team++){ + var dim1 = clusters[team]; + if(dim1 != null){ + for(int pathCost = 0; pathCost < dim1.length; pathCost++){ + var dim2 = dim1[pathCost]; + if(dim2 != null){ + var cluster = dim2[clusterIndex]; + if(cluster != null){ + updateCluster(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth); + clusterChanged(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth); + } + } + } + } + } + } + + private void updateClustersInner(int clusterIndex){ + for(int team = 0; team < clusters.length; team++){ + var dim1 = clusters[team]; + if(dim1 != null){ + for(int pathCost = 0; pathCost < dim1.length; pathCost++){ + var dim2 = dim1[pathCost]; + if(dim2 != null){ + var cluster = dim2[clusterIndex]; + if(cluster != null){ + updateInnerEdges(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth, cluster); + clusterChanged(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth); + } + } + } + } + } + } + + @Override + public void run(){ + long lastInvalidCheck = Time.millis() + invalidateCheckInterval; + + while(true){ + if(net.client()) return; + try{ + + + if(state.isPlaying()){ + queue.run(); + + clustersToUpdate.each(cluster -> { + updateClustersComplete(cluster); + + //just in case: don't redundantly update inner clusters after you've recalculated it entirely + clustersToInnerUpdate.remove(cluster); + }); + + clustersToInnerUpdate.each(cluster -> { + //only recompute the inner links + updateClustersInner(cluster); + }); + + clustersToInnerUpdate.clear(); + clustersToUpdate.clear(); + + //periodically check for invalidated paths + if(Time.timeSinceMillis(lastInvalidCheck) > invalidateCheckInterval){ + lastInvalidCheck = Time.millis(); + + var it = invalidRequests.iterator(); + while(it.hasNext()){ + var request = it.next(); + + //invalid request, ignore it + if(request.invalidated){ + it.remove(); + continue; + } + + long mapKey = Pack.longInt(request.destination, request.costId); + + var field = fields.get(mapKey); + + if(field != null){ + //it's only worth recalculating a path when the current frontier has finished; otherwise the unit will be following something incomplete. + if(field.frontier.isEmpty()){ + + //remove the field, to be recalculated next update one recalculatePath is processed + fields.remove(field.mapKey); + Core.app.post(() -> fieldList.remove(field)); + + //once the field is invalidated, make sure that all the requests that have it stored in their 'old' field, so units don't stutter during recalculations + for(var otherRequest : threadPathRequests){ + if(otherRequest.destination == request.destination){ + otherRequest.oldCache = field; + } + } + + //the recalculation is done next update, so multiple path requests in the same batch don't end up removing and recalculating the field multiple times. + queue.post(() -> recalculatePath(request)); + //it has been processed. + it.remove(); + } + }else{ //there's no field, presumably because a previous request already invalidated it. + queue.post(() -> recalculatePath(request)); + it.remove(); + } } } - try{ - Thread.sleep(updateInterval); - }catch(InterruptedException e){ - //stop looping when interrupted externally - return; + //each update time (not total!) no longer than maxUpdate + for(FieldCache cache : fields.values()){ + updateFields(cache, maxUpdate); } - }catch(Throwable e){ - //do not crash the pathfinding thread - Log.err(e); } + + try{ + Thread.sleep(updateInterval); + }catch(InterruptedException e){ + //stop looping when interrupted externally + return; + } + }catch(Throwable e){ + e.printStackTrace(); } } } - static class PathRequest{ - final PathfindThread thread; + @Struct + static class IntraEdgeStruct{ + @StructField(8) + int dir; + @StructField(8) + int portal; - volatile boolean done = false; - volatile boolean foundEnd = false; - volatile Unit unit; - volatile PathCost cost; - volatile int team; - volatile int lastWorldUpdate; - volatile boolean forcedRecalc; + float cost; + } - final Vec2 lastPos = new Vec2(); - float stuckTimer = 0f; - - final Vec2 destination = new Vec2(); - final Vec2 lastDestination = new Vec2(); - - //TODO only access on main thread?? - volatile int pathIndex; - - int rayPathIndex = -1; - IntSeq result = new IntSeq(); - volatile float raycastTimer; - - PathfindQueue frontier = new PathfindQueue(); - //node index -> node it came from - IntIntMap cameFrom = new IntIntMap(); - //node index -> total cost - IntFloatMap costs = new IntFloatMap(); - - int start, goal; - - long lastUpdateId; - long lastTime; - long forceRecalcTime; - - volatile int lastId, curId; - - public PathRequest(PathfindThread thread){ - this.thread = thread; - } - - public void forceRecalculate(){ - //keep it at 3 times/sec - if(Time.timeSinceMillis(forceRecalcTime) < 1000 / 3) return; - forcedRecalc = true; - forceRecalcTime = Time.millis(); - } - - void update(long maxUpdateNs){ - if(curId != lastId){ - clear(true); - } - lastId = curId; - - //re-do everything when world updates, but keep the old path around - if(forcedRecalc || (Time.timeSinceMillis(lastTime) > 1000 * 3 && (worldUpdateId != lastWorldUpdate || !destination.epsilonEquals(lastDestination, 2f)))){ - lastTime = Time.millis(); - lastWorldUpdate = worldUpdateId; - forcedRecalc = false; - clear(false); - } - - if(done) return; - - long ns = Time.nanos(); - int counter = 0; - - while(frontier.size > 0){ - int current = frontier.poll(); - - if(current == goal){ - foundEnd = true; - break; - } - - int cx = current % wwidth, cy = current / wwidth; - - for(Point2 point : Geometry.d4){ - int newx = cx + point.x, newy = cy + point.y; - int next = newx + wwidth * newy; - - if(newx >= wwidth || newy >= wheight || newx < 0 || newy < 0) continue; - - //in fallback mode, enemy walls are passable - if(tcost(team, cost, next) == impassable) continue; - - float add = tileCost(team, cost, current, next); - float currentCost = costs.get(current); - - if(add < 0) continue; - - //the cost can include an impassable enemy wall, so cap the cost if so and add the base cost instead - //essentially this means that any path with enemy walls will only count the walls once, preventing strange behavior like avoiding based on wall count - float newCost = currentCost >= wallImpassableCap && add >= wallImpassableCap ? currentCost + add - wallImpassableCap : currentCost + add; - - //a cost of 0 means "not set" - if(!costs.containsKey(next) || newCost < costs.get(next)){ - costs.put(next, newCost); - float priority = newCost + heuristic(next, goal); - frontier.add(next, priority); - cameFrom.put(next, current); - } - } - - //only check every N iterations to prevent nanoTime spam (slow) - if((counter ++) >= 100){ - counter = 0; - - //exit when out of time. - if(Time.timeSinceNanos(ns) > maxUpdateNs){ - return; - } - } - } - - lastTime = Time.millis(); - raycastTimer = 9999f; - result.clear(); - - pathIndex = 0; - rayPathIndex = -1; - - if(foundEnd){ - int cur = goal; - while(cur != start){ - result.add(cur); - cur = cameFrom.get(cur); - } - - result.reverse(); - - smoothPath(); - } - - //don't keep this around in memory, better to dump entirely - using clear() keeps around massive arrays for paths - frontier = new PathfindQueue(); - cameFrom = new IntIntMap(); - costs = new IntFloatMap(); - - done = true; - } - - void smoothPath(){ - int len = result.size; - if(len <= 2) return; - - int output = 1, input = 2; - - while(input < len){ - if(cast(team, cost, result.get(output - 1), result.get(input))){ - result.swap(output, input - 1); - output++; - } - input++; - } - - result.swap(output, input - 1); - result.size = output + 1; - } - - void clear(boolean resetCurrent){ - done = false; - - frontier = new PathfindQueue(20); - cameFrom.clear(); - costs.clear(); - - start = world.packArray(unit.tileX(), unit.tileY()); - goal = world.packArray(World.toTile(destination.x), World.toTile(destination.y)); - - cameFrom.put(start, start); - costs.put(start, 0); - - frontier.add(start, 0); - - foundEnd = false; - lastDestination.set(destination); - - if(resetCurrent){ - result.clear(); - } - } + @Struct + static class NodeIndexStruct{ + @StructField(22) + int cluster; + @StructField(2) + int dir; + @StructField(8) + int portal; } } diff --git a/core/src/mindustry/ai/HierarchyPathFinder.java b/core/src/mindustry/ai/HierarchyPathFinder.java deleted file mode 100644 index 7d43a2bdcd..0000000000 --- a/core/src/mindustry/ai/HierarchyPathFinder.java +++ /dev/null @@ -1,1471 +0,0 @@ -package mindustry.ai; - -import arc.*; -import arc.graphics.*; -import arc.graphics.g2d.*; -import arc.math.*; -import arc.math.geom.*; -import arc.struct.*; -import arc.util.*; -import mindustry.annotations.Annotations.*; -import mindustry.content.*; -import mindustry.core.*; -import mindustry.game.EventType.*; -import mindustry.game.*; -import mindustry.gen.*; -import mindustry.graphics.*; -import mindustry.world.*; - -import static mindustry.Vars.*; -import static mindustry.ai.Pathfinder.*; - -//https://webdocs.cs.ualberta.ca/~mmueller/ps/hpastar.pdf -//https://www.gameaipro.com/GameAIPro/GameAIPro_Chapter23_Crowd_Pathfinding_and_Steering_Using_Flow_Field_Tiles.pdf -public class HierarchyPathFinder implements Runnable{ - private static final long maxUpdate = Time.millisToNanos(12); - private static final int updateStepInterval = 200; - private static final int updateFPS = 30; - private static final int updateInterval = 1000 / updateFPS, invalidateCheckInterval = 1000; - - static final int clusterSize = 12; - - static final boolean debug = OS.hasProp("mindustry.debug"); - - static final int[] offsets = { - 1, 0, //right: bottom to top - 0, 1, //top: left to right - 0, 0, //left: bottom to top - 0, 0 //bottom: left to right - }; - - static final int[] moveDirs = { - 0, 1, - 1, 0, - 0, 1, - 1, 0 - }; - - static final int[] nextOffsets = { - 1, 0, - 0, 1, - -1, 0, - 0, -1 - }; - - //maps team -> pathCost -> flattened array of clusters in 2D - //(what about teams? different path costs?) - Cluster[][][] clusters; - - int cwidth, cheight; - - //temporarily used for resolving connections for intra-edges - IntSet usedEdges = new IntSet(); - //tasks to run on pathfinding thread - TaskQueue queue = new TaskQueue(); - - //individual requests based on unit - MAIN THREAD ONLY - ObjectMap unitRequests = new ObjectMap<>(); - - Seq threadPathRequests = new Seq<>(false); - - //TODO: very dangerous usage; - //TODO - it is accessed from the main thread - //TODO - it is written to on the pathfinding thread - //TODO - it does not include - //maps position in world in (x + y * width format) | type (bitpacked to long) to a cache of flow fields - LongMap fields = new LongMap<>(); - //MAIN THREAD ONLY - Seq fieldList = new Seq<>(false); - - //these are for inner edge A* (temporary!) - IntFloatMap innerCosts = new IntFloatMap(); - PathfindQueue innerFrontier = new PathfindQueue(); - - //ONLY modify on pathfinding thread. - IntSet clustersToUpdate = new IntSet(); - IntSet clustersToInnerUpdate = new IntSet(); - - //PATHFINDING THREAD - requests that should be recomputed - ObjectSet invalidRequests = new ObjectSet<>(); - - /** Current pathfinding thread */ - @Nullable Thread thread; - - //path requests are per-unit - static class PathRequest{ - final Unit unit; - final int destination, team, costId; - //resulting path of nodes - final IntSeq resultPath = new IntSeq(); - - //node index -> total cost - @Nullable IntFloatMap costs = new IntFloatMap(); - //node index (NodeIndex struct) -> node it came from TODO merge them, make properties of FieldCache? - @Nullable IntIntMap cameFrom = new IntIntMap(); - //frontier for A* - @Nullable PathfindQueue frontier = new PathfindQueue(); - - //main thread only! - long lastUpdateId = state.updateId; - - //both threads - volatile boolean notFound = false; - volatile boolean invalidated = false; - //old field assigned before everything was recomputed - @Nullable volatile FieldCache oldCache; - - boolean lastRaycastResult = false; - int lastRaycastTile, lastWorldUpdate; - int lastTile; - @Nullable Tile lastTargetTile; - - PathRequest(Unit unit, int team, int costId, int destination){ - this.unit = unit; - this.costId = costId; - this.team = team; - this.destination = destination; - } - } - - static class FieldCache{ - final PathCost cost; - final int costId; - final int team; - final int goalPos; - //frontier for flow fields - final IntQueue frontier = new IntQueue(); - //maps cluster index to field weights; 0 means uninitialized - final IntMap fields = new IntMap<>(); - final long mapKey; - - //main thread only! - long lastUpdateId = state.updateId; - - //TODO: how are the nodes merged? CAN they be merged? - - FieldCache(PathCost cost, int costId, int team, int goalPos){ - this.cost = cost; - this.team = team; - this.goalPos = goalPos; - this.costId = costId; - this.mapKey = Pack.longInt(goalPos, costId); - } - } - - static class Cluster{ - IntSeq[] portals = new IntSeq[4]; - //maps rotation + index of portal to list of IntraEdge objects - LongSeq[][] portalConnections = new LongSeq[4][]; - } - - public HierarchyPathFinder(){ - - Events.on(ResetEvent.class, event -> stop()); - - Events.on(WorldLoadEvent.class, event -> { - stop(); - - //TODO: can the pathfinding thread even see these? - unitRequests = new ObjectMap<>(); - fields = new LongMap<>(); - fieldList = new Seq<>(false); - - clusters = new Cluster[256][][]; - cwidth = Mathf.ceil((float)world.width() / clusterSize); - cheight = Mathf.ceil((float)world.height() / clusterSize); - - - start(); - }); - - Events.on(TileChangeEvent.class, e -> { - - e.tile.getLinkedTiles(t -> { - int x = t.x, y = t.y, mx = x % clusterSize, my = y % clusterSize, cx = x / clusterSize, cy = y / clusterSize, cluster = cx + cy * cwidth; - - //is at the edge of a cluster; this means the portals may have changed. - if(mx == 0 || my == 0 || mx == clusterSize - 1 || my == clusterSize - 1){ - - if(mx == 0) queueClusterUpdate(cx - 1, cy); //left - if(my == 0) queueClusterUpdate(cx, cy - 1); //bottom - if(mx == clusterSize - 1) queueClusterUpdate(cx + 1, cy); //right - if(my == clusterSize - 1) queueClusterUpdate(cx, cy + 1); //top - - queueClusterUpdate(cx, cy); - //TODO: recompute edge clusters too. - }else{ - //there is no need to recompute portals for block updates that are not on the edge. - queue.post(() -> clustersToInnerUpdate.add(cluster)); - } - }); - - //TODO: recalculate affected flow fields? or just all of them? how to reflow? - }); - - //invalidate paths - Events.run(Trigger.update, () -> { - for(var req : unitRequests.values()){ - //skipped N update -> drop it - if(req.lastUpdateId <= state.updateId - 10){ - req.invalidated = true; - //concurrent modification! - queue.post(() -> threadPathRequests.remove(req)); - Core.app.post(() -> unitRequests.remove(req.unit)); - } - } - - for(var field : fieldList){ - //skipped N update -> drop it - if(field.lastUpdateId <= state.updateId - 30){ - //make sure it's only modified on the main thread...? but what about calling get() on this thread?? - queue.post(() -> fields.remove(field.mapKey)); - Core.app.post(() -> fieldList.remove(field)); - } - } - }); - - if(debug){ - Events.run(Trigger.draw, () -> { - int team = player.team().id; - int cost = 0; - - Draw.draw(Layer.overlayUI, () -> { - Lines.stroke(1f); - - if(clusters[team] != null && clusters[team][cost] != null){ - for(int cx = 0; cx < cwidth; cx++){ - for(int cy = 0; cy < cheight; cy++){ - - var cluster = clusters[team][cost][cy * cwidth + cx]; - if(cluster != null){ - Lines.stroke(0.5f); - Draw.color(Color.gray); - Lines.stroke(1f); - - Lines.rect(cx * clusterSize * tilesize - tilesize/2f, cy * clusterSize * tilesize - tilesize/2f, clusterSize * tilesize, clusterSize * tilesize); - - - for(int d = 0; d < 4; d++){ - IntSeq portals = cluster.portals[d]; - if(portals != null){ - - for(int i = 0; i < portals.size; i++){ - int pos = portals.items[i]; - int from = Point2.x(pos), to = Point2.y(pos); - float width = tilesize * (Math.abs(from - to) + 1), height = tilesize; - - portalToVec(cluster, cx, cy, d, i, Tmp.v1); - - Draw.color(Color.brown); - Lines.ellipse(30, Tmp.v1.x, Tmp.v1.y, width / 2f, height / 2f, d * 90f - 90f); - - LongSeq connections = cluster.portalConnections[d] == null ? null : cluster.portalConnections[d][i]; - - if(connections != null){ - Draw.color(Color.forest); - for(int coni = 0; coni < connections.size; coni ++){ - long con = connections.items[coni]; - - portalToVec(cluster, cx, cy, IntraEdge.dir(con), IntraEdge.portal(con), Tmp.v2); - - float - x1 = Tmp.v1.x, y1 = Tmp.v1.y, - x2 = Tmp.v2.x, y2 = Tmp.v2.y; - Lines.line(x1, y1, x2, y2); - - } - } - } - } - } - } - } - } - } - - for(var fields : fieldList){ - try{ - for(var entry : fields.fields){ - int cx = entry.key % cwidth, cy = entry.key / cwidth; - for(int y = 0; y < clusterSize; y++){ - for(int x = 0; x < clusterSize; x++){ - int value = entry.value[x + y * clusterSize]; - Tmp.c1.a = 1f; - Lines.stroke(0.8f, Tmp.c1.fromHsv(value * 3f, 1f, 1f)); - Draw.alpha(0.5f); - Fill.square((x + cx * clusterSize) * tilesize, (y + cy * clusterSize) * tilesize, tilesize / 2f); - } - } - } - }catch(Exception ignored){} //probably has some concurrency issues when iterating but I don't care, this is for debugging - } - }); - - Draw.reset(); - }); - } - } - - void queueClusterUpdate(int cx, int cy){ - if(cx >= 0 && cy >= 0 && cx < cwidth && cy < cheight){ - queue.post(() -> clustersToUpdate.add(cx + cy * cwidth)); - } - } - - //debugging only! - void portalToVec(Cluster cluster, int cx, int cy, int direction, int portalIndex, Vec2 out){ - int pos = cluster.portals[direction].items[portalIndex]; - int from = Point2.x(pos), to = Point2.y(pos); - int addX = moveDirs[direction * 2], addY = moveDirs[direction * 2 + 1]; - float average = (from + to) / 2f; - - float - x = (addX * average + cx * clusterSize + offsets[direction * 2] * (clusterSize - 1) + nextOffsets[direction * 2] / 2f) * tilesize, - y = (addY * average + cy * clusterSize + offsets[direction * 2 + 1] * (clusterSize - 1) + nextOffsets[direction * 2 + 1] / 2f) * tilesize; - - out.set(x, y); - } - - /** Starts or restarts the pathfinding thread. */ - private void start(){ - stop(); - if(net.client()) return; - - thread = new Thread(this, "Control Pathfinder"); - thread.setPriority(Thread.MIN_PRIORITY); - thread.setDaemon(true); - thread.start(); - } - - /** Stops the pathfinding thread. */ - private void stop(){ - if(thread != null){ - thread.interrupt(); - thread = null; - } - queue.clear(); - } - - /** @return a cluster at coordinates; can be null if not cluster was created yet*/ - @Nullable Cluster getCluster(int team, int pathCost, int cx, int cy){ - return getCluster(team, pathCost, cx + cy * cwidth); - } - - /** @return a cluster at coordinates; can be null if not cluster was created yet*/ - @Nullable Cluster getCluster(int team, int pathCost, int clusterIndex){ - if(clusters == null) return null; - - Cluster[][] dim1 = clusters[team]; - - if(dim1 == null) return null; - - Cluster[] dim2 = dim1[pathCost]; - - if(dim2 == null) return null; - - return dim2[clusterIndex]; - } - - /** @return the cluster at specified coordinates; never null. */ - Cluster getCreateCluster(int team, int pathCost, int cx, int cy){ - return getCreateCluster(team, pathCost, cx + cy * cwidth); - } - - /** @return the cluster at specified coordinates; never null. */ - Cluster getCreateCluster(int team, int pathCost, int clusterIndex){ - Cluster result = getCluster(team, pathCost, clusterIndex); - if(result == null){ - return updateCluster(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth); - }else{ - return result; - } - } - - Cluster updateCluster(int team, int pathCost, int cx, int cy){ - //TODO: what if clusters are null for thread visibility reasons? - - Cluster[][] dim1 = clusters[team]; - - if(dim1 == null){ - dim1 = clusters[team] = new Cluster[Team.all.length][]; - } - - Cluster[] dim2 = dim1[pathCost]; - - if(dim2 == null){ - dim2 = dim1[pathCost] = new Cluster[cwidth * cheight]; - } - - Cluster cluster = dim2[cy * cwidth + cx]; - if(cluster == null){ - cluster = dim2[cy * cwidth + cx] = new Cluster(); - }else{ - //reset data - for(var p : cluster.portals){ - p.clear(); - } - } - - PathCost cost = idToCost(pathCost); - - for(int direction = 0; direction < 4; direction++){ - int otherX = cx + Geometry.d4x(direction), otherY = cy + Geometry.d4y(direction); - //out of bounds, no portals in this direction - if(otherX < 0 || otherY < 0 || otherX >= cwidth || otherY >= cheight){ - continue; - } - - Cluster other = dim2[otherX + otherY * cwidth]; - IntSeq portals; - - if(other == null){ - //create new portals at direction - portals = cluster.portals[direction] = new IntSeq(4); - }else{ - //share portals with the other cluster - portals = cluster.portals[direction] = other.portals[(direction + 2) % 4]; - - //clear the portals, they're being recalculated now - portals.clear(); - } - - int addX = moveDirs[direction * 2], addY = moveDirs[direction * 2 + 1]; - int - baseX = cx * clusterSize + offsets[direction * 2] * (clusterSize - 1), - baseY = cy * clusterSize + offsets[direction * 2 + 1] * (clusterSize - 1), - nextBaseX = baseX + Geometry.d4[direction].x, - nextBaseY = baseY + Geometry.d4[direction].y; - - int lastPortal = -1; - boolean prevSolid = true; - - for(int i = 0; i < clusterSize; i++){ - int x = baseX + addX * i, y = baseY + addY * i; - - //scan for portals - if(solid(team, cost, x, y) || solid(team, cost, nextBaseX + addX * i, nextBaseY + addY * i)){ - int previous = i - 1; - //hit a wall, create portals between the two points - if(!prevSolid && previous >= lastPortal){ - //portals are an inclusive range - portals.add(Point2.pack(previous, lastPortal)); - } - prevSolid = true; - }else{ - //empty area encountered, mark the location of portal start - if(prevSolid){ - lastPortal = i; - } - prevSolid = false; - } - } - - //at the end of the loop, close any un-initialized portals; this is copy pasted code - int previous = clusterSize - 1; - if(!prevSolid && previous >= lastPortal){ - //portals are an inclusive range - portals.add(Point2.pack(previous, lastPortal)); - } - } - - updateInnerEdges(team, cost, cx, cy, cluster); - - return cluster; - } - - void updateInnerEdges(int team, int cost, int cx, int cy, Cluster cluster){ - updateInnerEdges(team, idToCost(cost), cx, cy, cluster); - } - - void updateInnerEdges(int team, PathCost cost, int cx, int cy, Cluster cluster){ - int minX = cx * clusterSize, minY = cy * clusterSize, maxX = Math.min(minX + clusterSize - 1, wwidth - 1), maxY = Math.min(minY + clusterSize - 1, wheight - 1); - - usedEdges.clear(); - - //clear all connections, since portals changed, they need to be recomputed. - cluster.portalConnections = new LongSeq[4][]; - - for(int direction = 0; direction < 4; direction++){ - var portals = cluster.portals[direction]; - if(portals == null) continue; - - int addX = moveDirs[direction * 2], addY = moveDirs[direction * 2 + 1]; - - for(int i = 0; i < portals.size; i++){ - usedEdges.add(Point2.pack(direction, i)); - - int - portal = portals.items[i], - from = Point2.x(portal), to = Point2.y(portal), - average = (from + to) / 2, - x = (addX * average + cx * clusterSize + offsets[direction * 2] * (clusterSize - 1)), - y = (addY * average + cy * clusterSize + offsets[direction * 2 + 1] * (clusterSize - 1)); - - for(int otherDir = 0; otherDir < 4; otherDir++){ - var otherPortals = cluster.portals[otherDir]; - if(otherPortals == null) continue; - - for(int j = 0; j < otherPortals.size; j++){ - - if(!usedEdges.contains(Point2.pack(otherDir, j))){ - - int - other = otherPortals.items[j], - otherFrom = Point2.x(other), otherTo = Point2.y(other), - otherAverage = (otherFrom + otherTo) / 2, - ox = cx * clusterSize + offsets[otherDir * 2] * (clusterSize - 1), - oy = cy * clusterSize + offsets[otherDir * 2 + 1] * (clusterSize - 1), - otherX = (moveDirs[otherDir * 2] * otherAverage + ox), - otherY = (moveDirs[otherDir * 2 + 1] * otherAverage + oy); - - //duplicate portal; should never happen. - if(Point2.pack(x, y) == Point2.pack(otherX, otherY)){ - continue; - } - - float connectionCost = innerAstar( - team, cost, - minX, minY, maxX, maxY, - x + y * wwidth, - otherX + otherY * wwidth, - (moveDirs[otherDir * 2] * otherFrom + ox), - (moveDirs[otherDir * 2 + 1] * otherFrom + oy), - (moveDirs[otherDir * 2] * otherTo + ox), - (moveDirs[otherDir * 2 + 1] * otherTo + oy) - ); - - if(connectionCost != -1f){ - if(cluster.portalConnections[direction] == null) cluster.portalConnections[direction] = new LongSeq[cluster.portals[direction].size]; - if(cluster.portalConnections[otherDir] == null) cluster.portalConnections[otherDir] = new LongSeq[cluster.portals[otherDir].size]; - if(cluster.portalConnections[direction][i] == null) cluster.portalConnections[direction][i] = new LongSeq(8); - if(cluster.portalConnections[otherDir][j] == null) cluster.portalConnections[otherDir][j] = new LongSeq(8); - - //TODO: can there be duplicate edges?? - cluster.portalConnections[direction][i].add(IntraEdge.get(otherDir, j, connectionCost)); - cluster.portalConnections[otherDir][j].add(IntraEdge.get(direction, i, connectionCost)); - } - } - } - } - } - } - } - - //distance heuristic: manhattan - private static float heuristic(int a, int b){ - int x = a % wwidth, x2 = b % wwidth, y = a / wwidth, y2 = b / wwidth; - return Math.abs(x - x2) + Math.abs(y - y2); - } - - private static int tcost(int team, PathCost cost, int tilePos){ - return cost.getCost(team, pathfinder.tiles[tilePos]); - } - - private static float tileCost(int team, PathCost type, int a, int b){ - //currently flat cost - return cost(team, type, b); - } - - /** @return -1 if no path was found */ - float innerAstar(int team, PathCost cost, int minX, int minY, int maxX, int maxY, int startPos, int goalPos, int goalX1, int goalY1, int goalX2, int goalY2){ - var frontier = innerFrontier; - var costs = innerCosts; - - frontier.clear(); - costs.clear(); - - //TODO: this can be faster and more memory efficient by making costs a NxN array... probably? - costs.put(startPos, 0); - frontier.add(startPos, 0); - - if(goalX2 < goalX1){ - int tmp = goalX1; - goalX1 = goalX2; - goalX2 = tmp; - } - - if(goalY2 < goalY1){ - int tmp = goalY1; - goalY1 = goalY2; - goalY2 = tmp; - } - - while(frontier.size > 0){ - int current = frontier.poll(); - - int cx = current % wwidth, cy = current / wwidth; - - //found the goal (it's in the portal rectangle) - if((cx >= goalX1 && cy >= goalY1 && cx <= goalX2 && cy <= goalY2) || current == goalPos){ - return costs.get(current); - } - - for(Point2 point : Geometry.d4){ - int newx = cx + point.x, newy = cy + point.y; - int next = newx + wwidth * newy; - - if(newx > maxX || newy > maxY || newx < minX || newy < minY || tcost(team, cost, next) == impassable) continue; - - float add = tileCost(team, cost, current, next); - - if(add < 0) continue; - - float newCost = costs.get(current) + add; - - if(newCost < costs.get(next, Float.POSITIVE_INFINITY)){ - costs.put(next, newCost); - float priority = newCost + heuristic(next, goalPos); - frontier.add(next, priority); - } - } - } - - return -1f; - } - - int makeNodeIndex(int cx, int cy, int dir, int portal){ - //to make sure there's only one way to refer to each node, the direction must be 0 or 1 (referring to portals on the top or right edge) - - //direction can only be 2 if cluster X is 0 (left edge of map) - if(dir == 2 && cx != 0){ - dir = 0; - cx --; - } - - //direction can only be 3 if cluster Y is 0 (bottom edge of map) - if(dir == 3 && cy != 0){ - dir = 1; - cy --; - } - - return NodeIndex.get(cx + cy * cwidth, dir, portal); - } - - //uses A* to find the closest node index to specified coordinates - //this node is used in cluster A* - /** @return MAX_VALUE if no node is found */ - private int findClosestNode(int team, int pathCost, int tileX, int tileY){ - int cx = tileX / clusterSize, cy = tileY / clusterSize; - - if(cx < 0 || cy < 0 || cx >= cwidth || cy >= cheight){ - return Integer.MAX_VALUE; - } - - PathCost cost = idToCost(pathCost); - Cluster cluster = getCreateCluster(team, pathCost, cx, cy); - int minX = cx * clusterSize, minY = cy * clusterSize, maxX = Math.min(minX + clusterSize - 1, wwidth - 1), maxY = Math.min(minY + clusterSize - 1, wheight - 1); - - int bestPortalPair = Integer.MAX_VALUE; - float bestCost = Float.MAX_VALUE; - - //A* to every node, find the best one (I know there's a better algorithm for this, probably dijkstra) - for(int dir = 0; dir < 4; dir++){ - var portals = cluster.portals[dir]; - if(portals == null) continue; - - for(int j = 0; j < portals.size; j++){ - - int - other = portals.items[j], - otherFrom = Point2.x(other), otherTo = Point2.y(other), - otherAverage = (otherFrom + otherTo) / 2, - ox = cx * clusterSize + offsets[dir * 2] * (clusterSize - 1), - oy = cy * clusterSize + offsets[dir * 2 + 1] * (clusterSize - 1), - otherX = (moveDirs[dir * 2] * otherAverage + ox), - otherY = (moveDirs[dir * 2 + 1] * otherAverage + oy); - - float connectionCost = innerAstar( - team, cost, - minX, minY, maxX, maxY, - tileX + tileY * wwidth, - otherX + otherY * wwidth, - (moveDirs[dir * 2] * otherFrom + ox), - (moveDirs[dir * 2 + 1] * otherFrom + oy), - (moveDirs[dir * 2] * otherTo + ox), - (moveDirs[dir * 2 + 1] * otherTo + oy) - ); - - //better cost found, update and return - if(connectionCost != -1f && connectionCost < bestCost){ - bestPortalPair = Point2.pack(dir, j); - bestCost = connectionCost; - } - } - } - - if(bestPortalPair != Integer.MAX_VALUE){ - return makeNodeIndex(cx, cy, Point2.x(bestPortalPair), Point2.y(bestPortalPair)); - } - - - return Integer.MAX_VALUE; - } - - //distance heuristic: manhattan - private float clusterNodeHeuristic(int team, int pathCost, int nodeA, int nodeB){ - int - clusterA = NodeIndex.cluster(nodeA), - dirA = NodeIndex.dir(nodeA), - portalA = NodeIndex.portal(nodeA), - clusterB = NodeIndex.cluster(nodeB), - dirB = NodeIndex.dir(nodeB), - portalB = NodeIndex.portal(nodeB), - rangeA = getCreateCluster(team, pathCost, clusterA).portals[dirA].items[portalA], - rangeB = getCreateCluster(team, pathCost, clusterB).portals[dirB].items[portalB]; - - float - averageA = (Point2.x(rangeA) + Point2.y(rangeA)) / 2f, - x1 = (moveDirs[dirA * 2] * averageA + (clusterA % cwidth) * clusterSize + offsets[dirA * 2] * (clusterSize - 1) + nextOffsets[dirA * 2] / 2f), - y1 = (moveDirs[dirA * 2 + 1] * averageA + (clusterA / cwidth) * clusterSize + offsets[dirA * 2 + 1] * (clusterSize - 1) + nextOffsets[dirA * 2 + 1] / 2f), - - averageB = (Point2.x(rangeB) + Point2.y(rangeB)) / 2f, - x2 = (moveDirs[dirB * 2] * averageB + (clusterB % cwidth) * clusterSize + offsets[dirB * 2] * (clusterSize - 1) + nextOffsets[dirB * 2] / 2f), - y2 = (moveDirs[dirB * 2 + 1] * averageB + (clusterB / cwidth) * clusterSize + offsets[dirB * 2 + 1] * (clusterSize - 1) + nextOffsets[dirB * 2 + 1] / 2f); - - return Math.abs(x1 - x2) + Math.abs(y1 - y2); - } - - @Nullable IntSeq clusterAstar(PathRequest request, int pathCost, int startNodeIndex, int endNodeIndex){ - var result = request.resultPath; - - if(startNodeIndex == endNodeIndex){ - result.clear(); - result.add(startNodeIndex); - return result; - } - - var team = request.team; - - if(request.costs == null) request.costs = new IntFloatMap(); - if(request.cameFrom == null) request.cameFrom = new IntIntMap(); - if(request.frontier == null) request.frontier = new PathfindQueue(); - - //note: these are NOT cleared, it is assumed that this function cleans up after itself at the end - //is this a good idea? don't know, might hammer the GC with unnecessary objects too - var costs = request.costs; - var cameFrom = request.cameFrom; - var frontier = request.frontier; - - cameFrom.put(startNodeIndex, startNodeIndex); - costs.put(startNodeIndex, 0); - frontier.add(startNodeIndex, 0); - - boolean foundEnd = false; - - while(frontier.size > 0){ - int current = frontier.poll(); - - if(current == endNodeIndex){ - foundEnd = true; - break; - } - - int cluster = NodeIndex.cluster(current), dir = NodeIndex.dir(current), portal = NodeIndex.portal(current); - int cx = cluster % cwidth, cy = cluster / cwidth; - Cluster clust = getCreateCluster(team, pathCost, cluster); - LongSeq innerCons = clust.portalConnections[dir] == null || portal >= clust.portalConnections[dir].length ? null : clust.portalConnections[dir][portal]; - - //edges for the cluster the node is 'in' - if(innerCons != null){ - checkEdges(request, team, pathCost, current, endNodeIndex, cx, cy, innerCons); - } - - //edges that this node 'faces' from the other side - int nextCx = cx + Geometry.d4[dir].x, nextCy = cy + Geometry.d4[dir].y; - if(nextCx >= 0 && nextCy >= 0 && nextCx < cwidth && nextCy < cheight){ - Cluster nextCluster = getCreateCluster(team, pathCost, nextCx, nextCy); - int relativeDir = (dir + 2) % 4; - LongSeq outerCons = nextCluster.portalConnections[relativeDir] == null ? null : nextCluster.portalConnections[relativeDir][portal]; - if(outerCons != null){ - checkEdges(request, team, pathCost, current, endNodeIndex, nextCx, nextCy, outerCons); - } - } - } - - //null them out, so they get GC'ed later - //there's no reason to keep them around and waste memory, since this path may never be recalculated - request.costs = null; - request.cameFrom = null; - request.frontier = null; - - if(foundEnd){ - result.clear(); - - int cur = endNodeIndex; - while(cur != startNodeIndex){ - result.add(cur); - cur = cameFrom.get(cur); - } - - result.reverse(); - - return result; - } - return null; - } - - private void checkEdges(PathRequest request, int team, int pathCost, int current, int goal, int cx, int cy, LongSeq connections){ - for(int i = 0; i < connections.size; i++){ - long con = connections.items[i]; - float cost = IntraEdge.cost(con); - int otherDir = IntraEdge.dir(con), otherPortal = IntraEdge.portal(con); - int next = makeNodeIndex(cx, cy, otherDir, otherPortal); - - float newCost = request.costs.get(current) + cost; - - if(newCost < request.costs.get(next, Float.POSITIVE_INFINITY)){ - request.costs.put(next, newCost); - - request.frontier.add(next, newCost + clusterNodeHeuristic(team, pathCost, next, goal)); - request.cameFrom.put(next, current); - } - } - } - - private void updateFields(FieldCache cache, long nsToRun){ - var frontier = cache.frontier; - var fields = cache.fields; - var goalPos = cache.goalPos; - var pcost = cache.cost; - var team = cache.team; - - long start = Time.nanos(); - int counter = 0; - - //actually do the flow field part - while(frontier.size > 0){ - int tile = frontier.removeLast(); - int baseX = tile % wwidth, baseY = tile / wwidth; - int curWeightIndex = (baseX / clusterSize) + (baseY / clusterSize) * cwidth; - - //TODO: how can this be null??? serious problem! - int[] curWeights = fields.get(curWeightIndex); - if(curWeights == null) continue; - - int cost = curWeights[baseX % clusterSize + ((baseY % clusterSize) * clusterSize)]; - - if(cost != impassable){ - for(Point2 point : Geometry.d4){ - - int - dx = baseX + point.x, dy = baseY + point.y, - clx = dx / clusterSize, cly = dy / clusterSize; - - if(clx < 0 || cly < 0 || dx >= wwidth || dy >= wheight) continue; - - int nextWeightIndex = clx + cly * cwidth; - - int[] weights = nextWeightIndex == curWeightIndex ? curWeights : fields.get(nextWeightIndex); - - //out of bounds; not allowed to move this way because no weights were registered here - if(weights == null) continue; - - int newPos = tile + point.x + point.y * wwidth; - - //can't move back to the goal - if(newPos == goalPos) continue; - - if(dx - clx * clusterSize < 0 || dy - cly * clusterSize < 0) continue; - - int newPosArray = (dx - clx * clusterSize) + (dy - cly * clusterSize) * clusterSize; - - int otherCost = pcost.getCost(team, pathfinder.tiles[newPos]); - int oldCost = weights[newPosArray]; - - //a cost of 0 means uninitialized, OR it means we're at the goal position, but that's handled above - if((oldCost == 0 || oldCost > cost + otherCost) && otherCost != impassable){ - frontier.addFirst(newPos); - weights[newPosArray] = cost + otherCost; - } - } - } - - //every N iterations, check the time spent - this prevents extra calls to nano time, which itself is slow - if(nsToRun >= 0 && (counter++) >= updateStepInterval){ - counter = 0; - if(Time.timeSinceNanos(start) >= nsToRun){ - return; - } - } - } - } - - private void addFlowCluster(FieldCache cache, int cluster, boolean addingFrontier){ - addFlowCluster(cache, cluster % cwidth, cluster / cwidth, addingFrontier); - } - - private void addFlowCluster(FieldCache cache, int cx, int cy, boolean addingFrontier){ - //out of bounds - if(cx < 0 || cy < 0 || cx >= cwidth || cy >= cheight) return; - - var fields = cache.fields; - int key = cx + cy * cwidth; - - if(!fields.containsKey(key)){ - fields.put(key, new int[clusterSize * clusterSize]); - - if(addingFrontier){ - for(int dir = 0; dir < 4; dir++){ - int ox = cx + nextOffsets[dir * 2], oy = cy + nextOffsets[dir * 2 + 1]; - - if(ox < 0 || oy < 0 || ox >= cwidth || ox >= cheight) continue; - - var otherField = cache.fields.get(ox + oy * cwidth); - - if(otherField == null) continue; - - int - relOffset = (dir + 2) % 4, - movex = moveDirs[relOffset * 2], - movey = moveDirs[relOffset * 2 + 1], - otherx1 = offsets[relOffset * 2] * (clusterSize - 1), - othery1 = offsets[relOffset * 2 + 1] * (clusterSize - 1); - - //scan the edge of the cluster - for(int i = 0; i < clusterSize; i++){ - int x = otherx1 + movex * i, y = othery1 + movey * i; - - //check to make sure it's not 0 (uninitialized flowfield data) - if(otherField[x + y * clusterSize] > 0){ - int worldX = x + ox * clusterSize, worldY = y + oy * clusterSize; - - //add the world-relative position to the frontier, so it recalculates - cache.frontier.addFirst(worldX + worldY * wwidth); - - if(debug){ - Core.app.post(() -> Fx.placeBlock.at(worldX *tilesize, worldY * tilesize, 1f)); - } - } - } - } - } - } - } - - private void initializePathRequest(PathRequest request, int team, int costId, int unitX, int unitY, int goalX, int goalY){ - PathCost pcost = idToCost(costId); - - int goalPos = (goalX + goalY * wwidth); - - int node = findClosestNode(team, costId, unitX, unitY); - int dest = findClosestNode(team, costId, goalX, goalY); - - if(dest == Integer.MAX_VALUE){ - request.notFound = true; - //no node found (TODO: invalid state??) - return; - } - - var nodePath = clusterAstar(request, costId, node, dest); - - FieldCache cache = fields.get(Pack.longInt(goalPos, costId)); - //if true, extra values are added on the sides of existing field cells that face new cells. - boolean addingFrontier = true; - - //create the cache if it doesn't exist, and initialize it - if(cache == null){ - cache = new FieldCache(pcost, costId, team, goalPos); - fields.put(cache.mapKey, cache); - FieldCache fcache = cache; - //register field in main thread for iteration - Core.app.post(() -> fieldList.add(fcache)); - cache.frontier.addFirst(goalPos); - addingFrontier = false; //when it's a new field, there is no need to add to the frontier to merge the flowfield - } - - if(nodePath != null){ - int cx = unitX / clusterSize, cy = unitY / clusterSize; - - addFlowCluster(cache, cx, cy, addingFrontier); - - for(int i = -1; i < nodePath.size; i++){ - int - current = i == -1 ? node : nodePath.items[i], - cluster = NodeIndex.cluster(current), - dir = NodeIndex.dir(current), - dx = Geometry.d4[dir].x, - dy = Geometry.d4[dir].y, - ox = cluster % cwidth + dx, - oy = cluster / cwidth + dy; - - addFlowCluster(cache, cluster, addingFrontier); - - //store directional/flipped version of cluster - if(ox >= 0 && oy >= 0 && ox < cwidth && oy < cheight){ - int other = ox + oy * cwidth; - - addFlowCluster(cache, other, addingFrontier); - } - } - } - } - - private PathCost idToCost(int costId){ - return ControlPathfinder.costTypes.get(costId); - } - - public boolean getPathPosition(Unit unit, Vec2 destination, Vec2 mainDestination, Vec2 out, @Nullable boolean[] noResultFound){ - int costId = unit.type.pathCostId; - PathCost cost = idToCost(costId); - - int - team = unit.team.id, - tileX = unit.tileX(), - tileY = unit.tileY(), - packedPos = world.packArray(tileX, tileY), - destX = World.toTile(mainDestination.x), - destY = World.toTile(mainDestination.y), - actualDestX = World.toTile(destination.x), - actualDestY = World.toTile(destination.y), - destPos = destX + destY * wwidth; - - PathRequest request = unitRequests.get(unit); - - unit.hitboxTile(Tmp.r3); - //tile rect size has tile size factored in, since the ray cannot have thickness - float tileRectSize = tilesize + Tmp.r3.height; - - int lastRaycastTile = request == null || world.tileChanges != request.lastWorldUpdate ? -1 : request.lastRaycastTile; - boolean raycastResult = request != null && request.lastRaycastResult; - - //cache raycast results to run every time the world updates, and every tile the unit crosses - if(lastRaycastTile != packedPos){ - //near the destination, standard raycasting tends to break down, so use the more permissive 'near' variant that doesn't take into account edges of walls - raycastResult = unit.within(destination, tilesize * 2.5f) ? !raycastRect(unit.x, unit.y, destination.x, destination.y, team, cost, tileX, tileY, actualDestX, actualDestY, tileRectSize) : !raycast(team, cost, tileX, tileY, actualDestX, actualDestY); - - if(request != null){ - request.lastRaycastTile = packedPos; - request.lastRaycastResult = raycastResult; - request.lastWorldUpdate = world.tileChanges; - } - } - - //if the destination can be trivially reached in a straight line, do that. - if(raycastResult){ - out.set(destination); - return true; - } - - boolean any = false; - - long fieldKey = Pack.longInt(destPos, costId); - - //use existing request if it exists. - if(request != null && request.destination == destPos){ - request.lastUpdateId = state.updateId; - - Tile tileOn = unit.tileOn(), initialTileOn = tileOn; - //TODO: should fields be accessible from this thread? - FieldCache fieldCache = fields.get(fieldKey); - - if(fieldCache != null && tileOn != null){ - FieldCache old = request.oldCache; - //nullify the old field to be GCed, as it cannot be relevant anymore (this path is complete) - if(fieldCache.frontier.isEmpty() && old != null){ - request.oldCache = null; - } - - fieldCache.lastUpdateId = state.updateId; - int maxIterations = 30; //TODO higher/lower number? is this still too slow? - int i = 0; - boolean recalc = false; - - //TODO last pos can change if the flowfield changes. - if(initialTileOn.pos() != request.lastTile || request.lastTargetTile == null){ - boolean anyNearSolid = false; - - //find the next tile until one near a solid block is discovered - while(i ++ < maxIterations){ - int value = getCost(fieldCache, old, tileOn.x, tileOn.y); - - Tile current = null; - int minCost = 0; - for(int dir = 0; dir < 4; dir ++){ - Point2 point = Geometry.d4[dir]; - int dx = tileOn.x + point.x, dy = tileOn.y + point.y; - - Tile other = world.tile(dx, dy); - - if(other == null) continue; - - int packed = world.packArray(dx, dy); - int otherCost = getCost(fieldCache, old, dx, dy), relCost = otherCost - value; - - if(relCost > 2 || otherCost <= 0){ - anyNearSolid = true; - } - - if((value == 0 || otherCost < value) && otherCost != impassable && (otherCost != 0 || packed == destPos) && (current == null || otherCost < minCost) && passable(unit.team.id, cost, packed)){ - current = other; - minCost = otherCost; - } - } - - //TODO raycast spam = extremely slow - //...flowfield integration spam is also really slow. - if(!(current == null || (costId == costGround && current.dangerous() && !tileOn.dangerous()))){ - - //when anyNearSolid is false, no solid tiles have been encountered anywhere so far, so raycasting is a waste of time - if(anyNearSolid && !tileOn.dangerous() && raycastRect(unit.x, unit.y, current.x * tilesize, current.y * tilesize, team, cost, initialTileOn.x, initialTileOn.y, current.x, current.y, tileRectSize)){ - - //TODO this may be a mistake - if(tileOn == initialTileOn){ - recalc = true; - any = true; - } - - break; - }else{ - tileOn = current; - any = true; - - if(current.array() == destPos){ - break; - } - } - - }else{ - break; - } - } - - request.lastTargetTile = any ? tileOn : null; - if(debug && tileOn != null){ - Fx.placeBlock.at(tileOn.worldx(), tileOn.worldy(), 1); - } - } - - if(request.lastTargetTile != null){ - out.set(request.lastTargetTile); - request.lastTile = recalc ? -1 : initialTileOn.pos(); - return true; - } - } - }else if(request == null){ - - //queue new request. - unitRequests.put(unit, request = new PathRequest(unit, team, costId, destPos)); - - PathRequest f = request; - - //on the pathfinding thread: initialize the request - queue.post(() -> { - threadPathRequests.add(f); - recalculatePath(f); - }); - - out.set(destination); - - return true; - } - - if(noResultFound != null){ - noResultFound[0] = request.notFound; - } - return false; - } - - private void recalculatePath(PathRequest request){ - initializePathRequest(request, request.team, request.costId, request.unit.tileX(), request.unit.tileY(), request.destination % wwidth, request.destination / wwidth); - } - - private int getCost(FieldCache cache, FieldCache old, int x, int y){ - //fall back to the old flowfield when possible - it's best not to use partial results from the base cache - if(old != null){ - return getCost(old, x, y, false); - } - return getCost(cache, x, y, true); - } - - private int getCost(FieldCache cache, int x, int y, boolean requeue){ - int[] field = cache.fields.get(x / clusterSize + (y / clusterSize) * cwidth); - if(field == null){ - if(!requeue) return 0; - //request a new flow cluster if one wasn't found; this may be a spammed a bit, but the function will return early once it's created the first time - queue.post(() -> addFlowCluster(cache, x / clusterSize, y / clusterSize, true)); - return 0; - } - return field[(x % clusterSize) + (y % clusterSize) * clusterSize]; - } - - private static boolean raycast(int team, PathCost type, int x1, int y1, int x2, int y2){ - int ww = wwidth, wh = wheight; - int x = x1, dx = Math.abs(x2 - x), sx = x < x2 ? 1 : -1; - int y = y1, dy = Math.abs(y2 - y), sy = y < y2 ? 1 : -1; - int e2, err = dx - dy; - - while(x >= 0 && y >= 0 && x < ww && y < wh){ - if(avoid(team, type, x + y * wwidth)) return true; - if(x == x2 && y == y2) return false; - - //diagonal ver - e2 = 2 * err; - if(e2 > -dy){ - err -= dy; - x += sx; - } - - if(e2 < dx){ - err += dx; - y += sy; - } - } - - return true; - } - - private static boolean raycastNear(int team, PathCost type, int x1, int y1, int x2, int y2){ - int ww = wwidth, wh = wheight; - int x = x1, dx = Math.abs(x2 - x), sx = x < x2 ? 1 : -1; - int y = y1, dy = Math.abs(y2 - y), sy = y < y2 ? 1 : -1; - int err = dx - dy; - - - while(x >= 0 && y >= 0 && x < ww && y < wh){ - if(!passable(team, type, x + y * wwidth)) return true; - if(x == x2 && y == y2) return false; - - //no diagonal ver - if(2 * err + dy > dx - 2 * err){ - err -= dy; - x += sx; - }else{ - err += dx; - y += sy; - } - - } - - return true; - } - - private static boolean overlap(int team, PathCost type, int x, int y, float startX, float startY, float endX, float endY, float rectSize){ - if(x < 0 || y < 0 || x >= wwidth || y >= wheight) return false; - if(!passable(team, type, x + y * wwidth)){ - return Intersector.intersectSegmentRectangleFast(startX, startY, endX, endY, x * tilesize - rectSize/2f, y * tilesize - rectSize/2f, rectSize, rectSize); - } - return false; - } - - private static boolean raycastRect(float startX, float startY, float endX, float endY, int team, PathCost type, int x1, int y1, int x2, int y2, float rectSize){ - int ww = wwidth, wh = wheight; - int x = x1, dx = Math.abs(x2 - x), sx = x < x2 ? 1 : -1; - int y = y1, dy = Math.abs(y2 - y), sy = y < y2 ? 1 : -1; - int e2, err = dx - dy; - - while(x >= 0 && y >= 0 && x < ww && y < wh){ - if( - !passable(team, type, x + y * wwidth) || - overlap(team, type, x + 1, y, startX, startY, endX, endY, rectSize) || - overlap(team, type, x - 1, y, startX, startY, endX, endY, rectSize) || - overlap(team, type, x, y + 1, startX, startY, endX, endY, rectSize) || - overlap(team, type, x, y - 1, startX, startY, endX, endY, rectSize) - ) return true; - - if(x == x2 && y == y2) return false; - - //diagonal ver - e2 = 2 * err; - if(e2 > -dy){ - err -= dy; - x += sx; - } - - if(e2 < dx){ - err += dx; - y += sy; - } - } - - return true; - } - - private static boolean avoid(int team, PathCost type, int tilePos){ - int cost = cost(team, type, tilePos); - return cost == impassable || cost >= 2; - } - - private static boolean passable(int team, PathCost cost, int pos){ - int amount = cost.getCost(team, pathfinder.tiles[pos]); - //edge case: naval reports costs of 6000+ for non-liquids, even though they are not technically passable - return amount != impassable && !(cost == costTypes.get(costNaval) && amount >= 6000); - } - - private static boolean solid(int team, PathCost type, int x, int y){ - return x < 0 || y < 0 || x >= wwidth || y >= wheight || solid(team, type, x + y * wwidth, true); - } - - private static boolean solid(int team, PathCost type, int tilePos, boolean checkWall){ - int cost = cost(team, type, tilePos); - return cost == impassable || (checkWall && cost >= 6000); - } - - private static int cost(int team, PathCost cost, int tilePos){ - if(state.rules.limitMapArea && !Team.get(team).isAI()){ - int x = tilePos % wwidth, y = tilePos / wwidth; - if(x < state.rules.limitX || y < state.rules.limitY || x > state.rules.limitX + state.rules.limitWidth || y > state.rules.limitY + state.rules.limitHeight){ - return impassable; - } - } - return cost.getCost(team, pathfinder.tiles[tilePos]); - } - - private void clusterChanged(int team, int pathCost, int cx, int cy){ - int index = cx + cy * cwidth; - - for(var req : threadPathRequests){ - long mapKey = Pack.longInt(req.destination, pathCost); - var field = fields.get(mapKey); - if((field != null && field.fields.containsKey(index)) || req.notFound){ - invalidRequests.add(req); - } - } - - } - - private void updateClustersComplete(int clusterIndex){ - for(int team = 0; team < clusters.length; team++){ - var dim1 = clusters[team]; - if(dim1 != null){ - for(int pathCost = 0; pathCost < dim1.length; pathCost++){ - var dim2 = dim1[pathCost]; - if(dim2 != null){ - var cluster = dim2[clusterIndex]; - if(cluster != null){ - updateCluster(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth); - clusterChanged(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth); - } - } - } - } - } - } - - private void updateClustersInner(int clusterIndex){ - for(int team = 0; team < clusters.length; team++){ - var dim1 = clusters[team]; - if(dim1 != null){ - for(int pathCost = 0; pathCost < dim1.length; pathCost++){ - var dim2 = dim1[pathCost]; - if(dim2 != null){ - var cluster = dim2[clusterIndex]; - if(cluster != null){ - updateInnerEdges(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth, cluster); - clusterChanged(team, pathCost, clusterIndex % cwidth, clusterIndex / cwidth); - } - } - } - } - } - } - - @Override - public void run(){ - long lastInvalidCheck = Time.millis() + invalidateCheckInterval; - - while(true){ - if(net.client()) return; - try{ - - - if(state.isPlaying()){ - queue.run(); - - clustersToUpdate.each(cluster -> { - updateClustersComplete(cluster); - - //just in case: don't redundantly update inner clusters after you've recalculated it entirely - clustersToInnerUpdate.remove(cluster); - }); - - clustersToInnerUpdate.each(cluster -> { - //only recompute the inner links - updateClustersInner(cluster); - }); - - clustersToInnerUpdate.clear(); - clustersToUpdate.clear(); - - //periodically check for invalidated paths - if(Time.timeSinceMillis(lastInvalidCheck) > invalidateCheckInterval){ - lastInvalidCheck = Time.millis(); - - var it = invalidRequests.iterator(); - while(it.hasNext()){ - var request = it.next(); - - //invalid request, ignore it - if(request.invalidated){ - it.remove(); - continue; - } - - long mapKey = Pack.longInt(request.destination, request.costId); - - var field = fields.get(mapKey); - - if(field != null){ - //it's only worth recalculating a path when the current frontier has finished; otherwise the unit will be following something incomplete. - if(field.frontier.isEmpty()){ - - //remove the field, to be recalculated next update one recalculatePath is processed - fields.remove(field.mapKey); - Core.app.post(() -> fieldList.remove(field)); - - //once the field is invalidated, make sure that all the requests that have it stored in their 'old' field, so units don't stutter during recalculations - for(var otherRequest : threadPathRequests){ - if(otherRequest.destination == request.destination){ - otherRequest.oldCache = field; - } - } - - //the recalculation is done next update, so multiple path requests in the same batch don't end up removing and recalculating the field multiple times. - queue.post(() -> recalculatePath(request)); - //it has been processed. - it.remove(); - } - }else{ //there's no field, presumably because a previous request already invalidated it. - queue.post(() -> recalculatePath(request)); - it.remove(); - } - } - } - - //each update time (not total!) no longer than maxUpdate - for(FieldCache cache : fields.values()){ - updateFields(cache, maxUpdate); - } - } - - try{ - Thread.sleep(updateInterval); - }catch(InterruptedException e){ - //stop looping when interrupted externally - return; - } - }catch(Throwable e){ - e.printStackTrace(); - } - } - } - - @Struct - static class IntraEdgeStruct{ - @StructField(8) - int dir; - @StructField(8) - int portal; - - float cost; - } - - @Struct - static class NodeIndexStruct{ - @StructField(22) - int cluster; - @StructField(2) - int dir; - @StructField(8) - int portal; - } -} diff --git a/core/src/mindustry/ai/types/CommandAI.java b/core/src/mindustry/ai/types/CommandAI.java index 07a30878b1..dec38c14ed 100644 --- a/core/src/mindustry/ai/types/CommandAI.java +++ b/core/src/mindustry/ai/types/CommandAI.java @@ -4,7 +4,6 @@ import arc.math.*; import arc.math.geom.*; import arc.struct.*; import arc.util.*; -import mindustry.*; import mindustry.ai.*; import mindustry.core.*; import mindustry.entities.*; @@ -35,7 +34,6 @@ public class CommandAI extends AIController{ protected boolean stopAtTarget, stopWhenInRange; protected Vec2 lastTargetPos; - protected int pathId = -1; protected boolean blockingUnit; protected float timeSpentBlocked; @@ -251,7 +249,7 @@ public class CommandAI extends AIController{ timeSpentBlocked = 0f; } - move = hpath.getPathPosition(unit, vecMovePos, targetPos, vecOut, noFound) && (!blockingUnit || timeSpentBlocked > maxBlockTime); + move = controlPath.getPathPosition(unit, vecMovePos, targetPos, vecOut, noFound) && (!blockingUnit || timeSpentBlocked > maxBlockTime); //rare case where unit must be perfectly aligned (happens with 1-tile gaps) alwaysArrive = vecOut.epsilonEquals(unit.tileX() * tilesize, unit.tileY() * tilesize); //we've reached the final point if the returned coordinate is equal to the supplied input @@ -421,7 +419,6 @@ public class CommandAI extends AIController{ //this is an allocation, but it's relatively rarely called anyway, and outside mutations must be prevented targetPos = lastTargetPos = pos.cpy(); attackTarget = null; - pathId = Vars.controlPath.nextTargetId(); this.stopWhenInRange = stopWhenInRange; } @@ -436,7 +433,6 @@ public class CommandAI extends AIController{ public void commandTarget(Teamc moveTo, boolean stopAtTarget){ attackTarget = moveTo; this.stopAtTarget = stopAtTarget; - pathId = Vars.controlPath.nextTargetId(); } /* diff --git a/core/src/mindustry/ai/types/LogicAI.java b/core/src/mindustry/ai/types/LogicAI.java index 2ac8840d3f..9486ebea42 100644 --- a/core/src/mindustry/ai/types/LogicAI.java +++ b/core/src/mindustry/ai/types/LogicAI.java @@ -85,7 +85,7 @@ public class LogicAI extends AIController{ if(unit.isFlying()){ moveTo(Tmp.v1.set(moveX, moveY), 1f, 30f); }else{ - if(hpath.getPathPosition(unit, Tmp.v2.set(moveX, moveY), Tmp.v2, Tmp.v1, null)){ + if(controlPath.getPathPosition(unit, Tmp.v2.set(moveX, moveY), Tmp.v2, Tmp.v1, null)){ moveTo(Tmp.v1, 1f, Tmp.v2.epsilonEquals(Tmp.v1, 4.1f) ? 30f : 0f); } }