858 lines
27 KiB
C#
858 lines
27 KiB
C#
using UnityEngine;
|
|
using System.Collections.Generic;
|
|
|
|
namespace Pathfinding.Voxels {
|
|
/// <summary>
|
|
/// Voxelizer for recast graphs.
|
|
///
|
|
/// In comments: units wu are World Units, vx are Voxels
|
|
/// </summary>
|
|
public partial class Voxelize {
|
|
public List<RasterizationMesh> inputMeshes;
|
|
|
|
/// <summary>Maximum ledge height that is considered to still be traversable. [Limit: >=0] [Units: vx]</summary>
|
|
public readonly int voxelWalkableClimb;
|
|
|
|
/// <summary>
|
|
/// Minimum floor to 'ceiling' height that will still allow the floor area to
|
|
/// be considered walkable. [Limit: >= 3] [Units: vx]
|
|
/// </summary>
|
|
public readonly uint voxelWalkableHeight;
|
|
|
|
/// <summary>The xz-plane cell size to use for fields. [Limit: > 0] [Units: wu]</summary>
|
|
public readonly float cellSize = 0.2F;
|
|
|
|
/// <summary>The y-axis cell size to use for fields. [Limit: > 0] [Units: wu]</summary>
|
|
public readonly float cellHeight = 0.1F;
|
|
|
|
public int minRegionSize = 100;
|
|
|
|
/// <summary>The size of the non-navigable border around the heightfield. [Limit: >=0] [Units: vx]</summary>
|
|
public int borderSize = 0;
|
|
|
|
/// <summary>The maximum allowed length for contour edges along the border of the mesh. [Limit: >= 0] [Units: vx]</summary>
|
|
public float maxEdgeLength = 20;
|
|
|
|
/// <summary>The maximum slope that is considered walkable. [Limits: 0 <= value < 90] [Units: Degrees]</summary>
|
|
public float maxSlope = 30;
|
|
|
|
public RecastGraph.RelevantGraphSurfaceMode relevantGraphSurfaceMode;
|
|
|
|
/// <summary>The world AABB to rasterize</summary>
|
|
public Bounds forcedBounds;
|
|
|
|
public VoxelArea voxelArea;
|
|
public VoxelContourSet countourSet;
|
|
|
|
/// <summary>Transform from voxel space to world space</summary>
|
|
Pathfinding.Util.GraphTransform transform;
|
|
|
|
/// <summary>Transform from voxel space to graph space</summary>
|
|
public Pathfinding.Util.GraphTransform transformVoxel2Graph { get; private set; }
|
|
|
|
/// <summary>
|
|
/// Width in voxels.
|
|
/// Must match the <see cref="forcedBounds"/>
|
|
/// </summary>
|
|
public int width;
|
|
|
|
/// <summary>
|
|
/// Depth in voxels.
|
|
/// Must match the <see cref="forcedBounds"/>
|
|
/// </summary>
|
|
public int depth;
|
|
|
|
#region Debug
|
|
|
|
Vector3 voxelOffset = Vector3.zero;
|
|
|
|
public Vector3 CompactSpanToVector (int x, int z, int i) {
|
|
return voxelOffset+new Vector3((x+0.5f)*cellSize, voxelArea.compactSpans[i].y*cellHeight, (z+0.5f)*cellSize);
|
|
}
|
|
|
|
public void VectorToIndex (Vector3 p, out int x, out int z) {
|
|
p -= voxelOffset;
|
|
x = Mathf.RoundToInt((p.x / cellSize) - 0.5f);
|
|
z = Mathf.RoundToInt((p.z / cellSize) - 0.5f);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Constants /// <summary>@name Constants @{</summary>
|
|
|
|
public const uint NotConnected = 0x3f;
|
|
|
|
/// <summary>Unmotivated variable, but let's clamp the layers at 65535</summary>
|
|
const int MaxLayers = 65535;
|
|
|
|
/// <summary>TODO: : Check up on this variable</summary>
|
|
const int MaxRegions = 500;
|
|
|
|
const int UnwalkableArea = 0;
|
|
|
|
/// <summary>
|
|
/// If heightfield region ID has the following bit set, the region is on border area
|
|
/// and excluded from many calculations.
|
|
/// </summary>
|
|
const ushort BorderReg = 0x8000;
|
|
|
|
/// <summary>
|
|
/// If contour region ID has the following bit set, the vertex will be later
|
|
/// removed in order to match the segments and vertices at tile boundaries.
|
|
/// </summary>
|
|
const int RC_BORDER_VERTEX = 0x10000;
|
|
|
|
const int RC_AREA_BORDER = 0x20000;
|
|
|
|
const int VERTEX_BUCKET_COUNT = 1<<12;
|
|
|
|
/// <summary>Tessellate wall edges</summary>
|
|
public const int RC_CONTOUR_TESS_WALL_EDGES = 1 << 0;
|
|
|
|
/// <summary>Tessellate edges between areas</summary>
|
|
public const int RC_CONTOUR_TESS_AREA_EDGES = 1 << 1;
|
|
|
|
/// <summary>Tessellate edges at the border of the tile</summary>
|
|
public const int RC_CONTOUR_TESS_TILE_EDGES = 1 << 2;
|
|
|
|
/// <summary>Mask used with contours to extract region id.</summary>
|
|
const int ContourRegMask = 0xffff;
|
|
|
|
#endregion /// <summary>@}</summary>
|
|
|
|
readonly Vector3 cellScale;
|
|
|
|
public Voxelize (float ch, float cs, float walkableClimb, float walkableHeight, float maxSlope, float maxEdgeLength) {
|
|
cellSize = cs;
|
|
cellHeight = ch;
|
|
this.maxSlope = maxSlope;
|
|
|
|
cellScale = new Vector3(cellSize, cellHeight, cellSize);
|
|
|
|
voxelWalkableHeight = (uint)(walkableHeight/cellHeight);
|
|
voxelWalkableClimb = Mathf.RoundToInt(walkableClimb/cellHeight);
|
|
this.maxEdgeLength = maxEdgeLength;
|
|
}
|
|
|
|
public void Init () {
|
|
// Initialize the voxel area
|
|
if (voxelArea == null || voxelArea.width != width || voxelArea.depth != depth)
|
|
voxelArea = new VoxelArea(width, depth);
|
|
else voxelArea.Reset();
|
|
}
|
|
|
|
public void VoxelizeInput (Pathfinding.Util.GraphTransform graphTransform, Bounds graphSpaceBounds) {
|
|
// 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);
|
|
|
|
// 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];
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public void DebugDrawSpans () {
|
|
#if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST
|
|
int wd = voxelArea.width*voxelArea.depth;
|
|
var min = forcedBounds.min;
|
|
|
|
LinkedVoxelSpan[] spans = voxelArea.linkedSpans;
|
|
for (int z = 0, pz = 0; z < wd; z += voxelArea.width, pz++) {
|
|
for (int x = 0; x < voxelArea.width; x++) {
|
|
for (int s = z+x; s != -1 && spans[s].bottom != VoxelArea.InvalidSpanValue; s = spans[s].next) {
|
|
uint bottom = spans[s].top;
|
|
uint top = spans[s].next != -1 ? spans[spans[s].next].bottom : VoxelArea.MaxHeight;
|
|
|
|
if (bottom > top) {
|
|
Debug.Log(bottom + " " + top);
|
|
Debug.DrawLine(new Vector3(x*cellSize, bottom*cellHeight, pz*cellSize)+min, new Vector3(x*cellSize, top*cellHeight, pz*cellSize)+min, Color.yellow, 1);
|
|
}
|
|
//Debug.DrawRay (p,voxelArea.VectorDirection[d]*cellSize*0.5F,Color.red);
|
|
if (top - bottom < voxelWalkableHeight) {
|
|
//spans[s].area = UnwalkableArea;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#else
|
|
Debug.LogError("This debug method only works with !ASTAR_RECAST_CLASS_BASED_LINKED_LIST");
|
|
#endif
|
|
}
|
|
|
|
|
|
public void BuildCompactField () {
|
|
//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
|
|
}
|
|
}
|
|
}
|
|
|
|
public void BuildVoxelConnections () {
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void DrawLine (int a, int b, int[] indices, int[] verts, Color color) {
|
|
int p1 = (indices[a] & 0x0fffffff) * 4;
|
|
int p2 = (indices[b] & 0x0fffffff) * 4;
|
|
|
|
Debug.DrawLine(VoxelToWorld(verts[p1+0], verts[p1+1], verts[p1+2]), VoxelToWorld(verts[p2+0], verts[p2+1], verts[p2+2]), color);
|
|
}
|
|
|
|
#if ASTARDEBUG
|
|
Vector3 ConvertPos (int x, int y, int z) {
|
|
Vector3 p = Vector3.Scale(
|
|
new Vector3(
|
|
x+0.5F,
|
|
y,
|
|
(z/(float)voxelArea.width)+0.5F
|
|
)
|
|
, cellScale)
|
|
+voxelOffset;
|
|
|
|
return p;
|
|
}
|
|
#endif
|
|
|
|
/// <summary>
|
|
/// Convert from voxel coordinates to world coordinates.
|
|
/// (0,0,0) in voxel coordinates is a bottom corner of the bounding box.
|
|
/// (1,0,0) is one voxel in the +X direction of that.
|
|
/// </summary>
|
|
public Vector3 VoxelToWorld (int x, int y, int z) {
|
|
Vector3 p = Vector3.Scale(
|
|
new Vector3(
|
|
x,
|
|
y,
|
|
z
|
|
)
|
|
, cellScale)
|
|
+voxelOffset;
|
|
|
|
return p;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert from voxel coordinates to world coordinates.
|
|
/// (0,0,0) in voxel coordinates is a bottom corner of the bounding box.
|
|
/// (1,0,0) is one voxel in the +X direction of that.
|
|
/// </summary>
|
|
public Int3 VoxelToWorldInt3 (Int3 voxelPosition) {
|
|
var pos = voxelPosition * Int3.Precision;
|
|
|
|
pos = new Int3(Mathf.RoundToInt(pos.x * cellScale.x), Mathf.RoundToInt(pos.y * cellScale.y), Mathf.RoundToInt(pos.z * cellScale.z));
|
|
return pos +(Int3)voxelOffset;
|
|
}
|
|
|
|
Vector3 ConvertPosWithoutOffset (int x, int y, int z) {
|
|
Vector3 p = Vector3.Scale(
|
|
new Vector3(
|
|
x,
|
|
y,
|
|
(z/(float)voxelArea.width)
|
|
)
|
|
, cellScale)
|
|
+voxelOffset;
|
|
|
|
return p;
|
|
}
|
|
|
|
Vector3 ConvertPosition (int x, int z, int i) {
|
|
CompactVoxelSpan s = voxelArea.compactSpans[i];
|
|
|
|
return new Vector3(x*cellSize, s.y*cellHeight, (z/(float)voxelArea.width)*cellSize)+voxelOffset;
|
|
}
|
|
|
|
|
|
public void ErodeWalkableArea (int radius) {
|
|
ushort[] src = voxelArea.tmpUShortArr;
|
|
if (src == null || src.Length < voxelArea.compactSpanCount) {
|
|
src = voxelArea.tmpUShortArr = new ushort[voxelArea.compactSpanCount];
|
|
}
|
|
|
|
// Set all elements in src to 0xffff
|
|
Pathfinding.Util.Memory.MemSet<ushort>(src, 0xffff, sizeof(ushort));
|
|
|
|
CalculateDistanceField(src);
|
|
|
|
for (int i = 0; i < src.Length; i++) {
|
|
//Note multiplied with 2 because the distance field increments distance by 2 for each voxel (and 3 for diagonal)
|
|
if (src[i] < radius*2) {
|
|
voxelArea.areaTypes[i] = UnwalkableArea;
|
|
}
|
|
}
|
|
}
|
|
|
|
public void BuildDistanceField () {
|
|
ushort[] src = voxelArea.tmpUShortArr;
|
|
if (src == null || src.Length < voxelArea.compactSpanCount) {
|
|
src = voxelArea.tmpUShortArr = new ushort[voxelArea.compactSpanCount];
|
|
}
|
|
|
|
// Set all elements in src to 0xffff
|
|
Pathfinding.Util.Memory.MemSet<ushort>(src, 0xffff, sizeof(ushort));
|
|
|
|
voxelArea.maxDistance = CalculateDistanceField(src);
|
|
|
|
ushort[] dst = voxelArea.dist;
|
|
if (dst == null || dst.Length < voxelArea.compactSpanCount) {
|
|
dst = new ushort[voxelArea.compactSpanCount];
|
|
}
|
|
|
|
dst = BoxBlur(src, dst);
|
|
voxelArea.dist = dst;
|
|
}
|
|
|
|
/// <summary>TODO: Complete the ErodeVoxels function translation</summary>
|
|
[System.Obsolete("This function is not complete and should not be used")]
|
|
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];
|
|
*
|
|
* voxelOffset nni = (int)(voxelArea.compactCells[nnx+nnz].index+ns.GetConnection (2));
|
|
*
|
|
* if (src[nni]+3 < src[i]) {
|
|
* src[i] = (ushort)(src[nni]+3);
|
|
* }
|
|
* }
|
|
* }
|
|
* }
|
|
* }
|
|
* }*/
|
|
}
|
|
|
|
public void FilterLowHeightSpans (uint voxelWalkableHeight, float cs, float ch) {
|
|
int wd = voxelArea.width*voxelArea.depth;
|
|
|
|
//Filter all ledges
|
|
#if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST
|
|
LinkedVoxelSpan[] spans = voxelArea.linkedSpans;
|
|
for (int z = 0, pz = 0; z < wd; z += voxelArea.width, pz++) {
|
|
for (int x = 0; x < voxelArea.width; x++) {
|
|
for (int s = z+x; s != -1 && spans[s].bottom != VoxelArea.InvalidSpanValue; s = spans[s].next) {
|
|
uint bottom = spans[s].top;
|
|
uint top = spans[s].next != -1 ? spans[spans[s].next].bottom : VoxelArea.MaxHeight;
|
|
|
|
if (top - bottom < voxelWalkableHeight) {
|
|
spans[s].area = UnwalkableArea;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#else
|
|
for (int z = 0, pz = 0; z < wd; z += voxelArea.width, pz++) {
|
|
for (int x = 0; x < voxelArea.width; x++) {
|
|
for (VoxelSpan s = voxelArea.cells[z+x].firstSpan; s != null; s = s.next) {
|
|
uint bottom = s.top;
|
|
uint top = s.next != null ? s.next.bottom : VoxelArea.MaxHeight;
|
|
|
|
if (top - bottom < voxelWalkableHeight) {
|
|
s.area = UnwalkableArea;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
//Code almost completely ripped from Recast
|
|
public void FilterLedges (uint voxelWalkableHeight, int voxelWalkableClimb, float cs, float ch) {
|
|
int wd = voxelArea.width*voxelArea.depth;
|
|
|
|
#if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST
|
|
LinkedVoxelSpan[] spans = voxelArea.linkedSpans;
|
|
int[] DirectionX = voxelArea.DirectionX;
|
|
int[] DirectionZ = voxelArea.DirectionZ;
|
|
#endif
|
|
int width = voxelArea.width;
|
|
|
|
//Filter all ledges
|
|
for (int z = 0, pz = 0; z < wd; z += width, pz++) {
|
|
for (int x = 0; x < width; x++) {
|
|
#if !ASTAR_RECAST_CLASS_BASED_LINKED_LIST
|
|
if (spans[x+z].bottom == VoxelArea.InvalidSpanValue) continue;
|
|
|
|
for (int s = x+z; s != -1; s = spans[s].next) {
|
|
//Skip non-walkable spans
|
|
if (spans[s].area == UnwalkableArea) {
|
|
continue;
|
|
}
|
|
|
|
int bottom = (int)spans[s].top;
|
|
int top = spans[s].next != -1 ? (int)spans[spans[s].next].bottom : VoxelArea.MaxHeightInt;
|
|
|
|
int minHeight = VoxelArea.MaxHeightInt;
|
|
|
|
int aMinHeight = (int)spans[s].top;
|
|
int aMaxHeight = aMinHeight;
|
|
|
|
for (int d = 0; d < 4; d++) {
|
|
int nx = x+DirectionX[d];
|
|
int nz = z+DirectionZ[d];
|
|
|
|
//Skip out-of-bounds points
|
|
if (nx < 0 || nz < 0 || nz >= wd || nx >= width) {
|
|
spans[s].area = UnwalkableArea;
|
|
break;
|
|
}
|
|
|
|
int nsx = nx+nz;
|
|
|
|
int nbottom = -voxelWalkableClimb;
|
|
|
|
int ntop = spans[nsx].bottom != VoxelArea.InvalidSpanValue ? (int)spans[nsx].bottom : VoxelArea.MaxHeightInt;
|
|
|
|
if (System.Math.Min(top, ntop) - System.Math.Max(bottom, nbottom) > voxelWalkableHeight) {
|
|
minHeight = System.Math.Min(minHeight, nbottom - bottom);
|
|
}
|
|
|
|
//Loop through spans
|
|
if (spans[nsx].bottom != VoxelArea.InvalidSpanValue) {
|
|
for (int ns = nsx; ns != -1; ns = spans[ns].next) {
|
|
nbottom = (int)spans[ns].top;
|
|
ntop = spans[ns].next != -1 ? (int)spans[spans[ns].next].bottom : VoxelArea.MaxHeightInt;
|
|
|
|
if (System.Math.Min(top, ntop) - System.Math.Max(bottom, nbottom) > voxelWalkableHeight) {
|
|
minHeight = System.Math.Min(minHeight, nbottom - bottom);
|
|
|
|
if (System.Math.Abs(nbottom - bottom) <= voxelWalkableClimb) {
|
|
if (nbottom < aMinHeight) { aMinHeight = nbottom; }
|
|
if (nbottom > aMaxHeight) { aMaxHeight = nbottom; }
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (minHeight < -voxelWalkableClimb || (aMaxHeight - aMinHeight) > voxelWalkableClimb) {
|
|
spans[s].area = UnwalkableArea;
|
|
}
|
|
}
|
|
#else
|
|
for (VoxelSpan s = voxelArea.cells[z+x].firstSpan; s != null; s = s.next) {
|
|
//Skip non-walkable spans
|
|
if (s.area == UnwalkableArea) {
|
|
continue;
|
|
}
|
|
|
|
int bottom = (int)s.top;
|
|
int top = s.next != null ? (int)s.next.bottom : VoxelArea.MaxHeightInt;
|
|
|
|
int minHeight = VoxelArea.MaxHeightInt;
|
|
|
|
int aMinHeight = (int)s.top;
|
|
int aMaxHeight = (int)s.top;
|
|
|
|
for (int d = 0; d < 4; d++) {
|
|
int nx = x+voxelArea.DirectionX[d];
|
|
int nz = z+voxelArea.DirectionZ[d];
|
|
|
|
//Skip out-of-bounds points
|
|
if (nx < 0 || nz < 0 || nz >= wd || nx >= voxelArea.width) {
|
|
s.area = UnwalkableArea;
|
|
break;
|
|
}
|
|
|
|
VoxelSpan nsx = voxelArea.cells[nx+nz].firstSpan;
|
|
|
|
int nbottom = -voxelWalkableClimb;
|
|
|
|
int ntop = nsx != null ? (int)nsx.bottom : VoxelArea.MaxHeightInt;
|
|
|
|
if (System.Math.Min(top, ntop) - System.Math.Max(bottom, nbottom) > voxelWalkableHeight) {
|
|
minHeight = System.Math.Min(minHeight, nbottom - bottom);
|
|
}
|
|
|
|
//Loop through spans
|
|
for (VoxelSpan ns = nsx; ns != null; ns = ns.next) {
|
|
nbottom = (int)ns.top;
|
|
ntop = ns.next != null ? (int)ns.next.bottom : VoxelArea.MaxHeightInt;
|
|
|
|
if (System.Math.Min(top, ntop) - System.Math.Max(bottom, nbottom) > voxelWalkableHeight) {
|
|
minHeight = System.Math.Min(minHeight, nbottom - bottom);
|
|
|
|
if (System.Math.Abs(nbottom - bottom) <= voxelWalkableClimb) {
|
|
if (nbottom < aMinHeight) { aMinHeight = nbottom; }
|
|
if (nbottom > aMaxHeight) { aMaxHeight = nbottom; }
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (minHeight < -voxelWalkableClimb || (aMaxHeight - aMinHeight) > voxelWalkableClimb) {
|
|
s.area = UnwalkableArea;
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|