private bool lineOfSight(NodeSquare origin, NodeSquare destination)
{
return(!Physics.Raycast(origin.getRenderLoc(), (destination.getRenderLoc() - origin.getRenderLoc()).normalized));
}
private float distance(NodeSquare origin, NodeSquare destination)
{
return(Vector3.Distance(origin.getRenderLoc(), destination.getRenderLoc()));
}
public List <NodeSquare> rrt(NodeSquare start, NodeSquare goal)
{
Debug.Log("Starting RRT");
List <NodeSquare> solution = null;
float solutionDistance = 0;
List <NodeSquare> nodeList = new List <NodeSquare> ();
for (int i = 0; i < nodes.GetLength(0); i++)
{
for (int j = 0; j < nodes.GetLength(1); j++)
{
nodeList.Add(nodes[i, j]);
}
}
int nodeCount = nodeList.Count;
Dictionary <NodeSquare, NodeSquare> startEdges = new Dictionary <NodeSquare, NodeSquare> ();
HashSet <NodeSquare> startTree = new HashSet <NodeSquare> ();
startTree.Add(start);
bool success = false;
while (success == false || nodeList.Count > (nodeCount / 2))
{
int index = Random.Range(0, nodeList.Count);
NodeSquare rnd = nodeList [index];
if (!rnd.isPassable)
{
nodeList.Remove(rnd);
continue;
}
Debug.Log(nodeList.Count.ToString() + rnd.getLoc().ToString());
NodeSquare nearestStart = null;
foreach (NodeSquare node in startTree)
{
if (lineOfSight(node, rnd))
{
if (nearestStart == null || distance(rnd, node) < distance(rnd, nearestStart))
{
nearestStart = node;
}
}
}
if (nearestStart != null)
{
startTree.Add(rnd);
startEdges.Add(rnd, nearestStart);
nodeList.Remove(rnd);
Debug.DrawLine(nearestStart.getRenderLoc(), rnd.getRenderLoc(), Color.red, float.PositiveInfinity, false);
if (lineOfSight(rnd, goal))
{
List <NodeSquare> path = buildPath(startEdges, start, goal, rnd);
float pathDist = pathDistance(path);
if (solutionDistance == 0 || pathDist < solutionDistance)
{
solution = path;
solutionDistance = pathDist;
}
success = true;
Debug.DrawLine(rnd.getRenderLoc(), goal.getRenderLoc(), Color.red, float.PositiveInfinity, false);
}
}
}
return(solution);
}
