Majorly optimise bots movement AABB testing.

MCGalaxy/Bots/PlayerBot.cs

@@ -70,7 +70,7 @@ namespace MCGalaxy {
         
         public static void Add(PlayerBot bot, bool save = true) {
             bot.level.Bots.Add(bot);
-            bot.GlobalSpawn();            
+            bot.GlobalSpawn();
             if (save) BotsFile.Save(bot.level);
         }
 
@@ -87,7 +87,7 @@ namespace MCGalaxy {
         }
         
         public static void RemoveAllFromLevel(Level lvl) {
-            RemoveLoadedBots(lvl, false);           
+            RemoveLoadedBots(lvl, false);
             BotsFile.Save(lvl);
         }
         
@@ -113,40 +113,6 @@ namespace MCGalaxy {
             }
         }
         
-        public static void GlobalUpdatePosition() {
-            Level[] levels = LevelInfo.Loaded.Items;
-            for (int i = 0; i < levels.Length; i++) {
-                PlayerBot[] bots = levels[i].Bots.Items;
-                for (int j = 0; j < bots.Length; j++) { bots[j].UpdatePosition(); }
-            }
-        }
-        
-        void UpdatePosition() {
-            if (movement) {
-                double scale = Math.Ceiling(ServerConfig.PositionUpdateInterval / 25.0);
-                int steps = movementSpeed * (int)scale;
-                for (int i = 0; i < steps; i++)
-                    DoMove();
-            }
-            
-            Position pos = Pos; Orientation rot = Rot;
-            if (pos == lastPos && rot.HeadX == lastRot.HeadX && rot.RotY == lastRot.RotY) return;
-            lastPos = pos; lastRot = rot;
-            
-            // TODO: relative position updates, combine packets
-            byte[] packet = Packet.Teleport(id, pos, rot, false);
-            byte[] extPacket = Packet.Teleport(id, pos, rot, true);
-
-            Player[] players = PlayerInfo.Online.Items;
-            foreach (Player p in players) {
-                if (p.level != level) continue;
-                
-                if (p.hasExtPositions) p.Send(extPacket);
-                else p.Send(packet);
-            }
-        }
-
-        
         unsafe static byte NextFreeId(PlayerBot bot) {
             byte* used = stackalloc byte[256];
             for (int i = 0; i < 256; i++)
@@ -189,39 +155,118 @@ namespace MCGalaxy {
             cur++;
             if (cur == Instructions.Count) cur = 0;
         }
+
         
-        void DoMove() {
+        public static void GlobalUpdatePosition() {
+            Level[] levels = LevelInfo.Loaded.Items;
+            for (int i = 0; i < levels.Length; i++) {
+                PlayerBot[] bots = levels[i].Bots.Items;
+                for (int j = 0; j < bots.Length; j++) { bots[j].UpdatePosition(); }
+            }
+        }
+        
+        void UpdatePosition() {
+            if (movement) PerformMovement();
+            
+            Position pos = Pos; Orientation rot = Rot;
+            if (pos == lastPos && rot.HeadX == lastRot.HeadX && rot.RotY == lastRot.RotY) return;
+            lastPos = pos; lastRot = rot;
+            
+            // TODO: relative position updates, combine packets
+            byte[] packet = Packet.Teleport(id, pos, rot, false);
+            byte[] extPacket = Packet.Teleport(id, pos, rot, true);
+
+            Player[] players = PlayerInfo.Online.Items;
+            foreach (Player p in players) {
+                if (p.level != level) continue;
+                
+                if (p.hasExtPositions) p.Send(extPacket);
+                else p.Send(packet);
+            }
+        }
+        
+        static AABB[] downs = new AABB[16], ups = new AABB[16];
+        static int downsCount, upsCount;
+        
+        void RecalcDownExtent(ref AABB bb, int steps, int dx, int dz) {
+            AABB downExtent = bb.Adjust(dx * steps, -32, dz * steps);
+            downsCount = AABB.FindIntersectingSolids(downExtent, level, ref downs);
+        }
+        
+        void RecalcUpExtent(ref AABB bb, int steps, int dx, int dz) {
+            AABB upExtent = bb.Adjust(dx * steps, 32, dz * steps);
+            upsCount = AABB.FindIntersectingSolids(upExtent, level, ref ups);
+        }
+        
+        void PerformMovement() {
+            double scale = Math.Ceiling(ServerConfig.PositionUpdateInterval / 25.0);
+            int steps = movementSpeed * (int)scale;
+            
+            downsCount = -1;
+            for (int i = 0; i < steps; i++) DoMove(steps);
+        }
+        
+        void DoMove(int steps) {
             Position pos = Pos;
             AABB bb = ModelBB.OffsetPosition(pos);
+            int dx = Math.Sign(TargetPos.X - pos.X);
+            int dz = Math.Sign(TargetPos.Z - pos.Z);
+            
+            if (downsCount == -1) {
+                RecalcDownExtent(ref bb, steps, dx, dz);
+                RecalcUpExtent(ref bb, steps, dx, dz);
+            }
+            
             // Advance the AABB to the bot's next position
-            int dx = Math.Sign(TargetPos.X - Pos.X), dz = Math.Sign(TargetPos.Z - Pos.Z);
             bb = bb.Offset(dx, 0, dz);
             AABB bbCopy = bb;
             
             // Attempt to drop the bot down up to 1 block
             int hitY = -32;
             for (int dy = 0; dy >= -32; dy--) {
-                if (AABB.IntersectsSolidBlocks(bb, level)) { hitY = dy + 1; break; }
+                bool intersectsAny = false;
+                for (int i = 0; i < downsCount; i++) {
+                    if (AABB.Intersects(ref bb, ref downs[i])) { intersectsAny = true; break; }
+                }
+                
+                if (intersectsAny) { hitY = dy + 1; break; }
                 bb.Min.Y--; bb.Max.Y--;
-            }          
+            }
             
             // Does the bot fall down a block
             if (hitY < 0) {
-                pos.X += dx; pos.Y += hitY; pos.Z += dz;
+                pos.X += dx; pos.Y += hitY; pos.Z += dz; Pos = pos; 
-                Pos = pos; return;
+                RecalcDownExtent(ref bb, steps, dx, dz);
+                RecalcUpExtent(ref bb, steps, dx, dz);              
+                return;
             }
             
             // Attempt to move the bot up to 1 block
             bb = bbCopy;
+            
             for (int dy = 0; dy <= 32; dy++) {
-                if (!AABB.IntersectsSolidBlocks(bb, level)) {
+                bool intersectsAny = false;
-                    pos.X += dx; pos.Y += dy; pos.Z += dz;
+                for (int i = 0; i < upsCount; i++) {
-                    Pos = pos; return;
+                    if (AABB.Intersects(ref bb, ref ups[i])) { intersectsAny = true; break; }
+                }
+                
+                if (!intersectsAny) {
+                    pos.X += dx; pos.Y += dy; pos.Z += dz; Pos = pos; 
+                    
+                    if (dy != 0) {
+                        RecalcDownExtent(ref bb, steps, dx, dz);
+                        RecalcUpExtent(ref bb, steps, dx, dz);
+                    }
+                    return;
                 }
                 bb.Min.Y++; bb.Max.Y++;
             }
-            
+        }
-            /*
+        
+        
+        /*
+         * Old water/lava swimming code - TODO: need to fix.
+         * 
                 if ((ushort)(foundPos[1] / 32) > y) {
                     if (b1 == Block.water || b1 == Block.waterstill || b1 == Block.lava || b1 == Block.lavastill) {
                         if (Block.Walkthrough(b2)) {
@@ -235,6 +280,5 @@ namespace MCGalaxy {
                         pos[1] = (ushort)(pos[1] + (Math.Sign(foundPos[1] - pos[1])));
                     }
                 }*/
-        }
     }
 }

