public void UpdateDebugMesh(AstarPath astar)
{
#if UNITY_3_5
CreateDebugMesh();
meshRenderer.transform.position = Vector3.zero;
meshRenderer.transform.localScale = Vector3.one;
#endif
RecastGraph graph = target as RecastGraph;
if (graph != null && graph.nodes != null && graph.vectorVertices != null)
{
if (navmeshRender == null)
{
navmeshRender = new Mesh();
}
navmeshRender.Clear();
navmeshRender.vertices = graph.vectorVertices;
int[] tris = new int[graph.nodes.Length * 3];
Color[] vColors = new Color[graph.vectorVertices.Length];
for (int i = 0; i < graph.nodes.Length; i++)
{
MeshNode node = graph.nodes[i] as MeshNode;
tris[i * 3] = node.v1;
tris[i * 3 + 1] = node.v2;
tris[i * 3 + 2] = node.v3;
Color col = Mathfx.IntToColor(node.area, 1F);
vColors[node.v1] = col;
vColors[node.v2] = col;
vColors[node.v3] = col;
}
navmeshRender.triangles = tris;
navmeshRender.colors = vColors;
//meshRenderer.transform.position = graph.forcedBoundsCenter-graph.forcedBoundsSize*0.5F;
//meshRenderer.transform.localScale = Int3.Precision*Voxelize.CellScale;
navmeshRender.RecalculateNormals();
navmeshRender.RecalculateBounds();
if (navmeshMaterial == null)
{
navmeshMaterial = AssetDatabase.LoadAssetAtPath(AstarPathEditor.editorAssets + "/Materials/Navmesh.mat", typeof(Material)) as Material;
}
navmeshRender.hideFlags = HideFlags.HideAndDontSave;
#if UNITY_3_5
navmeshRenderer.material = navmeshMaterial;
#endif
}
}
public void UpdateDebugMesh(AstarPath astar)
{
CreateDebugMesh();
RecastGraph graph = target as RecastGraph;
//if (graph.useCRecast) {
meshRenderer.transform.position = Vector3.zero;
meshRenderer.transform.localScale = Vector3.one;
//} else {
//meshRenderer.transform.position = graph.forcedBounds.min;
//meshRenderer.transform.localScale = new Vector3(graph.cellSize * 100, 1, graph.cellSize * 100);
//}
if (graph != null && graph.nodes != null && graph.vectorVertices != null)
{
navmeshRender.Clear();
navmeshRender.vertices = graph.vectorVertices;
//Vector3[] vs = new Vector3[graph.vertices.Length];
//for (int i=0;i<graph.vertices.Length;i++) {
// vs[i] = graph.vertices[i];
//}
//navmeshRender.vertices = vs;
int[] tris = new int[graph.nodes.Length * 3];
Color[] vColors = new Color[graph.vectorVertices.Length];
for (int i = 0; i < graph.nodes.Length; i++)
{
MeshNode node = graph.nodes[i] as MeshNode;
tris[i * 3] = node.v1;
tris[i * 3 + 1] = node.v2;
tris[i * 3 + 2] = node.v3;
Color col = Mathfx.IntToColor(node.area, 1F);
vColors[node.v1] = col;
vColors[node.v2] = col;
vColors[node.v3] = col;
}
navmeshRender.triangles = tris;
navmeshRender.colors = vColors;
//meshRenderer.transform.position = graph.forcedBoundsCenter-graph.forcedBoundsSize*0.5F;
//meshRenderer.transform.localScale = Int3.Precision*Voxelize.CellScale;
navmeshRender.RecalculateNormals();
navmeshRender.RecalculateBounds();
if (navmeshMaterial == null)
{
navmeshMaterial = AssetDatabase.LoadAssetAtPath(AstarPathEditor.editorAssets + "/Materials/Navmesh.mat", typeof(Material)) as Material;
}
navmeshRenderer.material = navmeshMaterial;
}
}
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 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 = ClaimIntArr(simplified.Count, false); //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, Mathfx.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");
}
