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!"); } } } } }