public void BuildRegions()
{
AstarProfiler.StartProfile("Build Regions");
int w = voxelArea.width;
int d = voxelArea.depth;
int wd = w * d;
int spanCount = voxelArea.compactSpanCount;
#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
int expandIterations = 8;
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);
// 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++)
{
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)
{
basins.Add(new Int3(x, i, z));
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);
List <Int3> st1 = Pathfinding.Util.ListPool <Int3> .Claim(300);
List <Int3> st2 = Pathfinding.Util.ListPool <Int3> .Claim(300);
// Some debug code
//bool visited = new bool[voxelArea.compactSpanCount];
for (;; level -= 2)
{
int ocount = st1.Count;
int expandCount = 0;
if (ocount == 0)
{
//int c = 0;
for (int q = 0; q < basins.Count; q++)
{
if (srcReg[basins[q].y] == 0 && voxelArea.dist[basins[q].y] >= level)
{
srcReg[basins[q].y] = 1;
st1.Add(basins[q]);
// Some debug code
//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];
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)
{
srcReg[ni] = r;
//Debug.DrawRay (ConvertPosition(x,z,i),Vector3.up,AstarMath.IntToColor((int)level,0.6f));
st1.Add(new Int3(nx, ni, nz));
}
}
}
//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 (srcReg[basins[q].y] == 0 && voxelArea.dist[basins[q].y] >= level)
{
srcReg[basins[q].y] = 1;
st1.Add(basins[q]);
// Debug code
//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();
}
}
if (level == 0)
{
break;
}
}
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 (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(ref stack);
// Some debug code not currently used
/*
* 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.AstarMath.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");
}
/** Called when a path has completed.
* Will post process it and return it by calling #tmpPathCallback and #pathCallback */
public void OnPathComplete(Path p, bool runModifiers, bool sendCallbacks)
{
AstarProfiler.StartProfile("Seeker OnPathComplete");
if (p != null && p != path && sendCallbacks)
{
return;
}
if (this == null || p == null || p != path)
{
return;
}
if (!path.error && runModifiers)
{
AstarProfiler.StartProfile("Seeker Modifiers");
//This will send the path for post processing to modifiers attached to this Seeker
RunModifiers(ModifierPass.PostProcessOriginal, path);
//This will send the path for post processing to modifiers attached to this Seeker
RunModifiers(ModifierPass.PostProcess, path);
AstarProfiler.EndProfile();
}
if (sendCallbacks)
{
p.Claim(this);
AstarProfiler.StartProfile("Seeker Callbacks");
lastCompletedNodePath = p.path;
lastCompletedVectorPath = p.vectorPath;
//This will send the path to the callback (if any) specified when calling StartPath
if (tmpPathCallback != null)
{
tmpPathCallback(p);
}
//This will send the path to any script which has registered to the callback
if (pathCallback != null)
{
pathCallback(p);
}
//Recycle the previous path
if (prevPath != null)
{
prevPath.ReleaseSilent(this);
}
prevPath = p;
//If not drawing gizmos, then storing prevPath is quite unecessary
//So clear it and set prevPath to null
if (!drawGizmos)
{
ReleaseClaimedPath();
}
AstarProfiler.EndProfile();
}
AstarProfiler.EndProfile();
}
/// <summary>
/// Builds a polygon mesh from a contour set.
///
/// Warning: Currently locked to 3.
/// </summary>
/// <param name="cset">contour set to build a mesh from.</param>
/// <param name="nvp">Maximum allowed vertices per polygon.</param>
/// <param name="mesh">Results will be written to this mesh.</param>
public void BuildPolyMesh(VoxelContourSet cset, int nvp, out VoxelMesh mesh)
{
AstarProfiler.StartProfile("Build Poly Mesh");
nvp = 3;
int maxVertices = 0;
int maxTris = 0;
int maxVertsPerCont = 0;
for (int i = 0; i < cset.conts.Count; i++)
{
// Skip null contours.
if (cset.conts[i].nverts < 3)
{
continue;
}
maxVertices += cset.conts[i].nverts;
maxTris += cset.conts[i].nverts - 2;
maxVertsPerCont = System.Math.Max(maxVertsPerCont, cset.conts[i].nverts);
}
Int3[] verts = ArrayPool <Int3> .Claim(maxVertices);
int[] polys = ArrayPool <int> .Claim(maxTris *nvp);
int[] areas = ArrayPool <int> .Claim(maxTris);
Pathfinding.Util.Memory.MemSet <int>(polys, 0xff, sizeof(int));
int[] indices = ArrayPool <int> .Claim(maxVertsPerCont);
int[] tris = ArrayPool <int> .Claim(maxVertsPerCont * 3);
int vertexIndex = 0;
int polyIndex = 0;
int areaIndex = 0;
for (int i = 0; i < cset.conts.Count; i++)
{
VoxelContour cont = cset.conts[i];
// Skip degenerate contours
if (cont.nverts < 3)
{
continue;
}
for (int j = 0; j < cont.nverts; j++)
{
indices[j] = j;
// Convert the z coordinate from the form z*voxelArea.width which is used in other places for performance
cont.verts[j * 4 + 2] /= voxelArea.width;
}
// Triangulate the contour
int ntris = Triangulate(cont.nverts, cont.verts, ref indices, ref tris);
// Assign the correct vertex indices
int startIndex = vertexIndex;
for (int j = 0; j < ntris * 3; polyIndex++, j++)
{
//@Error sometimes
polys[polyIndex] = tris[j] + startIndex;
}
// Mark all triangles generated by this contour
// as having the area cont.area
for (int j = 0; j < ntris; areaIndex++, j++)
{
areas[areaIndex] = cont.area;
}
// Copy the vertex positions
for (int j = 0; j < cont.nverts; vertexIndex++, j++)
{
verts[vertexIndex] = new Int3(cont.verts[j * 4], cont.verts[j * 4 + 1], cont.verts[j * 4 + 2]);
}
}
mesh = new VoxelMesh
{
verts = Memory.ShrinkArray(verts, vertexIndex),
tris = Memory.ShrinkArray(polys, polyIndex),
areas = Memory.ShrinkArray(areas, areaIndex)
};
ArrayPool <Int3> .Release(ref verts);
ArrayPool <int> .Release(ref polys);
ArrayPool <int> .Release(ref areas);
ArrayPool <int> .Release(ref indices);
ArrayPool <int> .Release(ref tris);
AstarProfiler.EndProfile("Build Poly Mesh");
}
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");
// Copy srcReg and srcDist to dstReg and dstDist (but faster than a normal loop)
System.Buffer.BlockCopy(srcReg, 0, dstReg, 0, srcReg.Length * sizeof(ushort));
System.Buffer.BlockCopy(srcDist, 0, dstDist, 0, dstDist.Length * sizeof(ushort));
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
}
/** Builds a polygon mesh from a contour set.
*
* \param cset contour set to build a mesh from.
* \param nvp Maximum allowed vertices per polygon. \warning Currently locked to 3.
* \param mesh Results will be written to this mesh.
*/
public void BuildPolyMesh(VoxelContourSet cset, int nvp, out VoxelMesh mesh)
{
AstarProfiler.StartProfile("Build Poly Mesh");
nvp = 3;
int maxVertices = 0;
int maxTris = 0;
int maxVertsPerCont = 0;
for (int i = 0; i < cset.conts.Count; i++)
{
// Skip null contours.
if (cset.conts[i].nverts < 3)
{
continue;
}
maxVertices += cset.conts[i].nverts;
maxTris += cset.conts[i].nverts - 2;
maxVertsPerCont = AstarMath.Max(maxVertsPerCont, cset.conts[i].nverts);
}
if (maxVertices >= 65534)
{
Debug.LogWarning("To many vertices for unity to render - Unity might screw up rendering, but hopefully the navmesh will work ok");
//mesh = new VoxelMesh ();
//yield break;
//return;
}
/** \todo Could be cached to avoid allocations */
Int3[] verts = new Int3[maxVertices];
/** \todo Could be cached to avoid allocations */
int[] polys = new int[maxTris * nvp];
Pathfinding.Util.Memory.MemSet <int> (polys, 0xff, sizeof(int));
int[] indices = new int[maxVertsPerCont];
int[] tris = new int[maxVertsPerCont * 3];
int vertexIndex = 0;
int polyIndex = 0;
for (int i = 0; i < cset.conts.Count; i++)
{
VoxelContour cont = cset.conts[i];
//Skip null contours
if (cont.nverts < 3)
{
continue;
}
for (int j = 0; j < cont.nverts; j++)
{
indices[j] = j;
cont.verts[j * 4 + 2] /= voxelArea.width;
}
int ntris = Triangulate(cont.nverts, cont.verts, ref indices, ref tris);
int startIndex = vertexIndex;
for (int j = 0; j < ntris * 3; polyIndex++, j++)
{
//@Error sometimes
polys[polyIndex] = tris[j] + startIndex;
}
for (int j = 0; j < cont.nverts; vertexIndex++, j++)
{
verts[vertexIndex] = new Int3(cont.verts[j * 4], cont.verts[j * 4 + 1], cont.verts[j * 4 + 2]);
}
}
mesh = new VoxelMesh();
//yield break;
Int3[] trimmedVerts = new Int3[vertexIndex];
for (int i = 0; i < vertexIndex; i++)
{
trimmedVerts[i] = verts[i];
}
int[] trimmedTris = new int[polyIndex];
System.Buffer.BlockCopy(polys, 0, trimmedTris, 0, polyIndex * sizeof(int));
mesh.verts = trimmedVerts;
mesh.tris = trimmedTris;
// Some debugging
/*for (int i=0;i<mesh.tris.Length/3;i++) {
*
* int p = i*3;
*
* int p1 = mesh.tris[p];
* int p2 = mesh.tris[p+1];
* int p3 = mesh.tris[p+2];
*
* //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p1].x,mesh.verts[p1].y,mesh.verts[p1].z),ConvertPosCorrZ (mesh.verts[p2].x,mesh.verts[p2].y,mesh.verts[p2].z),Color.yellow);
* //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p1].x,mesh.verts[p1].y,mesh.verts[p1].z),ConvertPosCorrZ (mesh.verts[p3].x,mesh.verts[p3].y,mesh.verts[p3].z),Color.yellow);
* //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p3].x,mesh.verts[p3].y,mesh.verts[p3].z),ConvertPosCorrZ (mesh.verts[p2].x,mesh.verts[p2].y,mesh.verts[p2].z),Color.yellow);
*
* //Debug.DrawLine (ConvertPosCorrZ (verts[p1],0,verts[p1+2]),ConvertPosCorrZ (verts[p2],0,verts[p2+2]),Color.blue);
* //Debug.DrawLine (ConvertPosCorrZ (verts[p1],0,verts[p1+2]),ConvertPosCorrZ (verts[p3],0,verts[p3+2]),Color.blue);
* //Debug.DrawLine (ConvertPosCorrZ (verts[p2],0,verts[p2+2]),ConvertPosCorrZ (verts[p3],0,verts[p3+2]),Color.blue);
*
* }*/
AstarProfiler.EndProfile("Build Poly Mesh");
}
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(verts);
Pathfinding.Util.ListPool <int> .Release(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");
}
/** Builds a polygon mesh from a contour set.
* \param nvp Maximum allowed vertices per polygon. \note Currently locked to 3
*/
public void BuildPolyMesh(VoxelContourSet cset, int nvp, out VoxelMesh mesh)
{
AstarProfiler.StartProfile("Build Poly Mesh");
nvp = 3;
int maxVertices = 0;
int maxTris = 0;
int maxVertsPerCont = 0;
for (int i = 0; i < cset.conts.Count; i++)
{
// Skip null contours.
if (cset.conts[i].nverts < 3)
{
continue;
}
maxVertices += cset.conts[i].nverts;
maxTris += cset.conts[i].nverts - 2;
maxVertsPerCont = AstarMath.Max(maxVertsPerCont, cset.conts[i].nverts);
}
if (maxVertices >= 65534)
{
Debug.LogWarning("To many vertices for unity to render - Unity might screw up rendering, but hopefully the navmesh will work ok");
//mesh = new VoxelMesh ();
//yield break;
//return;
}
//int[] vflags = new int[maxVertices];
/** \todo Could be cached to avoid allocations */
Int3[] verts = new Int3[maxVertices];
/** \todo Could be cached to avoid allocations */
int[] polys = new int[maxTris * nvp];
//int[] regs = new int[maxTris];
//int[] areas = new int[maxTris];
Pathfinding.Util.Memory.MemSet <int> (polys, 0xff, sizeof(int));
//for (int i=0;i<polys.Length;i++) {
// polys[i] = 0xff;
//}
//int[] nexVert = new int[maxVertices];
//int[] firstVert = new int[VERTEX_BUCKET_COUNT];
int[] indices = new int[maxVertsPerCont];
int[] tris = new int[maxVertsPerCont * 3];
//ushort[] polys
int vertexIndex = 0;
int polyIndex = 0;
for (int i = 0; i < cset.conts.Count; i++)
{
VoxelContour cont = cset.conts[i];
//Skip null contours
if (cont.nverts < 3)
{
continue;
}
for (int j = 0; j < cont.nverts; j++)
{
indices[j] = j;
cont.verts[j * 4 + 2] /= voxelArea.width;
}
//yield return (GameObject.FindObjectOfType (typeof(MonoBehaviour)) as MonoBehaviour).StartCoroutine (
//Triangulate (cont.nverts, cont.verts, indices, tris);
int ntris = Triangulate(cont.nverts, cont.verts, ref indices, ref tris);
/*if (ntris > cont.nverts-2) {
* Debug.LogError (ntris + " "+cont.nverts+" "+cont.verts.Length+" "+(cont.nverts-2));
* }
*
* if (ntris > maxVertsPerCont) {
* Debug.LogError (ntris*3 + " "+maxVertsPerCont);
* }
*
* int tmp = polyIndex;
*
* Debug.Log (maxTris + " "+polyIndex+" "+polys.Length+" "+ntris+" "+(ntris*3) + " " + cont.nverts);*/
int startIndex = vertexIndex;
for (int j = 0; j < ntris * 3; polyIndex++, j++)
{
//@Error sometimes
polys[polyIndex] = tris[j] + startIndex;
}
/*int tmp2 = polyIndex;
* if (tmp+ntris*3 != tmp2) {
* Debug.LogWarning (tmp+" "+(tmp+ntris*3)+" "+tmp2+" "+ntris*3);
* }*/
for (int j = 0; j < cont.nverts; vertexIndex++, j++)
{
verts[vertexIndex] = new Int3(cont.verts[j * 4], cont.verts[j * 4 + 1], cont.verts[j * 4 + 2]);
}
}
mesh = new VoxelMesh();
//yield break;
Int3[] trimmedVerts = new Int3[vertexIndex];
for (int i = 0; i < vertexIndex; i++)
{
trimmedVerts[i] = verts[i];
}
int[] trimmedTris = new int[polyIndex];
System.Buffer.BlockCopy(polys, 0, trimmedTris, 0, polyIndex * sizeof(int));
//for (int i=0;i<polyIndex;i++) {
// trimmedTris[i] = polys[i];
//}
mesh.verts = trimmedVerts;
mesh.tris = trimmedTris;
/*for (int i=0;i<mesh.tris.Length/3;i++) {
*
* int p = i*3;
*
* int p1 = mesh.tris[p];
* int p2 = mesh.tris[p+1];
* int p3 = mesh.tris[p+2];
*
* //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p1].x,mesh.verts[p1].y,mesh.verts[p1].z),ConvertPosCorrZ (mesh.verts[p2].x,mesh.verts[p2].y,mesh.verts[p2].z),Color.yellow);
* //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p1].x,mesh.verts[p1].y,mesh.verts[p1].z),ConvertPosCorrZ (mesh.verts[p3].x,mesh.verts[p3].y,mesh.verts[p3].z),Color.yellow);
* //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p3].x,mesh.verts[p3].y,mesh.verts[p3].z),ConvertPosCorrZ (mesh.verts[p2].x,mesh.verts[p2].y,mesh.verts[p2].z),Color.yellow);
*
* //Debug.DrawLine (ConvertPosCorrZ (verts[p1],0,verts[p1+2]),ConvertPosCorrZ (verts[p2],0,verts[p2+2]),Color.blue);
* //Debug.DrawLine (ConvertPosCorrZ (verts[p1],0,verts[p1+2]),ConvertPosCorrZ (verts[p3],0,verts[p3+2]),Color.blue);
* //Debug.DrawLine (ConvertPosCorrZ (verts[p2],0,verts[p2+2]),ConvertPosCorrZ (verts[p3],0,verts[p3+2]),Color.blue);
*
* }*/
AstarProfiler.EndProfile("Build Poly Mesh");
}
/** Builds a polygon mesh from a contour set.
*
* \param cset contour set to build a mesh from.
* \param nvp Maximum allowed vertices per polygon. \warning Currently locked to 3.
* \param mesh Results will be written to this mesh.
*/
public void BuildPolyMesh(VoxelContourSet cset, int nvp, out VoxelMesh mesh)
{
AstarProfiler.StartProfile("Build Poly Mesh");
nvp = 3;
int maxVertices = 0;
int maxTris = 0;
int maxVertsPerCont = 0;
for (int i = 0; i < cset.conts.Count; i++)
{
// Skip null contours.
if (cset.conts[i].nverts < 3)
{
continue;
}
maxVertices += cset.conts[i].nverts;
maxTris += cset.conts[i].nverts - 2;
maxVertsPerCont = System.Math.Max(maxVertsPerCont, cset.conts[i].nverts);
}
if (maxVertices >= 65534)
{
Debug.LogWarning("To many vertices for unity to render - Unity might screw up rendering, but hopefully the navmesh will work ok");
}
/** \todo Could be cached to avoid allocations */
Int3[] verts = new Int3[maxVertices];
/** \todo Could be cached to avoid allocations */
int[] polys = new int[maxTris * nvp];
int[] areas = new int[maxTris];
Pathfinding.Util.Memory.MemSet <int>(polys, 0xff, sizeof(int));
int[] indices = new int[maxVertsPerCont];
int[] tris = new int[maxVertsPerCont * 3];
int vertexIndex = 0;
int polyIndex = 0;
int areaIndex = 0;
for (int i = 0; i < cset.conts.Count; i++)
{
VoxelContour cont = cset.conts[i];
// Skip degenerate contours
if (cont.nverts < 3)
{
continue;
}
for (int j = 0; j < cont.nverts; j++)
{
indices[j] = j;
cont.verts[j * 4 + 2] /= voxelArea.width;
}
// Triangulate the contour
int ntris = Triangulate(cont.nverts, cont.verts, ref indices, ref tris);
// Assign the correct vertex indices
int startIndex = vertexIndex;
for (int j = 0; j < ntris * 3; polyIndex++, j++)
{
//@Error sometimes
polys[polyIndex] = tris[j] + startIndex;
}
// Mark all triangles generated by this contour
// as having the area cont.area
for (int j = 0; j < ntris; areaIndex++, j++)
{
areas[areaIndex] = cont.area;
}
// Copy the vertex positions
for (int j = 0; j < cont.nverts; vertexIndex++, j++)
{
verts[vertexIndex] = new Int3(cont.verts[j * 4], cont.verts[j * 4 + 1], cont.verts[j * 4 + 2]);
}
}
mesh = new VoxelMesh();
Int3[] trimmedVerts = new Int3[vertexIndex];
for (int i = 0; i < vertexIndex; i++)
{
trimmedVerts[i] = verts[i];
}
int[] trimmedTris = new int[polyIndex];
int[] trimmedAreas = new int[areaIndex];
System.Buffer.BlockCopy(polys, 0, trimmedTris, 0, polyIndex * sizeof(int));
System.Buffer.BlockCopy(areas, 0, trimmedAreas, 0, areaIndex * sizeof(int));
mesh.verts = trimmedVerts;
mesh.tris = trimmedTris;
mesh.areas = trimmedAreas;
AstarProfiler.EndProfile("Build Poly Mesh");
}
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 BuildCompactField()
{
AstarProfiler.StartProfile("Build Compact Voxel Field");
//Build compact representation
int spanCount = voxelArea.GetSpanCount();
voxelArea.compactSpanCount = spanCount;
if (voxelArea.compactSpans == null || voxelArea.compactSpans.Length < spanCount)
{
voxelArea.compactSpans = new CompactVoxelSpan[spanCount];
voxelArea.areaTypes = new int[spanCount];
}
uint idx = 0;
int w = voxelArea.width;
int d = voxelArea.depth;
int wd = w * d;
if (this.voxelWalkableHeight >= 0xFFFF)
{
Debug.LogWarning("Too high walkable height to guarantee correctness. Increase voxel height or lower walkable height.");
}
#if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST
LinkedVoxelSpan[] spans = voxelArea.linkedSpans;
#endif
//Parallel.For (0, voxelArea.depth, delegate (int pz) {
for (int z = 0, pz = 0; z < wd; z += w, pz++)
{
for (int x = 0; x < w; x++)
{
#if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST
int spanIndex = x + z;
if (spans[spanIndex].bottom == VoxelArea.InvalidSpanValue)
{
voxelArea.compactCells[x + z] = new CompactVoxelCell(0, 0);
continue;
}
uint index = idx;
uint count = 0;
//Vector3 p = new Vector3(x,0,pz)*cellSize+voxelOffset;
while (spanIndex != -1)
{
if (spans[spanIndex].area != UnwalkableArea)
{
int bottom = (int)spans[spanIndex].top;
int next = spans[spanIndex].next;
int top = next != -1 ? (int)spans[next].bottom : VoxelArea.MaxHeightInt;
voxelArea.compactSpans[idx] = new CompactVoxelSpan((ushort)(bottom > 0xFFFF ? 0xFFFF : bottom), (uint)(top - bottom > 0xFFFF ? 0xFFFF : top - bottom));
voxelArea.areaTypes[idx] = spans[spanIndex].area;
idx++;
count++;
}
spanIndex = spans[spanIndex].next;
}
voxelArea.compactCells[x + z] = new CompactVoxelCell(index, count);
#else
VoxelSpan s = voxelArea.cells[x + z].firstSpan;
if (s == null)
{
voxelArea.compactCells[x + z] = new CompactVoxelCell(0, 0);
continue;
}
uint index = idx;
uint count = 0;
//Vector3 p = new Vector3(x,0,pz)*cellSize+voxelOffset;
while (s != null)
{
if (s.area != UnwalkableArea)
{
int bottom = (int)s.top;
int top = s.next != null ? (int)s.next.bottom : VoxelArea.MaxHeightInt;
voxelArea.compactSpans[idx] = new CompactVoxelSpan((ushort)Mathf.Clamp(bottom, 0, 0xffff), (uint)Mathf.Clamp(top - bottom, 0, 0xffff));
voxelArea.areaTypes[idx] = s.area;
idx++;
count++;
}
s = s.next;
}
voxelArea.compactCells[x + z] = new CompactVoxelCell(index, count);
#endif
}
}
AstarProfiler.EndProfile("Build Compact Voxel Field");
}
public void VoxelizeInput(Pathfinding.Util.GraphTransform graphTransform, Bounds graphSpaceBounds)
{
AstarProfiler.StartProfile("Build Navigation Mesh");
AstarProfiler.StartProfile("Voxelizing - Step 1");
// Transform from voxel space to graph space.
// then scale from voxel space (one unit equals one voxel)
// Finally add min
Matrix4x4 voxelMatrix = Matrix4x4.TRS(graphSpaceBounds.min, Quaternion.identity, Vector3.one) * Matrix4x4.Scale(new Vector3(cellSize, cellHeight, cellSize));
transformVoxel2Graph = new Pathfinding.Util.GraphTransform(voxelMatrix);
// Transform from voxel space to world space
// add half a voxel to fix rounding
transform = graphTransform * voxelMatrix * Matrix4x4.TRS(new Vector3(0.5f, 0, 0.5f), Quaternion.identity, Vector3.one);
int maximumVoxelYCoord = (int)(graphSpaceBounds.size.y / cellHeight);
AstarProfiler.EndProfile("Voxelizing - Step 1");
AstarProfiler.StartProfile("Voxelizing - Step 2 - Init");
// Cosine of the slope limit in voxel space (some tweaks are needed because the voxel space might be stretched out along the y axis)
float slopeLimit = Mathf.Cos(Mathf.Atan(Mathf.Tan(maxSlope * Mathf.Deg2Rad) * (cellSize / cellHeight)));
// Temporary arrays used for rasterization
float[] vTris = new float[3 * 3];
float[] vOut = new float[7 * 3];
float[] vRow = new float[7 * 3];
float[] vCellOut = new float[7 * 3];
float[] vCell = new float[7 * 3];
if (inputMeshes == null)
{
throw new System.NullReferenceException("inputMeshes not set");
}
// Find the largest lengths of vertex arrays and check for meshes which can be skipped
int maxVerts = 0;
for (int m = 0; m < inputMeshes.Count; m++)
{
maxVerts = System.Math.Max(inputMeshes[m].vertices.Length, maxVerts);
}
// Create buffer, here vertices will be stored multiplied with the local-to-voxel-space matrix
var verts = new Vector3[maxVerts];
AstarProfiler.EndProfile("Voxelizing - Step 2 - Init");
AstarProfiler.StartProfile("Voxelizing - Step 2");
// This loop is the hottest place in the whole rasterization process
// it usually accounts for around 50% of the time
for (int m = 0; m < inputMeshes.Count; m++)
{
RasterizationMesh mesh = inputMeshes[m];
var meshMatrix = mesh.matrix;
// Flip the orientation of all faces if the mesh is scaled in such a way
// that the face orientations would change
// This happens for example if a mesh has a negative scale along an odd number of axes
// e.g it happens for the scale (-1, 1, 1) but not for (-1, -1, 1) or (1,1,1)
var flipOrientation = VectorMath.ReversesFaceOrientations(meshMatrix);
Vector3[] vs = mesh.vertices;
int[] tris = mesh.triangles;
int trisLength = tris.Length;
// Transform vertices first to world space and then to voxel space
for (int i = 0; i < vs.Length; i++)
{
verts[i] = transform.InverseTransform(meshMatrix.MultiplyPoint3x4(vs[i]));
}
int mesharea = mesh.area;
for (int i = 0; i < trisLength; i += 3)
{
Vector3 p1 = verts[tris[i]];
Vector3 p2 = verts[tris[i + 1]];
Vector3 p3 = verts[tris[i + 2]];
if (flipOrientation)
{
var tmp = p1;
p1 = p3;
p3 = tmp;
}
int minX = (int)(Utility.Min(p1.x, p2.x, p3.x));
int minZ = (int)(Utility.Min(p1.z, p2.z, p3.z));
int maxX = (int)System.Math.Ceiling(Utility.Max(p1.x, p2.x, p3.x));
int maxZ = (int)System.Math.Ceiling(Utility.Max(p1.z, p2.z, p3.z));
minX = Mathf.Clamp(minX, 0, voxelArea.width - 1);
maxX = Mathf.Clamp(maxX, 0, voxelArea.width - 1);
minZ = Mathf.Clamp(minZ, 0, voxelArea.depth - 1);
maxZ = Mathf.Clamp(maxZ, 0, voxelArea.depth - 1);
// Check if the mesh is completely out of bounds
if (minX >= voxelArea.width || minZ >= voxelArea.depth || maxX <= 0 || maxZ <= 0)
{
continue;
}
Vector3 normal;
int area;
//AstarProfiler.StartProfile ("Rasterize...");
normal = Vector3.Cross(p2 - p1, p3 - p1);
float cosSlopeAngle = Vector3.Dot(normal.normalized, Vector3.up);
if (cosSlopeAngle < slopeLimit)
{
area = UnwalkableArea;
}
else
{
area = 1 + mesharea;
}
Utility.CopyVector(vTris, 0, p1);
Utility.CopyVector(vTris, 3, p2);
Utility.CopyVector(vTris, 6, p3);
for (int x = minX; x <= maxX; x++)
{
int nrow = clipper.ClipPolygon(vTris, 3, vOut, 1F, -x + 0.5F, 0);
if (nrow < 3)
{
continue;
}
nrow = clipper.ClipPolygon(vOut, nrow, vRow, -1F, x + 0.5F, 0);
if (nrow < 3)
{
continue;
}
float clampZ1 = vRow[2];
float clampZ2 = vRow[2];
for (int q = 1; q < nrow; q++)
{
float val = vRow[q * 3 + 2];
clampZ1 = System.Math.Min(clampZ1, val);
clampZ2 = System.Math.Max(clampZ2, val);
}
int clampZ1I = Mathf.Clamp((int)System.Math.Round(clampZ1), 0, voxelArea.depth - 1);
int clampZ2I = Mathf.Clamp((int)System.Math.Round(clampZ2), 0, voxelArea.depth - 1);
for (int z = clampZ1I; z <= clampZ2I; z++)
{
//AstarProfiler.StartFastProfile(1);
int ncell = clipper.ClipPolygon(vRow, nrow, vCellOut, 1F, -z + 0.5F, 2);
if (ncell < 3)
{
//AstarProfiler.EndFastProfile(1);
continue;
}
ncell = clipper.ClipPolygonY(vCellOut, ncell, vCell, -1F, z + 0.5F, 2);
if (ncell < 3)
{
//AstarProfiler.EndFastProfile(1);
continue;
}
//AstarProfiler.EndFastProfile(1);
//AstarProfiler.StartFastProfile(2);
float sMin = vCell[1];
float sMax = vCell[1];
for (int q = 1; q < ncell; q++)
{
float val = vCell[q * 3 + 1];
sMin = System.Math.Min(sMin, val);
sMax = System.Math.Max(sMax, val);
}
//AstarProfiler.EndFastProfile(2);
int maxi = (int)System.Math.Ceiling(sMax);
// Skip span if below or above the bounding box
if (maxi >= 0 && sMin <= maximumVoxelYCoord)
{
// Make sure mini >= 0
int mini = System.Math.Max(0, (int)sMin);
// Make sure the span is at least 1 voxel high
maxi = System.Math.Max(mini + 1, maxi);
voxelArea.AddLinkedSpan(z * voxelArea.width + x, (uint)mini, (uint)maxi, area, voxelWalkableClimb);
}
}
}
}
//AstarProfiler.EndFastProfile(0);
//AstarProfiler.EndProfile ("Rasterize...");
}
AstarProfiler.EndProfile("Voxelizing - Step 2");
}
public override void Apply(Path p, ModifierData source)
{
List <Node> path = p.path;
List <Vector3> vectorPath = p.vectorPath;
if (path == null || path.Count == 0 || vectorPath == null || vectorPath.Count != path.Count)
{
return;
}
//The graph index for the current nodes
int currentGraphIndex = path[0].graphIndex;
//First node which is in the graph currentGraphIndex
int currentGraphStart = 0;
List <Vector3> funnelPath = ListPool <Vector3> .Claim();
List <Vector3> left = ListPool <Vector3> .Claim();
List <Vector3> right = ListPool <Vector3> .Claim();
AstarProfiler.StartProfile("Construct Funnel");
for (int i = 0; i < path.Count; i++)
{
if (path[i].graphIndex != currentGraphIndex)
{
IFunnelGraph funnelGraph = AstarData.GetGraph(path[currentGraphStart]) as IFunnelGraph;
if (funnelGraph == null)
{
//Debug.Log ("Funnel Graph is null");
for (int j = currentGraphStart; j <= i; j++)
{
funnelPath.Add((Vector3)path[j].position);
}
}
else
{
AstarProfiler.StartProfile("Construct Funnel Real");
ConstructFunnel(funnelGraph, vectorPath, path, currentGraphStart, i - 1, funnelPath, left, right);
AstarProfiler.EndProfile();
}
currentGraphIndex = path[i].graphIndex;
currentGraphStart = i;
}
}
IFunnelGraph funnelGraph2 = AstarData.GetGraph(path[currentGraphStart]) as IFunnelGraph;
if (funnelGraph2 == null)
{
for (int j = currentGraphStart; j < path.Count - 1; j++)
{
funnelPath.Add((Vector3)path[j].position);
}
}
else
{
AstarProfiler.StartProfile("Construct Funnel Real");
ConstructFunnel(funnelGraph2, vectorPath, path, currentGraphStart, path.Count - 1, funnelPath, left, right);
AstarProfiler.EndProfile();
}
AstarProfiler.EndProfile();
ListPool <Vector3> .Release(p.vectorPath);
p.vectorPath = funnelPath;
ListPool <Vector3> .Release(left);
ListPool <Vector3> .Release(right);
#if ASTARDEBUG
for (int i = 0; i < p.vectorPath.Count - 1; i++)
{
Debug.DrawLine(p.vectorPath[i] + Vector3.up, p.vectorPath[i + 1] + Vector3.up, Color.magenta);
}
#endif
}
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");
}