| public AddLinkedSpan ( int index, uint bottom, uint top, int area, int voxelWalkableClimb ) : void | ||
| index | int | |
| bottom | uint | |
| top | uint | |
| area | int | |
| voxelWalkableClimb | int | |
| return | void | |
public static void MergeVoxelAreaData(VoxelArea source, VoxelArea merge, int voxelWalkableClimb)
{
#if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST
LinkedVoxelSpan[] spans1 = source.linkedSpans;
int wd = source.width*source.depth;
for (int x=0;x<wd;x++) {
int i = x;
if (spans1[i].bottom == VoxelArea.InvalidSpanValue) continue;
while (i != -1) {
merge.AddLinkedSpan(x,spans1[i].bottom,spans1[i].top,spans1[i].area,voxelWalkableClimb);
i = spans1[i].next;
}
}
#else
throw new System.NotImplementedException ("This method only works with !ASTAR_RECAST_CLASS_BASED_LINKED_LIST");
#endif
}
public void VoxelizeInput()
{
AstarProfiler.StartProfile("Build Navigation Mesh");
AstarProfiler.StartProfile("Voxelizing - Step 1");
Vector3 min = forcedBounds.min;
voxelOffset = min;
// Scale factor from world space to voxel space
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)));
// 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 (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];
// First subtract min, then scale to voxel space (one unit equals one voxel along the x and z axes)
// then subtract half a voxel to fix rounding
Matrix4x4 voxelMatrix = Matrix4x4.TRS(-new Vector3(0.5f, 0, 0.5f), Quaternion.identity, Vector3.one) * 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;
// 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(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]];
if (flipOrientation)
{
var tmp = p1;
p1 = p3;
p3 = tmp;
}
minX = (int)(Utility.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 = 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 = 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");
}
public static void MergeVoxelAreaData (VoxelArea source, VoxelArea merge, int voxelWalkableClimb) {
#if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST
LinkedVoxelSpan[] spans1 = source.linkedSpans;
int wd = source.width*source.depth;
for (int x=0;x<wd;x++) {
int i = x;
if (spans1[i].bottom == VoxelArea.InvalidSpanValue) continue;
while (i != -1) {
merge.AddLinkedSpan(x,spans1[i].bottom,spans1[i].top,spans1[i].area,voxelWalkableClimb);
i = spans1[i].next;
}
}
#else
throw new System.NotImplementedException ("This method only works with !ASTAR_RECAST_CLASS_BASED_LINKED_LIST");
#endif
}
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
var clipperOrig = new VoxelPolygonClipper(3);
var clipperX1 = new VoxelPolygonClipper(7);
var clipperX2 = new VoxelPolygonClipper(7);
var clipperZ1 = new VoxelPolygonClipper(7);
var clipperZ2 = new VoxelPolygonClipper(7);
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 = mesh.numTriangles;
// 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;
}
clipperOrig[0] = p1;
clipperOrig[1] = p2;
clipperOrig[2] = p3;
clipperOrig.n = 3;
for (int x = minX; x <= maxX; x++)
{
clipperOrig.ClipPolygonAlongX(ref clipperX1, 1f, -x + 0.5f);
if (clipperX1.n < 3)
{
continue;
}
clipperX1.ClipPolygonAlongX(ref clipperX2, -1F, x + 0.5F);
if (clipperX2.n < 3)
{
continue;
}
float clampZ1 = clipperX2.z[0];
float clampZ2 = clipperX2.z[0];
for (int q = 1; q < clipperX2.n; q++)
{
float val = clipperX2.z[q];
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++)
{
clipperX2.ClipPolygonAlongZWithYZ(ref clipperZ1, 1F, -z + 0.5F);
if (clipperZ1.n < 3)
{
continue;
}
clipperZ1.ClipPolygonAlongZWithY(ref clipperZ2, -1F, z + 0.5F);
if (clipperZ2.n < 3)
{
continue;
}
float sMin = clipperZ2.y[0];
float sMax = clipperZ2.y[0];
for (int q = 1; q < clipperZ2.n; q++)
{
float val = clipperZ2.y[q];
sMin = System.Math.Min(sMin, val);
sMax = System.Math.Max(sMax, val);
}
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 static void MergeVoxelAreaData (VoxelArea source, VoxelArea merge, int voxelWalkableClimb) {
LinkedVoxelSpan[] spans1 = source.linkedSpans;
int wd = source.width*source.depth;
for (int x=0;x<wd;x++) {
int i = x;
if (spans1[i].bottom == VoxelArea.InvalidSpanValue) continue;
while (i != -1) {
merge.AddLinkedSpan(x,spans1[i].bottom,spans1[i].top,spans1[i].area,voxelWalkableClimb);
i = spans1[i].next;
}
}
/*for (int c=0;c<cells1.Length;c++) {
int i1 = (int)cells1[c].index;
int i2 = (int)cells2[c].index;
int c1 = (int)i1 + (int)cells1[c].count;
int c2 = (int)i2 + (int)cells2[c].count;
CompactVoxelSpan last;
int lastIndex;
int lastAreaType;
if (i1 < c1 && (i2 >= c2 || spans1[i1].y < spans2[i2].y)) {
last = spans1[i1];
lastAreaType = source.areaTypes[i1];
lastIndex = i1;
i1++;
spanCount++;
} else if (i2 < c2) {
last = spans2[i2];
lastAreaType = merge.areaTypes[i2];
lastIndex = i2;
i2++;
spanCount++;
} else {
continue;
}
while (i1 < c1 || i2 < c2) {
CompactVoxelSpan span;
int areaType;
int spanIndex;
Debug.Log (i1 + " " + i2 + " " + c1 + " " + c2);
if (i1 < c1 && (i2 >= c2 || spans1[i1].y < spans2[i2].y)) {
span = spans1[i1];
spanIndex = i1;
areaType = source.areaTypes[i1];
i1++;
} else if (i2 < c2) {
span = spans2[i2];
areaType = merge.areaTypes[i2];
spanIndex = i2;
i2++;
} else {
throw new System.Exception ("This should not happen");
}
Debug.Log (span.y + " " + (last.y+last.h));
if (span.y > last.y+last.h) {
last = span;
spanCount++;
continue;
} else {
last.h = System.Math.Max(last.h,span.y+span.h - last.y);
}
}
}
CompactVoxelSpan[] spansResult = new CompactVoxelSpan[spanCount];
Debug.Log (spans1.Length + " : " + spans2.Length + " -> " + spanCount);
int area;
//1 is flagMergeDistance, when a walkable flag is favored before an unwalkable one
if (Mathfx.Abs ((int)(span.y+span.h) - (int)(last.y+span.h)) <= voxelWalkableClimb) {
area = Mathfx.Max (lastAreaType,areaType);
}
}
}*/
}
public static void MergeVoxelAreaData(VoxelArea source, VoxelArea merge, int voxelWalkableClimb)
{
LinkedVoxelSpan[] spans1 = source.linkedSpans;
int wd = source.width * source.depth;
for (int x = 0; x < wd; x++)
{
int i = x;
if (spans1[i].bottom == VoxelArea.InvalidSpanValue)
{
continue;
}
while (i != -1)
{
merge.AddLinkedSpan(x, spans1[i].bottom, spans1[i].top, spans1[i].area, voxelWalkableClimb);
i = spans1[i].next;
}
}
/*for (int c=0;c<cells1.Length;c++) {
*
* int i1 = (int)cells1[c].index;
* int i2 = (int)cells2[c].index;
* int c1 = (int)i1 + (int)cells1[c].count;
* int c2 = (int)i2 + (int)cells2[c].count;
*
* CompactVoxelSpan last;
* int lastIndex;
* int lastAreaType;
* if (i1 < c1 && (i2 >= c2 || spans1[i1].y < spans2[i2].y)) {
* last = spans1[i1];
* lastAreaType = source.areaTypes[i1];
* lastIndex = i1;
* i1++;
* spanCount++;
* } else if (i2 < c2) {
* last = spans2[i2];
* lastAreaType = merge.areaTypes[i2];
* lastIndex = i2;
* i2++;
* spanCount++;
* } else {
* continue;
* }
*
* while (i1 < c1 || i2 < c2) {
*
* CompactVoxelSpan span;
* int areaType;
* int spanIndex;
*
* Debug.Log (i1 + " " + i2 + " " + c1 + " " + c2);
* if (i1 < c1 && (i2 >= c2 || spans1[i1].y < spans2[i2].y)) {
* span = spans1[i1];
* spanIndex = i1;
* areaType = source.areaTypes[i1];
* i1++;
* } else if (i2 < c2) {
* span = spans2[i2];
* areaType = merge.areaTypes[i2];
* spanIndex = i2;
* i2++;
* } else {
* throw new System.Exception ("This should not happen");
* }
*
* Debug.Log (span.y + " " + (last.y+last.h));
* if (span.y > last.y+last.h) {
* last = span;
* spanCount++;
* continue;
* } else {
* last.h = System.Math.Max(last.h,span.y+span.h - last.y);
* }
* }
* }
*
* CompactVoxelSpan[] spansResult = new CompactVoxelSpan[spanCount];
*
* Debug.Log (spans1.Length + " : " + spans2.Length + " -> " + spanCount);
*
*
* int area;
* //1 is flagMergeDistance, when a walkable flag is favored before an unwalkable one
* if (Mathfx.Abs ((int)(span.y+span.h) - (int)(last.y+span.h)) <= voxelWalkableClimb) {
* area = Mathfx.Max (lastAreaType,areaType);
* }
* }
*
*
* }*/
}
