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