public float heuristic_cost_estimate(NodeSquare start, NodeSquare goal)
{
float distance = Mathf.Sqrt(Mathf.Pow(goal.getLoc().x - start.getLoc().x, 2) + Mathf.Pow(goal.getLoc().z - start.getLoc().z, 2));
return(distance);
}
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);
}
public List <NodeSquare> adjacent(NodeSquare current)
{
List <NodeSquare> adj = new List <NodeSquare>();
int x = (int)current.getLoc().x;
int z = (int)current.getLoc().z;
int max_x = nodes.GetLength(0);
int max_z = nodes.GetLength(1);
if (x - 1 >= 0 && z - 1 >= 0)
{
if (nodes[x - 1, z - 1].isPassable)
{
adj.Add(nodes [x - 1, z - 1]);
}
}
if (z - 1 >= 0)
{
if (nodes[x, z - 1].isPassable)
{
adj.Add(nodes [x, z - 1]);
}
}
if (x + 1 < max_x && z - 1 >= 0)
{
if (nodes[x + 1, z - 1].isPassable)
{
adj.Add(nodes [x + 1, z - 1]);
}
}
if (x - 1 >= 0)
{
if (nodes[x - 1, z].isPassable)
{
adj.Add(nodes [x - 1, z]);
}
}
if (x + 1 < max_x)
{
if (nodes[x + 1, z].isPassable)
{
adj.Add(nodes [x + 1, z]);
}
}
if (x - 1 >= 0 && z + 1 < max_z)
{
if (nodes[x - 1, z + 1].isPassable)
{
adj.Add(nodes [x - 1, z + 1]);
}
}
if (z + 1 < max_z)
{
if (nodes[x, z + 1].isPassable)
{
adj.Add(nodes [x, z + 1]);
}
}
if (x + 1 < max_x && z + 1 < max_z)
{
if (nodes[x + 1, z + 1].isPassable)
{
adj.Add(nodes [x + 1, z + 1]);
}
}
return(adj);
}
