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)AstarMath.Clamp(bottom, 0, 0xffff), (uint)AstarMath.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()
{
AstarProfiler.StartProfile("Build Navigation Mesh");
AstarProfiler.StartProfile("Voxelizing - Step 1");
//Debug.DrawLine (forcedBounds.min,forcedBounds.max,Color.blue);
Vector3 min = forcedBounds.min;
voxelOffset = min;
float ics = 1F / cellSize;
float ich = 1F / cellHeight;
AstarProfiler.EndProfile("Voxelizing - Step 1");
AstarProfiler.StartProfile("Voxelizing - Step 2 - Init");
float slopeLimit = Mathf.Cos(Mathf.Atan(Mathf.Tan(maxSlope * Mathf.Deg2Rad) * (ich * cellSize)));
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 (inputExtraMeshes == null)
{
throw new System.NullReferenceException("inputExtraMeshes 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 < inputExtraMeshes.Count; m++)
{
if (!inputExtraMeshes[m].bounds.Intersects(forcedBounds))
{
continue;
}
maxVerts = System.Math.Max(inputExtraMeshes[m].vertices.Length, maxVerts);
}
//Create buffer, here vertices will be stored multiplied with the local-to-voxel-space matrix
Vector3[] verts = new Vector3[maxVerts];
Matrix4x4 voxelMatrix = Matrix4x4.Scale(new Vector3(ics, ich, ics)) * Matrix4x4.TRS(-min, Quaternion.identity, Vector3.one);
AstarProfiler.EndProfile("Voxelizing - Step 2 - Init");
AstarProfiler.StartProfile("Voxelizing - Step 2");
for (int m = 0; m < inputExtraMeshes.Count; m++)
{
ExtraMesh mesh = inputExtraMeshes[m];
if (!mesh.bounds.Intersects(forcedBounds))
{
continue;
}
Matrix4x4 matrix = mesh.matrix;
matrix = voxelMatrix * matrix;
Vector3[] vs = mesh.vertices;
int[] tris = mesh.triangles;
int trisLength = tris.Length;
for (int i = 0; i < vs.Length; i++)
{
verts[i] = matrix.MultiplyPoint3x4(vs[i]);
}
//AstarProfiler.StartFastProfile(0);
int mesharea = mesh.area;
for (int i = 0; i < trisLength; i += 3)
{
Vector3 p1;
Vector3 p2;
Vector3 p3;
int minX;
int minZ;
int maxX;
int maxZ;
p1 = verts[tris[i]];
p2 = verts[tris[i + 1]];
p3 = verts[tris[i + 2]];
minX = (int)(Utility.Min(p1.x, p2.x, p3.x));
// (Mathf.Min (Mathf.Min (p1.x,p2.x),p3.x));
minZ = (int)(Utility.Min(p1.z, p2.z, p3.z));
maxX = (int)System.Math.Ceiling(Utility.Max(p1.x, p2.x, p3.x));
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);
if (minX >= voxelArea.width || minZ >= voxelArea.depth || maxX <= 0 || maxZ <= 0)
{
continue;
}
// Debug code
//Debug.DrawLine (p1*cellSize+min+Vector3.up*0.2F,p2*cellSize+voxelOffset+Vector3.up*0.1F,Color.red);
//Debug.DrawLine (p1*cellSize+min,p2*cellSize+voxelOffset,Color.red, 1);
//Debug.DrawLine (p2*cellSize+min,p3*cellSize+voxelOffset,Color.red, 1);
//Debug.DrawLine (p3*cellSize+min,p1*cellSize+voxelOffset,Color.red, 1);
Vector3 normal;
int area;
//AstarProfiler.StartProfile ("Rasterize...");
normal = Vector3.Cross(p2 - p1, p3 - p1);
float dot = Vector3.Dot(normal.normalized, Vector3.up);
if (dot < 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 = Utility.ClipPolygon(vTris, 3, vOut, 1F, -x + 0.5F, 0);
if (nrow < 3)
{
continue;
}
nrow = Utility.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 = AstarMath.Clamp((int)System.Math.Round(clampZ1), 0, voxelArea.depth - 1);
int clampZ2I = AstarMath.Clamp((int)System.Math.Round(clampZ2), 0, voxelArea.depth - 1);
for (int z = clampZ1I; z <= clampZ2I; z++)
{
//AstarProfiler.StartFastProfile(1);
int ncell = Utility.ClipPolygon(vRow, nrow, vCellOut, 1F, -z + 0.5F, 2);
if (ncell < 3)
{
//AstarProfiler.EndFastProfile(1);
continue;
}
ncell = Utility.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 the bounding box
if (maxi >= 0)
{
// 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");
}