public void RemoveUnWalkableNodesAndEdges()
{
float RayLength = 2.0f;
var NodesToRemove = new List <int>();
var EdgesToRemove = new List <UnityEdge>();
foreach (var Node in Graph.Nodes)
{
RaycastHit Hit;
var GraphNode = Graph.GetNode(Node.NodeIndex);
var Origin = GraphNode.Position + (Vector3.up * RayLength);
var bHitSomething = Physics.SphereCast(Origin, WalkableRadius, Vector3.down, out Hit, RayLength, WallLayerMask);
if (bHitSomething)
{
NodesToRemove.Add(Node.NodeIndex);
Debug.DrawRay(Origin, Vector3.down * RayLength, RayHitColor, 10);
}
else
{
Debug.DrawRay(Origin, Vector3.down * RayLength, DefaultRayColor, 5);
// Check to see if we can move to the end of any edges attached to this node
foreach (var Edge in Graph.Edges[Node.NodeIndex])
{
var FromNode = Graph.GetNode(Edge.FromNodeIndex);
var ToNode = Graph.GetNode(Edge.ToNodeIndex);
var FromNodePosition = FromNode.Position;
FromNodePosition.y += 1;
var Direction = Vector3.Normalize(ToNode.Position - FromNode.Position);
var bHitWall = Physics.CapsuleCast(FromNodePosition, FromNodePosition + Vector3.up * 0.25f, WalkableRadius, Direction, 1.25f, WallLayerMask);
if (bHitWall)
{
EdgesToRemove.Add(Graph.GetEdge(FromNode.NodeIndex, ToNode.NodeIndex));
EdgesToRemove.Add(Graph.GetEdge(ToNode.NodeIndex, FromNode.NodeIndex));
}
}
foreach (var Edge in EdgesToRemove)
{
Graph.RemoveEdge(Edge.FromNodeIndex, Edge.ToNodeIndex);
Graph.RemoveEdge(Edge.ToNodeIndex, Edge.FromNodeIndex);
}
}
}
foreach (var NodeIndex in NodesToRemove)
{
Graph.RemoveNode(NodeIndex);
}
}