MCGalaxy/Player/PlayerPhysics.cs

@@ -35,7 +35,7 @@ namespace MCGalaxy.Blocks.Physics {
                 if (block.BlockID == Block.Invalid) continue;
                 
                 AABB blockBB = p.level.blockAABBs[block.Index].Offset(x * 32, y * 32, z * 32);
-                if (!bb.Intersects(blockBB)) continue;
+                if (!AABB.Intersects(ref bb, ref blockBB)) continue;
                 
                 // We can activate only one walkthrough block per movement
                 if (!hitWalkthrough) {

MCGalaxy/util/Math/AABB.cs

@@ -61,6 +61,14 @@ namespace MCGalaxy.Maths {
             return bb;
         }
         
+        public AABB Adjust(int x, int y, int z) {
+            AABB bb = this;
+            if (x >= 0) { bb.Max.X += x; } else { bb.Min.X += x; }
+            if (y >= 0) { bb.Max.Y += y; } else { bb.Min.Y += y; }
+            if (z >= 0) { bb.Max.Z += z; } else { bb.Min.Z += z; }
+            return bb;
+        }
+        
         public AABB Expand(int amount) {
             AABB bb = this;
             bb.Min.X -= amount; bb.Min.Y -= amount; bb.Min.Z -= amount;
@@ -68,38 +76,18 @@ namespace MCGalaxy.Maths {
             return bb;
         }
         
-        /// <summary> Returns a new bounding box, with the minimum and maximum coordinates
-        /// of the original bounding box scaled away from origin the given value. </summary>
-        public AABB Scale(float scale) {
-            return new AABB(Min * scale, Max * scale);
-        }
-
         /// <summary> Determines whether this bounding box intersects
         /// the given bounding box on any axes. </summary>
-        public bool Intersects(AABB other) {
+        public static bool Intersects(ref AABB a, ref AABB b) {
-            if (Max.X >= other.Min.X && Min.X <= other.Max.X) {
+            if (a.Max.X >= b.Min.X && a.Min.X <= b.Max.X) {
-                if (Max.Y < other.Min.Y || Min.Y > other.Max.Y) {
+                if (a.Max.Y < b.Min.Y || a.Min.Y > b.Max.Y) {
                     return false;
                 }
-                return Max.Z >= other.Min.Z && Min.Z <= other.Max.Z;
+                return a.Max.Z >= b.Min.Z && a.Min.Z <= b.Max.Z;
             }
             return false;
         }
-        
+
-        /// <summary> Determines whether this bounding box entirely contains
-        /// the given bounding box on all axes. </summary>
-        public bool Contains(AABB other) {
-            return other.Min.X >= Min.X && other.Min.Y >= Min.Y && other.Min.Z >= Min.Z &&
-                other.Max.X <= Max.X && other.Max.Y <= Max.Y && other.Max.Z <= Max.Z;
-        }
-        
-        /// <summary> Determines whether this bounding box entirely contains
-        /// the coordinates on all axes. </summary>
-        public bool Contains(Vec3S32 P) {
-            return P.X >= Min.X && P.Y >= Min.Y && P.Z >= Min.Z &&
-                P.X <= Max.X && P.Y <= Max.Y && P.Z <= Max.Z;
-        }
-        
         public override string ToString() {
             return Min + " : " + Max;
         }
@@ -127,7 +115,7 @@ namespace MCGalaxy.Maths {
             float maxScale = model.CaselessEq("chibi") ? 3 : 2;
             if (scale > maxScale) scale = maxScale;
             
-            return baseBB.Scale(scale);
+            return new AABB(baseBB.Min * scale, baseBB.Max * scale);
         }
         
         static Vec3S32 BaseSize(string model) {
@@ -154,7 +142,7 @@ namespace MCGalaxy.Maths {
                 ExtBlock block = lvl.GetBlock(xP, yP, zP);
                 
                 AABB blockBB = lvl.blockAABBs[block.Index].Offset(x * 32, y * 32, z * 32);
-                if (!bb.Intersects(blockBB)) continue;
+                if (!AABB.Intersects(ref bb, ref blockBB)) continue;
                 
                 BlockDefinition def = lvl.GetBlockDef(block);
                 if (def != null) {
@@ -165,5 +153,38 @@ namespace MCGalaxy.Maths {
             }
             return false;
         }
+        
+        public static int FindIntersectingSolids(AABB bb, Level lvl, ref AABB[] aabbs) {
+            Vec3S32 min = bb.BlockMin, max = bb.BlockMax;
+            int volume = (max.X - min.X + 1) * (max.Y - min.Y + 1) * (max.Z - min.Z + 1);
+            if (volume > aabbs.Length) aabbs = new AABB[volume];
+            int count = 0;
+            
+            for (int y = min.Y; y <= max.Y; y++)
+                for (int z = min.Z; z <= max.Z; z++)
+                    for (int x = min.X; x <= max.X; x++)
+            {
+                ExtBlock block = lvl.GetBlock((ushort)x, (ushort)y, (ushort)z);               
+                AABB blockBB = lvl.blockAABBs[block.Index];
+                
+                blockBB.Min.X += x * 32; blockBB.Min.Y += y * 32; blockBB.Min.Z += z * 32;
+                blockBB.Max.X += x * 32; blockBB.Max.Y += y * 32; blockBB.Max.Z += z * 32;
+                if (!AABB.Intersects(ref bb, ref blockBB)) continue;
+                
+                BlockDefinition def = lvl.GetBlockDef(block);
+                bool solid = false;
+                
+                if (def != null) {
+                    solid = CollideType.IsSolid(def.CollideType);
+                } else {
+                    solid = block.BlockID == Block.Invalid || !Block.Walkthrough(Block.Convert(block.BlockID));
+                }
+                if (!solid) continue;
+                
+                aabbs[count] = blockBB;
+                count++;
+            }
+            return count;
+        }
     }
 }