diff --git a/core/src/mindustry/ai/ControlPathfinder.java b/core/src/mindustry/ai/ControlPathfinder.java index cabb297270..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,13 +19,13 @@ import mindustry.world.*; import static mindustry.Vars.*; import static mindustry.ai.Pathfinder.*; -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) -> @@ -61,295 +63,1201 @@ public class ControlPathfinder{ ((PathTile.team(tile) != team && PathTile.team(tile) != 0) && PathTile.solid(tile) ? wallImpassableCap : 0) + (PathTile.nearGround(tile) || PathTile.nearSolid(tile) ? 6 : 0); - public static boolean showDebug = false; + public static final int + costIdGround = 0, + costIdHover = 1, + costIdLegs = 2, + costIdNaval = 3; - //static access probably faster than object access - static int wwidth, wheight; - //increments each tile change - static volatile int worldUpdateId; + public static final Seq costTypes = Seq.with( + costGround, + costHover, + costLegs, + costNaval + ); - /** Current pathfinding threads, contents may be null */ - @Nullable PathfindThread[] threads; - /** for unique target IDs */ - int lastTargetId = 1; - /** requests per-unit */ - ObjectMap requests = new ObjectMap<>(); + 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 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; @@ -360,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){ @@ -382,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; @@ -435,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){ @@ -463,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/RtsAI.java b/core/src/mindustry/ai/RtsAI.java index 6b37777c29..92025da099 100644 --- a/core/src/mindustry/ai/RtsAI.java +++ b/core/src/mindustry/ai/RtsAI.java @@ -77,6 +77,7 @@ public class RtsAI{ } public void update(){ + if(timer.get(timeUpdate, 60f * 2f)){ assignSquads(); checkBuilding(); diff --git a/core/src/mindustry/ai/types/CommandAI.java b/core/src/mindustry/ai/types/CommandAI.java index 1fc6a83a8d..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; @@ -205,6 +203,8 @@ public class CommandAI extends AIController{ } } + boolean alwaysArrive = false; + if(targetPos != null){ boolean move = true, isFinalPoint = commandQueue.size == 0; vecOut.set(targetPos); @@ -221,6 +221,7 @@ public class CommandAI extends AIController{ } if(unit.isGrounded() && stance != UnitStance.ram){ + //TODO: blocking enable or disable? if(timer.get(timerTarget3, avoidInterval)){ Vec2 dstPos = Tmp.v1.trns(unit.rotation, unit.hitSize/2f); float max = unit.hitSize/2f; @@ -248,8 +249,9 @@ public class CommandAI extends AIController{ timeSpentBlocked = 0f; } - //if you've spent 3 seconds stuck, something is wrong, move regardless - move = Vars.controlPath.getPathPosition(unit, pathId, vecMovePos, 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 isFinalPoint &= vecMovePos.epsilonEquals(vecOut, 4.1f); @@ -277,7 +279,7 @@ public class CommandAI extends AIController{ attackTarget != null && unit.within(attackTarget, engageRange) && stance != UnitStance.ram ? engageRange : unit.isGrounded() ? 0f : attackTarget != null && stance != UnitStance.ram ? engageRange : - 0f, unit.isFlying() ? 40f : 100f, false, null, isFinalPoint); + 0f, unit.isFlying() ? 40f : 100f, false, null, isFinalPoint || alwaysArrive); } } @@ -417,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; } @@ -432,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 d334f9dec1..9486ebea42 100644 --- a/core/src/mindustry/ai/types/LogicAI.java +++ b/core/src/mindustry/ai/types/LogicAI.java @@ -3,7 +3,6 @@ package mindustry.ai.types; import arc.math.*; import arc.struct.*; import arc.util.*; -import mindustry.*; import mindustry.ai.*; import mindustry.entities.units.*; import mindustry.gen.*; @@ -86,7 +85,7 @@ public class LogicAI extends AIController{ if(unit.isFlying()){ moveTo(Tmp.v1.set(moveX, moveY), 1f, 30f); }else{ - if(Vars.controlPath.getPathPosition(unit, lastPathId, Tmp.v2.set(moveX, moveY), 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); } } diff --git a/core/src/mindustry/content/Fx.java b/core/src/mindustry/content/Fx.java index 944bd87168..246eec104a 100644 --- a/core/src/mindustry/content/Fx.java +++ b/core/src/mindustry/content/Fx.java @@ -2584,7 +2584,7 @@ public class Fx{ if(!(e.data instanceof Vec2[] vec)) return; Draw.color(e.color); - Lines.stroke(1f); + Lines.stroke(2f); if(vec.length == 2){ Lines.line(vec[0].x, vec[0].y, vec[1].x, vec[1].y); @@ -2596,5 +2596,15 @@ public class Fx{ } Draw.reset(); + }), + debugRect = new Effect(90f, 1000000000000f, e -> { + if(!(e.data instanceof Rect rect)) return; + + Draw.color(e.color); + Lines.stroke(2f); + + Lines.rect(rect); + + Draw.reset(); }); } diff --git a/core/src/mindustry/entities/comp/HitboxComp.java b/core/src/mindustry/entities/comp/HitboxComp.java index a444f350ab..178ab1c228 100644 --- a/core/src/mindustry/entities/comp/HitboxComp.java +++ b/core/src/mindustry/entities/comp/HitboxComp.java @@ -68,7 +68,7 @@ abstract class HitboxComp implements Posc, Sized, QuadTreeObject{ public void hitboxTile(Rect rect){ //tile hitboxes are never bigger than a tile, otherwise units get stuck - float size = Math.min(hitSize * 0.66f, 7.9f); + float size = Math.min(hitSize * 0.66f, 7.8f); //TODO: better / more accurate version is //float size = hitSize * 0.85f; //- for tanks? diff --git a/core/src/mindustry/entities/comp/TankComp.java b/core/src/mindustry/entities/comp/TankComp.java index 38946709ac..986a17cb56 100644 --- a/core/src/mindustry/entities/comp/TankComp.java +++ b/core/src/mindustry/entities/comp/TankComp.java @@ -51,7 +51,7 @@ abstract class TankComp implements Posc, Flyingc, Hitboxc, Unitc, ElevationMovec } //calculate overlapping tiles so it slows down when going "over" walls - int r = Math.max(Math.round(hitSize * 0.6f / tilesize), 1); + int r = Math.max((int)(hitSize * 0.6f / tilesize), 0); int solids = 0, total = (r*2+1)*(r*2+1); for(int dx = -r; dx <= r; dx++){ diff --git a/core/src/mindustry/entities/comp/UnitComp.java b/core/src/mindustry/entities/comp/UnitComp.java index c33e020693..eb9b64f032 100644 --- a/core/src/mindustry/entities/comp/UnitComp.java +++ b/core/src/mindustry/entities/comp/UnitComp.java @@ -402,7 +402,7 @@ abstract class UnitComp implements Healthc, Physicsc, Hitboxc, Statusc, Teamc, I return type.allowLegStep && type.legPhysicsLayer ? PhysicsProcess.layerLegs : isGrounded() ? PhysicsProcess.layerGround : PhysicsProcess.layerFlying; } - /** @return pathfinder path type for calculating costs */ + /** @return pathfinder path type for calculating costs. This is used for wave AI only. (TODO: remove) */ public int pathType(){ return Pathfinder.costGround; } diff --git a/core/src/mindustry/entities/units/AIController.java b/core/src/mindustry/entities/units/AIController.java index 009e56d6ad..6025ea611c 100644 --- a/core/src/mindustry/entities/units/AIController.java +++ b/core/src/mindustry/entities/units/AIController.java @@ -341,7 +341,7 @@ public class AIController implements UnitController{ vec.setLength(speed * length); } - //do not move when infinite vectors are used or if its zero. + //ignore invalid movement values if(vec.isNaN() || vec.isInfinite() || vec.isZero()) return; if(!unit.type.omniMovement && unit.type.rotateMoveFirst){ diff --git a/core/src/mindustry/type/UnitType.java b/core/src/mindustry/type/UnitType.java index fbd7e599d7..64e7c87785 100644 --- a/core/src/mindustry/type/UnitType.java +++ b/core/src/mindustry/type/UnitType.java @@ -288,6 +288,8 @@ public class UnitType extends UnlockableContent implements Senseable{ /** Function used for calculating cost of moving with ControlPathfinder. Does not affect "normal" flow field pathfinding. */ public @Nullable PathCost pathCost; + /** ID for path cost, to be used in the control path finder. This is the value that actually matters; do not assign manually. Set in init(). */ + public int pathCostId; /** A sample of the unit that this type creates. Do not modify! */ public @Nullable Unit sample; @@ -693,6 +695,9 @@ public class UnitType extends UnlockableContent implements Senseable{ ControlPathfinder.costGround; } + pathCostId = ControlPathfinder.costTypes.indexOf(pathCost); + if(pathCostId == -1) pathCostId = 0; + if(flying){ envEnabled |= Env.space; }