Pathfinding fixes

core/src/mindustry/ai/ControlPathfinder.java

@@ -344,6 +344,10 @@ public class ControlPathfinder{
         requests.clear();
     }
 
+    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);
+    }
+
     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;

core/src/mindustry/ai/Pathfinder.java

@@ -159,7 +159,7 @@ public class Pathfinder implements Runnable{
             if(other != null){
                 Floor floor = other.floor();
                 boolean osolid = other.solid();
-                if(floor.isLiquid) nearLiquid = true;
+                if(floor.isLiquid && floor.isDeep()) nearLiquid = true;
                 //TODO potentially strange behavior when teamPassable is false for other teams?
                 if(osolid && !other.block().teamPassable) nearSolid = true;
                 if(!floor.isLiquid) nearGround = true;

core/src/mindustry/ai/UnitGroup.java

@@ -1,11 +1,11 @@
 package mindustry.ai;
 
 import arc.*;
-import arc.func.*;
 import arc.graphics.*;
 import arc.math.*;
 import arc.math.geom.*;
 import arc.struct.*;
+import arc.util.*;
 import mindustry.*;
 import mindustry.ai.Pathfinder.*;
 import mindustry.async.*;
@@ -16,11 +16,13 @@ import mindustry.world.blocks.environment.*;
 
 public class UnitGroup{
     public Seq<Unit> units = new Seq<>();
-    public float[] positions;
+    public int collisionLayer;
-    public float minSpeed = 999999f;
+    public volatile float[] positions, originalPositions;
     public volatile boolean valid;
+    public float minSpeed = 999999f;
     
     public void calculateFormation(Vec2 dest, int collisionLayer){
+        this.collisionLayer = collisionLayer;
 
         float cx = 0f, cy = 0f;
         for(Unit unit : units){
@@ -137,31 +139,11 @@ public class UnitGroup{
                 }
             }
 
+            originalPositions = positions.clone();
+
             //raycast from the destination to the offset to make sure it's reachable
-            if(collisionLayer != PhysicsProcess.layerFlying){
             for(int a = 0; a < units.size; a ++){
-                    //coordinates in world space
+                updateRaycast(a, dest, v1);
-                    float
-                    x = positions[a * 2] + dest.x,
-                    y = positions[a * 2 + 1] + dest.y;
-
-                    Unit unit = units.get(a);
-
-                    PathCost cost = unit.type.pathCost;
-                    int res = ControlPathfinder.raycastFast(unit.team.id, cost, World.toTile(dest.x), World.toTile(dest.y), World.toTile(x), World.toTile(y));
-
-                    //collision found, make th destination the point right before the collision
-                    if(res != 0){
-                        v1.set(Point2.x(res) * Vars.tilesize - dest.x, Point2.y(res) * Vars.tilesize - dest.y);
-                        v1.setLength(Math.max(v1.len() - Vars.tilesize - 4f, 0));
-                        positions[a * 2] = v1.x;
-                        positions[a * 2 + 1] = v1.y;
-                    }
-
-                    if(ControlPathfinder.showDebug){
-                        Core.app.post(() -> Fx.debugLine.at(unit.x, unit.y, 0f, Color.green, new Vec2[]{new Vec2(dest.x, dest.y), new Vec2(x, y)}));
-                    }
-                }
             }
 
             valid = true;
@@ -178,274 +160,34 @@ public class UnitGroup{
         });
     }
 
-    public static class IntQuadTree{
+    public void updateRaycast(int index, Vec2 dest){
-        protected final Rect tmp = new Rect();
+        updateRaycast(index, dest, Tmp.v1);
-        protected static final int maxObjectsPerNode = 5;
-
-        public IntQuadTreeProvider prov;
-        public Rect bounds;
-        public IntSeq objects = new IntSeq(false, 10);
-        public IntQuadTree botLeft, botRight, topLeft, topRight;
-        public boolean leaf = true;
-        public int totalObjects;
-
-        public IntQuadTree(Rect bounds, IntQuadTreeProvider prov){
-            this.bounds = bounds;
-            this.prov = prov;
     }
 
-        protected void split(){
+    private void updateRaycast(int index, Vec2 dest, Vec2 v1){
-            if(!leaf) return;
+        if(collisionLayer != PhysicsProcess.layerFlying){
 
-            float subW = bounds.width / 2;
+            //coordinates in world space
-            float subH = bounds.height / 2;
+            float
+                x = originalPositions[index * 2] + dest.x,
+                y = originalPositions[index * 2 + 1] + dest.y;
 
-            if(botLeft == null){
+            Unit unit = units.get(index);
-                botLeft = newChild(new Rect(bounds.x, bounds.y, subW, subH));
-                botRight = newChild(new Rect(bounds.x + subW, bounds.y, subW, subH));
-                topLeft = newChild(new Rect(bounds.x, bounds.y + subH, subW, subH));
-                topRight = newChild(new Rect(bounds.x + subW, bounds.y + subH, subW, subH));
-            }
-            leaf = false;
 
-            // Transfer objects to children if they fit entirely in one
+            PathCost cost = unit.type.pathCost;
-            for(int i = 0; i < objects.size; i ++){
+            int res = ControlPathfinder.raycastFast(unit.team.id, cost, World.toTile(dest.x), World.toTile(dest.y), World.toTile(x), World.toTile(y));
-                int obj = objects.items[i];
+
-                hitbox(obj);
+            //collision found, make th destination the point right before the collision
-                IntQuadTree child = getFittingChild(tmp);
+            if(res != 0){
-                if(child != null){
+                v1.set(Point2.x(res) * Vars.tilesize - dest.x, Point2.y(res) * Vars.tilesize - dest.y);
-                    child.insert(obj);
+                v1.setLength(Math.max(v1.len() - Vars.tilesize - 4f, 0));
-                    objects.removeIndex(i);
+                positions[index * 2] = v1.x;
-                    i --;
+                positions[index * 2 + 1] = v1.y;
-                }
-            }
             }
 
-        protected void unsplit(){
+            if(ControlPathfinder.showDebug){
-            if(leaf) return;
+                Core.app.post(() -> Fx.debugLine.at(unit.x, unit.y, 0f, Color.green, new Vec2[]{new Vec2(dest.x, dest.y), new Vec2(x, y)}));
-            objects.addAll(botLeft.objects);
-            objects.addAll(botRight.objects);
-            objects.addAll(topLeft.objects);
-            objects.addAll(topRight.objects);
-            botLeft.clear();
-            botRight.clear();
-            topLeft.clear();
-            topRight.clear();
-            leaf = true;
-        }
-
-        /**
-         * Inserts an object into this node or its child nodes. This will split a leaf node if it exceeds the object limit.
-         */
-        public void insert(int obj){
-            hitbox(obj);
-            if(!bounds.overlaps(tmp)){
-                // New object not in quad tree, ignoring
-                // throw an exception?
-                return;
-            }
-
-            totalObjects ++;
-
-            if(leaf && objects.size + 1 > maxObjectsPerNode) split();
-
-            if(leaf){
-                // Leaf, so no need to add to children, just add to root
-                objects.add(obj);
-            }else{
-                hitbox(obj);
-                // Add to relevant child, or root if can't fit completely in a child
-                IntQuadTree child = getFittingChild(tmp);
-                if(child != null){
-                    child.insert(obj);
-                }else{
-                    objects.add(obj);
             }
         }
     }
-
-        /**
-         * Removes an object from this node or its child nodes.
-         */
-        public boolean remove(int obj){
-            boolean result;
-            if(leaf){
-                // Leaf, no children, remove from root
-                result = objects.removeValue(obj);
-            }else{
-                // Remove from relevant child
-                hitbox(obj);
-                IntQuadTree child = getFittingChild(tmp);
-
-                if(child != null){
-                    result = child.remove(obj);
-                }else{
-                    // Or root if object doesn't fit in a child
-                    result = objects.removeValue(obj);
-                }
-
-                if(totalObjects <= maxObjectsPerNode) unsplit();
-            }
-            if(result){
-                totalObjects --;
-            }
-            return result;
-        }
-
-        /** Removes all objects. */
-        public void clear(){
-            objects.clear();
-            totalObjects = 0;
-            if(!leaf){
-                topLeft.clear();
-                topRight.clear();
-                botLeft.clear();
-                botRight.clear();
-            }
-            leaf = true;
-        }
-
-        protected IntQuadTree getFittingChild(Rect boundingBox){
-            float verticalMidpoint = bounds.x + (bounds.width / 2);
-            float horizontalMidpoint = bounds.y + (bounds.height / 2);
-
-            // Object can completely fit within the top quadrants
-            boolean topQuadrant = boundingBox.y > horizontalMidpoint;
-            // Object can completely fit within the bottom quadrants
-            boolean bottomQuadrant = boundingBox.y < horizontalMidpoint && (boundingBox.y + boundingBox.height) < horizontalMidpoint;
-
-            // Object can completely fit within the left quadrants
-            if(boundingBox.x < verticalMidpoint && boundingBox.x + boundingBox.width < verticalMidpoint){
-                if(topQuadrant){
-                    return topLeft;
-                }else if(bottomQuadrant){
-                    return botLeft;
-                }
-            }else if(boundingBox.x > verticalMidpoint){ // Object can completely fit within the right quadrants
-                if(topQuadrant){
-                    return topRight;
-                }else if(bottomQuadrant){
-                    return botRight;
-                }
-            }
-
-            // Else, object needs to be in parent cause it can't fit completely in a quadrant
-            return null;
-        }
-
-        /**
-         * Processes objects that may intersect the given rectangle.
-         * <p>
-         * This will never result in false positives.
-         */
-        public void intersect(float x, float y, float width, float height, Intc out){
-            if(!leaf){
-                if(topLeft.bounds.overlaps(x, y, width, height)) topLeft.intersect(x, y, width, height, out);
-                if(topRight.bounds.overlaps(x, y, width, height)) topRight.intersect(x, y, width, height, out);
-                if(botLeft.bounds.overlaps(x, y, width, height)) botLeft.intersect(x, y, width, height, out);
-                if(botRight.bounds.overlaps(x, y, width, height)) botRight.intersect(x, y, width, height, out);
-            }
-
-            IntSeq objects = this.objects;
-
-            for(int i = 0; i < objects.size; i++){
-                int item = objects.items[i];
-                hitbox(item);
-                if(tmp.overlaps(x, y, width, height)){
-                    out.get(item);
-                }
-            }
-        }
-
-        /**
-         * @return whether an object overlaps this rectangle.
-         * This will never result in false positives.
-         */
-        public boolean any(float x, float y, float width, float height){
-            if(!leaf){
-                if(topLeft.bounds.overlaps(x, y, width, height) && topLeft.any(x, y, width, height)) return true;
-                if(topRight.bounds.overlaps(x, y, width, height) && topRight.any(x, y, width, height)) return true;
-                if(botLeft.bounds.overlaps(x, y, width, height) && botLeft.any(x, y, width, height)) return true;
-                if(botRight.bounds.overlaps(x, y, width, height) && botRight.any(x, y, width, height))return true;
-            }
-
-            IntSeq objects = this.objects;
-
-            for(int i = 0; i < objects.size; i++){
-                int item = objects.items[i];
-                hitbox(item);
-                if(tmp.overlaps(x, y, width, height)){
-                    return true;
-                }
-            }
-            return false;
-        }
-
-        /**
-         * Processes objects that may intersect the given rectangle.
-         * <p>
-         * This will never result in false positives.
-         */
-        public void intersect(Rect rect, Intc out){
-            intersect(rect.x, rect.y, rect.width, rect.height, out);
-        }
-
-        /**
-         * Fills the out parameter with any objects that may intersect the given rectangle.
-         * <p>
-         * This will result in false positives, but never a false negative.
-         */
-        public void intersect(Rect toCheck, IntSeq out){
-            intersect(toCheck.x, toCheck.y, toCheck.width, toCheck.height, out);
-        }
-
-        /**
-         * Fills the out parameter with any objects that may intersect the given rectangle.
-         */
-        public void intersect(float x, float y, float width, float height, IntSeq out){
-            if(!leaf){
-                if(topLeft.bounds.overlaps(x, y, width, height)) topLeft.intersect(x, y, width, height, out);
-                if(topRight.bounds.overlaps(x, y, width, height)) topRight.intersect(x, y, width, height, out);
-                if(botLeft.bounds.overlaps(x, y, width, height)) botLeft.intersect(x, y, width, height, out);
-                if(botRight.bounds.overlaps(x, y, width, height)) botRight.intersect(x, y, width, height, out);
-            }
-
-            IntSeq objects = this.objects;
-
-            for(int i = 0; i < objects.size; i++){
-                int item = objects.items[i];
-                hitbox(item);
-                if(tmp.overlaps(x, y, width, height)){
-                    out.add(item);
-                }
-            }
-        }
-
-        /** Adds all quadtree objects to the specified Seq. */
-        public void getObjects(IntSeq out){
-            out.addAll(objects);
-
-            if(!leaf){
-                topLeft.getObjects(out);
-                topRight.getObjects(out);
-                botLeft.getObjects(out);
-                botRight.getObjects(out);
-            }
-        }
-
-        protected IntQuadTree newChild(Rect rect){
-            return new IntQuadTree(rect, prov);
-        }
-
-        protected void hitbox(int t){
-            prov.hitbox(t, tmp);
-        }
-
-        /**Represents an object in a QuadTree.*/
-        public interface IntQuadTreeProvider{
-            /**Fills the out parameter with this element's rough bounding box. This should never be smaller than the actual object, but may be larger.*/
-            void hitbox(int object, Rect out);
-        }
-    }
-
 }

core/src/mindustry/ai/types/CommandAI.java

@@ -1,5 +1,6 @@
 package mindustry.ai.types;
 
+import arc.math.*;
 import arc.math.geom.*;
 import arc.struct.*;
 import arc.util.*;
@@ -16,7 +17,7 @@ import mindustry.world.meta.*;
 import static mindustry.Vars.*;
 
 public class CommandAI extends AIController{
-    protected static final int maxCommandQueueSize = 50;
+    protected static final int maxCommandQueueSize = 50, avoidInterval = 5;
     protected static final Vec2 vecOut = new Vec2(), vecMovePos = new Vec2();
     protected static final boolean[] noFound = {false};
 
@@ -32,6 +33,8 @@ public class CommandAI extends AIController{
     protected boolean stopAtTarget, stopWhenInRange;
     protected Vec2 lastTargetPos;
     protected int pathId = -1;
+    protected boolean blockingUnit;
+    protected float timeSpentBlocked;
 
     /** Stance, usually related to firing mode. */
     public UnitStance stance = UnitStance.shoot;
@@ -205,7 +208,29 @@ public class CommandAI extends AIController{
             }
 
             if(unit.isGrounded() && stance != UnitStance.ram){
-                move = Vars.controlPath.getPathPosition(unit, pathId, vecMovePos, vecOut, noFound);
+                if(timer.get(timerTarget3, avoidInterval)){
+                    Vec2 dstPos = Tmp.v1.trns(unit.rotation, unit.hitSize/2f);
+                    float max = unit.hitSize/2f;
+                    float radius = Math.max(7f, max);
+                    float margin = 4f;
+                    blockingUnit = Units.nearbyCheck(unit.x + dstPos.x - radius/2f, unit.y + dstPos.y - radius/2f, radius, radius,
+                        u -> u != unit && u.within(unit, u.hitSize/2f + unit.hitSize/2f + margin) && u.controller() instanceof CommandAI ai && ai.targetPos != null &&
+                        //stop for other unit only if it's closer to the target
+                        (ai.targetPos.equals(targetPos) && u.dst2(targetPos) < unit.dst2(targetPos)) &&
+                        //don't stop if they're facing the same way
+                        !Angles.within(unit.rotation, u.rotation, 15f) &&
+                        //must be near an obstacle, stopping in open ground is pointless
+                        ControlPathfinder.isNearObstacle(unit, unit.tileX(), unit.tileY(), u.tileX(), u.tileY()));
+                }
+
+                if(blockingUnit){
+                    timeSpentBlocked += Time.delta;
+                }else{
+                    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 > 60f * 3f);
                 //we've reached the final point if the returned coordinate is equal to the supplied input
                 isFinalPoint &= vecMovePos.epsilonEquals(vecOut, 4.1f);
 
@@ -278,6 +303,11 @@ public class CommandAI extends AIController{
             if(prev != null && stance == UnitStance.patrol){
                 commandQueue.add(prev.cpy());
             }
+
+            //make sure spot in formation is reachable
+            if(group != null){
+                group.updateRaycast(groupIndex, next instanceof Vec2 position ? position : Tmp.v3.set(next));
+            }
         }else{
             if(group != null){
                 group = null;

core/src/mindustry/entities/EntityGroup.java

@@ -145,6 +145,12 @@ public class EntityGroup<T extends Entityc> implements Iterable<T>{
         tree.intersect(x, y, width, height, out);
     }
 
+    public boolean intersect(float x, float y, float width, float height, Boolf<? super T> out){
+        //don't waste time for empty groups
+        if(isEmpty()) return false;
+        return tree.intersect(x, y, width, height, out);
+    }
+
     public Seq<T> intersect(float x, float y, float width, float height){
         intersectArray.clear();
         //don't waste time for empty groups

core/src/mindustry/entities/Units.java

@@ -20,22 +20,18 @@ public class Units{
     private static final Rect hitrect = new Rect();
     private static Unit result;
     private static float cdist, cpriority;
-    private static boolean boolResult;
     private static int intResult;
     private static Building buildResult;
 
     //prevents allocations in anyEntities
     private static boolean anyEntityGround;
     private static float aeX, aeY, aeW, aeH;
-    private static final Cons<Unit> anyEntityLambda = unit -> {
+    private static final Boolf<Unit> anyEntityLambda = unit -> {
-        if(boolResult) return;
         if((unit.isGrounded() && !unit.type.allowLegStep) == anyEntityGround){
             unit.hitboxTile(hitrect);
-
+            return hitrect.overlaps(aeX, aeY, aeW, aeH);
-            if(hitrect.overlaps(aeX, aeY, aeW, aeH)){
-                boolResult = true;
-            }
         }
+        return false;
     };
 
     @Remote(called = Loc.server)
@@ -162,31 +158,26 @@ public class Units{
     }
 
     public static boolean anyEntities(float x, float y, float width, float height, boolean ground){
-        boolResult = false;
         anyEntityGround = ground;
         aeX = x;
         aeY = y;
         aeW = width;
         aeH = height;
 
-        nearby(x, y, width, height, anyEntityLambda);
+        return nearbyCheck(x, y, width, height, anyEntityLambda);
-        return boolResult;
     }
 
+    /** Note that this checks the tile hitbox, not the standard hitbox. */
     public static boolean anyEntities(float x, float y, float width, float height, Boolf<Unit> check){
-        boolResult = false;
 
-        nearby(x, y, width, height, unit -> {
+        return nearbyCheck(x, y, width, height, unit -> {
-            if(boolResult) return;
             if(check.get(unit)){
                 unit.hitboxTile(hitrect);
 
-                if(hitrect.overlaps(x, y, width, height)){
+                return hitrect.overlaps(x, y, width, height);
-                    boolResult = true;
-                }
             }
+            return false;
         });
-        return boolResult;
     }
 
     /** Returns the nearest damaged tile. */
@@ -428,6 +419,14 @@ public class Units{
         Groups.unit.intersect(x, y, width, height, cons);
     }
 
+    /**
+     * Iterates over all units in a rectangle.
+     * @return whether a unit was found.
+     * */
+    public static boolean nearbyCheck(float x, float y, float width, float height, Boolf<Unit> cons){
+        return Groups.unit.intersect(x, y, width, height, cons);
+    }
+
     /** Iterates over all units in a rectangle. */
     public static void nearby(Rect rect, Cons<Unit> cons){
         nearby(rect.x, rect.y, rect.width, rect.height, cons);

gradle.properties

@@ -25,4 +25,4 @@ org.gradle.caching=true
 #used for slow jitpack builds; TODO see if this actually works
 org.gradle.internal.http.socketTimeout=100000
 org.gradle.internal.http.connectionTimeout=100000
-archash=0c61237b32
+archash=443db6a9c5