public CompiledNavMesh FindSubgraphUnderPoint(float x, float y, float z, float maxFall = 20f)
{
CompiledNavMesh bestGraph = null;
float bestDistance = maxFall;
foreach (var sg in Subgraphs)
{
if (!sg.BoundsContainsXY(x, y))
{
continue;
}
var node = sg.FindFloorUnderPoint(x, y, z, maxFall);
if (node.blockIndex != -1)
{
float hitZ = NavMeshUtil.DecodeHeight(sg.Z1, sg.Z2, node.encZ);
if (hitZ <= z)
{
float d = z - hitZ;
if (d < bestDistance)
{
bestDistance = d;
bestGraph = sg;
}
}
}
}
return(bestGraph);
}
public NodeId GetNeighbor(NodeId start, int direction)
{
NodeId result = new NodeId(-1, 0, 0xFF);
if ((int)(start.directionFlags & direction) == 0)
{
var offset = NavMeshUtil.OffsetFromDirectionFlag(direction);
int neighborIndex = start.blockIndex + (offset.Y * m_blockWidth + offset.X);
ForeachHeightAtIndex(neighborIndex, (entry) =>
{
float height = entry.GetRealHeight(m_z1, m_z2);
// add .5 because the step isn't always exact.
if (Math.Abs(NavMeshUtil.DecodeHeight(m_z1, m_z2, start.encZ) - height) > m_maxZStep + 0.5f)
{
return(true); // keep looping
}
result = new NodeId(neighborIndex, entry.encZ, entry.directionFlags);
return(false); // stop loop
});
if (result.blockIndex == -1)
{
throw new InvalidOperationException("expected to find neighbor");
}
}
return(result);
}
private void CollectEdgesBetweenFloorLists(int bx, int by,
List <NavMeshBuilderFloorDesc> center, int nx, int ny,
Dictionary <EdgeVertex, List <EdgeVertex> > edgesOut)
{
List <NavMeshBuilderFloorDesc> neighbor = GetFloorDescListNoCheck(nx, ny);
if (neighbor == null)
{
return;
}
var directionToNeighbor = NavMeshUtil.DetermineDirection(bx, by, nx, ny);
var directionFromNeighbor = NavMeshUtil.GetInverseDirection(directionToNeighbor);
foreach (var floor in center)
{
// check if direction to neighbor is blocked.
if ((floor.DirectionFlags & directionToNeighbor) != 0)
{
continue;
}
List <EdgeVertex> list = null;
foreach (var neighborFloor in neighbor)
{
// check if direction from neighbor is blocked.
if ((neighborFloor.DirectionFlags & directionFromNeighbor) != 0)
{
continue;
}
if (Math.Abs(floor.Z100i - neighborFloor.Z100i) / 100.0f > m_maxZStep)
{
continue;
}
if (list == null)
{
var p0 = new EdgeVertex {
BX = (ushort)bx, BY = (ushort)by, Z100i = floor.Z100i
};
if (!edgesOut.TryGetValue(p0, out list))
{
list = new List <EdgeVertex>();
edgesOut.Add(p0, list);
}
}
list.Add(new EdgeVertex {
BX = (ushort)nx, BY = (ushort)ny, Z100i = neighborFloor.Z100i
});
break; // each floor connects to 0 or 1 neighbor in 1 direction.
}
}
}
public void ForeachHeightAtXY(int blockX, int blockY, Action <int, float> action)
{
if (blockX < 0 || blockY < 0 || blockX >= m_blockWidth || blockY >= m_blockHeight)
{
throw new ArgumentOutOfRangeException();
}
int blockIndex = blockY * m_blockWidth + blockX;
ForeachHeightAtIndex(blockIndex, (entry) =>
{
action(entry.directionFlags, NavMeshUtil.DecodeHeight(m_z1, m_z2, entry.encZ));
return(true); // continue looping
});
}
// bx/by must be offset to neighbor. returns height of neighbor, or -Inf if no edge.
// there should be 0 or 1 edge per direction. neighboring nodes
// should have a matching edge flag in the inverse direction.
public float GetEdge(int startBlockX, int startBlockY, float startFloor, int startFlags, int directionFlag)
{
if ((startFlags & directionFlag) != 0)
{
return(float.NegativeInfinity);
}
Point ofs = NavMeshUtil.OffsetFromDirectionFlag(directionFlag);
uint neighbor = m_grid[(startBlockY + ofs.Y) * m_blockWidth + (startBlockX + ofs.X)];
int count = (int)(neighbor >> 24);
if (count == 1)
{
var entry = Entry.UnpackSingleEntry(neighbor);
float height = entry.GetRealHeight(m_z1, m_z2);
if (Math.Abs(startFloor - height) <= m_maxZStep + 0.1f) // TODO added +.1 for epsilon...
{
return(height);
}
}
else if (count > 1)
{
int index = (int)(neighbor & 0xFFFFFF);
for (int i = 0; i < count; i++, index += 3)
{
var entry = Entry.UnpackMultiEntry(m_multiheights, index);
float height = entry.GetRealHeight(m_z1, m_z2);
if (Math.Abs(startFloor - height) <= m_maxZStep + 0.1f) // TODO added +.1 for epsilon...
{
return(height);
}
}
}
throw new InvalidOperationException("neighbor should exist for direction");
}
/*
* height grid:
* each entry:
* 0xFF000000 - number of heights (if <= 1, inline. else index to list).
* 0x00FFFFFF - index to heights, or 3 byte single height. max distinct = 16 million (4096^2).
*
* height format (3 bytes):
* 0xFF - 8 directions, from top cw, top TR R BR bot BL left TL.
* 0xFFFF - encoded height = (height_value - z1) * 0xFFFF / (z2-z1).
*
* if 8dir = 0xFF, space is impassable. impassable should always be inline (no index) with floor count 0.
*
* each node edge is implied by neighbor having a close height that is not blocked in that direction.
*/
public CompiledNavMesh Build(HashSet <EdgeVertex> subgraph,
int startBX, int startBY, int newBlockWidth, int newBlockHeight,
float x1, float y1, // world XY of the lower bound of the mesh bounding box.
int z1, int z2) // min and max z values, range is used for encoding height
{
if (startBX < 0 || startBY < 0 || newBlockWidth <= 0 || newBlockHeight <= 0 ||
startBX + newBlockWidth > m_blockWidth || startBY + newBlockHeight > m_blockHeight)
{
throw new ArgumentOutOfRangeException();
}
if (z1 < 0 || z2 <= z1)
{
throw new ArgumentOutOfRangeException();
}
int endBX = startBX + newBlockWidth;
int endBY = startBY + newBlockHeight;
uint[] grid = new uint[newBlockWidth * newBlockHeight];
var multiheights = new List <byte>(newBlockWidth * newBlockHeight); // arbitrary capacity
var bytes = new List <byte>(); // temp storage per cell
int iSub = 0; // index to new grid for subgraph
for (int y = startBY; y < endBY; y++)
{
int iAll = y * m_blockWidth + startBX; // index to floor cell
for (int x = startBX; x < endBX; x++, iAll++, iSub++)
{
bytes.Clear();
if (m_floorData[iAll] != null)
{
foreach (var floor in m_floorData[iAll])
{
ushort encHeight = NavMeshUtil.EncodeHeight(z1, z2, floor.Z100i / 100.0f);
var flags = floor.DirectionFlags;
if (flags == 0xFF)
{
continue;
}
// exclude vertices that are not part of the subgraph.
if (!subgraph.Contains(new EdgeVertex {
BX = (ushort)x, BY = (ushort)y, Z100i = floor.Z100i
}))
{
continue;
}
bytes.Add(flags);
bytes.Add((byte)(encHeight));
bytes.Add((byte)(encHeight >> 8));
}
}
if (bytes.Count == 3)
{
grid[iSub] = (1 << 24) | // count 1
((uint)bytes[0] << 16) | // flags
((uint)bytes[2] << 8) | // encoded height
bytes[1];
}
else if (bytes.Count > 3)
{
if (bytes.Count > 255 * 3)
{
throw new InvalidOperationException("too many heights");
}
int index = StoreBytesInMultiFloorSet(multiheights, bytes);
grid[iSub] = (uint)((bytes.Count / 3) << 24) | (uint)index;
}
else
{
grid[iSub] = 0x00FF0000;
}
}
}
if (multiheights.Count > 0xFFFFFF)
{
throw new InvalidOperationException("height limit exceeded");
}
Debug.WriteLine($"Compiled NavMesh {newBlockWidth}x{newBlockHeight}x{z2-z1} bytes: grid:{grid.Length*4}, Zs:{multiheights.Count}");
var compiledNavMesh = new CompiledNavMesh(newBlockWidth, newBlockHeight, m_step, m_maxZStep, x1, y1, z1, z2, grid, multiheights.ToArray());
compiledNavMesh.Validate();
return(compiledNavMesh);
}
// TODO - need to avoid creating lines that cross blocked node edges.
private List <Vector3> reducePath(List <Vector3> original)
{
if (original.Count < 2)
{
if (original.Count > 0)
{
original.RemoveAt(0); // remove redundant starting point
}
return(original);
}
var result = new List <Vector3>();
int lastDirection = 0;
int primaryDir = 0;
int secondaryDir = 0;
int majorDir = 0; // this is the direction that is allowed to repeat.
float previousDeltaZ = Math.Abs(original[0].Z - original[1].Z);
for (int i = 2; ; i++)
{
if (i >= original.Count)
{
result.Add(original.Last());
break;
}
Vector3 previousPoint = original[i - 1];
Vector3 currentPoint = original[i];
int direction = NavMeshUtil.determineDirectionFromWorld(original[i - 1], original[i]);
float currentDeltaZ = Math.Abs(original[i].Z - original[i - 1].Z);
bool emit = true;
// if no major Z difference, check for continuous line.
if (Math.Abs(currentDeltaZ - previousDeltaZ) < 0.2f) // TODO - tune this number - check height/smoothness in client.
{
emit = false;
if (lastDirection == 0)
{
// first time through
primaryDir = direction;
}
else if (secondaryDir == 0 && NavMeshUtil.isDirectionAdjacent(lastDirection, direction))
{
// init secondary on first adjacent step
secondaryDir = direction;
}
else if (majorDir == 0 && lastDirection == direction)
{
// init major dir on first double sequence
majorDir = direction;
}
else if ((direction == majorDir) ||
(direction == primaryDir && lastDirection == secondaryDir) ||
(direction == secondaryDir && lastDirection == primaryDir))
{
// allow step
}
else
{
emit = true;
}
}
lastDirection = direction;
previousDeltaZ = currentDeltaZ;
if (emit)
{
// emit the previous point.
result.Add(previousPoint);
primaryDir = 0;
secondaryDir = 0;
majorDir = 0;
}
}
return(result);
}
public Vector3 WorldPointFromNode(NodeId node)
{
return(WorldFromBlockIndex(node.blockIndex, NavMeshUtil.DecodeHeight(m_z1, m_z2, node.encZ)));
}
public NodeId GetNeighbor(NodeId start, int offX, int offY)
{
return(GetNeighbor(start, NavMeshUtil.DetermineDirection(0, 0, offX, offY)));
}
public float GetRealHeight(int z1, int z2)
{
return(NavMeshUtil.DecodeHeight(z1, z2, encZ));
}
// WARNING! on failure, m_floorData will be left in a bad state.
private void FixDirectionFlagsToMatchSubgraphs(Dictionary <EdgeVertex, List <EdgeVertex> > edges,
List <HashSet <EdgeVertex> > subgraphs)
{
// mark all directions as blocked, then for each subgraph
// visit edges/vertices and unblock directions.
// clear all to 'blocked'
foreach (var fl in m_floorData)
{
if (fl != null)
{
for (int i = 0; i < fl.Count; i++)
{
fl[i] = new NavMeshBuilderFloorDesc(fl[i].Z100i, 0xFF);
}
}
}
foreach (var sg in subgraphs)
{
foreach (var v in sg)
{
var neighborEdges = edges[v];
foreach (var n in neighborEdges)
{
var dirToNeighbor = NavMeshUtil.DetermineDirection(v.BX, v.BY, n.BX, n.BY);
var dirFromNeighbor = NavMeshUtil.GetInverseDirection(dirToNeighbor);
// update 'to' neighbor
// need to find which floor data entry is mine
var meContainedInList = m_floorData[v.BY * m_blockWidth + v.BX];
// next, find me
bool foundMe = false;
for (int j = 0; j < meContainedInList.Count; j++)
{
if (meContainedInList[j].Z100i == v.Z100i)
{
// unblock me to neighbor
meContainedInList[j] = new NavMeshBuilderFloorDesc(v.Z100i,
(byte)(meContainedInList[j].DirectionFlags & ~dirToNeighbor));
foundMe = true;
break;
}
}
if (!foundMe)
{
throw new InvalidOperationException("vertex should exist in floor data!");
}
// update 'from' neighbor
var neighborContainedInList = m_floorData[n.BY * m_blockWidth + n.BX];
bool foundNeighbor = false;
for (int j = 0; j < neighborContainedInList.Count; j++)
{
if (neighborContainedInList[j].Z100i == n.Z100i)
{
// unblock neighbor to me
neighborContainedInList[j] = new NavMeshBuilderFloorDesc(n.Z100i,
(byte)(neighborContainedInList[j].DirectionFlags & ~dirFromNeighbor));
foundNeighbor = true;
break;
}
}
if (!foundNeighbor)
{
throw new InvalidOperationException("neighbor vertex should exist in floor data!");
}
}
}
}
}
