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