123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710 |
- using System.Collections.Generic;
- using UnityEngine;
- namespace Pathfinding.Voxels {
- public partial class Voxelize {
- public bool FloodRegion (int x, int z, int i, uint level, ushort r, ushort[] srcReg, ushort[] srcDist, Int3[] stack, int[] flags = null, bool[] closed = null) {
- int area = voxelArea.areaTypes[i];
-
- int stackSize = 1;
- stack[0] = new Int3 {
- x = x,
- y = i,
- z = z,
- };
- srcReg[i] = r;
- srcDist[i] = 0;
- int lev = (int)(level >= 2 ? level-2 : 0);
- int count = 0;
-
- var DirectionX = voxelArea.DirectionX;
- var DirectionZ = voxelArea.DirectionZ;
- var compactCells = voxelArea.compactCells;
- var compactSpans = voxelArea.compactSpans;
- var areaTypes = voxelArea.areaTypes;
- var dist = voxelArea.dist;
- while (stackSize > 0) {
- stackSize--;
- var c = stack[stackSize];
-
- int ci = c.y;
- int cx = c.x;
- int cz = c.z;
- CompactVoxelSpan cs = compactSpans[ci];
-
-
- ushort ar = 0;
-
-
-
- for (int dir = 0; dir < 4; dir++) {
-
- if (cs.GetConnection(dir) != NotConnected) {
- int ax = cx + DirectionX[dir];
- int az = cz + DirectionZ[dir];
- int ai = (int)compactCells[ax+az].index + cs.GetConnection(dir);
- if (areaTypes[ai] != area)
- continue;
- ushort nr = srcReg[ai];
- if ((nr & BorderReg) == BorderReg)
- continue;
- if (nr != 0 && nr != r) {
- ar = nr;
-
- break;
- }
-
- int dir2 = (dir+1) & 0x3;
- var neighbour2 = compactSpans[ai].GetConnection(dir2);
-
- if (neighbour2 != NotConnected) {
- int ax2 = ax + DirectionX[dir2];
- int az2 = az + DirectionZ[dir2];
- int ai2 = (int)compactCells[ax2+az2].index + neighbour2;
- if (areaTypes[ai2] != area)
- continue;
- ushort nr2 = srcReg[ai2];
- if ((nr2 & BorderReg) == BorderReg)
- continue;
- if (nr2 != 0 && nr2 != r) {
- ar = nr2;
-
- break;
- }
- }
- }
- }
- if (ar != 0) {
- srcReg[ci] = 0;
- srcDist[ci] = (ushort)0xFFFF;
- continue;
- }
- count++;
- if (closed != null) {
- closed[ci] = true;
- }
-
- for (int dir = 0; dir < 4; ++dir) {
- if (cs.GetConnection(dir) != NotConnected) {
- int ax = cx + DirectionX[dir];
- int az = cz + DirectionZ[dir];
- int ai = (int)compactCells[ax+az].index + cs.GetConnection(dir);
- if (areaTypes[ai] != area)
- continue;
- if (srcReg[ai] == 0) {
- if (dist[ai] >= lev && flags[ai] == 0) {
- srcReg[ai] = r;
- srcDist[ai] = 0;
- stack[stackSize] = new Int3 {
- x = ax,
- y = ai,
- z = az,
- };
- stackSize++;
- } else if (flags != null) {
- flags[ai] = r;
- srcDist[ai] = 2;
- }
- }
- }
- }
- }
- return count > 0;
- }
- public void MarkRectWithRegion (int minx, int maxx, int minz, int maxz, ushort region, ushort[] srcReg) {
- int md = maxz * voxelArea.width;
- for (int z = minz*voxelArea.width; z < md; z += voxelArea.width) {
- for (int x = minx; x < maxx; x++) {
- CompactVoxelCell c = voxelArea.compactCells[z+x];
- for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; i++) {
- if (voxelArea.areaTypes[i] != UnwalkableArea) {
- srcReg[i] = region;
- }
- }
- }
- }
- }
- public ushort CalculateDistanceField (ushort[] src) {
- int wd = voxelArea.width*voxelArea.depth;
-
- 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];
- int nc = 0;
- for (int d = 0; d < 4; d++) {
- if (s.GetConnection(d) != NotConnected) {
-
-
-
-
-
-
- nc++;
- } else {
- break;
- }
- }
- if (nc != 4) {
- src[i] = 0;
- }
- }
- }
- }
-
- 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) {
-
- int nx = x+voxelArea.DirectionX[0];
- int nz = z+voxelArea.DirectionZ[0];
- int ni = (int)(voxelArea.compactCells[nx+nz].index+s.GetConnection(0));
- if (src[ni]+2 < src[i]) {
- src[i] = (ushort)(src[ni]+2);
- }
- CompactVoxelSpan ns = voxelArea.compactSpans[ni];
- if (ns.GetConnection(3) != NotConnected) {
-
- 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) {
-
- 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) {
-
- int nnx = nx+voxelArea.DirectionX[2];
- int nnz = nz+voxelArea.DirectionZ[2];
- int nni = (int)(voxelArea.compactCells[nnx+nnz].index+ns.GetConnection(2));
- if (src[nni]+3 < src[i]) {
- src[i] = (ushort)(src[nni]+3);
- }
- }
- }
- }
- }
- }
-
- for (int z = wd-voxelArea.width; z >= 0; z -= voxelArea.width) {
- for (int x = voxelArea.width-1; x >= 0; 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(2) != NotConnected) {
-
- int nx = x+voxelArea.DirectionX[2];
- int nz = z+voxelArea.DirectionZ[2];
- int ni = (int)(voxelArea.compactCells[nx+nz].index+s.GetConnection(2));
- if (src[ni]+2 < src[i]) {
- src[i] = (ushort)(src[ni]+2);
- }
- CompactVoxelSpan ns = voxelArea.compactSpans[ni];
- if (ns.GetConnection(1) != NotConnected) {
-
- int nnx = nx+voxelArea.DirectionX[1];
- int nnz = nz+voxelArea.DirectionZ[1];
- int nni = (int)(voxelArea.compactCells[nnx+nnz].index+ns.GetConnection(1));
- if (src[nni]+3 < src[i]) {
- src[i] = (ushort)(src[nni]+3);
- }
- }
- }
- if (s.GetConnection(1) != NotConnected) {
-
- int nx = x+voxelArea.DirectionX[1];
- int nz = z+voxelArea.DirectionZ[1];
- int ni = (int)(voxelArea.compactCells[nx+nz].index+s.GetConnection(1));
- if (src[ni]+2 < src[i]) {
- src[i] = (ushort)(src[ni]+2);
- }
- CompactVoxelSpan ns = voxelArea.compactSpans[ni];
- if (ns.GetConnection(0) != NotConnected) {
-
- int nnx = nx+voxelArea.DirectionX[0];
- int nnz = nz+voxelArea.DirectionZ[0];
- int nni = (int)(voxelArea.compactCells[nnx+nnz].index+ns.GetConnection(0));
- if (src[nni]+3 < src[i]) {
- src[i] = (ushort)(src[nni]+3);
- }
- }
- }
- }
- }
- }
- ushort maxDist = 0;
- for (int i = 0; i < voxelArea.compactSpanCount; i++) {
- maxDist = System.Math.Max(src[i], maxDist);
- }
- return maxDist;
- }
- public ushort[] BoxBlur (ushort[] src, ushort[] dst) {
- ushort thr = 20;
- int wd = voxelArea.width*voxelArea.depth;
- for (int z = wd-voxelArea.width; z >= 0; z -= voxelArea.width) {
- for (int x = voxelArea.width-1; x >= 0; 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];
- ushort cd = src[i];
- if (cd < thr) {
- dst[i] = cd;
- continue;
- }
- int total = (int)cd;
- for (int d = 0; d < 4; d++) {
- if (s.GetConnection(d) != NotConnected) {
- int nx = x+voxelArea.DirectionX[d];
- int nz = z+voxelArea.DirectionZ[d];
- int ni = (int)(voxelArea.compactCells[nx+nz].index+s.GetConnection(d));
- total += (int)src[ni];
- CompactVoxelSpan ns = voxelArea.compactSpans[ni];
- int d2 = (d+1) & 0x3;
- if (ns.GetConnection(d2) != NotConnected) {
- int nnx = nx+voxelArea.DirectionX[d2];
- int nnz = nz+voxelArea.DirectionZ[d2];
- int nni = (int)(voxelArea.compactCells[nnx+nnz].index+ns.GetConnection(d2));
- total += (int)src[nni];
- } else {
- total += cd;
- }
- } else {
- total += cd*2;
- }
- }
- dst[i] = (ushort)((total+5)/9F);
- }
- }
- }
- return dst;
- }
- public void BuildRegions () {
-
- int w = voxelArea.width;
- int d = voxelArea.depth;
- int wd = w*d;
- int spanCount = voxelArea.compactSpanCount;
- int expandIterations = 8;
- ushort[] srcReg = Util.ArrayPool<ushort>.Claim(spanCount);
- ushort[] srcDist = Util.ArrayPool<ushort>.Claim(spanCount);
- bool[] closed = Util.ArrayPool<bool>.Claim(spanCount);
- int[] spanFlags = Util.ArrayPool<int>.Claim(spanCount);
- Int3[] stack = Util.ArrayPool<Int3>.Claim(spanCount);
-
- Util.Memory.MemSet(srcReg, (ushort)0, sizeof(ushort));
- Util.Memory.MemSet(srcDist, (ushort)0xFFFF, sizeof(ushort));
- Util.Memory.MemSet(closed, false, sizeof(bool));
- Util.Memory.MemSet(spanFlags, 0, sizeof(int));
- var DirectionX = voxelArea.DirectionX;
- var DirectionZ = voxelArea.DirectionZ;
- var spanDistances = voxelArea.dist;
- var areaTypes = voxelArea.areaTypes;
- var compactCells = voxelArea.compactCells;
- 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++;
- Int3[][] buckedSortedSpans = new Int3[(voxelArea.maxDistance)/2 + 1][];
- int[] sortedSpanCounts = new int[buckedSortedSpans.Length];
- for (int i = 0; i < buckedSortedSpans.Length; i++) buckedSortedSpans[i] = new Int3[16];
-
-
-
- for (int z = 0, pz = 0; z < wd; z += w, pz++) {
- for (int x = 0; x < voxelArea.width; x++) {
- CompactVoxelCell c = compactCells[z+x];
- for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; i++) {
- if ((srcReg[i] & BorderReg) == BorderReg)
- continue;
- if (areaTypes[i] == UnwalkableArea)
- continue;
- int distIndex = voxelArea.dist[i] / 2;
- if (sortedSpanCounts[distIndex] >= buckedSortedSpans[distIndex].Length) {
- var newBuffer = new Int3[sortedSpanCounts[distIndex]*2];
- buckedSortedSpans[distIndex].CopyTo(newBuffer, 0);
- buckedSortedSpans[distIndex] = newBuffer;
- }
- buckedSortedSpans[distIndex][sortedSpanCounts[distIndex]++] = new Int3(x, i, z);
- }
- }
- }
-
- Queue<Int3> srcQue = new Queue<Int3>();
- Queue<Int3> dstQue = new Queue<Int3>();
-
- for (int distIndex = buckedSortedSpans.Length - 1; distIndex >= 0; distIndex--) {
- var level = (uint)distIndex * 2;
- var spans = buckedSortedSpans[distIndex];
- var spansAtLevel = sortedSpanCounts[distIndex];
- for (int i = 0; i < spansAtLevel; i++) {
- int spanIndex = spans[i].y;
-
- if (spanFlags[spanIndex] != 0 && srcReg[spanIndex] == 0) {
- srcReg[spanIndex] = (ushort)spanFlags[spanIndex];
- srcQue.Enqueue(spans[i]);
- closed[spanIndex] = true;
- }
- }
-
- for (int expansionIteration = 0; expansionIteration < expandIterations && srcQue.Count > 0; expansionIteration++) {
- while (srcQue.Count > 0) {
- Int3 spanInfo = srcQue.Dequeue();
- var area = areaTypes[spanInfo.y];
- var span = voxelArea.compactSpans[spanInfo.y];
- var region = srcReg[spanInfo.y];
- closed[spanInfo.y] = true;
- ushort nextDist = (ushort)(srcDist[spanInfo.y] + 2);
-
- for (int dir = 0; dir < 4; dir++) {
- var neighbour = span.GetConnection(dir);
- if (neighbour == NotConnected) continue;
- int nx = spanInfo.x + DirectionX[dir];
- int nz = spanInfo.z + DirectionZ[dir];
- int ni = (int)compactCells[nx+nz].index + neighbour;
- if ((srcReg[ni] & BorderReg) == BorderReg)
- continue;
-
- if (area == areaTypes[ni]) {
- if (nextDist < srcDist[ni]) {
- if (spanDistances[ni] < level) {
- srcDist[ni] = nextDist;
- spanFlags[ni] = region;
- } else if (!closed[ni]) {
- srcDist[ni] = nextDist;
- if (srcReg[ni] == 0) dstQue.Enqueue(new Int3(nx, ni, nz));
- srcReg[ni] = region;
- }
- }
- }
- }
- }
- Util.Memory.Swap(ref srcQue, ref dstQue);
- }
-
- for (int i = 0; i < spansAtLevel; i++) {
- var info = spans[i];
- if (srcReg[info.y] == 0) {
- if (!FloodRegion(info.x, info.z, info.y, level, regionId, srcReg, srcDist, stack, spanFlags, closed)) {
-
-
- } else {
- regionId++;
- }
- }
- }
- }
- voxelArea.maxRegions = regionId;
-
- FilterSmallRegions(srcReg, minRegionSize, voxelArea.maxRegions);
-
- var compactSpans = voxelArea.compactSpans;
- for (int i = 0; i < spanCount; i++) {
- compactSpans[i].reg = srcReg[i];
- }
-
- Util.ArrayPool<ushort>.Release(ref srcReg);
- Util.ArrayPool<ushort>.Release(ref srcDist);
- Util.ArrayPool<bool>.Release(ref closed);
- Util.ArrayPool<int>.Release(ref spanFlags);
- Util.ArrayPool<Int3>.Release(ref stack);
-
- }
-
-
-
-
- static int union_find_find (int[] arr, int x) {
- if (arr[x] < 0) return x;
- return arr[x] = union_find_find(arr, arr[x]);
- }
-
-
-
-
- static void union_find_union (int[] arr, int a, int b) {
- a = union_find_find(arr, a);
- b = union_find_find(arr, b);
- if (a == b) return;
- if (arr[a] > arr[b]) {
- int tmp = a;
- a = b;
- b = tmp;
- }
- arr[a] += arr[b];
- arr[b] = a;
- }
-
- public void FilterSmallRegions (ushort[] reg, int minRegionSize, int maxRegions) {
- RelevantGraphSurface c = RelevantGraphSurface.Root;
-
- bool anySurfaces = !RelevantGraphSurface.ReferenceEquals(c, null) && (relevantGraphSurfaceMode != RecastGraph.RelevantGraphSurfaceMode.DoNotRequire);
-
- if (!anySurfaces && minRegionSize <= 0) {
- return;
- }
- int[] counter = new int[maxRegions];
- ushort[] bits = voxelArea.tmpUShortArr;
- if (bits == null || bits.Length < maxRegions) {
- bits = voxelArea.tmpUShortArr = new ushort[maxRegions];
- }
- Util.Memory.MemSet(counter, -1, sizeof(int));
- Util.Memory.MemSet(bits, (ushort)0, maxRegions, sizeof(ushort));
- int nReg = counter.Length;
- int wd = voxelArea.width*voxelArea.depth;
- const int RelevantSurfaceSet = 1 << 1;
- const int BorderBit = 1 << 0;
-
-
- int RelevantSurfaceCheck = RelevantSurfaceSet | ((relevantGraphSurfaceMode == RecastGraph.RelevantGraphSurfaceMode.OnlyForCompletelyInsideTile) ? BorderBit : 0x0);
- if (anySurfaces) {
-
- while (!RelevantGraphSurface.ReferenceEquals(c, null)) {
- int x, z;
- this.VectorToIndex(c.Position, out x, out z);
-
- if (x >= 0 && z >= 0 && x < voxelArea.width && z < voxelArea.depth) {
- int y = (int)((c.Position.y - voxelOffset.y)/cellHeight);
- int rad = (int)(c.maxRange / cellHeight);
- CompactVoxelCell cell = voxelArea.compactCells[x+z*voxelArea.width];
- for (int i = (int)cell.index; i < cell.index+cell.count; i++) {
- CompactVoxelSpan s = voxelArea.compactSpans[i];
- if (System.Math.Abs(s.y - y) <= rad && reg[i] != 0) {
- bits[union_find_find(counter, (int)reg[i] & ~BorderReg)] |= RelevantSurfaceSet;
- }
- }
- }
- c = c.Next;
- }
- }
- for (int z = 0, pz = 0; z < wd; z += voxelArea.width, pz++) {
- for (int x = 0; x < voxelArea.width; x++) {
- CompactVoxelCell cell = voxelArea.compactCells[x+z];
- for (int i = (int)cell.index; i < cell.index+cell.count; i++) {
- CompactVoxelSpan s = voxelArea.compactSpans[i];
- int r = (int)reg[i];
- if ((r & ~BorderReg) == 0) continue;
- if (r >= nReg) {
- bits[union_find_find(counter, r & ~BorderReg)] |= BorderBit;
- continue;
- }
- int k = union_find_find(counter, r);
-
- counter[k]--;
- 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);
- int r2 = (int)reg[ni];
- if (r != r2 && (r2 & ~BorderReg) != 0) {
- if ((r2 & BorderReg) != 0) {
- bits[k] |= BorderBit;
- } else {
- union_find_union(counter, k, r2);
- }
-
- }
- }
-
- }
- }
- }
-
- for (int i = 0; i < counter.Length; i++) bits[union_find_find(counter, i)] |= bits[i];
- for (int i = 0; i < counter.Length; i++) {
- int ctr = union_find_find(counter, i);
-
- if ((bits[ctr] & BorderBit) != 0) counter[ctr] = -minRegionSize-2;
-
-
- if (anySurfaces && (bits[ctr] & RelevantSurfaceCheck) == 0) counter[ctr] = -1;
- }
- for (int i = 0; i < voxelArea.compactSpanCount; i++) {
- int r = (int)reg[i];
- if (r >= nReg) {
- continue;
- }
- if (counter[union_find_find(counter, r)] >= -minRegionSize-1) {
- reg[i] = 0;
- }
- }
- }
- }
- }
|