public static void DeserializeVoxelAreaCompactData(byte[] bytes, VoxelArea target) { #if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST System.IO.MemoryStream stream = new System.IO.MemoryStream(bytes); System.IO.BinaryReader reader = new System.IO.BinaryReader(stream); int width = reader.ReadInt32(); int depth = reader.ReadInt32(); if (target.width != width) throw new System.ArgumentException ("target VoxelArea has a different width than the data ("+target.width + " != " + width + ")"); if (target.depth != depth) throw new System.ArgumentException ("target VoxelArea has a different depth than the data ("+target.depth + " != " + depth + ")"); CompactVoxelCell[] cells = new CompactVoxelCell[reader.ReadInt32()]; CompactVoxelSpan[] spans = new CompactVoxelSpan[reader.ReadInt32()]; int[] areas = new int[reader.ReadInt32()]; for (int i=0;i<cells.Length;i++) { cells[i].index = reader.ReadUInt32(); cells[i].count = reader.ReadUInt32(); } for (int i=0;i<spans.Length;i++) { spans[i].con = reader.ReadUInt32(); spans[i].h = reader.ReadUInt32(); spans[i].reg = reader.ReadInt32(); spans[i].y = reader.ReadUInt16(); } for (int i=0;i<areas.Length;i++) { areas[i] = reader.ReadInt32(); } target.compactCells = cells; target.compactSpans = spans; target.areaTypes = areas; #else throw new System.NotImplementedException ("This method only works with !ASTAR_RECAST_CLASS_BASED_LINKED_LIST"); #endif }
void FloodOnes(List <Int3> st1, ushort[] regs, uint level, ushort reg) { for (int j = 0; j < st1.Count; j++) { int x = st1[j].x; int i = st1[j].y; int z = st1[j].z; regs[i] = reg; CompactVoxelSpan s = voxelArea.compactSpans[i]; int area = voxelArea.areaTypes[i]; for (int dir = 0; dir < 4; dir++) { if (s.GetConnection(dir) == NotConnected) { continue; } int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; int ni = (int)voxelArea.compactCells[nx + nz].index + s.GetConnection(dir); if (area != voxelArea.areaTypes[ni]) { continue; } if (regs[ni] == 1) { //if ((int)srcDist[ni]+2 < (int)d2) //{ //visited[i] = true; regs[ni] = reg; st1.Add(new Int3(nx, ni, nz)); //d2 = (ushort)(srcDist[ni]+2); //} } } } }
public static void DeserializeVoxelAreaCompactData(byte[] bytes, VoxelArea target) { System.IO.MemoryStream stream = new System.IO.MemoryStream(bytes); System.IO.BinaryReader reader = new System.IO.BinaryReader(stream); int width = reader.ReadInt32(); int depth = reader.ReadInt32(); if (target.width != width) { throw new System.ArgumentException("target VoxelArea has a different width than the data (" + target.width + " != " + width + ")"); } if (target.depth != depth) { throw new System.ArgumentException("target VoxelArea has a different depth than the data (" + target.depth + " != " + depth + ")"); } CompactVoxelCell[] cells = new CompactVoxelCell[reader.ReadInt32()]; CompactVoxelSpan[] spans = new CompactVoxelSpan[reader.ReadInt32()]; int[] areas = new int[reader.ReadInt32()]; for (int i = 0; i < cells.Length; i++) { cells[i].index = reader.ReadUInt32(); cells[i].count = reader.ReadUInt32(); } for (int i = 0; i < spans.Length; i++) { spans[i].con = reader.ReadUInt32(); spans[i].h = reader.ReadUInt32(); spans[i].reg = reader.ReadInt32(); spans[i].y = reader.ReadUInt16(); } for (int i = 0; i < areas.Length; i++) { areas[i] = reader.ReadInt32(); } target.compactCells = cells; target.compactSpans = spans; target.areaTypes = areas; }
public static void DeserializeVoxelAreaCompactData (byte[] bytes, VoxelArea target) { #if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST System.IO.MemoryStream stream = new System.IO.MemoryStream(bytes); System.IO.BinaryReader reader = new System.IO.BinaryReader(stream); int width = reader.ReadInt32(); int depth = reader.ReadInt32(); if (target.width != width) throw new System.ArgumentException ("target VoxelArea has a different width than the data ("+target.width + " != " + width + ")"); if (target.depth != depth) throw new System.ArgumentException ("target VoxelArea has a different depth than the data ("+target.depth + " != " + depth + ")"); CompactVoxelCell[] cells = new CompactVoxelCell[reader.ReadInt32()]; CompactVoxelSpan[] spans = new CompactVoxelSpan[reader.ReadInt32()]; int[] areas = new int[reader.ReadInt32()]; for (int i=0;i<cells.Length;i++) { cells[i].index = reader.ReadUInt32(); cells[i].count = reader.ReadUInt32(); } for (int i=0;i<spans.Length;i++) { spans[i].con = reader.ReadUInt32(); spans[i].h = reader.ReadUInt32(); spans[i].reg = reader.ReadInt32(); spans[i].y = reader.ReadUInt16(); } for (int i=0;i<areas.Length;i++) { areas[i] = reader.ReadInt32(); } target.compactCells = cells; target.compactSpans = spans; target.areaTypes = areas; #else throw new System.NotImplementedException ("This method only works with !ASTAR_RECAST_CLASS_BASED_LINKED_LIST"); #endif }
public void BuildRegions() { AstarProfiler.StartProfile("Build Regions"); int w = voxelArea.width; int d = voxelArea.depth; int wd = w * d; int expandIterations = 8; int spanCount = voxelArea.compactSpanCount; //new List<int>(1024); //List<int> visited = new List<int>(1024); #if ASTAR_RECAST_BFS ushort[] srcReg = voxelArea.tmpUShortArr; if (srcReg.Length < spanCount) { srcReg = voxelArea.tmpUShortArr = new ushort[spanCount]; } Pathfinding.Util.Memory.MemSet <ushort> (srcReg, 0, sizeof(ushort)); #else List <int> stack = Pathfinding.Util.ListPool <int> .Claim(1024); ushort[] srcReg = new ushort[spanCount]; ushort[] srcDist = new ushort[spanCount]; ushort[] dstReg = new ushort[spanCount]; ushort[] dstDist = new ushort[spanCount]; #endif ushort regionId = 2; MarkRectWithRegion(0, borderSize, 0, d, (ushort)(regionId | BorderReg), srcReg); regionId++; MarkRectWithRegion(w - borderSize, w, 0, d, (ushort)(regionId | BorderReg), srcReg); regionId++; MarkRectWithRegion(0, w, 0, borderSize, (ushort)(regionId | BorderReg), srcReg); regionId++; MarkRectWithRegion(0, w, d - borderSize, d, (ushort)(regionId | BorderReg), srcReg); regionId++; #if ASTAR_RECAST_BFS uint level = 0; List <Int3> basins = Pathfinding.Util.ListPool <Int3> .Claim(100); //new List<Int3>(); // Find "basins" DebugReplay.BeginGroup("Basins"); for (int z = 0, pz = 0; z < wd; z += w, pz++) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[z + x]; for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++) { CompactVoxelSpan s = voxelArea.compactSpans[i]; bool anyBelow = false; if (voxelArea.areaTypes[i] == UnwalkableArea || srcReg[i] != 0) { continue; } for (int dir = 0; dir < 4; dir++) { if (s.GetConnection(dir) != NotConnected) { int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; int ni2 = (int)(voxelArea.compactCells[nx + nz].index + s.GetConnection(dir)); if (voxelArea.dist[i] < voxelArea.dist[ni2]) { anyBelow = true; break; } //CompactVoxelSpan ns = voxelArea.compactSpans[ni]; } } if (!anyBelow) { //Debug.DrawRay (this.ConvertPosition(x,z,i),Vector3.down,Color.red); DebugReplay.DrawCube(this.ConvertPosition(x, z, i), cellScale, Color.red); basins.Add(new Int3(x, i, z)); //System.Console.WriteLine ("Basin at " + voxelArea.dist[i]); level = System.Math.Max(level, voxelArea.dist[i]); } } } } //Start at maximum possible distance. & ~1 is rounding down to an even value level = (uint)((level + 1) & ~1); DebugReplay.EndGroup(); DebugReplay.BeginGroup("BFS"); List <Int3> st1 = Pathfinding.Util.ListPool <Int3> .Claim(300); List <Int3> st2 = Pathfinding.Util.ListPool <Int3> .Claim(300); //bool visited = new bool[voxelArea.compactSpanCount]; for (;; level -= 2) { DebugReplay.BeginGroup("BFS " + level); //System.Console.WriteLine ("Starting level " + level + " with st1.Count = " + st1.Count); int ocount = st1.Count; int expandCount = 0; if (ocount == 0) { //int c = 0; for (int q = 0; q < basins.Count; q++) { if (voxelArea.dist[basins[q].y] >= level) { DebugReplay.DrawCube(this.ConvertPosition(basins[q].x, basins[q].z, basins[q].y) + Vector3.up, cellScale, new Color(0, 1, 0, 0.5f)); } if (srcReg[basins[q].y] == 0 && voxelArea.dist[basins[q].y] >= level) { srcReg[basins[q].y] = 1; st1.Add(basins[q]); //c++; //visited[basins[i].y] = true; } } } for (int j = 0; j < st1.Count; j++) { int x = st1[j].x; int i = st1[j].y; int z = st1[j].z; ushort r = srcReg[i]; CompactVoxelSpan s = voxelArea.compactSpans[i]; int area = voxelArea.areaTypes[i]; DebugReplay.DrawCube(this.ConvertPosition(x, z, i), cellScale, Mathfx.IntToColor(srcReg[i], 0.7f)); bool anyAbove = false; for (int dir = 0; dir < 4; dir++) { if (s.GetConnection(dir) == NotConnected) { continue; } int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; int ni = (int)voxelArea.compactCells[nx + nz].index + s.GetConnection(dir); if (area != voxelArea.areaTypes[ni]) { continue; } if (voxelArea.dist[ni] < level) { anyAbove = true; continue; } if (srcReg[ni] == 0) { bool same = false; for (int v = (int)voxelArea.compactCells[nx + nz].index, vt = (int)voxelArea.compactCells[nx + nz].index + (int)voxelArea.compactCells[nx + nz].count; v < vt; v++) { if (srcReg[v] == srcReg[i]) { same = true; break; } } if (!same) { //if ((int)srcDist[ni]+2 < (int)d2) //{ //visited[i] = true; srcReg[ni] = r; //Debug.DrawRay (ConvertPosition(x,z,i),Vector3.up,Mathfx.IntToColor((int)level,0.6f)); //if (dstReg[ni] == 0) st1.Add(new Int3(nx, ni, nz)); //d2 = (ushort)(srcDist[ni]+2); //} } } } //Still on the edge if (anyAbove) { st2.Add(st1[j]); } if (j == ocount - 1) { expandCount++; ocount = st1.Count; if (expandCount == 8 || j == st1.Count - 1) { //int c = 0; for (int q = 0; q < basins.Count; q++) { if (voxelArea.dist[basins[q].y] >= level) { DebugReplay.DrawCube(this.ConvertPosition(basins[q].x, basins[q].z, basins[q].y) + Vector3.up, cellScale, new Color(0, 1, 0, 0.5f)); } if (srcReg[basins[q].y] == 0 && voxelArea.dist[basins[q].y] >= level) { srcReg[basins[q].y] = 1; st1.Add(basins[q]); //c++; //visited[basins[i].y] = true; } } } } } List <Int3> tmpList = st1; st1 = st2; st2 = tmpList; st2.Clear(); //System.Console.WriteLine ("Flooding basins"); for (int i = 0; i < basins.Count; i++) { if (srcReg[basins[i].y] == 1) { st2.Add(basins[i]); FloodOnes(st2, srcReg, level, regionId); regionId++; st2.Clear(); } } //System.Console.WriteLine ("Added " + c + " basins"); DebugReplay.EndGroup(); if (level == 0) { break; } } DebugReplay.EndGroup(); Pathfinding.Util.ListPool <Int3> .Release(st1); Pathfinding.Util.ListPool <Int3> .Release(st2); Pathfinding.Util.ListPool <Int3> .Release(basins); // Filter out small regions. voxelArea.maxRegions = regionId; FilterSmallRegions(srcReg, minRegionSize, voxelArea.maxRegions); // Write the result out. for (int i = 0; i < voxelArea.compactSpanCount; i++) { voxelArea.compactSpans[i].reg = srcReg[i]; } #else /// ====== Use original recast code ====== // //Start at maximum possible distance. & ~1 is rounding down to an even value uint level = (uint)((voxelArea.maxDistance + 1) & ~1); int count = 0; while (level > 0) { level = level >= 2 ? level - 2 : 0; AstarProfiler.StartProfile("--Expand Regions"); if (ExpandRegions(expandIterations, level, srcReg, srcDist, dstReg, dstDist, stack) != srcReg) { ushort[] tmp = srcReg; srcReg = dstReg; dstReg = tmp; tmp = srcDist; srcDist = dstDist; dstDist = tmp; } AstarProfiler.EndProfile("--Expand Regions"); AstarProfiler.StartProfile("--Mark Regions"); // Mark new regions with IDs. // Find "basins" for (int z = 0, pz = 0; z < wd; z += w, pz++) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[z + x]; for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++) { if (voxelArea.dist[i] < level || srcReg[i] != 0 || voxelArea.areaTypes[i] == UnwalkableArea) { continue; } if (FloodRegion(x, z, i, level, regionId, srcReg, srcDist, stack)) { regionId++; } } } } AstarProfiler.EndProfile("--Mark Regions"); count++; //if (count == 10) { // return; //} } if (ExpandRegions(expandIterations * 8, 0, srcReg, srcDist, dstReg, dstDist, stack) != srcReg) { ushort[] tmp = srcReg; srcReg = dstReg; dstReg = tmp; tmp = srcDist; srcDist = dstDist; dstDist = tmp; } // Filter out small regions. voxelArea.maxRegions = regionId; FilterSmallRegions(srcReg, minRegionSize, voxelArea.maxRegions); // Write the result out. for (int i = 0; i < voxelArea.compactSpanCount; i++) { voxelArea.compactSpans[i].reg = srcReg[i]; } Pathfinding.Util.ListPool <int> .Release(stack); /* * int sCount = voxelArea.GetSpanCount (); * Vector3[] debugPointsTop = new Vector3[sCount]; * Vector3[] debugPointsBottom = new Vector3[sCount]; * Color[] debugColors = new Color[sCount]; * * int debugPointsCount = 0; * //int wd = voxelArea.width*voxelArea.depth; * * for (int z=0, pz = 0;z < wd;z += voxelArea.width, pz++) { * for (int x=0;x < voxelArea.width;x++) { * * Vector3 p = new Vector3(x,0,pz)*cellSize+forcedBounds.min; * * //CompactVoxelCell c = voxelArea.compactCells[x+z]; * CompactVoxelCell c = voxelArea.compactCells[x+z]; * //if (c.count == 0) { * // Debug.DrawRay (p,Vector3.up,Color.red); * //} * * //for (int i=(int)c.index, ni = (int)(c.index+c.count);i<ni;i++) * * for (int i = (int)c.index; i < c.index+c.count; i++) { * CompactVoxelSpan s = voxelArea.compactSpans[i]; * //CompactVoxelSpan s = voxelArea.compactSpans[i]; * * p.y = ((float)(s.y+0.1F))*cellHeight+forcedBounds.min.y; * * debugPointsTop[debugPointsCount] = p; * * p.y = ((float)s.y)*cellHeight+forcedBounds.min.y; * debugPointsBottom[debugPointsCount] = p; * * debugColors[debugPointsCount] = Pathfinding.Mathfx.IntToColor(s.reg,0.7f);//s.reg == 1 ? Color.green : (s.reg == 2 ? Color.yellow : Color.red); * debugPointsCount++; * * //Debug.DrawRay (p,Vector3.up*0.5F,Color.green); * } * } * } * * DebugUtility.DrawCubes (debugPointsTop,debugPointsBottom,debugColors, cellSize);*/ #endif AstarProfiler.EndProfile("Build Regions"); }
public ushort[] BoxBlur(ushort[] src, ushort[] dst) { ushort thr = 20; int wd = voxelArea.width * voxelArea.depth; for (int z = wd - voxelArea.width; z >= 0; z -= voxelArea.width) { for (int x = voxelArea.width - 1; x >= 0; x--) { CompactVoxelCell c = voxelArea.compactCells[x + z]; for (int i = (int)c.index, ci = (int)(c.index + c.count); i < ci; i++) { CompactVoxelSpan s = voxelArea.compactSpans[i]; ushort cd = src[i]; if (cd < thr) { dst[i] = cd; continue; } int total = (int)cd; for (int d = 0; d < 4; d++) { if (s.GetConnection(d) != NotConnected) { int nx = x + voxelArea.DirectionX[d]; int nz = z + voxelArea.DirectionZ[d]; int ni = (int)(voxelArea.compactCells[nx + nz].index + s.GetConnection(d)); total += (int)src[ni]; CompactVoxelSpan ns = voxelArea.compactSpans[ni]; int d2 = (d + 1) & 0x3; if (ns.GetConnection(d2) != NotConnected) { int nnx = nx + voxelArea.DirectionX[d2]; int nnz = nz + voxelArea.DirectionZ[d2]; int nni = (int)(voxelArea.compactCells[nnx + nnz].index + ns.GetConnection(d2)); total += (int)src[nni]; } else { total += cd; } } else { total += cd * 2; } } dst[i] = (ushort)((total + 5) / 9F); } } } return(dst); }
public ushort CalculateDistanceField(ushort[] src) { int wd = voxelArea.width * voxelArea.depth; //Mark boundary cells for (int z = 0; z < wd; z += voxelArea.width) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[x + z]; for (int i = (int)c.index, ci = (int)(c.index + c.count); i < ci; i++) { CompactVoxelSpan s = voxelArea.compactSpans[i]; //int area = voxelArea.areaTypes[i]; int nc = 0; for (int d = 0; d < 4; d++) { if (s.GetConnection(d) != NotConnected) { /*int nx = x+voxelArea.DirectionX[d]; * int nz = z+voxelArea.DirectionZ[d]; * * int ni = (int)(voxelArea.compactCells[nx+nz].index+s.GetConnection (d));*/ //This function (CalculateDistanceField) is used for both ErodeWalkableArea and by itself. //The C++ recast source uses different code for those two cases, but I have found it works with one function //the voxelArea.areaTypes[ni] will actually only be one of two cases when used from ErodeWalkableArea //so it will have the same effect as // if (area != UnwalkableArea) { //This line is the one where the differ most nc++; #if FALSE if (area == voxelArea.areaTypes[ni]) { nc++; } else { //No way we can reach 4 break; } #endif } else { break; } } if (nc != 4) { src[i] = 0; } } } } //Pass 1 for (int z = 0; z < wd; z += voxelArea.width) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[x + z]; for (int i = (int)c.index, ci = (int)(c.index + c.count); i < ci; i++) { CompactVoxelSpan s = voxelArea.compactSpans[i]; if (s.GetConnection(0) != NotConnected) { // (-1,0) int nx = x + voxelArea.DirectionX[0]; int nz = z + voxelArea.DirectionZ[0]; int ni = (int)(voxelArea.compactCells[nx + nz].index + s.GetConnection(0)); if (src[ni] + 2 < src[i]) { src[i] = (ushort)(src[ni] + 2); } CompactVoxelSpan ns = voxelArea.compactSpans[ni]; if (ns.GetConnection(3) != NotConnected) { // (-1,0) + (0,-1) = (-1,-1) int nnx = nx + voxelArea.DirectionX[3]; int nnz = nz + voxelArea.DirectionZ[3]; int nni = (int)(voxelArea.compactCells[nnx + nnz].index + ns.GetConnection(3)); if (src[nni] + 3 < src[i]) { src[i] = (ushort)(src[nni] + 3); } } } if (s.GetConnection(3) != NotConnected) { // (0,-1) int nx = x + voxelArea.DirectionX[3]; int nz = z + voxelArea.DirectionZ[3]; int ni = (int)(voxelArea.compactCells[nx + nz].index + s.GetConnection(3)); if (src[ni] + 2 < src[i]) { src[i] = (ushort)(src[ni] + 2); } CompactVoxelSpan ns = voxelArea.compactSpans[ni]; if (ns.GetConnection(2) != NotConnected) { // (0,-1) + (1,0) = (1,-1) int nnx = nx + voxelArea.DirectionX[2]; int nnz = nz + voxelArea.DirectionZ[2]; int nni = (int)(voxelArea.compactCells[nnx + nnz].index + ns.GetConnection(2)); if (src[nni] + 3 < src[i]) { src[i] = (ushort)(src[nni] + 3); } } } } } } //Pass 2 for (int z = wd - voxelArea.width; z >= 0; z -= voxelArea.width) { for (int x = voxelArea.width - 1; x >= 0; x--) { CompactVoxelCell c = voxelArea.compactCells[x + z]; for (int i = (int)c.index, ci = (int)(c.index + c.count); i < ci; i++) { CompactVoxelSpan s = voxelArea.compactSpans[i]; if (s.GetConnection(2) != NotConnected) { // (-1,0) int nx = x + voxelArea.DirectionX[2]; int nz = z + voxelArea.DirectionZ[2]; int ni = (int)(voxelArea.compactCells[nx + nz].index + s.GetConnection(2)); if (src[ni] + 2 < src[i]) { src[i] = (ushort)(src[ni] + 2); } CompactVoxelSpan ns = voxelArea.compactSpans[ni]; if (ns.GetConnection(1) != NotConnected) { // (-1,0) + (0,-1) = (-1,-1) int nnx = nx + voxelArea.DirectionX[1]; int nnz = nz + voxelArea.DirectionZ[1]; int nni = (int)(voxelArea.compactCells[nnx + nnz].index + ns.GetConnection(1)); if (src[nni] + 3 < src[i]) { src[i] = (ushort)(src[nni] + 3); } } } if (s.GetConnection(1) != NotConnected) { // (0,-1) int nx = x + voxelArea.DirectionX[1]; int nz = z + voxelArea.DirectionZ[1]; int ni = (int)(voxelArea.compactCells[nx + nz].index + s.GetConnection(1)); if (src[ni] + 2 < src[i]) { src[i] = (ushort)(src[ni] + 2); } CompactVoxelSpan ns = voxelArea.compactSpans[ni]; if (ns.GetConnection(0) != NotConnected) { // (0,-1) + (1,0) = (1,-1) int nnx = nx + voxelArea.DirectionX[0]; int nnz = nz + voxelArea.DirectionZ[0]; int nni = (int)(voxelArea.compactCells[nnx + nnz].index + ns.GetConnection(0)); if (src[nni] + 3 < src[i]) { src[i] = (ushort)(src[nni] + 3); } } } } } } ushort maxDist = 0; for (int i = 0; i < voxelArea.compactSpanCount; i++) { maxDist = System.Math.Max(src[i], maxDist); } return(maxDist); }
Vector3 ConvertPosition(int x, int z, int i) { CompactVoxelSpan s = voxelArea.compactSpans[i]; return(new Vector3(x * cellSize, s.y * cellHeight, (z / (float)voxelArea.width) * cellSize) + voxelOffset); }
public ushort[] ExpandRegions(int maxIterations, uint level, ushort[] srcReg, ushort[] srcDist, ushort[] dstReg, ushort[] dstDist, List <int> stack) { AstarProfiler.StartProfile("---Expand 1"); int w = voxelArea.width; int d = voxelArea.depth; int wd = w * d; #if ASTAR_RECAST_BFS && FALSE List <int> st1 = new List <int>(); List <int> st2 = new List <int>(); for (int z = 0, pz = 0; z < wd; z += w, pz++) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[z + x]; for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++) { if (voxelArea.dist[i] >= level && srcReg[i] == 0 && voxelArea.areaTypes[i] != UnwalkableArea) { st2.Add(x); st2.Add(z); st2.Add(i); //Debug.DrawRay (ConvertPosition(x,z,i),Vector3.up*0.5F,Color.cyan); } } } } throw new System.NotImplementedException(); return(null); #else // Find cells revealed by the raised level. stack.Clear(); for (int z = 0, pz = 0; z < wd; z += w, pz++) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[z + x]; for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++) { if (voxelArea.dist[i] >= level && srcReg[i] == 0 && voxelArea.areaTypes[i] != UnwalkableArea) { stack.Add(x); stack.Add(z); stack.Add(i); //Debug.DrawRay (ConvertPosition(x,z,i),Vector3.up*0.5F,Color.cyan); } } } } AstarProfiler.EndProfile("---Expand 1"); AstarProfiler.StartProfile("---Expand 2"); int iter = 0; int stCount = stack.Count; if (stCount > 0) { while (true) { int failed = 0; AstarProfiler.StartProfile("---- Copy"); System.Buffer.BlockCopy(srcReg, 0, dstReg, 0, srcReg.Length * sizeof(ushort)); System.Buffer.BlockCopy(srcDist, 0, dstDist, 0, dstDist.Length * sizeof(ushort)); /*//NOTE: half of execution time is spent here, optimize!! * for (int i=0;i<srcReg.Length;i++) { * dstReg[i] = srcReg[i]; * } * * for (int i=0;i<srcDist.Length;i++) { * dstDist[i] = srcDist[i]; * }*/ AstarProfiler.EndProfile("---- Copy"); for (int j = 0; j < stCount; j += 3) { if (j >= stCount) { break; } int x = stack[j]; int z = stack[j + 1]; int i = stack[j + 2]; if (i < 0) { //Debug.DrawRay (ConvertPosition(x,z,i),Vector3.up*2,Color.blue); failed++; continue; } ushort r = srcReg[i]; ushort d2 = 0xffff; CompactVoxelSpan s = voxelArea.compactSpans[i]; int area = voxelArea.areaTypes[i]; for (int dir = 0; dir < 4; dir++) { if (s.GetConnection(dir) == NotConnected) { continue; } int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; int ni = (int)voxelArea.compactCells[nx + nz].index + s.GetConnection(dir); if (area != voxelArea.areaTypes[ni]) { continue; } if (srcReg[ni] > 0 && (srcReg[ni] & BorderReg) == 0) { if ((int)srcDist[ni] + 2 < (int)d2) { r = srcReg[ni]; d2 = (ushort)(srcDist[ni] + 2); } } } if (r != 0) { stack[j + 2] = -1; // mark as used dstReg[i] = r; dstDist[i] = d2; } else { failed++; //Debug.DrawRay (ConvertPosition(x,z,i),Vector3.up*2,Color.red); } } // Swap source and dest. ushort[] tmp = srcReg; srcReg = dstReg; dstReg = tmp; tmp = srcDist; srcDist = dstDist; dstDist = tmp; if (failed * 3 >= stCount) { //Debug.Log("Failed count broke "+failed); break; } if (level > 0) { iter++; if (iter >= maxIterations) { //Debug.Log("Iterations broke"); break; } } } } AstarProfiler.EndProfile("---Expand 2"); return(srcReg); #endif }
public bool FloodRegion(int x, int z, int i, uint level, ushort r, ushort[] srcReg, ushort[] srcDist, Int3[] stack, int[] flags = null, bool[] closed = null) { int area = voxelArea.areaTypes[i]; // Flood f mark region. int stackSize = 1; stack[0] = new Int3 { x = x, y = i, z = z, }; srcReg[i] = r; srcDist[i] = 0; int lev = (int)(level >= 2 ? level - 2 : 0); int count = 0; // Store these in local variables (for performance, avoids an extra indirection) var DirectionX = voxelArea.DirectionX; var DirectionZ = voxelArea.DirectionZ; var compactCells = voxelArea.compactCells; var compactSpans = voxelArea.compactSpans; var areaTypes = voxelArea.areaTypes; var dist = voxelArea.dist; while (stackSize > 0) { stackSize--; var c = stack[stackSize]; //Similar to the Pop operation of an array, but Pop is not implemented in List<> int ci = c.y; int cx = c.x; int cz = c.z; CompactVoxelSpan cs = compactSpans[ci]; //Debug.DrawRay (ConvertPosition(cx,cz,ci),Vector3.up, Color.cyan); // Check if any of the neighbours already have a valid region set. ushort ar = 0; // Loop through four neighbours // then check one neighbour of the neighbour // to get the diagonal neighbour for (int dir = 0; dir < 4; dir++) { // 8 connected if (cs.GetConnection(dir) != NotConnected) { int ax = cx + DirectionX[dir]; int az = cz + DirectionZ[dir]; int ai = (int)compactCells[ax + az].index + cs.GetConnection(dir); if (areaTypes[ai] != area) { continue; } ushort nr = srcReg[ai]; if ((nr & BorderReg) == BorderReg) // Do not take borders into account. { continue; } if (nr != 0 && nr != r) { ar = nr; // Found a valid region, skip checking the rest break; } // Rotate dir 90 degrees int dir2 = (dir + 1) & 0x3; var neighbour2 = compactSpans[ai].GetConnection(dir2); // Check the diagonal connection if (neighbour2 != NotConnected) { int ax2 = ax + DirectionX[dir2]; int az2 = az + DirectionZ[dir2]; int ai2 = (int)compactCells[ax2 + az2].index + neighbour2; if (areaTypes[ai2] != area) { continue; } ushort nr2 = srcReg[ai2]; if ((nr2 & BorderReg) == BorderReg) // Do not take borders into account. { continue; } if (nr2 != 0 && nr2 != r) { ar = nr2; // Found a valid region, skip checking the rest break; } } } } if (ar != 0) { srcReg[ci] = 0; srcDist[ci] = (ushort)0xFFFF; continue; } count++; if (closed != null) { closed[ci] = true; } // Expand neighbours. for (int dir = 0; dir < 4; ++dir) { if (cs.GetConnection(dir) != NotConnected) { int ax = cx + DirectionX[dir]; int az = cz + DirectionZ[dir]; int ai = (int)compactCells[ax + az].index + cs.GetConnection(dir); if (areaTypes[ai] != area) { continue; } if (srcReg[ai] == 0) { if (dist[ai] >= lev && flags[ai] == 0) { srcReg[ai] = r; srcDist[ai] = 0; stack[stackSize] = new Int3 { x = ax, y = ai, z = az, }; stackSize++; } else if (flags != null) { flags[ai] = r; srcDist[ai] = 2; } } } } } return(count > 0); }
public int GetCornerHeight(int x, int z, int i, int dir, ref bool isBorderVertex) { CompactVoxelSpan s = voxelArea.compactSpans[i]; int ch = (int)s.y; //dir + clockwise direction int dirp = (dir + 1) & 0x3; //int dirp = (dir+3) & 0x3; uint[] regs = new uint[4]; regs[0] = (uint)voxelArea.compactSpans[i].reg | ((uint)voxelArea.areaTypes[i] << 16); if (s.GetConnection(dir) != NotConnected) { int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; int ni = (int)voxelArea.compactCells[nx + nz].index + s.GetConnection(dir); CompactVoxelSpan ns = voxelArea.compactSpans[ni]; ch = System.Math.Max(ch, (int)ns.y); regs[1] = (uint)ns.reg | ((uint)voxelArea.areaTypes[ni] << 16); if (ns.GetConnection(dirp) != NotConnected) { int nx2 = nx + voxelArea.DirectionX[dirp]; int nz2 = nz + voxelArea.DirectionZ[dirp]; int ni2 = (int)voxelArea.compactCells[nx2 + nz2].index + ns.GetConnection(dirp); CompactVoxelSpan ns2 = voxelArea.compactSpans[ni2]; ch = System.Math.Max(ch, (int)ns2.y); regs[2] = (uint)ns2.reg | ((uint)voxelArea.areaTypes[ni2] << 16); } } if (s.GetConnection(dirp) != NotConnected) { int nx = x + voxelArea.DirectionX[dirp]; int nz = z + voxelArea.DirectionZ[dirp]; int ni = (int)voxelArea.compactCells[nx + nz].index + s.GetConnection(dirp); CompactVoxelSpan ns = voxelArea.compactSpans[ni]; ch = System.Math.Max(ch, (int)ns.y); regs[3] = (uint)ns.reg | ((uint)voxelArea.areaTypes[ni] << 16); if (ns.GetConnection(dir) != NotConnected) { int nx2 = nx + voxelArea.DirectionX[dir]; int nz2 = nz + voxelArea.DirectionZ[dir]; int ni2 = (int)voxelArea.compactCells[nx2 + nz2].index + ns.GetConnection(dir); CompactVoxelSpan ns2 = voxelArea.compactSpans[ni2]; ch = System.Math.Max(ch, (int)ns2.y); regs[2] = (uint)ns2.reg | ((uint)voxelArea.areaTypes[ni2] << 16); } } // Check if the vertex is special edge vertex, these vertices will be removed later. for (int j = 0; j < 4; ++j) { int a = j; int b = (j + 1) & 0x3; int c = (j + 2) & 0x3; int d = (j + 3) & 0x3; // The vertex is a border vertex there are two same exterior cells in a row, // followed by two interior cells and none of the regions are out of bounds. bool twoSameExts = (regs[a] & regs[b] & BorderReg) != 0 && regs[a] == regs[b]; bool twoInts = ((regs[c] | regs[d]) & BorderReg) == 0; bool intsSameArea = (regs[c] >> 16) == (regs[d] >> 16); bool noZeros = regs[a] != 0 && regs[b] != 0 && regs[c] != 0 && regs[d] != 0; if (twoSameExts && twoInts && intsSameArea && noZeros) { isBorderVertex = true; break; } } return(ch); }
public void WalkContour(int x, int z, int i, ushort[] flags, List <int> verts) { // Choose the first non-connected edge int dir = 0; while ((flags[i] & (ushort)(1 << dir)) == 0) { dir++; } int startDir = dir; int startI = i; int area = voxelArea.areaTypes[i]; int iter = 0; #if ASTARDEBUG Vector3 previousPos; Vector3 currentPos; previousPos = ConvertPos( x, 0, z ); Vector3 previousPos2 = ConvertPos( x, 0, z ); #endif while (iter++ < 40000) { //Are we facing a region edge if ((flags[i] & (ushort)(1 << dir)) != 0) { #if ASTARDEBUG Vector3 pos = ConvertPos(x, 0, z) + new Vector3((voxelArea.DirectionX[dir] != 0) ? Mathf.Sign(voxelArea.DirectionX[dir]) : 0, 0, (voxelArea.DirectionZ[dir]) != 0 ? Mathf.Sign(voxelArea.DirectionZ[dir]) : 0) * 0.6F; //int dir2 = (dir+1) & 0x3; //pos += new Vector3 ((voxelArea.DirectionX[dir2] != 0) ? Mathf.Sign(voxelArea.DirectionX[dir2]) : 0,0,(voxelArea.DirectionZ[dir2]) != 0 ? Mathf.Sign(voxelArea.DirectionZ[dir2]) : 0)*1.2F; //Debug.DrawLine (ConvertPos (x,0,z),pos,Color.cyan); Debug.DrawLine(previousPos2, pos, Color.blue); previousPos2 = pos; #endif //Choose the edge corner bool isBorderVertex = false; bool isAreaBorder = false; int px = x; int py = GetCornerHeight(x, z, i, dir, ref isBorderVertex); int pz = z; switch (dir) { case 0: pz += voxelArea.width;; break; case 1: px++; pz += voxelArea.width; break; case 2: px++; break; } /*case 1: px++; break; * case 2: px++; pz += voxelArea.width; break; * case 3: pz += voxelArea.width; break; */ int r = 0; CompactVoxelSpan s = voxelArea.compactSpans[i]; if (s.GetConnection(dir) != NotConnected) { int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; int ni = (int)voxelArea.compactCells[nx + nz].index + s.GetConnection(dir); r = (int)voxelArea.compactSpans[ni].reg; if (area != voxelArea.areaTypes[ni]) { isAreaBorder = true; } } if (isBorderVertex) { r |= RC_BORDER_VERTEX; } if (isAreaBorder) { r |= RC_AREA_BORDER; } verts.Add(px); verts.Add(py); verts.Add(pz); verts.Add(r); //Debug.DrawRay (previousPos,new Vector3 ((dir == 1 || dir == 2) ? 1 : 0, 0, (dir == 0 || dir == 1) ? 1 : 0),Color.cyan); flags[i] = (ushort)(flags[i] & ~(1 << dir)); // Remove visited edges dir = (dir + 1) & 0x3; // Rotate CW } else { int ni = -1; int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; CompactVoxelSpan s = voxelArea.compactSpans[i]; if (s.GetConnection(dir) != NotConnected) { CompactVoxelCell nc = voxelArea.compactCells[nx + nz]; ni = (int)nc.index + s.GetConnection(dir); } if (ni == -1) { Debug.LogWarning("Degenerate triangles might have been generated.\n" + "Usually this is not a problem, but if you have a static level, try to modify the graph settings slightly to avoid this edge case."); return; } x = nx; z = nz; i = ni; // & 0x3 is the same as % 4 (modulo 4) dir = (dir + 3) & 0x3; // Rotate CCW #if ASTARDEBUG currentPos = ConvertPos( x, 0, z ); Debug.DrawLine(previousPos + Vector3.up * 0, currentPos, Color.blue); previousPos = currentPos; #endif } if (startI == i && startDir == dir) { break; } } #if ASTARDEBUG Color col = new Color(Random.value, Random.value, Random.value); for (int q = 0, j = (verts.Count / 4) - 1; q < (verts.Count / 4); j = q, q++) { int i4 = q * 4; int j4 = j * 4; Vector3 p1 = ConvertPosWithoutOffset( verts[i4 + 0], verts[i4 + 1], verts[i4 + 2] ); Vector3 p2 = ConvertPosWithoutOffset( verts[j4 + 0], verts[j4 + 1], verts[j4 + 2] ); Debug.DrawLine(p1, p2, col); } #endif }
public void BuildContours(float maxError, int maxEdgeLength, VoxelContourSet cset, int buildFlags) { AstarProfiler.StartProfile("Build Contours"); AstarProfiler.StartProfile("- Init"); int w = voxelArea.width; int d = voxelArea.depth; int wd = w * d; //cset.bounds = voxelArea.bounds; int maxContours = Mathf.Max(8 /*Max Regions*/, 8); //cset.conts = new VoxelContour[maxContours]; List <VoxelContour> contours = new List <VoxelContour>(maxContours); AstarProfiler.EndProfile("- Init"); AstarProfiler.StartProfile("- Mark Boundaries"); //cset.nconts = 0; //NOTE: This array may contain any data, but since we explicitly set all data in it before we use it, it's OK. ushort[] flags = voxelArea.tmpUShortArr; if (flags.Length < voxelArea.compactSpanCount) { flags = voxelArea.tmpUShortArr = new ushort[voxelArea.compactSpanCount]; } // Mark boundaries. (@?) for (int z = 0; z < wd; z += voxelArea.width) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[x + z]; for (int i = (int)c.index, ci = (int)(c.index + c.count); i < ci; i++) { ushort res = 0; CompactVoxelSpan s = voxelArea.compactSpans[i]; if (s.reg == 0 || (s.reg & BorderReg) == BorderReg) { flags[i] = 0; continue; } for (int dir = 0; dir < 4; dir++) { int r = 0; if (s.GetConnection(dir) != NotConnected) { int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; int ni = (int)voxelArea.compactCells[nx + nz].index + s.GetConnection(dir); r = voxelArea.compactSpans[ni].reg; } //@TODO - Why isn't this inside the previous IF if (r == s.reg) { res |= (ushort)(1 << dir); } } //Inverse, mark non connected edges. flags[i] = (ushort)(res ^ 0xf); } } } AstarProfiler.EndProfile("- Mark Boundaries"); AstarProfiler.StartProfile("- Simplify Contours"); List <int> verts = Pathfinding.Util.ListPool <int> .Claim(256); //new List<int> (256); List <int> simplified = Pathfinding.Util.ListPool <int> .Claim(64); //new List<int> (64); for (int z = 0; z < wd; z += voxelArea.width) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[x + z]; for (int i = (int)c.index, ci = (int)(c.index + c.count); i < ci; i++) { //CompactVoxelSpan s = voxelArea.compactSpans[i]; if (flags[i] == 0 || flags[i] == 0xf) { flags[i] = 0; continue; } int reg = voxelArea.compactSpans[i].reg; if (reg == 0 || (reg & BorderReg) == BorderReg) { continue; } int area = voxelArea.areaTypes[i]; verts.Clear(); simplified.Clear(); WalkContour(x, z, i, flags, verts); SimplifyContour(verts, simplified, maxError, maxEdgeLength, buildFlags); RemoveDegenerateSegments(simplified); VoxelContour contour = new VoxelContour(); contour.verts = Pathfinding.Util.ArrayPool <int> .Claim(simplified.Count); //simplified.ToArray (); for (int j = 0; j < simplified.Count; j++) { contour.verts[j] = simplified[j]; } #if ASTAR_RECAST_INCLUDE_RAW_VERTEX_CONTOUR //Not used at the moment, just debug stuff contour.rverts = ClaimIntArr(verts.Count); for (int j = 0; j < verts.Count; j++) { contour.rverts[j] = verts[j]; } #endif contour.nverts = simplified.Count / 4; contour.reg = reg; contour.area = area; contours.Add(contour); #if ASTARDEBUG for (int q = 0, j = (simplified.Count / 4) - 1; q < (simplified.Count / 4); j = q, q++) { int i4 = q * 4; int j4 = j * 4; Vector3 p1 = Vector3.Scale( new Vector3( simplified[i4 + 0], simplified[i4 + 1], (simplified[i4 + 2] / (float)voxelArea.width) ), cellScale) + voxelOffset; Vector3 p2 = Vector3.Scale( new Vector3( simplified[j4 + 0], simplified[j4 + 1], (simplified[j4 + 2] / (float)voxelArea.width) ) , cellScale) + voxelOffset; if (CalcAreaOfPolygon2D(contour.verts, contour.nverts) > 0) { Debug.DrawLine(p1, p2, AstarMath.IntToColor(reg, 0.5F)); } else { Debug.DrawLine(p1, p2, Color.red); } } #endif } } } Pathfinding.Util.ListPool <int> .Release(ref verts); Pathfinding.Util.ListPool <int> .Release(ref simplified); AstarProfiler.EndProfile("- Simplify Contours"); AstarProfiler.StartProfile("- Fix Contours"); // Check and merge droppings. // Sometimes the previous algorithms can fail and create several contours // per area. This pass will try to merge the holes into the main region. for (int i = 0; i < contours.Count; i++) { VoxelContour cont = contours[i]; // Check if the contour is would backwards. if (CalcAreaOfPolygon2D(cont.verts, cont.nverts) < 0) { // Find another contour which has the same region ID. int mergeIdx = -1; for (int j = 0; j < contours.Count; j++) { if (i == j) { continue; } if (contours[j].nverts > 0 && contours[j].reg == cont.reg) { // Make sure the polygon is correctly oriented. if (CalcAreaOfPolygon2D(contours[j].verts, contours[j].nverts) > 0) { mergeIdx = j; break; } } } if (mergeIdx == -1) { Debug.LogError("rcBuildContours: Could not find merge target for bad contour " + i + "."); } else { // Debugging // Debug.LogWarning ("Fixing contour"); VoxelContour mcont = contours[mergeIdx]; // Merge by closest points. int ia = 0, ib = 0; GetClosestIndices(mcont.verts, mcont.nverts, cont.verts, cont.nverts, ref ia, ref ib); if (ia == -1 || ib == -1) { Debug.LogWarning("rcBuildContours: Failed to find merge points for " + i + " and " + mergeIdx + "."); continue; } #if ASTARDEBUG int p4 = ia * 4; int p42 = ib * 4; Vector3 p12 = Vector3.Scale( new Vector3( mcont.verts[p4 + 0], mcont.verts[p4 + 1], (mcont.verts[p4 + 2] / (float)voxelArea.width) ), cellScale) + voxelOffset; Vector3 p22 = Vector3.Scale( new Vector3( cont.verts[p42 + 0], cont.verts[p42 + 1], (cont.verts[p42 + 2] / (float)voxelArea.width) ) , cellScale) + voxelOffset; Debug.DrawLine(p12, p22, Color.green); #endif if (!MergeContours(ref mcont, ref cont, ia, ib)) { Debug.LogWarning("rcBuildContours: Failed to merge contours " + i + " and " + mergeIdx + "."); continue; } contours[mergeIdx] = mcont; contours[i] = cont; #if ASTARDEBUG Debug.Log(mcont.nverts); for (int q = 0, j = (mcont.nverts) - 1; q < (mcont.nverts); j = q, q++) { int i4 = q * 4; int j4 = j * 4; Vector3 p1 = Vector3.Scale( new Vector3( mcont.verts[i4 + 0], mcont.verts[i4 + 1], (mcont.verts[i4 + 2] / (float)voxelArea.width) ), cellScale) + voxelOffset; Vector3 p2 = Vector3.Scale( new Vector3( mcont.verts[j4 + 0], mcont.verts[j4 + 1], (mcont.verts[j4 + 2] / (float)voxelArea.width) ) , cellScale) + voxelOffset; Debug.DrawLine(p1, p2, Color.red); //} } #endif } } } cset.conts = contours; AstarProfiler.EndProfile("- Fix Contours"); AstarProfiler.EndProfile("Build Contours"); }
public void BuildVoxelConnections() { AstarProfiler.StartProfile("Build Voxel Connections"); int wd = voxelArea.width * voxelArea.depth; CompactVoxelSpan[] spans = voxelArea.compactSpans; CompactVoxelCell[] cells = voxelArea.compactCells; // Build voxel connections for (int z = 0, pz = 0; z < wd; z += voxelArea.width, pz++) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = cells[x + z]; for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++) { CompactVoxelSpan s = spans[i]; spans[i].con = 0xFFFFFFFF; for (int d = 0; d < 4; d++) { int nx = x + voxelArea.DirectionX[d]; int nz = z + voxelArea.DirectionZ[d]; if (nx < 0 || nz < 0 || nz >= wd || nx >= voxelArea.width) { continue; } CompactVoxelCell nc = cells[nx + nz]; for (int k = (int)nc.index, nk = (int)(nc.index + nc.count); k < nk; k++) { CompactVoxelSpan ns = spans[k]; int bottom = System.Math.Max(s.y, ns.y); int top = System.Math.Min((int)s.y + (int)s.h, (int)ns.y + (int)ns.h); if ((top - bottom) >= voxelWalkableHeight && System.Math.Abs((int)ns.y - (int)s.y) <= voxelWalkableClimb) { uint connIdx = (uint)k - nc.index; if (connIdx > MaxLayers) { Debug.LogError("Too many layers"); continue; } spans[i].SetConnection(d, connIdx); break; } } } } } } AstarProfiler.EndProfile("Build Voxel Connections"); }
public void DebugDrawCompactSpans() { int sCount = voxelArea.compactSpans.Length; Vector3[] debugPointsTop = new Vector3[sCount]; Vector3[] debugPointsBottom = new Vector3[sCount]; Color[] debugColors = new Color[sCount]; int debugPointsCount = 0; int wd = voxelArea.width * voxelArea.depth; var min = forcedBounds.min; for (int z = 0, pz = 0; z < wd; z += voxelArea.width, pz++) { for (int x = 0; x < voxelArea.width; x++) { Vector3 p = new Vector3(x, 0, pz) * cellSize + min; //CompactVoxelCell c = voxelArea.compactCells[x+z]; CompactVoxelCell c = voxelArea.compactCells[x + z]; //if (c.count == 0) { // Debug.DrawRay (p,Vector3.up,Color.red); //} for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++) { CompactVoxelSpan s = voxelArea.compactSpans[i]; p.y = ((float)(s.y + 0.1F)) * cellHeight + min.y; debugPointsTop[debugPointsCount] = p; p.y = ((float)s.y) * cellHeight + min.y; debugPointsBottom[debugPointsCount] = p; Color col = Color.black; switch (s.reg) { case 0: col = Color.red; break; case 1: col = Color.green; break; case 2: col = Color.yellow; break; case 3: col = Color.magenta; break; } debugColors[debugPointsCount] = col; //Color.Lerp (Color.black, Color.white , (float)dst[i] / (float)voxelArea.maxDistance);//(float)(Mathf.Abs(dst[i]-src[i])) / (float)5);//s.area == 1 ? Color.green : (s.area == 2 ? Color.yellow : Color.red); debugPointsCount++; //Debug.DrawRay (p,Vector3.up*0.5F,Color.green); } } } DebugUtility.DrawCubes(debugPointsTop, debugPointsBottom, debugColors, cellSize); }
public void BuildVoxelConnections() { AstarProfiler.StartProfile("Build Voxel Connections"); int wd = voxelArea.width * voxelArea.depth; CompactVoxelSpan[] spans = voxelArea.compactSpans; CompactVoxelCell[] cells = voxelArea.compactCells; //Build voxel connections for (int z = 0, pz = 0; z < wd; z += voxelArea.width, pz++) { //System.Threading.ManualResetEvent[] handles = new System.Threading.ManualResetEvent[voxelArea.depth]; //This will run the loop in multiple threads (speedup by ? 40%) //Parallel.For (0, voxelArea.depth, delegate (int pz) { //System.Threading.WaitCallback del = delegate (System.Object _pz) { //int pz = (int)_pz; //int z = pz*voxelArea.width; for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = cells[x + z]; for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++) { CompactVoxelSpan s = spans[i]; spans[i].con = 0xFFFFFFFF; for (int d = 0; d < 4; d++) { int nx = x + voxelArea.DirectionX[d]; int nz = z + voxelArea.DirectionZ[d]; if (nx < 0 || nz < 0 || nz >= wd || nx >= voxelArea.width) { continue; } CompactVoxelCell nc = cells[nx + nz]; for (int k = (int)nc.index, nk = (int)(nc.index + nc.count); k < nk; k++) { CompactVoxelSpan ns = spans[k]; int bottom = System.Math.Max(s.y, ns.y); int top = System.Math.Min((int)s.y + (int)s.h, (int)ns.y + (int)ns.h); if ((top - bottom) >= voxelWalkableHeight && System.Math.Abs((int)ns.y - (int)s.y) <= voxelWalkableClimb) { uint connIdx = (uint)k - nc.index; if (connIdx > MaxLayers) { Debug.LogError("Too many layers"); continue; } spans[i].SetConnection(d, connIdx); break; } } } } } //handles[pz].Set (); //}; //}); } /*for (int z=0, pz = 0;z < wd;z += voxelArea.width, pz++) { * handles[pz] = new System.Threading.ManualResetEvent(false); * System.Threading.ThreadPool.QueueUserWorkItem (del, pz); * } * * System.Threading.WaitHandle.WaitAll (handles);*/ AstarProfiler.EndProfile("Build Voxel Connections"); }
/** Filters out or merges small regions. */ public void FilterSmallRegions(ushort[] reg, int minRegionSize, int maxRegions) { /*int maxID = 0; * for (int i=0;i<reg.Length;i++) { * * maxID = Mathf.Max (maxID,(int)reg[i]); * }*/ RelevantGraphSurface c = RelevantGraphSurface.Root; bool anySurfaces = c != null && (relevantGraphSurfaceMode != RecastGraph.RelevantGraphSurfaceMode.DoNotRequire); // Nothing to do here if (!anySurfaces && minRegionSize <= 0) { return; } int[] counter = new int[maxRegions]; ushort[] bits = voxelArea.tmpUShortArr; if (bits == null || bits.Length < maxRegions) { bits = voxelArea.tmpUShortArr = new ushort[maxRegions]; } Util.Memory.MemSet(counter, -1, sizeof(int)); Util.Memory.MemSet(bits, (ushort)0, maxRegions, sizeof(ushort)); int nReg = counter.Length; int wd = voxelArea.width * voxelArea.depth; const int RelevantSurfaceSet = 1 << 1; const int BorderBit = 1 << 0; // Mark RelevantGraphSurfaces // If they can also be adjacent to tile borders, this will also include the BorderBit int RelevantSurfaceCheck = RelevantSurfaceSet | ((relevantGraphSurfaceMode == RecastGraph.RelevantGraphSurfaceMode.OnlyForCompletelyInsideTile) ? BorderBit : 0x0); if (anySurfaces) { while (c != null) { int x, z; this.VectorToIndex(c.Position, out x, out z); // Out of bounds if (x < 0 || z < 0 || x >= voxelArea.width || z >= voxelArea.depth) { c = c.Next; continue; } int y = (int)((c.Position.y - voxelOffset.y) / cellHeight); int rad = (int)(c.maxRange / cellHeight); CompactVoxelCell cell = voxelArea.compactCells[x + z * voxelArea.width]; for (int i = (int)cell.index; i < cell.index + cell.count; i++) { CompactVoxelSpan s = voxelArea.compactSpans[i]; if (System.Math.Abs(s.y - y) <= rad && reg[i] != 0) { bits[union_find_find(counter, (int)reg[i] & ~BorderReg)] |= RelevantSurfaceSet; } } c = c.Next; } } for (int z = 0, pz = 0; z < wd; z += voxelArea.width, pz++) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell cell = voxelArea.compactCells[x + z]; for (int i = (int)cell.index; i < cell.index + cell.count; i++) { CompactVoxelSpan s = voxelArea.compactSpans[i]; int r = (int)reg[i]; if ((r & ~BorderReg) == 0) { continue; } if (r >= nReg) //Probably border { bits[union_find_find(counter, r & ~BorderReg)] |= BorderBit; continue; } int k = union_find_find(counter, r); // Count this span counter[k]--; for (int dir = 0; dir < 4; dir++) { if (s.GetConnection(dir) == NotConnected) { continue; } int nx = x + voxelArea.DirectionX[dir]; int nz = z + voxelArea.DirectionZ[dir]; int ni = (int)voxelArea.compactCells[nx + nz].index + s.GetConnection(dir); int r2 = (int)reg[ni]; if (r != r2 && (r2 & ~BorderReg) != 0) { if ((r2 & BorderReg) != 0) { bits[k] |= BorderBit; } else { union_find_union(counter, k, r2); } //counter[r] = minRegionSize; } } //counter[r]++; } } } // Propagate bits for (int i = 0; i < counter.Length; i++) { bits[union_find_find(counter, i)] |= bits[i]; } for (int i = 0; i < counter.Length; i++) { int ctr = union_find_find(counter, i); // Adjacent to border if ((bits[ctr] & BorderBit) != 0) { counter[ctr] = -minRegionSize - 2; } // Not in any relevant surface // or it is adjacent to a border (see RelevantSurfaceCheck) if (anySurfaces && (bits[ctr] & RelevantSurfaceCheck) == 0) { counter[ctr] = -1; } } for (int i = 0; i < voxelArea.compactSpanCount; i++) { int r = (int)reg[i]; if (r >= nReg) { continue; } if (counter[union_find_find(counter, r)] >= -minRegionSize - 1) { reg[i] = 0; } } }
public void ErodeVoxels(int radius) { if (radius > 255) { Debug.LogError("Max Erode Radius is 255"); radius = 255; } int wd = voxelArea.width * voxelArea.depth; int[] dist = new int[voxelArea.compactSpanCount]; for (int i = 0; i < dist.Length; i++) { dist[i] = 0xFF; } for (int z = 0; z < wd; z += voxelArea.width) { for (int x = 0; x < voxelArea.width; x++) { CompactVoxelCell c = voxelArea.compactCells[x + z]; for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++) { if (voxelArea.areaTypes[i] != UnwalkableArea) { CompactVoxelSpan s = voxelArea.compactSpans[i]; int nc = 0; for (int dir = 0; dir < 4; dir++) { if (s.GetConnection(dir) != NotConnected) { nc++; } } //At least one missing neighbour if (nc != 4) { dist[i] = 0; } } } } } //int nd = 0; //Pass 1 /*for (int z=0;z < wd;z += voxelArea.width) { * for (int x=0;x < voxelArea.width;x++) { * * CompactVoxelCell c = voxelArea.compactCells[x+z]; * * for (int i= (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { * CompactVoxelSpan s = voxelArea.compactSpans[i]; * * if (s.GetConnection (0) != NotConnected) { * // (-1,0) * int nx = x+voxelArea.DirectionX[0]; * int nz = z+voxelArea.DirectionZ[0]; * * int ni = (int)(voxelArea.compactCells[nx+nz].index+s.GetConnection (0)); * CompactVoxelSpan ns = voxelArea.compactSpans[ni]; * * if (dist[ni]+2 < dist[i]) { * dist[i] = (ushort)(dist[ni]+2); * } * * if (ns.GetConnection (3) != NotConnected) { * // (-1,0) + (0,-1) = (-1,-1) * int nnx = nx+voxelArea.DirectionX[3]; * int nnz = nz+voxelArea.DirectionZ[3]; * * int nni = (int)(voxelArea.compactCells[nnx+nnz].index+ns.GetConnection (3)); * * if (src[nni]+3 < src[i]) { * src[i] = (ushort)(src[nni]+3); * } * } * } * * if (s.GetConnection (3) != NotConnected) { * // (0,-1) * int nx = x+voxelArea.DirectionX[3]; * int nz = z+voxelArea.DirectionZ[3]; * * int ni = (int)(voxelArea.compactCells[nx+nz].index+s.GetConnection (3)); * * if (src[ni]+2 < src[i]) { * src[i] = (ushort)(src[ni]+2); * } * * CompactVoxelSpan ns = voxelArea.compactSpans[ni]; * * if (ns.GetConnection (2) != NotConnected) { * * // (0,-1) + (1,0) = (1,-1) * int nnx = nx+voxelArea.DirectionX[2]; * int nnz = nz+voxelArea.DirectionZ[2]; * * int nni = (int)(voxelArea.compactCells[nnx+nnz].index+ns.GetConnection (2)); * * if (src[nni]+3 < src[i]) { * src[i] = (ushort)(src[nni]+3); * } * } * } * } * } * }*/ }
public bool FloodRegion(int x, int z, int i, uint level, ushort r, ushort[] srcReg, ushort[] srcDist, List <int> stack) { int area = voxelArea.areaTypes[i]; // Flood fill mark region. stack.Clear(); stack.Add(x); stack.Add(z); stack.Add(i); srcReg[i] = r; srcDist[i] = 0; int lev = (int)(level >= 2 ? level - 2 : 0); int count = 0; while (stack.Count > 0) { //Similar to the Pop operation of an array, but Pop is not implemented in List<> int ci = stack[stack.Count - 1]; stack.RemoveAt(stack.Count - 1); int cz = stack[stack.Count - 1]; stack.RemoveAt(stack.Count - 1); int cx = stack[stack.Count - 1]; stack.RemoveAt(stack.Count - 1); CompactVoxelSpan cs = voxelArea.compactSpans[ci]; //Debug.DrawRay (ConvertPosition(cx,cz,ci),Vector3.up, Color.cyan); // Check if any of the neighbours already have a valid region set. ushort ar = 0; for (int dir = 0; dir < 4; dir++) { // 8 connected if (cs.GetConnection(dir) != NotConnected) { int ax = cx + voxelArea.DirectionX[dir]; int az = cz + voxelArea.DirectionZ[dir]; int ai = (int)voxelArea.compactCells[ax + az].index + cs.GetConnection(dir); if (voxelArea.areaTypes[ai] != area) { continue; } ushort nr = srcReg[ai]; if ((nr & BorderReg) == BorderReg) // Do not take borders into account. { continue; } if (nr != 0 && nr != r) { ar = nr; } CompactVoxelSpan aspan = voxelArea.compactSpans[ai]; int dir2 = (dir + 1) & 0x3; if (aspan.GetConnection(dir2) != NotConnected) { int ax2 = ax + voxelArea.DirectionX[dir2]; int az2 = az + voxelArea.DirectionZ[dir2]; int ai2 = (int)voxelArea.compactCells[ax2 + az2].index + aspan.GetConnection(dir2); if (voxelArea.areaTypes[ai2] != area) { continue; } nr = srcReg[ai2]; if (nr != 0 && nr != r) { ar = nr; } } } } if (ar != 0) { srcReg[ci] = 0; continue; } count++; // Expand neighbours. for (int dir = 0; dir < 4; ++dir) { if (cs.GetConnection(dir) != NotConnected) { int ax = cx + voxelArea.DirectionX[dir]; int az = cz + voxelArea.DirectionZ[dir]; int ai = (int)voxelArea.compactCells[ax + az].index + cs.GetConnection(dir); if (voxelArea.areaTypes[ai] != area) { continue; } if (voxelArea.dist[ai] >= lev && srcReg[ai] == 0) { srcReg[ai] = r; srcDist[ai] = 0; stack.Add(ax); stack.Add(az); stack.Add(ai); } } } } return(count > 0); }