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 MarkRectWithRegion(int minx, int maxx, int minz, int maxz, ushort region, ushort[] srcReg)
{
int md = maxz * voxelArea.width;
for (int z = minz * voxelArea.width; z < md; z += voxelArea.width)
{
for (int x = minx; x < maxx; x++)
{
CompactVoxelCell c = voxelArea.compactCells[z + x];
for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++)
{
if (voxelArea.areaTypes[i] != UnwalkableArea)
{
srcReg[i] = region;
}
}
}
}
}
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 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 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");
}
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 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);
}
/** 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 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 void BuildRegions()
{
/*System.Diagnostics.Stopwatch w0 = new System.Diagnostics.Stopwatch();
* System.Diagnostics.Stopwatch w1 = new System.Diagnostics.Stopwatch();
* System.Diagnostics.Stopwatch w2 = new System.Diagnostics.Stopwatch();
* System.Diagnostics.Stopwatch w3 = new System.Diagnostics.Stopwatch();
* System.Diagnostics.Stopwatch w4 = new System.Diagnostics.Stopwatch();
* System.Diagnostics.Stopwatch w5 = new System.Diagnostics.Stopwatch();
* w3.Start();*/
int w = voxelArea.width;
int d = voxelArea.depth;
int wd = w * d;
int spanCount = voxelArea.compactSpanCount;
int expandIterations = 8;
ushort[] srcReg = Util.ArrayPool <ushort> .Claim(spanCount);
ushort[] srcDist = Util.ArrayPool <ushort> .Claim(spanCount);
bool[] closed = Util.ArrayPool <bool> .Claim(spanCount);
int[] spanFlags = Util.ArrayPool <int> .Claim(spanCount);
Int3[] stack = Util.ArrayPool <Int3> .Claim(spanCount);
// The array pool arrays may contain arbitrary data. We need to zero it out.
Util.Memory.MemSet(srcReg, (ushort)0, sizeof(ushort));
Util.Memory.MemSet(srcDist, (ushort)0xFFFF, sizeof(ushort));
Util.Memory.MemSet(closed, false, sizeof(bool));
Util.Memory.MemSet(spanFlags, 0, sizeof(int));
var DirectionX = voxelArea.DirectionX;
var DirectionZ = voxelArea.DirectionZ;
var spanDistances = voxelArea.dist;
var areaTypes = voxelArea.areaTypes;
var compactCells = voxelArea.compactCells;
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++;
Int3[][] buckedSortedSpans = new Int3[(voxelArea.maxDistance) / 2 + 1][];
int[] sortedSpanCounts = new int[buckedSortedSpans.Length];
for (int i = 0; i < buckedSortedSpans.Length; i++)
{
buckedSortedSpans[i] = new Int3[16];
}
//w3.Stop();
//w5.Start();
// Bucket sort the spans based on distance
for (int z = 0, pz = 0; z < wd; z += w, pz++)
{
for (int x = 0; x < voxelArea.width; x++)
{
CompactVoxelCell c = compactCells[z + x];
for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; i++)
{
if ((srcReg[i] & BorderReg) == BorderReg) // Do not take borders into account.
{
continue;
}
if (areaTypes[i] == UnwalkableArea)
{
continue;
}
int distIndex = voxelArea.dist[i] / 2;
if (sortedSpanCounts[distIndex] >= buckedSortedSpans[distIndex].Length)
{
var newBuffer = new Int3[sortedSpanCounts[distIndex] * 2];
buckedSortedSpans[distIndex].CopyTo(newBuffer, 0);
buckedSortedSpans[distIndex] = newBuffer;
}
buckedSortedSpans[distIndex][sortedSpanCounts[distIndex]++] = new Int3(x, i, z);
}
}
}
//w5.Stop();
Queue <Int3> srcQue = new Queue <Int3>();
Queue <Int3> dstQue = new Queue <Int3>();
// Go through spans in reverse order (i.e largest distances first)
for (int distIndex = buckedSortedSpans.Length - 1; distIndex >= 0; distIndex--)
{
var level = (uint)distIndex * 2;
var spans = buckedSortedSpans[distIndex];
var spansAtLevel = sortedSpanCounts[distIndex];
for (int i = 0; i < spansAtLevel; i++)
{
int spanIndex = spans[i].y;
// This span is adjacent to a region, so we should start the BFS search from it
if (spanFlags[spanIndex] != 0 && srcReg[spanIndex] == 0)
{
srcReg[spanIndex] = (ushort)spanFlags[spanIndex];
srcQue.Enqueue(spans[i]);
closed[spanIndex] = true;
}
}
// Expand a few iterations out from every known node
for (int expansionIteration = 0; expansionIteration < expandIterations && srcQue.Count > 0; expansionIteration++)
{
while (srcQue.Count > 0)
{
Int3 spanInfo = srcQue.Dequeue();
var area = areaTypes[spanInfo.y];
var span = voxelArea.compactSpans[spanInfo.y];
var region = srcReg[spanInfo.y];
closed[spanInfo.y] = true;
ushort nextDist = (ushort)(srcDist[spanInfo.y] + 2);
// Go through the neighbours of the span
for (int dir = 0; dir < 4; dir++)
{
var neighbour = span.GetConnection(dir);
if (neighbour == NotConnected)
{
continue;
}
int nx = spanInfo.x + DirectionX[dir];
int nz = spanInfo.z + DirectionZ[dir];
int ni = (int)compactCells[nx + nz].index + neighbour;
if ((srcReg[ni] & BorderReg) == BorderReg) // Do not take borders into account.
{
continue;
}
// Do not combine different area types
if (area == areaTypes[ni])
{
if (nextDist < srcDist[ni])
{
if (spanDistances[ni] < level)
{
srcDist[ni] = nextDist;
spanFlags[ni] = region;
}
else if (!closed[ni])
{
srcDist[ni] = nextDist;
if (srcReg[ni] == 0)
{
dstQue.Enqueue(new Int3(nx, ni, nz));
}
srcReg[ni] = region;
}
}
}
}
}
Util.Memory.Swap(ref srcQue, ref dstQue);
}
// Find the first span that has not been seen yet and start a new region that expands from there
for (int i = 0; i < spansAtLevel; i++)
{
var info = spans[i];
if (srcReg[info.y] == 0)
{
if (!FloodRegion(info.x, info.z, info.y, level, regionId, srcReg, srcDist, stack, spanFlags, closed))
{
// The starting voxel was already adjacent to an existing region so we skip flooding it.
// It will be visited in the next area expansion.
}
else
{
regionId++;
}
}
}
}
voxelArea.maxRegions = regionId;
// Filter out small regions.
FilterSmallRegions(srcReg, minRegionSize, voxelArea.maxRegions);
// Write the result out.
var compactSpans = voxelArea.compactSpans;
for (int i = 0; i < spanCount; i++)
{
compactSpans[i].reg = srcReg[i];
}
// Pool arrays
Util.ArrayPool <ushort> .Release(ref srcReg);
Util.ArrayPool <ushort> .Release(ref srcDist);
Util.ArrayPool <bool> .Release(ref closed);
Util.ArrayPool <int> .Release(ref spanFlags);
Util.ArrayPool <Int3> .Release(ref stack);
//Debug.Log(w0.Elapsed.TotalMilliseconds.ToString("0.0") + " " + w1.Elapsed.TotalMilliseconds.ToString("0.0") + " " + w2.Elapsed.TotalMilliseconds.ToString("0.0") + " " + w3.Elapsed.TotalMilliseconds.ToString("0.0") + " " + w4.Elapsed.TotalMilliseconds.ToString("0.0") + " " + w5.Elapsed.TotalMilliseconds.ToString("0.0"));
}
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");
}
