public bool FindPath()
{
// Debug.Log ("PathFound"+PathFound.ToString());
qpNode start_node = qpManager.Instance.nodes[0];
qpNode end_node = qpManager.Instance.nodes[0];
for (int i = 0; i < qpManager.Instance.nodes.Count; i++)
{
//Debug.Log (qpManager.Instance.nodes[i].GetCoordinate ().ToString());
if (qpManager.Instance.nodes[i].GetCoordinate() == new Vector3(0.5f, 0.5f, 0.1f))
{
start_node = qpManager.Instance.nodes[i];
}
if (qpManager.Instance.nodes[i].GetCoordinate() == new Vector3(9.5f, 9.5f, 0.1f))
{
end_node = qpManager.Instance.nodes[i];
}
}
List <qpNode> a = AStar(start_node, end_node);
Debug.Log(start_node.GetCoordinate().ToString());
Debug.Log(end_node.GetCoordinate().ToString());
if (a.Count == 0)
{
return(false);
}
return(true);
}
/// <summary>
/// Update is called once per frame
/// </summary>
protected void FixedUpdate()
{
_verifyNodes();
_move();
if (DrawPathInEditor && Path.Count > 0)
{
for (int i = 1; i < Path.Count; i++)
{
qpNode prevNode = Path[i - 1];
Debug.DrawLine(prevNode.GetCoordinate() + Offset, Path[i].GetCoordinate() + Offset, Color.red, 0, false);
}
}
}
private List <qpNode> _findNodesNear(qpNode target, float radius)
{
List <qpNode> retList = new List <qpNode>();
foreach (qpNode node in nodes)
{
if (target != node && Vector3.Distance(target.GetCoordinate(), node.GetCoordinate()) < radius)
{
//Debug.Log("node found at:" + Vector3.Distance(target.transform.position, node.transform.position));
retList.Add(node);
}
}
return(retList);
}
/// <summary>
/// Used for A*
/// </summary>
public float CalculateG(qpNode parent)
{
return(Vector3.Distance(_coordinate, parent.GetCoordinate()));
}
/// <summary>
/// Used for A*
/// </summary>
public float CalculateH(qpNode end)
{
return(Vector3.Distance(_coordinate, end.GetCoordinate()));
}
/// <summary>
/// Performs an A* algorithm
/// </summary>
/// <param name="start">Starting node</param>
/// <param name="end">Destination Node</param>
/// <returns>The fastest path from start node to the end node</returns>
protected List <qpNode> AStar(qpNode start, qpNode end)
{
Debug.Log("astar from " + start.GetCoordinate() + " to " + end.GetCoordinate());
List <qpNode> path = new List <qpNode>(); //will hold the final path
bool complete = (end == null || start == null) ? true : false; //Regulates the main while loop of the algorithm
List <qpNode> closedList = new List <qpNode>(); //Closed list for the best candidates.
List <qpNode> openList = new List <qpNode>(); //Open list for all candidates(A home for all).
qpNode candidate = start; //The current node candidate which is being analyzed in the algorithm.
openList.Add(start); //Start node is added to the openlist
if (start == null || end == null)
{
return(null); //algoritmen cannot be executed if either start or end node are null.
}
int astarSteps = 0;
while (openList.Count > 0 && !complete) //ALGORITHM STARTS HERE.
{
astarSteps++;
if (candidate == end) //If current candidate is end, the algorithm has been completed and the path can be build.
{
DestinationNode = end;
complete = true;
bool pathComplete = false;
qpNode node = end;
while (!pathComplete)
{
path.Add(node);
if (node == start)
{
pathComplete = true;
}
node = node.GetParent();
}
}
List <qpNode> allNodes = (DiagonalMovement ? candidate.Contacts : candidate.NonDiagonalContacts);
List <qpNode> potentialNodes = new List <qpNode>();
foreach (qpNode n in allNodes)
{
if (n.traverseable)
{
potentialNodes.Add(n);
}
}
foreach (qpNode n in potentialNodes)
{
bool inClosed = closedList.Contains(n);
bool inOpen = openList.Contains(n);
//Mark candidate as parent if not in open nor closed.
if (!inClosed && !inOpen)
{
n.SetParent(candidate);
openList.Add(n);
}
//But if in open, then calculate which is the better parent: Candidate or current parent.
else if (inOpen)
{
float math2 = n.GetParent().GetG();
float math1 = candidate.GetG();
if (math2 > math1)
{
//candidate is the better parent as it has a lower combined g value.
n.SetParent(candidate);
}
}
}
//Calculate h, g and total
if (openList.Count == 0)
{
break;
}
openList.RemoveAt(0);
if (openList.Count == 0)
{
break;
}
//the below one-lined for loop,if conditional and method call updates all nodes in openlist.
for (int i = 0; i < openList.Count; i++)
{
openList[i].CalculateTotal(start, end);
}
openList.Sort(delegate(qpNode node1, qpNode node2)
{
return(node1.GetTotal().CompareTo(node2.GetTotal()));
});
candidate = openList[0];
closedList.Add(candidate);
}
Debug.Log("astar completed in " + astarSteps + " steps. Path found:" + complete);
path.Reverse();
return(path);
}
