using System; namespace Chop.Game.Rooms { // CREDITS TO CECER1 FOR MAKING THIS! public class IHIPathfinder { private byte[,] CollisionMap; private float[,] Height; private SquareState[,] SquareMap; public float MinJump = 1.2F; public float MaxJump = 3.0F; public void ApplyCollisionMap(byte[,] Map, float[,] height, SquareState[,] Squares) { if (CollisionMap != null) lock (CollisionMap) { CollisionMap = Map; Height = height; SquareMap = Squares; } else { CollisionMap = Map; Height = height; SquareMap = Squares; } } public byte[] Path(byte StartX, byte StartY, byte EndX, byte EndY) { IHIPathfinderValues Values; lock (CollisionMap) { Values = new IHIPathfinderValues(CollisionMap, Height); if (SquareMap[EndX, EndY] == SquareState.Closed) return new byte[0]; if (EndX >= CollisionMap.GetLength(0) || EndY >= CollisionMap.GetLength(1) || (CollisionMap[EndX, EndY] == 0 && (SquareMap[EndX, EndY] == SquareState.Closed || SquareMap[EndX, EndY] == SquareState.User)) || (StartX == EndX && StartY == EndY)) return new byte[0]; #region Init if (StartX > CollisionMap.GetUpperBound(0) || StartY > CollisionMap.GetUpperBound(1)) return new byte[0]; Values.LastID++; Values.Count++; Values.BinaryHeap[Values.Count] = Values.LastID; Values.X[Values.LastID] = StartX; Values.Y[Values.LastID] = StartY; Values.H[Values.LastID] = (ushort)GetH(StartX, StartY, EndX, EndY); Values.Parent[Values.LastID] = 0; Values.G[Values.LastID] = 0; Values.F[Values.LastID] = (ushort)(Values.G[Values.LastID] + Values.H[Values.LastID]); #endregion while (Values.Count != 0) { Values.Location = Values.BinaryHeap[1]; if (Values.X[Values.Location] == EndX && Values.Y[Values.Location] == EndY) break; Move(Values); Add(-1, -1, EndX, EndY, Values); Add(-1, 1, EndX, EndY, Values); Add(1, -1, EndX, EndY, Values); Add(1, 1, EndX, EndY, Values); Add(-1, 0, EndX, EndY, Values); Add(0, -1, EndX, EndY, Values); Add(1, 0, EndX, EndY, Values); Add(0, 1, EndX, EndY, Values); } } if (Values.Count == 0) return new byte[0]; while (Values.X[Values.Parent[Values.Location]] != StartX || Values.Y[Values.Parent[Values.Location]] != StartY) { Values.Location = Values.Parent[Values.Location]; } return new byte[] { Values.X[Values.Location], Values.Y[Values.Location] }; } int GetH(int X, int Y, int EndX, int EndY) { return (Math.Abs(X + EndX) + Math.Abs(Y + EndY)); } private void Add(sbyte iX, sbyte iY, byte EndX, byte EndY, IHIPathfinderValues Values) { byte X = (byte)(Values.X[Values.Location] + iX); byte Y = (byte)(Values.Y[Values.Location] + iY); ushort Parent = Values.Location; if (X > CollisionMap.GetUpperBound(0) || Y > CollisionMap.GetUpperBound(1)) return; if (Values.Tiles[X, Y] == 2) return; if (((CollisionMap[X, Y] == 0 && (SquareMap[X, Y] == SquareState.Closed || SquareMap[X, Y] == SquareState.User)) || (CollisionMap[X, Y] == 2 && (X != EndX || Y != EndY)))) return; float Z = Values.Z[X, Y]; float pZ = Values.Z[Values.X[Parent], Values.Y[Parent]]; if (Z > pZ + MinJump || Z < pZ - MaxJump) // TODO: Max/Min jump height from database return; if (Parent > 0) { if (Values.X[Parent] != X && Values.Y[Parent] != Y) { if (CollisionMap[X, Values.Y[Parent]] == 0 || CollisionMap[X, Values.Y[Parent]] == 2) return; if (CollisionMap[Values.X[Parent], Y] == 0 || CollisionMap[Values.X[Parent], Y] == 2) return; } } if (Values.Tiles[X, Y] == 1) { ushort i = 1; for (; i <= Values.Count; i++) { if (Values.X[i] == X && Values.Y[i] == Y) break; } if (Values.X[i] == EndX || Values.Y[i] == EndY) { if (10 + Values.G[Parent] < Values.G[i]) Values.Parent[i] = Parent; } else if (14 + Values.G[Parent] < Values.G[i]) Values.Parent[i] = Parent; return; } Values.LastID++; Values.Count++; Values.BinaryHeap[Values.Count] = Values.LastID; Values.X[Values.LastID] = X; Values.Y[Values.LastID] = Y; Values.H[Values.LastID] = (ushort)GetH(X, Y, EndX, EndY); Values.Parent[Values.LastID] = Parent; if (X == Values.X[Parent] || Y == Values.Y[Parent]) Values.G[Values.LastID] = (ushort)(10 + Values.G[Parent]); else Values.G[Values.LastID] = (ushort)(14 + Values.G[Parent]); Values.F[Values.LastID] = (ushort)(Values.G[Values.LastID] + Values.H[Values.LastID]); for (ushort C = Values.Count; C != 1; C /= 2) { if (Values.F[Values.BinaryHeap[C]] <= Values.F[Values.BinaryHeap[C / 2]]) { ushort Temp = Values.BinaryHeap[C / 2]; Values.BinaryHeap[C / 2] = Values.BinaryHeap[C]; Values.BinaryHeap[C] = Temp; } else break; } Values.Tiles[X, Y] = 1; } private static void Move(IHIPathfinderValues Values) { Values.Tiles[Values.X[Values.BinaryHeap[1]], Values.Y[Values.BinaryHeap[1]]] = 2; Values.BinaryHeap[1] = Values.BinaryHeap[Values.Count]; Values.Count--; ushort location = 1; while (true) { ushort high = location; if (2 * high + 1 <= Values.Count) { if (Values.F[Values.BinaryHeap[high]] >= Values.F[Values.BinaryHeap[2 * high]]) location = (ushort)(2 * high); if (Values.F[Values.BinaryHeap[location]] >= Values.F[Values.BinaryHeap[2 * high + 1]]) location = (ushort)(2 * high + 1); } else if (2 * high <= Values.Count) { if (Values.F[Values.BinaryHeap[high]] >= Values.F[Values.BinaryHeap[2 * high]]) location = (ushort)(2 * high); } if (high != location) { ushort Temp = Values.BinaryHeap[high]; Values.BinaryHeap[high] = Values.BinaryHeap[location]; Values.BinaryHeap[location] = Temp; } else break; } } } public class IHIPathfinderValues { public byte[,] Tiles; public float[,] Z; public byte[] X; public byte[] Y; public ushort[] H; public ushort[] G; public ushort[] F; public ushort Count; public ushort LastID; public ushort[] BinaryHeap; public ushort[] Parent; public ushort Location; public IHIPathfinderValues(byte[,] CollisionMap, float[,] Height) { Tiles = new byte[CollisionMap.GetLength(0), CollisionMap.GetLength(1)]; X = new byte[Tiles.Length]; Y = new byte[X.Length]; Z = Height; H = new ushort[X.Length]; G = new ushort[X.Length]; F = new ushort[X.Length]; Count = 0; LastID = 0; BinaryHeap = new ushort[X.Length]; Parent = new ushort[X.Length]; Location = 0; } } }