public CNRSearchNode(CNavRect Rect, Vector2 position, float cost, float pathCost, CNRSearchNode next)
{
mRect = Rect;
mPosition = position;
mCost = cost;
mPathCost = pathCost;
mNextMove = next;
}
public CNavRectPortal(Vector2 A, Vector2 B, CNavRect RectA, CNavRect RectB)
{
mA = A;
mB = B;
// NOTE: The centre is at half size for the pathfinder.
mCentre = ((B - A) * 0.5f + A) * 0.5f;
mRectA = RectA;
mRectB = RectB;
}
/// <summary>
/// Method that switfly finds the best path from start to end. Doesn't reverse outcome.
/// </summary>
private static CNRSearchNode _FindReversedPath(CNavRectMesh NavMesh, Vector2 Start, Vector2 End, int OccupiedID)
{
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
Start.x = Mathf.Clamp(Start.x, 0, 99);
Start.y = Mathf.Clamp(Start.y, 0, 99);
End.x = Mathf.Clamp(End.x, 0, 99);
End.y = Mathf.Clamp(End.y, 0, 99);
CNavRect startRect = null;
CNavRect endRect = null;
startRect = NavMesh.mRectLookup[(int)(Start.x * 2.0f), (int)(Start.y * 2.0f)];
endRect = NavMesh.mRectLookup[(int)(End.x * 2.0f), (int)(End.y * 2.0f)];
if (startRect == null || endRect == null)
{
return(null);
}
int occupiedID = 10000 + OccupiedID;
if ((startRect.mFlags > 10000 && startRect.mFlags != occupiedID) ||
(endRect.mFlags > 10000 && endRect.mFlags != occupiedID))
{
return(null);
}
// TODO: Ideally we only clamp if we must.
Start.x = Mathf.Clamp(Start.x, startRect.mRect.xMin * 0.5f + 0.25f, startRect.mRect.xMax * 0.5f - 0.25f);
Start.y = Mathf.Clamp(Start.y, startRect.mRect.yMin * 0.5f + 0.25f, startRect.mRect.yMax * 0.5f - 0.25f);
End.x = Mathf.Clamp(End.x, endRect.mRect.xMin * 0.5f + 0.25f, endRect.mRect.xMax * 0.5f - 0.25f);
End.y = Mathf.Clamp(End.y, endRect.mRect.yMin * 0.5f + 0.25f, endRect.mRect.yMax * 0.5f - 0.25f);
double rectFindTime = sw.Elapsed.TotalMilliseconds;
CNRSearchNode startNode = new CNRSearchNode(startRect, Start, 0, 0, null);
CNRMinHeap openList = new CNRMinHeap();
openList.Add(startNode);
_brRects[startRect.mIndex] = ++_brTurn;
double allocateTime = sw.Elapsed.TotalMilliseconds - rectFindTime;
CNRSearchNode current = null;
int steps = 0;
while (openList.HasNext())
{
++steps;
current = openList.ExtractFirst();
if (current.mRect == endRect)
{
sw.Stop();
//Debug.Log("Steps: " + steps + " Allocs: " + searchNodeCount + " T: " + sw.Elapsed.TotalMilliseconds + "ms F: " + rectFindTime + "ms A: " + allocateTime + "ms");
float moveCost = (End - current.mPosition).SqrMagnitude();
return(new CNRSearchNode(endRect, End, 0, moveCost + current.mPathCost, current));
}
for (int i = 0; i < current.mRect.mPortals.Count; ++i)
{
CNavRectPortal p = current.mRect.mPortals[i];
// TODO: Some way to ditch this check?
CNavRect r = p.mRectA != current.mRect ? p.mRectA : p.mRectB;
if (_brRects[r.mIndex] == _brTurn)
{
continue;
}
if (r.mFlags > 10000 && r.mFlags != occupiedID)
{
continue;
}
_brRects[r.mIndex] = _brTurn;
float moveCost = (p.mCentre - current.mPosition).SqrMagnitude();
float costToEnd = (End - p.mCentre).SqrMagnitude();
float currentPathCost = current.mPathCost + moveCost;
float entirePathCost = currentPathCost + costToEnd;
CNRSearchNode node = new CNRSearchNode(r, p.mCentre, entirePathCost, currentPathCost, current);
openList.Add(node);
}
}
sw.Stop();
//Debug.Log("(FAILED) Steps: " + steps + " Time: " + sw.Elapsed.TotalMilliseconds + "ms");
return(null);
}
public CNavRectEdge(Vector2 A, Vector2 B, CNavRect Rect)
{
mA = A;
mB = B;
mRect = Rect;
}
public void Generate(CMap Map, int PlayerId)
{
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
mRects.Clear();
mEdges.Clear();
mPortals.Clear();
Array.Clear(mRectLookup, 0, mRectLookup.Length);
for (int i = 0; i < Map.mWidth * 2 + 1; ++i)
{
mXAxisEdges[i].Clear();
mYAxisEdges[i].Clear();
}
for (int iX = 0; iX < 7; ++iX)
{
for (int iY = 0; iY < 7; ++iY)
{
mBuckets[iX, iY].Clear();
}
}
CCollisionTile[,] tileMap = Map.mLocalCollisionTiles[PlayerId];
double setupTime = sw.Elapsed.TotalMilliseconds;
// Touch all cells and expand rects
for (int iY = 0; iY < Map.mWidth * 2; ++iY)
{
for (int iX = 0; iX < Map.mWidth * 2; ++iX)
{
CCollisionTile tile = tileMap[iX, iY];
if (mRectLookup[iX, iY] == null && tile.mOccupied >= 0)
{
bool expandX = true;
bool expandY = true;
int sizeX = 1;
int sizeY = 1;
//int type = tile.mOccupied + 10000 * (tile.mSolid ? 1 : 0);
int type = (tile.mOccupied + 10000) * (tile.mSolid ? 1 : 0);
while (expandX || expandY)
{
// Check expansion in X
if (expandX)
{
expandX = _CanExpandX(mRectLookup, tileMap, iX + sizeX - 1, iY, sizeY, type);
}
// Check expansion in Y
if (expandY)
{
expandY = _CanExpandY(mRectLookup, tileMap, iX, iY + sizeY - 1, sizeX, type);
}
// Check corner expansion if we do both
if (expandX && expandY)
{
// If we fail here, then prefer expand X.
if (mRectLookup[iX + sizeX, iY + sizeY] != null ||
//tileMap[iX + sizeX, iY + sizeY].mOccupied + 10000 * (tileMap[iX + sizeX, iY + sizeY].mSolid ? 1 : 0) != type ||
(tileMap[iX + sizeX, iY + sizeY].mOccupied + 10000) * (tileMap[iX + sizeX, iY + sizeY].mSolid ? 1 : 0) != type ||
tileMap[iX + sizeX, iY + sizeY].mWallXSolid ||
tileMap[iX + sizeX, iY + sizeY].mWallZSolid)
{
expandY = false;
}
}
if ((iX + sizeX) % 32 == 0)
{
expandX = false;
}
if ((iY + sizeY) % 32 == 0)
{
expandY = false;
}
if (expandX)
{
++sizeX;
}
if (expandY)
{
++sizeY;
}
}
// Add nav rect.
CNavRect r = new CNavRect();
r.mIndex = mRects.Count;
r.mRect = new Rect(iX, iY, sizeX, sizeY);
r.mFlags = type;
r.mRoomId = -1;
mRects.Add(r);
mBuckets[iX / 32, iY / 32].Add(r);
// Clear expanded area
for (int jX = iX; jX < iX + sizeX; ++jX)
{
for (int jY = iY; jY < iY + sizeY; ++jY)
{
mRectLookup[jX, jY] = r;
}
}
}
}
}
double expansionTime = sw.Elapsed.TotalMilliseconds;
// Go through each rect and determine accesible edges.
for (int i = 0; i < mRects.Count; ++i)
{
Rect r = mRects[i].mRect;
//-----------------------------------------------------------------------------
// Bottom X
//-----------------------------------------------------------------------------
int start = (int)r.xMin;
int run = 0;
int y = (int)r.y;
int x = (int)r.x;
for (int j = start; j < (int)r.xMax; ++j)
{
if (tileMap[j, y].mWallXSolid)
{
if (run != 0)
{
// Add run to this point
CNavRectEdge e = new CNavRectEdge(new Vector2(start, y), new Vector2(start + run, y), mRects[i]);
mEdges.Add(e);
mXAxisEdges[y].Add(e);
run = 0;
}
start = j + 1;
}
else
{
if (run != 0 && tileMap[j, y - 1].mWallZSolid)
{
// Add run to this point and start with 1 already in run
CNavRectEdge e = new CNavRectEdge(new Vector2(start, y), new Vector2(start + run, y), mRects[i]);
mEdges.Add(e);
mXAxisEdges[y].Add(e);
start = j;
run = 0;
}
++run;
}
}
// Add final non-blocked edge.
if (run != 0)
{
CNavRectEdge e = new CNavRectEdge(new Vector2(start, y), new Vector2(start + run, y), mRects[i]);
mEdges.Add(e);
mXAxisEdges[y].Add(e);
}
//-----------------------------------------------------------------------------
// Top X
//-----------------------------------------------------------------------------
start = (int)r.xMin;
run = 0;
y = (int)r.yMax;
x = (int)r.x;
for (int j = start; j < (int)r.xMax; ++j)
{
if (tileMap[j, y].mWallXSolid)
{
if (run != 0)
{
// Add run to this point
CNavRectEdge e = new CNavRectEdge(new Vector2(start, y), new Vector2(start + run, y), mRects[i]);
mEdges.Add(e);
mXAxisEdges[y].Add(e);
run = 0;
}
start = j + 1;
}
else
{
if (run != 0 && tileMap[j, y].mWallZSolid)
{
// Add run to this point and start with 1 already in run
CNavRectEdge e = new CNavRectEdge(new Vector2(start, y), new Vector2(start + run, y), mRects[i]);
mEdges.Add(e);
mXAxisEdges[y].Add(e);
start = j;
run = 0;
}
++run;
}
}
// Add final non-blocked edge.
if (run != 0)
{
CNavRectEdge e = new CNavRectEdge(new Vector2(start, y), new Vector2(start + run, y), mRects[i]);
mEdges.Add(e);
mXAxisEdges[y].Add(e);
}
//-----------------------------------------------------------------------------
// Left Y
//-----------------------------------------------------------------------------
start = (int)r.yMin;
run = 0;
y = (int)r.y;
x = (int)r.x;
for (int j = start; j < (int)r.yMax; ++j)
{
if (tileMap[x, j].mWallZSolid)
{
if (run != 0)
{
// Add run to this point
CNavRectEdge e = new CNavRectEdge(new Vector2(x, start), new Vector2(x, start + run), mRects[i]);
mEdges.Add(e);
mYAxisEdges[x].Add(e);
run = 0;
}
start = j + 1;
}
else
{
if (run != 0 && tileMap[x - 1, j].mWallXSolid)
{
// Add run to this point and start with 1 already in run
CNavRectEdge e = new CNavRectEdge(new Vector2(x, start), new Vector2(x, start + run), mRects[i]);
mEdges.Add(e);
mYAxisEdges[x].Add(e);
start = j;
run = 0;
}
++run;
}
}
// Add final non-blocked edge.
if (run != 0)
{
CNavRectEdge e = new CNavRectEdge(new Vector2(x, start), new Vector2(x, start + run), mRects[i]);
mEdges.Add(e);
mYAxisEdges[x].Add(e);
}
//-----------------------------------------------------------------------------
// Right Y
//-----------------------------------------------------------------------------
start = (int)r.yMin;
run = 0;
y = (int)r.y;
x = (int)r.xMax;
for (int j = start; j < (int)r.yMax; ++j)
{
if (tileMap[x, j].mWallZSolid)
{
if (run != 0)
{
// Add run to this point
CNavRectEdge e = new CNavRectEdge(new Vector2(x, start), new Vector2(x, start + run), mRects[i]);
mEdges.Add(e);
mYAxisEdges[x].Add(e);
run = 0;
}
start = j + 1;
}
else
{
if (run != 0 && tileMap[x, j].mWallXSolid)
{
// Add run to this point and start with 1 already in run
CNavRectEdge e = new CNavRectEdge(new Vector2(x, start), new Vector2(x, start + run), mRects[i]);
mEdges.Add(e);
mYAxisEdges[x].Add(e);
start = j;
run = 0;
}
++run;
}
}
// Add final non-blocked edge.
if (run != 0)
{
CNavRectEdge e = new CNavRectEdge(new Vector2(x, start), new Vector2(x, start + run), mRects[i]);
mEdges.Add(e);
mYAxisEdges[x].Add(e);
}
}
double edgesTime = sw.Elapsed.TotalMilliseconds;
// Generate portals.
for (int k = 0; k < Map.mWidth * 2 + 1; ++k)
{
List <CNavRectEdge> edges = mXAxisEdges[k];
for (int i = 0; i < edges.Count; ++i)
{
for (int j = i + 1; j < edges.Count; ++j)
{
float min = Mathf.Max(edges[i].mA.x, edges[j].mA.x);
float max = Mathf.Min(edges[i].mB.x, edges[j].mB.x);
if (min < max)
{
CNavRectPortal p = new CNavRectPortal(new Vector2(min, edges[i].mA.y), new Vector2(max, edges[i].mA.y), edges[i].mRect, edges[j].mRect);
mPortals.Add(p);
edges[i].mRect.mPortals.Add(p);
edges[j].mRect.mPortals.Add(p);
}
}
}
edges = mYAxisEdges[k];
for (int i = 0; i < edges.Count; ++i)
{
for (int j = i + 1; j < edges.Count; ++j)
{
float min = Mathf.Max(edges[i].mA.y, edges[j].mA.y);
float max = Mathf.Min(edges[i].mB.y, edges[j].mB.y);
if (min < max)
{
CNavRectPortal p = new CNavRectPortal(new Vector2(edges[i].mA.x, min), new Vector2(edges[i].mA.x, max), edges[i].mRect, edges[j].mRect);
mPortals.Add(p);
edges[i].mRect.mPortals.Add(p);
edges[j].mRect.mPortals.Add(p);
}
}
}
}
double portalsTime = sw.Elapsed.TotalMilliseconds;
// Generate room IDs.
int roomCounter = 1;
List <CNavRect> rectStack = new List <CNavRect>();
int rectScan = 0;
while (rectScan < mRects.Count)
{
if (mRects[rectScan].mRoomId == -1)
{
if (mRects[rectScan].mFlags >= 10000)
{
mRects[rectScan].mRoomId = 0;
}
else
{
rectStack.Add(mRects[rectScan]);
mRects[rectScan].mRoomId = roomCounter;
while (rectStack.Count > 0)
{
CNavRect rect = rectStack[0];
rectStack.RemoveAt(0);
for (int i = 0; i < rect.mPortals.Count; ++i)
{
CNavRectPortal p = rect.mPortals[i];
if (p.mRectA.mFlags < 10000 && p.mRectA.mRoomId == -1)
{
rectStack.Add(p.mRectA);
p.mRectA.mRoomId = roomCounter;
}
if (p.mRectB.mFlags < 10000 && p.mRectB.mRoomId == -1)
{
rectStack.Add(p.mRectB);
p.mRectB.mRoomId = roomCounter;
}
}
}
}
++roomCounter;
}
++rectScan;
}
double roomTime = sw.Elapsed.TotalMilliseconds;
sw.Stop();
Debug.Log("NavRect: " + sw.Elapsed.TotalMilliseconds.ToString("0.000") +
"ms Setup: " + setupTime.ToString("0.000") +
"ms Rects(" + mRects.Count + "): " + (expansionTime - setupTime).ToString("0.000") +
"ms Edges(" + mEdges.Count + "): " + (edgesTime - expansionTime).ToString("0.000") +
"ms Portals(" + mPortals.Count + "): " + (portalsTime - edgesTime).ToString("0.000") +
"ms Rooms: " + (roomTime - portalsTime).ToString("0.000") + "ms");
}
